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.
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);
}