How To Code Two Bone IK in Unity

You might be aware that I usually post tutorials on VR game mechanics, so why would I write one on Inverse Kinematics of all things? Well, one of the said VR game mechanics I’ve been trying to program is body tracking (or more accurately body estimation) and to do that you need at least good two bone IK. So I looked into existing solutions and found that all of the free IK assets are… lacking. And no-one wants to pay $90 for Final IK. Luckily I’m here to give my hard work out for free.

The Code

using UnityEngine;

[ExecuteInEditMode]
public class IK : MonoBehaviour
{
    public Transform Upper;//root of upper arm
    public Transform Lower;//root of lower arm
    public Transform End;//root of hand
    public Transform Target;//target position of hand
    public Transform Pole;//direction to bend towards 
    public float UpperElbowRotation;//Rotation offsetts
    public float LowerElbowRotation;

    private float a;//values for use in cos rule
    private float b;
    private float c;
    private Vector3 en;//Normal of plane we want our arm to be on
    
    void Update()
    { 
        a = Lower.localPosition.magnitude;
        b = End.localPosition.magnitude;
        c = Vector3.Distance(Upper.position, Target.position);
        en = Vector3.Cross(Target.position-Upper.position, Pole.position-Upper.position);
        Debug.Log("The angle is: " + CosAngle(a,b,c));
        Debug.DrawLine(Upper.position,Target.position);
        Debug.DrawLine((Upper.position+Target.position)/2,Lower.position);

        //Set the rotation of the upper arm
        Upper.rotation = Quaternion.LookRotation(Target.position-Upper.position, Quaternion.AngleAxis(UpperElbowRotation, Lower.position - Upper.position) * (en));
        Upper.rotation *= Quaternion.Inverse(Quaternion.FromToRotation(Vector3.forward, Lower.localPosition));
        Upper.rotation=  Quaternion.AngleAxis(-CosAngle(a, c, b), -en)*Upper.rotation;

        //set the rotation of the lower arm
        Lower.rotation =  Quaternion.LookRotation(Target.position - Lower.position, Quaternion.AngleAxis(LowerElbowRotation, End.position - Lower.position) * (en));
        Lower.rotation *= Quaternion.Inverse(Quaternion.FromToRotation(Vector3.forward, End.localPosition));

        //Lower.LookAt(Lower, Pole.position - Upper.position);
        //Lower.rotation = Quaternion.AngleAxis(CosAngle(a, b, c), en);
    }

    //function that finds angles using the cosine rule 
    float CosAngle(float a, float b, float c) {
        if ( !float.IsNaN(Mathf.Acos((-(c * c) + (a * a) + (b * b)) / (-2 * a * b)) * Mathf.Rad2Deg))
        {
            return Mathf.Acos((-(c * c) + (a * a) + (b * b)) / (2 * a * b)) * Mathf.Rad2Deg;
        }
        else
        {
            return 1;
        }
    }
}

Now, this might look like a bunch of gibberish but the concept actually pretty simple. First It rotates the upper bone to point towards the target with the correct rotation relative to the pole, then using the cosine rule it finds the correct amount of bend to apply to the arm. Then after it finds that we rotate the upper arm by that amount using the normal of the plane made by the target, shoulder, and pole. After that, we just rotate the lower arm to look towards the target and we are done! If you want to make it so that the end rotates with the target, you can add that yourself with like one line of code.

Implementation

I built this code as a drag and drop solution, so all you need is a chain of three-game objects for the arm and a target and pole object. Drag and drop them in and it should just work. If the arm rotation is wrong just tweak the respective values and that should just work perfectly.

Liked it? Take a second to support WireWhiz on Patreon!