0

I have tried to make a Inverse Kinematic formula in C# with Unity although I cannot seem to get work. The join moves when i move the target (purple point on second image) but they do not move correctly to the target. I believe the formulas are right, and have a feeling i messed up with radians and degrees, and maybe with the Quaternions or the coordinate system.

The formulas

Inverse kinematic result

public class IK : MonoBehaviour
{
    public double A1, A2, A3;
    
    public Transform J1, J2, J3;
    public Transform target;

    void Update()
    {
        InverseKinematic();
        //DoInverseKinematic();
    }

    void InverseKinematic()
    {
        double x = target.position.x;
        double y = target.position.y;
        double z = target.position.z;

        double T1 = Math.Atan(y / x);

        double R1 = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
        double R2 = z - A1;

        double P2 = Math.Atan(R2 / R1) * Mathf.Rad2Deg;

        double R3 = Math.Sqrt(Math.Pow(R1, 2) + Math.Pow(R2, 2));

        double P1 = Math.Acos((Math.Pow(A3, 2) - Math.Pow(A2, 2) - Math.Pow(R3, 2)) / (-2*A2*R3));

        double T2 = P2 - P1;

        double P3 = Math.Acos((Math.Pow(R3, 2) - Math.Pow(A2, 2) - Math.Pow(A3, 2)) / (-2 * A2 * A3));

        double T3 = 180 - P3;
        
        Debug.Log(T1 + " - " + T2 + " - " + T3);

        J1.rotation = Quaternion.AngleAxis((float)T1 * Mathf.Rad2Deg, Vector3.up);
        J2.rotation = Quaternion.AngleAxis((float)T2 * Mathf.Rad2Deg, Vector3.forward);
        J3.rotation = Quaternion.AngleAxis((float)T3 * Mathf.Rad2Deg, Vector3.forward);

    }
  • Hi Olav- nice geometry diagram! It looks like you're retrieving a value in radians from Math.Acos, but after that you're subtracting that value from 180 degrees. Try subtracting it from PI instead. – gremto Jul 11 '23 at 08:39

0 Answers0