private static void RunTests()
{
Debug.Log(new Quaternion(0, 1, 2, 3) * new Quaternion(0, 1, 2, 3));
Debug.Log(new MyQuaternion(0, 1, 2, 3) * new MyQuaternion(0, 1, 2, 3));
Debug.Log(Quaternion.AngleAxis(90f, Vector3.forward));
Debug.Log(MyQuaternion.AngleAxis(90f, Vector3.forward));
Debug.Log(Quaternion.AngleAxis(75f, Vector3.forward + Vector3.up));
Debug.Log(MyQuaternion.AngleAxis(75f, Vector3.forward + Vector3.up));
Debug.Log(Quaternion.Euler(new Vector3(0f, 0f, 45f)));
Debug.Log(MyQuaternion.Euler(new Vector3(0f, 0f, 45f)));
}
void Update()
{
// Copy the joints positions to work with
for (int i = 0; i < joints.Length; i++)
{
copy[i] = new MyVector3(joints[i].position.x, joints[i].position.y, joints[i].position.z); //Copy the joints
if (i < joints.Length - 1)
{
distances[i] = MyVector3.Distance(joints[i + 1].position, joints[i].position); //Calculate the distances
}
}
done = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude < treshold_condition;
if (!done)
{
float targetRootDist = MyVector3.Distance(copy[0], new MyVector3(target.position.x, target.position.y, target.position.z));
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (new MyVector3(target.position.x, target.position.y, target.position.z) - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + new MyVector3(target.position.x, target.position.y, target.position.z) * lambda;
}
}
else
{
MyVector3 b = copy[0];
float difA = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude;
// The target is reachable
while (difA > treshold_condition)
{
// STAGE 1: FORWARD REACHING
copy[copy.Length - 1] = new MyVector3(target.position.x, target.position.y, target.position.z);
for (int i = copy.Length - 2; i > 0; i--)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i] = copy[i + 1] * (1 - lambda) + copy[i] * lambda;
}
// STAGE 2: BACKWARD REACHING
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + copy[i + 1] * lambda;
}
difA = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
MyQuaternion parentQuat = new MyQuaternion(joints[i + 1].rotation);
MyQuaternion myQuat = new MyQuaternion(joints[i].rotation);
MyVector3 A = new MyVector3(joints[i + 1].position - joints[i].position);
MyVector3 B = copy[i + 1] - copy[i];
float cosa = MyVector3.Dot(MyVector3.Normalize(A), MyVector3.Normalize(B));
float sina = MyVector3.Cross(MyVector3.Normalize(A), MyVector3.Normalize(B)).magnitude;
float alpha = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
MyVector3 myAxis = MyVector3.Normalize(MyVector3.Cross(A, B));
//Vector3 axis = new Vector3(myAxis.x, myAxis.y, myAxis.z);
myQuat = MyQuaternion.AngleAxis(alpha, ref myAxis);
Quaternion quat = new Quaternion(myQuat.x, myQuat.y, myQuat.z, myQuat.w);
joints[i].rotation = quat * joints[i].rotation;
//joints[i].rotation = Quaternion.AngleAxis(alpha, axis) * joints[i].rotation;
myQuat = new MyQuaternion(joints[i].rotation);
float localAngle = MyQuaternion.Angle(parentQuat, myQuat);
if (Mathf.Abs(localAngle) > maxRotation)
{
joints[i + 1].rotation = joints[i].rotation;
}
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
}
}
}
// Running the solver - all the joints are iterated through once every frame
void Update()
{
// if the target hasn't been reached
if (!done)
{
// if the Max number of tries hasn't been reached
if (tries <= Mtries)
{
// starting from the second last joint (the last being the end effector)
// going back up to the root
for (int i = joints.Length - 2; i >= 0; i--)
{
// The vector from the ith joint to the end effector
MyVector3 r1 = new MyVector3(joints[joints.Length - 1].position - joints[i].position);
// The vector from the ith joint to the target
MyVector3 r2 = tpos - new MyVector3(joints[i].position);
// to avoid dividing by tiny numbers
if (r1.magnitude * r2.magnitude <= 0.001f)
{
cos[i] = 1.0f;
sin[i] = 0.0f;
}
else
{
// find the components using dot and cross product
cos[i] = MyVector3.Dot(r1, r2) / (r1.magnitude * r2.magnitude);
sin[i] = MyVector3.Cross(r1, r2).magnitude / (r1.magnitude * r2.magnitude);
}
// The axis of rotation
MyVector3 axis = MyVector3.Cross(r1, r2);
// find the angle between r1 and r2 (and clamp values if needed avoid errors)
theta[i] = Mathf.Acos(cos[i]);
//Optional. correct angles if needed, depending on angles invert angle if sin component is negative
if (sin[i] < 0)
{
theta[i] *= -1;
}
// obtain an angle value between -pi and pi, and then convert to degrees
theta[i] *= Mathf.Rad2Deg;
// rotate the ith joint along the axis by theta degrees in the world space.
MyQuaternion quat = MyQuaternion.AngleAxis(theta[i], ref axis);
Quaternion fQuat = new Quaternion(quat.x, quat.y, quat.z, quat.w);
joints[i].rotation = fQuat * joints[i].rotation;
}
// increment tries
tries++;
}
}
// find the difference in the positions of the end effector and the target
float dif = (tpos - new MyVector3(joints[joints.Length - 1].position)).magnitude;
// if target is within reach (within epsilon) then the process is done
if (dif < epsilon)
{
done = true;
}
// if it isn't, then the process should be repeated
else
{
done = false;
}
// the target has moved, reset tries to 0 and change tpos
if (new MyVector3(targ.position) != tpos)
{
tries = 0;
tpos = new MyVector3(targ.position);
}
}
