I struggle desperately with maths, to try and improve I've been trying to create an IK solution in Unity.
I have made a simple CCD algorithm which behaves as intended: https://www.youtube.com/watch?v=tZvdZGUxbbc
But when I try adding constraints to the joint angles the behaviour is innacurate and distorted: https://www.youtube.com/watch?v=k1YY9KqqaYY
Currently I have a for loop which iterates through all the joints in the chain while target is not reached and current attempts < attemptsthreshold.
Once the corrective rotation for each joint is calculated, I apply it to joint[i]. I then check the rotation at each joint, clamping it within it's min-max range.
Here is the code which applies rotation, then checks the rotation limits:
Vector3 vectorCurrJointToEndEffector = (joints[joints.Length - 1].transform.position - joints[i].transform.position).normalized; //normalised vector from current joint to end effector position
Vector3 vectorCurrJointToTarget = (target.transform.position - joints[i].transform.position).normalized; //normalised vector from current joint to targetposition
float cosineAngle = Vector3.Dot(vectorCurrJointToEndEffector, vectorCurrJointToTarget); //dot product gives the cosine of the angle for the corrective rotation
//limit possibilities
if (cosineAngle < 1)
{
Vector3 crossProduct = Vector3.Cross(vectorCurrJointToEndEffector, vectorCurrJointToTarget).normalized; //normalised cross product gives the axis on which to rotate the joint
float toRotateDegrees = Mathf.Acos(cosineAngle) * Mathf.Rad2Deg; //calculate joint rotation in degrees
joints[i].transform.rotation = Quaternion.AngleAxis(toRotateDegrees, crossProduct) * joints[i].transform.rotation; //apply joint rotation
joints[i].transform.rotation = Quaternion.AngleAxis(toRotateDegrees, crossProduct) * joints[i].transform.rotation; //apply joint rotation
//clamp current joint rotation within set limits
Vector3 currentJointEulerAngles = joints[i].joint.transform.localRotation.eulerAngles;
if (currentJointEulerAngles.x > 180f)
currentJointEulerAngles.x -= 360f;
currentJointEulerAngles.x = Mathf.Clamp(currentJointEulerAngles.x, joints[i].XaxisMin, joints[i].XaxisMax);
if (currentJointEulerAngles.y > 180f)
currentJointEulerAngles.y -= 360f;
currentJointEulerAngles.y = Mathf.Clamp(currentJointEulerAngles.y, joints[i].YaxisMin, joints[i].YaxisMax);
if (currentJointEulerAngles.z > 180f)
currentJointEulerAngles.z -= 360f;
currentJointEulerAngles.z = Mathf.Clamp(currentJointEulerAngles.z, joints[i].ZaxisMin, joints[i].ZaxisMax);
joints[i].joint.transform.localEulerAngles = currentJointEulerAngles;
As shown in the second video, the result is very poor.
Could anyone suggest a solution and possibly an explanation of why this does not work?
updated, corrected typo in pasted code.