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