SteamVR_IK.cs

//======= Copyright (c) Valve Corporation, All rights reserved. ===============
//
// Purpose: Simple two bone ik solver.
//
//=============================================================================

using UnityEngine;

public class SteamVR_IK : MonoBehaviour
{
    public Transform target;
    public Transform start, joint, end;
    public Transform poleVector, upVector;

    public float blendPct = 1.0f;

    [HideInInspector]
    public Transform startXform, jointXform, endXform;

    void LateUpdate()
    {
        const float epsilon = 0.001f;
        if (blendPct < epsilon)
            return;

        var preUp = upVector ? upVector.up : Vector3.Cross(end.position - start.position, joint.position - start.position).normalized;

        var targetPosition = target.position;
        var targetRotation = target.rotation;

        Vector3 forward, up, result = joint.position;
        Solve(start.position, targetPosition, poleVector.position,
            (joint.position - start.position).magnitude,
            (end.position - joint.position).magnitude,
            ref result, out forward, out up);

        if (up == Vector3.zero)
            return;

        var startPosition = start.position;
        var jointPosition = joint.position;
        var endPosition = end.position;

        var startRotationLocal = start.localRotation;
        var jointRotationLocal = joint.localRotation;
        var endRotationLocal = end.localRotation;

        var startParent = start.parent;
        var jointParent = joint.parent;
        var endParent = end.parent;

        var startScale = start.localScale;
        var jointScale = joint.localScale;
        var endScale = end.localScale;

        if (startXform == null)
        {
            startXform = new GameObject("startXform").transform;
            startXform.parent = transform;
        }

        startXform.position = startPosition;
        startXform.LookAt(joint, preUp);
        start.parent = startXform;

        if (jointXform == null)
        {
            jointXform = new GameObject("jointXform").transform;
            jointXform.parent = startXform;
        }

        jointXform.position = jointPosition;
        jointXform.LookAt(end, preUp);
        joint.parent = jointXform;

        if (endXform == null)
        {
            endXform = new GameObject("endXform").transform;
            endXform.parent = jointXform;
        }

        endXform.position = endPosition;
        end.parent = endXform;

        startXform.LookAt(result, up);
        jointXform.LookAt(targetPosition, up);
        endXform.rotation = targetRotation;

        start.parent = startParent;
        joint.parent = jointParent;
        end.parent = endParent;

        end.rotation = targetRotation; // optionally blend?

        // handle blending in/out
        if (blendPct < 1.0f)
        {
            start.localRotation = Quaternion.Slerp(startRotationLocal, start.localRotation, blendPct);
            joint.localRotation = Quaternion.Slerp(jointRotationLocal, joint.localRotation, blendPct);
            end.localRotation = Quaternion.Slerp(endRotationLocal, end.localRotation, blendPct);
        }

        // restore scale so it doesn't blow out
        start.localScale = startScale;
        joint.localScale = jointScale;
        end.localScale = endScale;
    }

    public static bool Solve(
        Vector3 start, // shoulder / hip
        Vector3 end, // desired hand / foot position
        Vector3 poleVector, // point to aim elbow / knee toward
        float jointDist, // distance from start to elbow / knee
        float targetDist, // distance from joint to hand / ankle
        ref Vector3 result, // original and output elbow / knee position
        out Vector3 forward, out Vector3 up) // plane formed by root, joint and target
    {
        var totalDist = jointDist + targetDist;
        var start2end = end - start;
        var poleVectorDir = (poleVector - start).normalized;
        var baseDist = start2end.magnitude;

        result = start;

        const float epsilon = 0.001f;
        if (baseDist < epsilon)
        {
            // move jointDist toward jointTarget
            result += poleVectorDir * jointDist;

            forward = Vector3.Cross(poleVectorDir, Vector3.up);
            up = Vector3.Cross(forward, poleVectorDir).normalized;
        }
        else
        {
            forward = start2end * (1.0f / baseDist);
            up = Vector3.Cross(forward, poleVectorDir).normalized;

            if (baseDist + epsilon < totalDist)
            {
                // calculate the area of the triangle to determine its height
                var p = (totalDist + baseDist) * 0.5f; // half perimeter
                if (p > jointDist + epsilon && p > targetDist + epsilon)
                {
                    var A = Mathf.Sqrt(p * (p - jointDist) * (p - targetDist) * (p - baseDist));
                    var height = 2.0f * A / baseDist; // distance of joint from line between root and target

                    var dist = Mathf.Sqrt((jointDist * jointDist) - (height * height));
                    var right = Vector3.Cross(up, forward); // no need to normalized - already orthonormal

                    result += (forward * dist) + (right * height);
                    return true; // in range
                }
                else
                {
                    // move jointDist toward jointTarget
                    result += poleVectorDir * jointDist;
                }
            }
            else
            {
                // move elboDist toward target
                result += forward * jointDist;
            }
        }

        return false; // edge cases
    }
}