/// <summary> /// Core function that relies on the law of cosines in order to /// determine the angles between two bones. /// </summary> /// <param name="rState">State object containing information about what is to be solved and the results</param> public static void SolveIK(ref IKSolverState rState, float rBone2Extension = 0f) { if (rState.Bones == null || rState.Bones.Count != 2) { return; } // Extract out the data BoneControllerBone lBone1 = rState.Bones[0]; BoneControllerBone lBone2 = rState.Bones[1]; // Grab basic bone info. We need the end's bind rotation so that it will keep the cosine equations // on a single plane after limits are processed. float lBone1Length = Vector3.Distance(lBone1.Transform.position, lBone2.Transform.position); Vector3 lBone1Position = lBone1.Transform.position; Quaternion lBone1Rotation = lBone1.Transform.rotation; Vector3 lBone1BendAxis = rState.BoneBendAxes[0]; float lBone2Length = lBone2.Length + rBone2Extension; Vector3 lBone2Position = lBone2.Transform.position; Quaternion lBone2Rotation = lBone2.Transform.rotation; Vector3 lBone2BendAxis = rState.BoneBendAxes[1]; Vector3 lBone3Position = lBone2Position + (lBone2Rotation * lBone2.BindRotation * lBone2.ToBoneForward * (Vector3.forward * lBone2Length)); // Check if our final position is too far. If so, we need to bring it in Vector3 lTargetPosition = rState.TargetPosition; float lBone1ToTargetLength = Vector3.Distance(lBone1Position, lTargetPosition); if (lBone1ToTargetLength > lBone1Length + lBone2Length) { // We remove a tiny bit of length so we never end up with a // bone angle of 0. This allows us to account for the bend axis. lBone1ToTargetLength = lBone1Length + lBone2Length;// -0.0000f; Vector3 lDirection = (lTargetPosition - lBone1Position).normalized; lTargetPosition = lBone1Position + (lDirection * lBone1ToTargetLength); } // Grab the angle between the target vector and the mid bone. Then, create the final rotation vector for the first bone float lAngle = (-(lBone2Length * lBone2Length) + (lBone1Length * lBone1Length) + (lBone1ToTargetLength * lBone1ToTargetLength)) / (2f * lBone1Length * lBone1ToTargetLength); float lBone1Angle = Mathf.Acos(Mathf.Clamp(lAngle, -1f, 1f)) * Mathf.Rad2Deg; // The bind rotation in world coordinates Quaternion lBaseRootRotation = (rState.UseBindRotation ? lBone1.WorldBindRotation : lBone1.Transform.rotation * lBone1.ToBoneForward); // Grab the rotation that gets us from the base vector to the target vector. This is the hypotenuse. Quaternion lToTargetRotation = Quaternion.FromToRotation(lBaseRootRotation * Vector3.forward, lTargetPosition - lBone1Position); // Determine the axis we'll rotate the root bone around Vector3 lRootBendAxis = Vector3.zero; if (rState.UsePlaneNormal) { lRootBendAxis = Vector3Ext.PlaneNormal(lBone1Position, lBone2Position, lBone3Position); } else { lRootBendAxis = lToTargetRotation * lBaseRootRotation * lBone1BendAxis; } // Rotate from the base rotation to the target rotation and finally to the correct rotation (based on the angle) lBone1Rotation = Quaternion.AngleAxis(lBone1Angle, lRootBendAxis) * lToTargetRotation * lBaseRootRotation; // Now we can determine the position of the second bone lBone2Position = lBone1Position + (lBone1Rotation * (Vector3.forward * lBone1Length)); // Want to ensure we don't end up with a '0' look direction. Otherwise, we'll get infinite errors. if (Vector3.SqrMagnitude(lTargetPosition - lBone2Position) > 0.001f) { // Grabbing the rotation of the second bone is easier since we just look at the target Vector3 lForward = lTargetPosition - lBone2Position; Vector3 lRight = lBone1Rotation * lBone2BendAxis; Vector3 lUp = Vector3.Cross(lForward, lRight).normalized; lBone2Rotation = Quaternion.LookRotation(lForward, lUp); } // Return the results rState.Rotations.Clear(); rState.AddRotation(lBone1, lBone1Rotation); rState.AddRotation(lBone2, lBone2Rotation); // Set the position valudes (for debugging) rState.BonePositions.Clear(); rState.BonePositions.Add(lBone1Position); rState.BonePositions.Add(lBone2Position); rState.BonePositions.Add(lBone2Position + (lBone2Rotation * (Vector3.forward * lBone2Length))); // Debug if (rState.IsDebugEnabled) { DebugDraw.DrawOctahedronOverlay(lBone1Position, Quaternion.identity, 0.03f, Color.red, 1f); DebugDraw.DrawOctahedronOverlay(lBone2Position, Quaternion.identity, 0.03f, Color.green, 1f); DebugDraw.DrawOctahedronOverlay(lBone3Position, Quaternion.identity, 0.03f, Color.blue, 1f); DebugDraw.DrawOctahedronOverlay(lTargetPosition, Quaternion.identity, 0.03f, Color.magenta, 1f); DebugDraw.DrawLineOverlay(lBone1Position, lBone1Position + (lBone1Rotation * (Vector3.forward * 0.5f)), 0.01f, Color.blue, 0.75f); DebugDraw.DrawLineOverlay(lBone1Position, lBone1Position + (lBone1Rotation * (Vector3.up * 0.5f)), 0.01f, Color.green, 0.75f); DebugDraw.DrawLineOverlay(lBone1Position, lBone1Position + (lBone1Rotation * (Vector3.right * 0.5f)), 0.01f, Color.red, 0.75f); DebugDraw.DrawLineOverlay(lBone2Position, lBone2Position + (lBone2Rotation * Vector3.forward), 0.02f, Color.blue, 0.5f); DebugDraw.DrawLineOverlay(lBone2Position, lBone2Position + (lBone2Rotation * Vector3.up), 0.02f, Color.green, 0.5f); DebugDraw.DrawLineOverlay(lBone2Position, lBone2Position + (lBone2Rotation * Vector3.right), 0.02f, Color.red, 0.5f); } }
/// <summary> /// Determine the final position and rotation of the bones in a chain. The /// first bone will remain fixed, while the last bone will attempt to reach the target /// position. /// </summary> /// <param name="rBoneChainRoot">List of BoneControllerBones we'll be solving for.</param> /// <param name="rBoneChainEffector">Vector3 the last bone in the chain is attempting to reach.</param> /// <remarks>Note that the last bone (effector) will have a bone length of 0. This joint is the effector.</remarks> public static void SolveIK(ref IKSolverState rState) { lBonePositions.Clear(); lBoneRotations.Clear(); BoneControllerBone lBoneChainRoot = rState.Bones[0]; BoneControllerBone lBoneChainEnd = rState.Bones[rState.Bones.Count - 1]; // Positions of each bone. This allows us to iterate the positions //List<Vector3> lBonePositions = rState.BonePositions; // Rotations of each bone. This allows us to iterate the rotations //List<Quaternion> lBoneRotations = rState.BoneRotations; Vector3 lBoneForward = lBoneChainRoot.BoneForward; Quaternion lToBoneForward = lBoneChainRoot.ToBoneForward; // Build the chain that we'll be processing List <BoneControllerBone> lBones = new List <BoneControllerBone>(); // Store the root start position Vector3 lRootPosition = lBoneChainRoot.Transform.position; // Add the end point of the last bone in our chain // as our last point. We do this so we rotate this last bone correctly lBones.Add(null); lBonePositions.Add(lBoneChainEnd.Transform.position + (lBoneChainEnd.Transform.rotation * (lBoneForward * lBoneChainEnd.Length))); // Insert each ancestor BoneControllerBone lParent = lBoneChainEnd; while (lParent != null) { lBones.Insert(0, lParent); lBonePositions.Insert(0, lParent.Transform.position); lParent = (lParent == lBoneChainRoot ? null : lParent = lParent.Parent); } // Since a bone can have multiple children, we want the length that // follows this chain. So, we'll work backwards. float lTotalLength = 0f; List <float> lBoneLengths = new List <float>(); for (int i = 0; i < lBonePositions.Count - 2; i++) { float lLength = Vector3.Distance(lBonePositions[i], lBonePositions[i + 1]); lTotalLength += lLength; lBoneLengths.Add(lLength); } // Add the last lengths since we can't determine them. The second // to last one is the end point of our chain lTotalLength += lBoneChainEnd.Length; lBoneLengths.Add(lBoneChainEnd.Length); lBoneLengths.Add(0); //// We need to determine if we can even reach the target. If not, we'll define //// a target we can reach //if (lTotalLength > Vector3.Distance(rBoneChainRoot.Transform.position, rTargetPosition)) //{ // Vector3 lDirection = (rTargetPosition - rBoneChainRoot.Transform.position).normalized; // rTargetPosition = rBoneChainRoot.Transform.position + (lDirection * lTotalLength); //} // Perform the solution bool lIterate = true; int lIterationCount = 0; while (lIterate && lIterationCount < BoneController.MaxIterations) { lIterationCount++; // First, reposition from the end and iterate backwards. Grab the // line the new position should be on and based on the bone length, // position it. We don't need to change the position of the first // bone since it needs to be fixed as the root. lBonePositions[lBonePositions.Count - 1] = rState.TargetPosition; for (int i = lBonePositions.Count - 2; i >= 0; i--) { Vector3 lDirection = lBonePositions[i + 1].DirectionTo(lBonePositions[i]); lBonePositions[i] = lBonePositions[i + 1] + (lDirection * lBoneLengths[i]); } // Second, reposition the start and iterate forward. Grab the // line the new position should be on and based on the bone length, // position it. lBonePositions[0] = lRootPosition; for (int i = 1; i < lBonePositions.Count; i++) { Vector3 lDirection = lBonePositions[i - 1].DirectionTo(lBonePositions[i]); lBonePositions[i] = lBonePositions[i - 1] + (lDirection * lBoneLengths[i - 1]); } // Enforce limits lBoneRotations.Clear(); for (int i = 0; i < lBonePositions.Count - 1; i++) { Vector3 lNextPosition = lBonePositions[i + 1]; Vector3 lDirectionForward = (lNextPosition - lBones[i]._Transform.position).normalized; // For the arm, the forward direction points down (as the rotation axis of the elbow). So, that's what we'll get. // For the arm, we use this directly as it's the "up" vector for the "look" rotation Vector3 lUpAxis = (lBones[i]._Joint == null ? Vector3.forward : lBones[i]._Joint._UpAxis); // Using the bind pose "rotation axis", grab an "up" direction for our look rotation Vector3 lDirectionUp = lBones[i].WorldBindRotation * lUpAxis; //lDirectionUp = Quaternion.AngleAxis(90, lDirectionUp) * lDirectionForward; // Create the rotation based on typical forward and up. Then, we need to point from // the typical forward direction to the 'BoneForward' Quaternion lWorldRotation = Quaternion.LookRotation(lDirectionForward, lDirectionUp); lWorldRotation = lWorldRotation * lToBoneForward; // Convert the world rotation we want to a rotation that is relative to the bone Quaternion lLocalRotation = lBones[i].TransformWorldRotationToLocalRotation(lWorldRotation); if (lBones[i]._Joint != null) { // Extract out the limitations so we can adjust the reach Quaternion lSwing = Quaternion.identity; Quaternion lTwist = Quaternion.identity; lLocalRotation.DecomposeSwingTwist(lBoneForward, ref lSwing, ref lTwist); lBones[i]._Joint.ApplyLimits(ref lSwing, ref lTwist); lLocalRotation = lSwing * lTwist; } // Store the resulting local rotations lBoneRotations.Add(lLocalRotation); } // Determine the new positions based on the final rotations. This is // important since the rotations may have been limited Vector3 lParentPosition = lBones[0]._Transform.position; Quaternion lParentRotation = (lBones[0]._Transform.parent != null ? lBones[0]._Transform.parent.rotation : Quaternion.identity); for (int i = 1; i < lBonePositions.Count; i++) { int iMinus1 = i - 1; lParentRotation = lParentRotation * lBones[iMinus1].BindRotation * lBoneRotations[iMinus1]; lBonePositions[i] = lParentPosition + (lParentRotation * (lBoneForward * lBoneLengths[iMinus1])); lParentPosition = lBonePositions[i]; } // If our last position is close to our target, we can stop float lDistance = Vector3.Distance(lBonePositions[lBonePositions.Count - 1], rState.TargetPosition); if (lDistance < 0.01f) { lIterate = false; } } // We'll report the new rotations that we calculated earlier rState.Swings.Clear(); rState.Twists.Clear(); rState.Rotations.Clear(); for (int i = 0; i < lBoneRotations.Count; i++) { Quaternion lSwing = Quaternion.identity; Quaternion lTwist = Quaternion.identity; lBoneRotations[i].DecomposeSwingTwist(lBoneForward, ref lSwing, ref lTwist); rState.AddRotation(lBones[i], lSwing, lTwist); } // The final positions we'll be moving to //return lBonePositions; }