void Update()
{
// Copy the joints positions to work with
for (int i = 0; i < joints.Length; i++)
{
copy[i] = new MyVec(joints[i].position.x, joints[i].position.y, joints[i].position.z); //Copy the joints
if (i < joints.Length - 1)
{
distances[i] = MyVec.Distance(joints[i + 1].position, joints[i].position); //Calculate the distances
}
}
done = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude < treshold_condition;
if (!done)
{
float targetRootDist = MyVec.Distance(copy[0], new MyVec(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 MyVec(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 MyVec(target.position.x, target.position.y, target.position.z) * lambda;
}
}
else
{
MyVec b = copy[0];
float difA = (copy[copy.Length - 1] - new MyVec(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 MyVec(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 MyVec(target.position.x, target.position.y, target.position.z)).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i < joints.Length - 1; i++)
{
//TODO
MyVec A = new MyVec(joints[i + 1].position - joints[i].position);
MyVec B = copy[i + 1] - copy[i];
float cosa = MyVec.Dot(MyVec.Normalize(A), MyVec.Normalize(B));
float sina = MyVec.Cross(MyVec.Normalize(A), MyVec.Normalize(B)).magnitude;
float alpha = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
MyVec myAxis = MyVec.Normalize(MyVec.Cross(A, B));
//Vector3 axis = new Vector3(myAxis.x, myAxis.y, myAxis.z);
MyQuat myQuat = MyQuat.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;
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
if (i == 2)
{
// print(joints[i].rotation.z);
}
if ((joints[i].rotation.z > 0.5f || joints[i].rotation.z < -0.5f) && i > 0)
{
joints[i].rotation = joints[i - 1].rotation;
//joints[i].rotation = new Quaternion(joints[i].rotation.x, joints[i].rotation.y, , joints[i - 1].rotation.w);
}
}
}
}
void Update()
{
// 1.Copy the joints positions to work with.
//Calculate also the distances between joints
//TODO1
Vec3 tpos = new Vec3(target.position);
copy[0] = new Vec3(joints[0].position);
for (int i = 0; i < copy.Length - 1; i++)
{
Vec3 temp = new Vec3(joints[i + 1].position);
copy[i + 1] = temp;
distances[i] = Vec3.Distance(copy[i + 1], copy[i]);
}
//done = TODO2
done = Vec3.Mod(copy[copy.Length - 1] - tpos) < treshold_condition;
if (!done)
{
float targetRootDist = Vec3.Distance(copy[0], tpos);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
//TODO3
for (int i = 0; i < copy.Length - 1; i++)
{
float r = Vec3.Distance(tpos, copy[i]);
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * tpos);
}
}
else
{
float comvulguis = Vec3.Distance(tpos, copy[copy.Length - 1]);
Vec3 b = copy[0];
int iter = 0;
// The target is reachable
while (comvulguis > treshold_condition && iter < maxIterations)
{
iter++;
// STAGE 1: FORWARD REACHING
//TODO
//Debug.Log("FABRIK iteration:" + iter);
copy[copy.Length - 1] = tpos;
for (int i = copy.Length - 2; i > 0; i--)
{
float r = Vec3.Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / r;
copy[i] = (1 - lambda) * copy[i + 1] + (lambda * copy[i]);
}
// STAGE 2: BACKWARD REACHING
//TODO
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = Vec3.Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * copy[i + 1]);
}
comvulguis = Vec3.Distance(copy[copy.Length - 1], tpos);
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
//TODO4
//without rotations of the different pieces:
//joints[i + 1].position = copy[i + 1];
//with rotations of the different pieces:
Vec3 temp1 = new Vec3(joints[i + 1].position);
Vec3 temp2 = new Vec3(joints[i].position);
Vec3 init = temp1 - temp2;
Vec3 now = copy[i + 1] - copy[i];
//float angle = Mathf.Acos(Vector3.Dot(init.normalized, now.normalized))*Mathf.Rad2Deg;
float cosa = Vec3.Dot(Vec3.Normalize(init), Vec3.Normalize(now));
float sina = Vec3.Mod(Vec3.Cross(Vec3.Normalize(init), Vec3.Normalize(now)));
float angle = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
Vec3 axis = Vec3.Normalize(Vec3.Cross(init, now));
MyQuat temp = new MyQuat(joints[i].rotation);
MyQuat q = MyQuat.AngleAxis(angle, axis) * temp;
Quaternion trueQ = new Quaternion(q.x, q.y, q.z, q.w);
joints[i].rotation = trueQ;
Vector3 v = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
joints[i + 1].position = v;
}
}
}
