// Apply constraints after Update
void LateUpdate()
{
int jointLevel = 0;
Transform T = transform;
Transform refT = reference;
Quaternion q = QuaternionUtils.RelativeRotation(rotations[jointLevel], refT.localRotation);
Vector3 axis;
float angle;
q.ToAngleAxis(out angle, out axis);
//Debug.Log("Angle: " + angle + " Axis: " + axis);
if (angle <= constraints[jointLevel])
{
T.localRotation = rotations[0] * Quaternion.AngleAxis(angle, axis);
//T.localRotation = refT.localRotation;
}
else
{
angle = Mathf.Clamp(angle, 0, constraints[jointLevel]);
T.localRotation = rotations[0] * Quaternion.AngleAxis(angle, axis);
}
T = T.GetChild(1);
refT = refT.GetChild(1);
jointLevel++;
while (T != null && T.childCount == 2)
{
Vector3 p0 = T.position;
Vector3 p1old = T.GetChild(1).position;
Vector3 p1new = refT.GetChild(1).position;
q = QuaternionUtils.RelativeRotation(rotations[jointLevel], refT.localRotation);
q.ToAngleAxis(out angle, out axis);
angle = (float)QuaternionUtils.NormalizeAngle(angle);
//Debug.Log("Angle: " + angle + " Axis: " + axis);
// if the rotation cumplies with the constraints, apply it
if (angle <= constraints[jointLevel] && angle >= -constraints[jointLevel])
{
//T.localRotation = Quaternion.AngleAxis(angle, axis);
T.localRotation = refT.localRotation;
//T.rotation = r * T.rotation;
}
// else clamp before aplying
else
{
angle = Mathf.Clamp(angle, -constraints[jointLevel], constraints[jointLevel]);
float x = Mathf.Clamp((float)QuaternionUtils.NormalizeAngle(q.eulerAngles.x),
-constraints[jointLevel], constraints[jointLevel]);
float y = Mathf.Clamp((float)QuaternionUtils.NormalizeAngle(q.eulerAngles.y),
-constraints[jointLevel], constraints[jointLevel]);
float z = Mathf.Clamp((float)QuaternionUtils.NormalizeAngle(q.eulerAngles.z),
-constraints[jointLevel], constraints[jointLevel]);
T.localEulerAngles = new Vector3(x, y, z);
//T.localRotation = Quaternion.AngleAxis(angle, axis);
//r = Quaternion.AngleAxis(diff, axis);
//T.rotation = r * T.rotation;
}
T = T.GetChild(1);
refT = refT.GetChild(1);
jointLevel++;
}
}
// Apply FABRIK forward setp on the joints[i] (with constraints)
private void ForwardReachingJoint(int i)
{
// Position of the current joint
Vector3 CurrentPos = joints[i].position;
// (Imaginary) Position of where the current joint's child should be if it wasn't moved
Vector3 CurrentEndPos = CurrentPos + distanceDir[i];
Debug.Assert(Vector3.Distance(CurrentEndPos, CurrentPos) - distanceDir[i].magnitude < Mathf.Epsilon);
// (Real) Position of the current joint's child
Vector3 ChildPos = joints[i + 1].position;
// Offset between the real and the imaginary positions
//Vector3 Offset = ChildPos - CurrentEndPos;
// Distance between current joint and its child real position
float CurrentToChildDist = Vector3.Distance(CurrentPos, ChildPos);
// Distance between current joint and its imaginary child (we already stored it, and should never change!)
float RealCurrentToChildDist = distanceDir[i].magnitude;
Debug.Assert(Vector3.Distance(CurrentPos, CurrentEndPos) - RealCurrentToChildDist < Mathf.Epsilon);
// Ratio between the 2 distances above (used to linearly interpolate)
float Ratio = RealCurrentToChildDist / CurrentToChildDist;
// Linear interpolation between the current position and its child
Vector3 CurrentNewPos = (1 - Ratio) * ChildPos + Ratio * CurrentPos;
// Rotation from the old joint orientation (towards Imaginary) to the new one (towards Real)
// Ref. (0)
Vector3 OldDir = CurrentEndPos - CurrentPos;
Vector3 NewDir = ChildPos - CurrentPos;
Quaternion Rotation = Quaternion.identity;
if (Vector3.Dot(OldDir.normalized, NewDir.normalized) < 1)
{
Rotation = Quaternion.FromToRotation(OldDir.normalized, NewDir.normalized);
}
// Ref. (1)
// Move child position (Real) to where the joint thinks it should be (Imaginary)
joints[i + 1].position = CurrentEndPos;
// Store current child's rotation, to restore it later
Quaternion ChildRot = joints[i + 1].rotation;
// Ref. (2)
// Rotate current joint so that the link is looking in the direction of the child position
joints[i].rotation = Rotation * joints[i].rotation;
// Position joint in its appropriate position (on the line pointing to the child)
joints[i].position = CurrentNewPos;
// Ref. (3)
// Restore child rotation
joints[i + 1].rotation = ChildRot;
distanceDir[i] = joints[i + 1].position - joints[i].position;
if (constraints[i + 1] != null)
{
// Store child rotation
ChildRot = joints[i + 1].rotation;
// Ref. (4)
// Apply constraint (on the child!)
constraints[i + 1].Constrain();
// Compute constraint quaternion from the current child's orientation and its previous orientation
Quaternion constraint = QuaternionUtils.RelativeRotation(ChildRot, joints[i + 1].rotation);
// Ref. (5)
// Restore child orientation, since we want to apply the constraint on the current joint
joints[i + 1].rotation = ChildRot;
// Apply constraint (on the parent), it moves the children positions away from the target
joints[i].rotation = joints[i].rotation * Quaternion.Inverse(constraint);
joints[i + 1].rotation = joints[i + 1].rotation * constraint;
// Ref. (6)
// Reposition current joint so that the end effector still matches the target position
Vector3 offset = joints[joints.Length - 1].position - target.position;
joints[i].position -= offset;
}
}