private Vector3 BackwardReach(FreeJoint joint)
{
if (endEffectorToTargetMap.ContainsKey(joint))
{
/* This is an end effector, set it to the corresponding target position */
joint.position = endEffectorToTargetMap[joint];
ForwardReach(joint);
}
else if (joint.childCount == 0)
{
/* This chain has no end effectors in it. Don't change anything */
return(joint.Parent.position);
}
else
{
Vector3 centroid = Vector3.zero;
for (int i = 0; i < joint.childCount; i++)
{
FreeJoint child = joint.GetChild(i);
centroid += BackwardReach(child);
}
/* Set joint position as centroid of new positions calculated by this joint's children */
joint.position = centroid / joint.childCount;
}
if (joint.Parent != null)
{
Vector3 v = joint.Parent.position - joint.position;
Vector3 newParentPos = joint.position + jointLengthMap[joint] * v.normalized;
return(newParentPos);
}
return(Vector3.zero);
}
private void SetBonesTransformsToFitJoints(Transform trans, FreeJoint joint)
{
/*
* for (int i = 0; i < trans.childCount; i++)
* {
* Transform childTrans = trans.GetChild(i);
* FreeJoint childJoint = joint.GetChild(i);
* Vector3 currentOffset = trans.InverseTransformPoint(childTrans.position);
* Vector3 desiredOffset = trans.InverseTransformPoint(childJoint.position);
* trans.localRotation *= Quaternion.FromToRotation(currentOffset, desiredOffset);
* SetBonesTransformsToFitJoints(childTrans, childJoint);
* }
*/
Vector3 originalChildrenCentroid = Vector3.zero;
Vector3 newChildrenCentroid = Vector3.zero;
for (int i = 0; i < trans.childCount; i++)
{
Transform childTrans = trans.GetChild(i);
FreeJoint childJoint = joint.GetChild(i);
originalChildrenCentroid += trans.InverseTransformPoint(childTrans.position);
newChildrenCentroid += trans.InverseTransformPoint(childJoint.position);
}
trans.localRotation *= Quaternion.FromToRotation(originalChildrenCentroid, newChildrenCentroid);
for (int i = 0; i < trans.childCount; i++)
{
Transform childTrans = trans.GetChild(i);
FreeJoint childJoint = joint.GetChild(i);
SetBonesTransformsToFitJoints(childTrans, childJoint);
}
}
public FABRIKSolver(Transform rootTrans, Dictionary <Transform, Vector3> endEffectorTransToTargetMap)
{
this.rootTrans = rootTrans;
this.jointLengthMap = new Dictionary <FreeJoint, float>();
this.endEffectorToTargetMap = new Dictionary <FreeJoint, Vector3>();
this.root = CreateFreeJointTree(rootTrans, endEffectorTransToTargetMap);
}
private void ForwardReach(FreeJoint joint)
{
for (int i = 0; i < joint.childCount; i++)
{
FreeJoint child = joint.GetChild(i);
Vector3 v = child.position - joint.position;
Vector3 newChildPos = joint.position + jointLengthMap[child] * v.normalized;
child.position = newChildPos;
ForwardReach(child);
}
}
private FreeJoint CreateFreeJointTree(Transform trans, Dictionary <Transform, Vector3> endEffectorTransToTargetMap)
{
FreeJoint joint = new FreeJoint(trans.position);
for (int i = 0; i < trans.childCount; i++)
{
Transform childTrans = trans.GetChild(i);
FreeJoint childJoint = CreateFreeJointTree(childTrans, endEffectorTransToTargetMap);
childJoint.Parent = joint;
jointLengthMap[childJoint] = Vector3.Distance(childTrans.position, trans.position);
if (endEffectorTransToTargetMap.ContainsKey(childTrans))
{
endEffectorToTargetMap[childJoint] = endEffectorTransToTargetMap[childTrans];
}
}
return(joint);
}
