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... }