public void AdjustLeg(int leg, Vector3 desiredAnklePosition, bool secondPass) {
LegInfo legInfo = legs[leg];
LegState legState = legStates[leg];
// Store original foot alignment
Quaternion qAnkleOrigRotation;
if (!secondPass) {
// Footbase rotation in character space
Quaternion objectToFootBaseRotation = legStates[leg].footBaseRotation * Quaternion.Inverse(rotation);
qAnkleOrigRotation = objectToFootBaseRotation * legInfo.ankle.rotation;
}
else {
qAnkleOrigRotation = legInfo.ankle.rotation;
}
// Choose IK solver
IKSolver ikSolver;
if (legInfo.legChain.Length==3) ikSolver = new IK1JointAnalytic();
else ikSolver = new IKSimple();
// Solve the inverse kinematics
ikSolver.Solve( legInfo.legChain, desiredAnklePosition );
// Calculate the desired new joint positions
Vector3 pHip = legInfo.hip.position;
Vector3 pAnkle = legInfo.ankle.position;
if (!secondPass) {
// Find alignment that is only rotates in horizontal plane
// and keeps local ankle angle
Quaternion horizontalRotation = Quaternion.FromToRotation(
forward,
Util.ProjectOntoPlane(legStates[leg].footBaseRotation*Vector3.forward,up)
) * legInfo.ankle.rotation;
// Apply original foot alignment when foot is grounded
legInfo.ankle.rotation = Quaternion.Slerp(
horizontalRotation, // only horizontal rotation (keep local angle)
qAnkleOrigRotation, // rotates to slope of ground
1-legState.GetFootGrounding(legState.cycleTime)
);
}
else {
// Rotate leg around hip-ankle axis by half amount of what the foot is rotated
Vector3 hipAnkleVector = pAnkle-pHip;
Quaternion legAxisRotate = Quaternion.Slerp(
Quaternion.identity,
Quaternion.FromToRotation(
Util.ProjectOntoPlane(forward,hipAnkleVector),
Util.ProjectOntoPlane(legStates[leg].footBaseRotation*Vector3.forward,hipAnkleVector)
),
0.5f
);
legInfo.hip.rotation = legAxisRotate * legInfo.hip.rotation;
// Apply foot alignment found in first pass
legInfo.ankle.rotation = qAnkleOrigRotation;
}
}
