public void MultiJointSolve()
{
List <IK_Joint> chainRoot = fork_1.GetPathFromFork();
List <IK_Joint> chain1 = left_end.GetPathFromFork();
List <IK_Joint> chain2 = right_end.GetPathFromFork();
Vector3 chainRoot_origin = chainRoot[0].worldPosition;
//left_end.target = target1.position;
//right_end.target = target2.position;
// Work backwards to fork
//BackwardsAdjust(chain1, target1.position);
//BackwardsAdjust(chain2, target2.position);
Vector3 center = (chain1[1].worldPosition + chain2[1].worldPosition) / 2;
fork_1.target = center;
Solve(fork_1, false);
Vector3 chain1_origin = chain1[0].worldPosition;
Vector3 chain2_origin = chain2[0].worldPosition;
// Work forwards from fork
ForwardsAdjust(chain1, chain1_origin);
ForwardsAdjust(chain2, chain2_origin);
}
public void Solve(IK_Joint endPoint, bool requiredInRange)
{
Vector3 target = endPoint.target;
int iterationNum = 0;
float error = 0;
List <IK_Joint> path = endPoint.GetPathFromFork();
// Check if can reach
if (requiredInRange)
{
float totalLimbDistance = 0;
float requiredDistance = 0;
foreach (IK_Joint joint in path)
{
totalLimbDistance += joint.distanceToParent;
if (joint.jointType == IK_Joint.JointTypeEnum.Root)
{
requiredDistance = (joint.worldPosition - target).magnitude;
}
}
if (totalLimbDistance < requiredDistance)
{
return;
}
}
// Start adjusting the positions of joints
do
{
Vector3 originPos = path[0].worldPosition; // Used later
BackwardsAdjust(path, target); // Backwards through the joints
ForwardsAdjust(path, originPos); // Forwards through the joints
error = (path[path.Count - 1].worldPosition - target).magnitude;
iterationNum++;
} while (iterationNum < ITERATION_COUNT && (error > MARGIN_OF_ERROR && iterationNum != 0));
// While iteration num is below ITERATION_COUNT and the MARGIN_OF_ERROR hasn't been reached yet...
}
