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