void Start()
{
if (!endEffector.transform.IsChildOf(rootJoint.transform))
{
Debug.Log("[IK.Solver] End effector is not a child of root joint.");
return;
}
List <IKJoint> joints = new List <IKJoint>();
IKJoint joint = rootJoint;
while (true)
{
joints.Add(joint);
_chainLen += joint.boneLength;
if (joint == endEffector)
{
break;
}
joint = joint.GetChildJoint();
}
_joints = joints.ToArray();
_solution = new Vector3[_joints.Length];
}
void Awake()
{
IKJoint childJoint = GetChildJoint();
if (childJoint)
{
_boneLength = Vector3.Magnitude(childJoint.transform.localPosition);
_refOrientation = Quaternion.LookRotation(childJoint.transform.localPosition);
}
}
void LateUpdate()
{
if (_joints == null)
{
return;
}
Vector3 startPos = rootJoint.transform.position;
Vector3 targetPos = target.position;
// initialize solution
for (int i = 0; i < _solution.Length; i++)
{
_solution[i] = _joints[i].transform.position;
}
// target unreachable
if (_chainLen < Vector3.Distance(targetPos, rootJoint.transform.position))
{
targetPos = rootJoint.transform.position + _chainLen * Vector3.Normalize(targetPos - rootJoint.transform.position);
}
// FABRIK iterations
for (int k = 0; k < maxIterations; k++)
{
// inward
_solution[_solution.Length - 1] = targetPos;
for (int i = _solution.Length - 1; i > 0; i--)
{
Vector3 v = Vector3.Normalize(_solution[i] - _solution[i - 1]);
_solution[i - 1] = _solution[i] - _joints[i - 1].boneLength * v;
}
// outward
_solution[0] = startPos;
for (int i = 1; i < _solution.Length; i++)
{
Vector3 v = Vector3.Normalize(_solution[i] - _solution[i - 1]);
_solution[i] = _solution[i - 1] + _joints[i - 1].boneLength * v;
}
if (Vector3.Distance(_solution[_solution.Length - 1], targetPos) < (tolerance + 0.001))
{
break;
}
}
// update joints
for (int i = 0; i < _solution.Length - 1; i++)
{
IKJoint joint = _joints[i];
joint.transform.rotation = Quaternion.LookRotation(_solution[i + 1] - joint.transform.position) * Quaternion.Inverse(joint.refOrientation);
}
}
