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