/// <summary>
/// Returns an element back to the pool.
/// </summary>
/// <param name="rEdge"></param>
public static void Release(IKSolverState rInstance)
{
// Ensure we have a valid object
if (rInstance.BoneLengths == null)
{
return;
}
// Clear the values
rInstance.TargetPosition = Vector3.zero;
rInstance.UsePlaneNormal = false;
rInstance.UseBindRotation = false;
rInstance.IsDebugEnabled = false;
if (rInstance.Bones == null)
{
rInstance.Bones = new List <BoneControllerBone>();
}
rInstance.Bones.Clear();
if (rInstance.BoneLengths == null)
{
rInstance.BoneLengths = new List <float>();
}
rInstance.BoneLengths.Clear();
if (rInstance.BonePositions == null)
{
rInstance.BonePositions = new List <Vector3>();
}
rInstance.BonePositions.Clear();
if (rInstance.BoneBendAxes == null)
{
rInstance.BoneBendAxes = new List <Vector3>();
}
rInstance.BoneBendAxes.Clear();
if (rInstance.Rotations == null)
{
rInstance.Rotations = new Dictionary <BoneControllerBone, Quaternion>();
}
rInstance.Rotations.Clear();
if (rInstance.Swings == null)
{
rInstance.Swings = new Dictionary <BoneControllerBone, Quaternion>();
}
rInstance.Swings.Clear();
if (rInstance.Twists == null)
{
rInstance.Twists = new Dictionary <BoneControllerBone, Quaternion>();
}
rInstance.Twists.Clear();
// Send it back to the pool
sPool.Release(rInstance);
}
/// <summary>
/// Pulls an object from the pool.
/// </summary>
/// <returns></returns>
public static IKSolverState Allocate()
{
// Grab the next available object
IKSolverState lInstance = sPool.Allocate();
// Initialize
lInstance.TargetPosition = Vector3.zero;
lInstance.UsePlaneNormal = false;
lInstance.UseBindRotation = false;
lInstance.IsDebugEnabled = false;
if (lInstance.Bones == null)
{
lInstance.Bones = new List <BoneControllerBone>();
}
if (lInstance.BoneLengths == null)
{
lInstance.BoneLengths = new List <float>();
}
if (lInstance.BonePositions == null)
{
lInstance.BonePositions = new List <Vector3>();
}
if (lInstance.BoneBendAxes == null)
{
lInstance.BoneBendAxes = new List <Vector3>();
}
if (lInstance.Rotations == null)
{
lInstance.Rotations = new Dictionary <BoneControllerBone, Quaternion>();
}
if (lInstance.Swings == null)
{
lInstance.Swings = new Dictionary <BoneControllerBone, Quaternion>();
}
if (lInstance.Twists == null)
{
lInstance.Twists = new Dictionary <BoneControllerBone, Quaternion>();
}
return(lInstance);
}
/// <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;
}
/// <summary>
/// Process the motor each frame so that it can update the bone rotations.
/// This is the function that should be overridden in each motor
/// </summary>
/// <param name="rDeltaTime">Delta time to use for the update</param>
/// <param name="rUpdate">Determines if it is officially time to do the update</param>
protected override void Update(float rDeltaTime, bool rUpdate)
{
// TRT 02/04/2016 a - Protection for some errors I saw when going from debug to running
if (mBones.Count < 3)
{
return;
}
if (mSkeleton == null)
{
return;
}
if (object.ReferenceEquals(mSkeleton, null))
{
return;
}
if (object.ReferenceEquals(mSkeleton.gameObject, null))
{
return;
}
// Shortcuts for easy access
BoneControllerBone lUpperLeg = mBones[0];
BoneControllerBone lLowerLeg = mBones[1];
BoneControllerBone lFoot = mBones[2];
BoneControllerBone lToes = (mBones.Count > 3 ? mBones[3] : null);
// Get out if we don't have valid bones
if (lUpperLeg == null || lLowerLeg == null || lFoot == null)
{
return;
}
// Ensure we have valid values
if (lToes != null)
{
if (_FootToeDistance == 0f)
{
_FootToeDistance = mBones[3]._Transform.position.y - mBones[2]._Transform.position.y;
}
}
if (_FootForwardToBind == Quaternion.identity)
{
Vector3 lBindGroundForward = Vector3.Cross(mSkeleton.transform.up, lFoot.WorldBindRotation * -Vector3.right);
Quaternion lBindForwardRotation = Quaternion.LookRotation(lBindGroundForward, mSkeleton.transform.up);
_FootForwardToBind = Quaternion.Inverse(lBindForwardRotation) * lFoot.WorldBindRotation;
}
// Ensure we have the correct amount of bone infos... we should
while (_BoneInfo.Count < mBones.Count)
{
FootPlacementMotorBone lBoneInfo = new FootPlacementMotorBone();
_BoneInfo.Add(lBoneInfo);
}
// If it's time to update, cast out to find the collision point and
// generate the new positions.
if (rUpdate)
{
bool lUseCurrentRotation = true;
Transform lOwnerTransform = mSkeleton.gameObject.transform;
// Heel cast
Vector3 lHeelStart = lFoot.Transform.position;
lHeelStart = lHeelStart + (lOwnerTransform.up * _RaycastStartDistance);
float lHeelRaycastDistance = _RaycastStartDistance + _FootToeDistance + _ToeSoleDistance + (_AllowLegExtension ? _RaycastExtensionDistance : 0f);
Vector3 lHeelEnd = lHeelStart - (lOwnerTransform.up * lHeelRaycastDistance);
bool lHeelCollision = RaycastExt.SafeRaycast(lHeelStart, -lOwnerTransform.up, out sCollisionInfo1, lHeelRaycastDistance, _GroundingLayers, mSkeleton._RootTransform, mSkeleton.BoneTransforms);
// Toe cast
bool lToeCollision = false;
Vector3 lToeEnd = Vector3.zero;
if (lToes != null)
{
Vector3 lToeStart = lToes.Transform.position;
lToeStart = lToeStart + (lOwnerTransform.up * _RaycastStartDistance);
float lToeRaycastDistance = _RaycastStartDistance + _ToeSoleDistance + (_AllowLegExtension ? _RaycastExtensionDistance : 0f);
lToeEnd = lToeStart - (lOwnerTransform.up * lToeRaycastDistance);
lToeCollision = RaycastExt.SafeRaycast(lToeStart, -lOwnerTransform.up, out sCollisionInfo2, lToeRaycastDistance, _GroundingLayers, mSkeleton._RootTransform, mSkeleton.BoneTransforms);
}
// Prepare some variables in case we'll need to continue
Vector3 lFootTarget = Vector3.zero;
Vector3 lGroundNormal = Vector3.up;
// We only need to process if there is a collision
if (lHeelCollision || lToeCollision)
{
lUseCurrentRotation = false;
// Test if we actually hit anything
bool lUseHeel = true;
if (!lHeelCollision || (lToeCollision && (sCollisionInfo2.point.y - lToeEnd.y > sCollisionInfo1.point.y - lHeelEnd.y)))
{
lUseHeel = false;
}
lGroundNormal = (lUseHeel ? sCollisionInfo1 : sCollisionInfo2).normal;
// Determine the actual foot bone target
if (lUseHeel)
{
lFootTarget = sCollisionInfo1.point + (lOwnerTransform.up * _FootToeDistance);
}
else
{
lFootTarget = sCollisionInfo2.point + ((lFoot.Transform.position - lToes.Transform.position).normalized * lFoot.Length);
}
// If we aren't allowed to extend the leg, but we need to... stop
if (!_AllowLegExtension)
{
// TRT 02/04/2016 a - When than animation curls the leg and pulls the foot up, we
// don't want to force the foot to the ground. So, we disable the IK. The problem is
// if there's a tiny shuffling animation, we could flicker the IK on and off.
float lLegFootAnimationDistance = (lFoot._Transform.position - lUpperLeg._Transform.position).sqrMagnitude;
float lLegFootTargetDistance = (lFootTarget - lUpperLeg._Transform.position).sqrMagnitude;
//if (lLegFootNew >= lLegFootOld)
float lLegDelta = lLegFootTargetDistance - lLegFootAnimationDistance;
if (lLegDelta > _MaxDeltaDistance)
{
lUseCurrentRotation = true;
}
}
}
// If we're using the current rotations, we need to remove the targets that
// may have been set. We do this so we can smoothly blend to the current rotation
// as set by animations.
if (lUseCurrentRotation)
{
if (lUpperLeg != null)
{
BoneControllerBone lBone = lUpperLeg;
FootPlacementMotorBone lBoneInfo = _BoneInfo[mBones.IndexOf(lBone)];
lBoneInfo.RotationTarget = lBone.Transform.rotation * lBone.ToBoneForward;
lBoneInfo.Rotation = Quaternion.Lerp(lBoneInfo.Rotation, lBoneInfo.RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? lBoneInfo.RotationLerp : 1f));
lBone.SetWorldRotation(lBoneInfo.Rotation, _BoneWeight);
if (lBone.ApplyLimitsInFrame)
{
lBone.ApplyLimitsInFrame = _ApplyLimits;
}
}
if (lLowerLeg != null)
{
BoneControllerBone lBone = lLowerLeg;
FootPlacementMotorBone lBoneInfo = _BoneInfo[mBones.IndexOf(lBone)];
lBoneInfo.RotationTarget = lBone.Transform.rotation * lBone.ToBoneForward;
lBoneInfo.Rotation = Quaternion.Lerp(lBoneInfo.Rotation, lBoneInfo.RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? lBoneInfo.RotationLerp : 1f));
lBone.SetWorldRotation(lBoneInfo.Rotation, _BoneWeight);
if (lBone.ApplyLimitsInFrame)
{
lBone.ApplyLimitsInFrame = _ApplyLimits;
}
}
if (lFoot != null && _RotateFootToGround)
{
RotateFoot(lOwnerTransform, lLowerLeg, lFoot, lToes, lFootTarget, lGroundNormal, lHeelCollision, lToeCollision);
}
}
// If we get there, we need to bend the legs because there is a collision
// or an extension
else
{
// Only perform the solve if there enough movement involved. Otherwise,
// we're wasting resources.
//float lTargetDistance = Vector3.Distance(lFoot.Transform.position, lFootTarget);
// TRT 02/04/2016 a - With minor movements in animation, we can see the solver popping on and off.
// So, we'll force the solver to run each frame no matter what. It's not that big of a hit.
//if (lTargetDistance > FootGround2BoneMotor.MIN_TARGET_DISTANCE)
{
//HingeSwingAndTwistJoint lLowerJoint = lLowerLeg.Joint as HingeSwingAndTwistJoint;
// Since we have a target, solve
IKSolverState lState = IKSolverState.Allocate();
lState.TargetPosition = lFootTarget;
lState.UseBindRotation = _UseBindRotation;
lState.UsePlaneNormal = _UsePlaneNormal;
lState.IsDebugEnabled = _IsDebugEnabled;
lState.Bones.Add(lUpperLeg);
lState.Bones.Add(lLowerLeg);
lState.BoneBendAxes.Add(_BoneInfo[0].BendAxis);
lState.BoneBendAxes.Add(_BoneInfo[1].BendAxis);
CosineSolver.SolveIK(ref lState);
// Process the results of the solve. We use the enumerator to
// avoid garbage from the ForEach
Dictionary <BoneControllerBone, Quaternion> .Enumerator lEnumerator = lState.Rotations.GetEnumerator();
while (lEnumerator.MoveNext())
{
BoneControllerBone lBone = lEnumerator.Current.Key;
FootPlacementMotorBone lBoneInfo = _BoneInfo[mBones.IndexOf(lBone)];
// The current rotation we will lerp from. We remove the trailing rotation offset because we'll add it later
Quaternion lCurrentRotation = lBone.Transform.rotation * lBone.ToBoneForward;
// Rotation as determined by the target
Quaternion lTargetRotation = lState.Rotations[lBone] * Quaternion.Euler(0f, 0f, lBoneInfo.Twist);
// Determine the final rotation based on weight
lBoneInfo.RotationTarget = Quaternion.Lerp(lCurrentRotation, lTargetRotation, _Weight * lBoneInfo.Weight);
// Slowly move towards the rotation we determined
lBoneInfo.Rotation = Quaternion.Lerp(lBoneInfo.Rotation, lBoneInfo.RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? lBoneInfo.RotationLerp : 1f));
// Set the world rotation
lBone.SetWorldRotation(lBoneInfo.Rotation, _BoneWeight);
if (lBone.ApplyLimitsInFrame)
{
lBone.ApplyLimitsInFrame = _ApplyLimits;
}
}
//// Process the results of the solve
//foreach (BoneControllerBone lBone in lState.Rotations.Keys)
//{
// FootPlacementMotorBone lBoneInfo = _BoneInfo[mBones.IndexOf(lBone)];
// // The current rotation we will lerp from. We remove the trailing rotation offset because we'll add it later
// Quaternion lCurrentRotation = lBone.Transform.rotation * lBone.ToBoneForward;
// // Rotation as determined by the target
// Quaternion lTargetRotation = lState.Rotations[lBone] * Quaternion.Euler(0f, 0f, lBoneInfo.Twist);
// // Determine the final rotation based on weight
// lBoneInfo.RotationTarget = Quaternion.Lerp(lCurrentRotation, lTargetRotation, _Weight * lBoneInfo.Weight);
// // Slowly move towards the rotation we determined
// lBoneInfo.Rotation = Quaternion.Lerp(lBoneInfo.Rotation, lBoneInfo.RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? lBoneInfo.RotationLerp : 1f));
// // Set the world rotation
// lBone.SetWorldRotation(lBoneInfo.Rotation, _BoneWeight);
// if (lBone.ApplyLimitsInFrame) { lBone.ApplyLimitsInFrame = _ApplyLimits; }
//}
//DebugDraw.DrawLineOverlay(lUpperLeg.Transform.position, lUpperLeg.Transform.position + (lUpperLeg.Transform.rotation * lUpperLeg.ToBoneForward * (Vector3.forward * 0.5f)), 0.02f, Color.blue, 0.5f);
//DebugDraw.DrawLineOverlay(lUpperLeg.Transform.position, lUpperLeg.Transform.position + (lUpperLeg.Transform.rotation * lUpperLeg.ToBoneForward * (Vector3.up * 0.5f)), 0.02f, Color.green, 0.5f);
//DebugDraw.DrawLineOverlay(lUpperLeg.Transform.position, lUpperLeg.Transform.position + (lUpperLeg.Transform.rotation * lUpperLeg.ToBoneForward * (Vector3.right * 0.5f)), 0.02f, Color.red, 0.5f);
// Set the foot rotations. This may change based on the collisions
if (lFoot != null && _RotateFootToGround)
{
RotateFoot(lOwnerTransform, lLowerLeg, lFoot, lToes, lFootTarget, lGroundNormal, lHeelCollision, lToeCollision);
}
// Clean up
IKSolverState.Release(lState);
}
}
// Keep track of the last position so we can test for movement
mLastPosition = mSkeleton.transform.position;
}
// If it's not on a consistant update, we just want to reset the
// last rotations that we found.
else
{
for (int i = 0; i < mBones.Count; i++)
{
BoneControllerBone lBone = mBones[i];
if (lBone == null || lBone == lToes)
{
continue;
}
if (lBone == lFoot)
{
Vector3 lMovement = mSkeleton.transform.position - mLastPosition;
if (!_RotateFootOnMovement && (Mathf.Abs(lMovement.x) > 0.001f || Mathf.Abs(lMovement.z) > 0.001f))
{
continue;
}
}
lBone.SetWorldRotation(_BoneInfo[i].Rotation, _BoneWeight);
}
}
}
/// <summary>
/// Process the motor each frame so that it can update the bone rotations.
/// This is the function that should be overridden in each motor
/// </summary>
/// <param name="rDeltaTime">Delta time to use for the update</param>
/// <param name="rUpdate">Determines if it is officially time to do the update</param>
protected override void Update(float rDeltaTime, bool rUpdate)
{
if (mBones == null || mBones.Count < 2)
{
return;
}
if (_TargetTransform == null && _TargetTransformName.Length > 0)
{
GameObject lObject = GameObject.Find(_TargetTransformName);
if (lObject != null)
{
_TargetTransform = lObject.transform;
}
}
if (_TargetTransform != null && !_TargetTransform.gameObject.activeInHierarchy)
{
return;
}
// Ensure we have the correct amount of bone infos... we should
while (_BoneInfo.Count < mBones.Count)
{
LimbReachMotorBone lBoneInfo = new LimbReachMotorBone();
_BoneInfo.Add(lBoneInfo);
}
// If it's time to update, determine the positions we need to be
// at and lerp towards them.
if (rUpdate)
{
// Grab the target. Priority is given to the transform
Vector3 lTargetPosition = (_TargetTransform != null ? _TargetTransform.position : _TargetPosition);
if (lTargetPosition == Vector3.zero)
{
return;
}
// Simplify the bone names
BoneControllerBone lBoneChainRoot = mBones[0];
BoneControllerBone lBoneChainEnd = mBones[1];
// If we have valid bones, solve
if (lBoneChainRoot != null && lBoneChainEnd != null)
{
//HingeSwingAndTwistJoint lEndJoint = lBoneChainEnd.Joint as HingeSwingAndTwistJoint;
IKSolverState lState = IKSolverState.Allocate();
lState.TargetPosition = lTargetPosition;
lState.UseBindRotation = _UseBindRotation;
lState.UsePlaneNormal = _UsePlaneNormal;
lState.IsDebugEnabled = _IsDebugEnabled;
lState.Bones.Add(lBoneChainRoot);
lState.Bones.Add(lBoneChainEnd);
lState.BoneBendAxes.Add(_BoneInfo[0].BendAxis);
lState.BoneBendAxes.Add(_BoneInfo[1].BendAxis);
CosineSolver.SolveIK(ref lState, _Bone2Extension);
// Process the results of the solve. We use the enumerator to
// avoid garbage from the ForEach
Dictionary <BoneControllerBone, Quaternion> .Enumerator lEnumerator = lState.Rotations.GetEnumerator();
while (lEnumerator.MoveNext())
{
BoneControllerBone lBone = lEnumerator.Current.Key;
int lIndex = mBones.IndexOf(lBone);
// The current rotation we will lerp from. We remove the trailing rotation offset because we'll add it later
Quaternion lCurrentRotation = lBone.Transform.rotation * lBone.ToBoneForward;
// Rotation based on the target position
Quaternion lTargetRotation = lState.Rotations[lBone] * Quaternion.Euler(0f, 0f, _BoneInfo[lIndex].Twist);
// Rotation as determined by the target
_BoneInfo[lIndex].RotationTarget = Quaternion.Lerp(lCurrentRotation, lTargetRotation, _Weight * _BoneInfo[lIndex].Weight);
// Slowly move towards the rotation we determined
_BoneInfo[lIndex].Rotation = Quaternion.Lerp(_BoneInfo[lIndex].Rotation, _BoneInfo[lIndex].RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? _BoneInfo[lIndex].RotationLerp : 1f));
// Set the world rotation
lBone.SetWorldRotation(_BoneInfo[lIndex].Rotation, _BoneWeight);
if (lBone.ApplyLimitsInFrame)
{
lBone.ApplyLimitsInFrame = _ApplyLimits;
}
}
//foreach (BoneControllerBone lBone in lState.Rotations.Keys)
//{
// int lIndex = mBones.IndexOf(lBone);
// // The current rotation we will lerp from. We remove the trailing rotation offset because we'll add it later
// Quaternion lCurrentRotation = lBone.Transform.rotation * lBone.ToBoneForward;
// // Rotation based on the target position
// Quaternion lTargetRotation = lState.Rotations[lBone] * Quaternion.Euler(0f, 0f, _BoneInfo[lIndex].Twist);
// // Rotation as determined by the target
// _BoneInfo[lIndex].RotationTarget = Quaternion.Lerp(lCurrentRotation, lTargetRotation, _Weight * _BoneInfo[lIndex].Weight);
// // Slowly move towards the rotation we determined
// _BoneInfo[lIndex].Rotation = Quaternion.Lerp(_BoneInfo[lIndex].Rotation, _BoneInfo[lIndex].RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? _BoneInfo[lIndex].RotationLerp : 1f));
// // Set the world rotation
// lBone.SetWorldRotation(_BoneInfo[lIndex].Rotation, _BoneWeight);
// if (lBone.ApplyLimitsInFrame) { lBone.ApplyLimitsInFrame = _ApplyLimits; }
//}
IKSolverState.Release(lState);
}
}
// If it's not on a consistant update, we just want to reset the
// last rotations that we found.
else
{
for (int i = 0; i < mBones.Count; i++)
{
BoneControllerBone lBone = mBones[i];
if (lBone == null)
{
continue;
}
lBone.SetWorldRotation(_BoneInfo[i].Rotation, _BoneWeight);
}
}
}