/// <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.Count == 0)
{
return;
}
// Process the motor info at start up
if (!mIsInitialized)
{
// Ensure we have the correct amount of bone infos... we should
while (_BoneInfo.Count < mBones.Count)
{
RotationBone lBoneInfo = new RotationBone();
_BoneInfo.Add(lBoneInfo);
}
// Initialize the euler angles that are our base
for (int i = 0; i < mBones.Count; i++)
{
_BoneInfo[i].Euler = Vector3.zero;
_BoneInfo[i].BaseRotation = mBones[i]._Transform.rotation;
//_BoneInfo[i].Euler = mBones[i]._Transform.rotation.eulerAngles;
}
// Flag that we've initialized
mIsInitialized = true;
}
// If it's time to update, determine the positions we need to be
// at and lerp towards them.
if (rUpdate)
{
// Process each bone
for (int i = 0; i < mBones.Count; i++)
{
BoneControllerBone lBone = mBones[i];
RotationBone lBoneInfo = _BoneInfo[i];
// 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 we're moving towards
Quaternion lTargetRotation = lBone._Transform.rotation;
// Speed this frame
Vector3 lRotationSpeed = lBoneInfo.RotationSpeed * rDeltaTime;
// Update the angles we're rotating to
lBoneInfo.Euler += lRotationSpeed;
// Rotate based on the axis
if (lBoneInfo.RotationAxis == EnumIKBoneRotationAxis.BONE)
{
lTargetRotation = _BoneInfo[i].BaseRotation * Quaternion.Euler(lBoneInfo.Euler);
}
else if (lBoneInfo.RotationAxis == EnumIKBoneRotationAxis.MODEL)
{
lTargetRotation = _BoneInfo[i].BaseRotation * Quaternion.Euler(lBoneInfo.Euler) * lBone._ToBoneForward;
}
else
{
lTargetRotation = lTargetRotation * lBone._ToBoneForward;
}
// Rotation as determined by the target
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, Quaternion.identity, _BoneWeight);
}
}
// 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, Quaternion.identity, _BoneWeight);
}
}
}
/// <summary>
/// Rotates the foot to match the ground. This function will use the
/// collision values as best as it can
/// </summary>
/// <param name="rOwnerTransform"></param>
/// <param name="rFoot"></param>
/// <param name="rToes"></param>
/// <param name="rFootTarget"></param>
/// <param name="rGroundNormal"></param>
/// <param name="rHeelCollision"></param>
/// <param name="rToeCollision"></param>
private void RotateFoot(Transform rOwnerTransform, BoneControllerBone rLowerLeg, BoneControllerBone rFoot, BoneControllerBone rToes, Vector3 rFootTarget, Vector3 rGroundNormal, bool rHeelCollision, bool rToeCollision)
{
BoneControllerBone lBone = rFoot;
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 based on the target position
Quaternion lTargetRotation = lCurrentRotation;
// Usually we don't want to rotate if we're moving. This way we don't cause floppy foot
Vector3 lMovement = mSkeleton.transform.position - mLastPosition;
if (_RotateFootOnMovement || (Mathf.Abs(lMovement.x) <= 0.001f && Mathf.Abs(lMovement.z) <= 0.001f))
{
// If we're meant to rotate the feet, do it
float lGroundAngle = Vector3.Angle(rGroundNormal, mSkeleton.transform.up);
if ((_RotateFootToGroundMinAngle == 0f || Mathf.Abs(lGroundAngle) > _RotateFootToGroundMinAngle) && Mathf.Abs(lGroundAngle) < 60f)
{
if (rHeelCollision && rToeCollision)
{
Vector3 lGroundForward = Vector3.Cross(rGroundNormal, lCurrentRotation * -Vector3.right);
//float lDot = Vector3.Dot(mSkeleton.transform.forward, lGroundForward);
//if (lDot < 0f) { lGroundForward = -lGroundForward; }
lTargetRotation = Quaternion.LookRotation(lGroundForward, rGroundNormal) * _FootForwardToBind;
}
else if (rHeelCollision || rToeCollision)
{
if (rToes == null || !_RotateFootRequiresBoth)
{
if (rHeelCollision)
{
//Quaternion lHeelRotation = Quaternion.AngleAxis(90f, rFoot.WorldBindRotation * Vector3.right);
//Vector3 lToeForward = sCollisionInfo1.point + ((lHeelRotation * rGroundNormal).normalized * rFoot._Length);
//lTargetRotation = Quaternion.LookRotation(lToeForward - rFootTarget, rGroundNormal);
Vector3 lGroundForward = Vector3.Cross(rGroundNormal, lCurrentRotation * -Vector3.right);
//float lDot = Vector3.Dot(mSkeleton.transform.forward, lGroundForward);
//if (lDot < 0f) { lGroundForward = -lGroundForward; }
lTargetRotation = Quaternion.LookRotation(lGroundForward, rGroundNormal) * _FootForwardToBind;
}
else
{
//lTargetRotation = Quaternion.LookRotation(sCollisionInfo2.point - rFootTarget, rGroundNormal);
Vector3 lGroundForward = Vector3.Cross(rGroundNormal, lCurrentRotation * -Vector3.right);
//float lDot = Vector3.Dot(mSkeleton.transform.forward, lGroundForward);
//if (lDot < 0f) { lGroundForward = -lGroundForward; }
lTargetRotation = Quaternion.LookRotation(lGroundForward, rGroundNormal) * _FootForwardToBind;
}
}
}
}
}
// Rotation as determined by the target
lBoneInfo.RotationTarget = Quaternion.Lerp(lCurrentRotation, lTargetRotation, _Weight * lBoneInfo.Weight);
// Slowly move towards the rotation we determined
lBoneInfo.Rotation = Quaternion.Lerp(lBoneInfo.Rotation, lBoneInfo.RotationTarget, (!mIsFirstUpdate ? lBoneInfo.RotationLerp : 1f));
// Set the world rotation
lBone.SetWorldRotation(lBoneInfo.Rotation, _BoneWeight);
if (lBone.ApplyLimitsInFrame)
{
lBone.ApplyLimitsInFrame = _ApplyLimits;
}
}
/// <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);
}
}
}
/// <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.Count == 0)
{
return;
}
// Ensure we always have one more than the number of bones. The
// last one is the target position for the last bone.
while (_BoneInfo.Count <= mBones.Count)
{
BoneControllerBone lBone = (_BoneInfo.Count < mBones.Count ? mBones[_BoneInfo.Count] : null);
AddBoneInfo(mBones.Count, lBone);
}
// If it's time to update, determine the positions we need to be
// at and lerp towards them.
if (rUpdate)
{
// The first bone doesn't move on it's own. It's dragged by it's parent
_BoneInfo[0].Velocity = _BoneInfo[0].Position - _BoneInfo[0].PrevPosition;
_BoneInfo[0].PrevPosition = _BoneInfo[0].Position;
_BoneInfo[0].Position = mBones[0]._Transform.position;
// Initialize the data if it isn't
for (int i = 1; i < _BoneInfo.Count; i++)
{
// Initialize any bone info that hasn't currently been set.
if (_BoneInfo[i].PrevPosition.sqrMagnitude == 0f)
{
if (i < mBones.Count)
{
_BoneInfo[i].Velocity = Vector3.zero;
_BoneInfo[i].PrevPosition = mBones[i]._Transform.position;
_BoneInfo[i].Position = mBones[i]._Transform.position;
}
else
{
int lPrevIndex = i - 1;
_BoneInfo[i].Velocity = Vector3.zero;
_BoneInfo[i].PrevPosition = mBones[lPrevIndex]._Transform.position + (mBones[lPrevIndex]._Transform.rotation * (mBones[lPrevIndex].BoneForward * mBones[lPrevIndex].Length));
_BoneInfo[i].Position = mBones[lPrevIndex]._Transform.position + (mBones[lPrevIndex]._Transform.rotation * (mBones[lPrevIndex].BoneForward * mBones[lPrevIndex].Length));
}
}
}
// Drag the bones one after the other
//bool lCollision = false;
for (int i = 0; i < mBones.Count; i++)
{
// Collisions will stop movement
if (_IsCollisionEnabled)
{
_BoneInfo[i].Collision = ProcessBoneCollisions(i);
}
// If we're not colliding, allow the bones to move
if (!_BoneInfo[i].Collision)
{
// Add gravity to pull the bones down
if (!_IsGravityEnabled)
{
_BoneInfo[i + 1].Velocity = Vector3.zero;
}
else
{
_BoneInfo[i + 1].Velocity = _Gravity * rDeltaTime * GetGravity(i);
}
// Add pull velocity so that the bones chase after the positions
_BoneInfo[i + 1].Velocity += (_BoneInfo[i + 1].Position - _BoneInfo[i + 1].PrevPosition);
// Determine the new position (which may be too much)
Vector3 lNewPosition = _BoneInfo[i + 1].PrevPosition + _BoneInfo[i + 1].Velocity;
// Ensure the new position stays connected with the previous bones
Vector3 lDirection = (lNewPosition - _BoneInfo[i].Position).normalized;
Vector3 lDragPosition = _BoneInfo[i].Position + (lDirection * mBones[i].Length);
// Finally, set the new position
_BoneInfo[i + 1].Position = lDragPosition;
}
}
// With the bone positions determined, now we can compute the final positions
for (int i = 0; i < mBones.Count; i++)
{
// The original pose the bones were bound to
Vector3 lBaseEndPosition = Vector3.zero;
if (_BoneInfo[i].UseBindPosition)
{
lBaseEndPosition = _BoneInfo[i].Position + (mBones[i].WorldBindRotation * (Vector3.forward * mBones[i].Length));
}
else
{
lBaseEndPosition = (i < mBones.Count - 1 ? mBones[i + 1].Transform.position : mBones[i].WorldEndPosition);
}
// Determine the bending based on the stiffness of each bone
float lStiffness = GetStiffness(i);
if (_BoneInfo[i].Collision)
{
lStiffness = 0f;
}
Vector3 lNewEndPosition = Vector3.Lerp(_BoneInfo[i + 1].Position, lBaseEndPosition, lStiffness);
// Since we lerped, the value may not actually be the right position
// based on our bone length. So, we need to reprocess the lengths again
Vector3 lDirection = (lNewEndPosition - _BoneInfo[i].Position).normalized;
lNewEndPosition = _BoneInfo[i].Position + (lDirection * mBones[i].Length);
// Finally, set the final positions
_BoneInfo[i + 1].Position = lNewEndPosition;
// Rotation based on the positions
Vector3 lTargetForward = _BoneInfo[i + 1].Position - _BoneInfo[i].Position;
if (lTargetForward.sqrMagnitude != 0f)
{
lTargetForward = lTargetForward.normalized;
// TRT 2/21/2016 - Updated to reflect bone's actual values
//_BoneInfo[i].RotationTarget = Quaternion.LookRotation(lTargetForward.normalized, mBones[i].Transform.up);
// TRT 3/8/2016 - Wasn't using the current bone's actual values :(
//_BoneInfo[i].RotationTarget = Quaternion.LookRotation(lTargetForward.normalized, mBones[i]._BoneUp);
// TRT 7/9/2016 - Wasn't trying to get back to original rotation
//Vector3 lCurrentUp = (mBones[i].Transform.rotation * mBones[i].ToBoneForward) * Vector3.up;
//_BoneInfo[i].RotationTarget = Quaternion.LookRotation(lTargetForward, lCurrentUp);
// TRT 7/9/2016 - Want to make sure the bones try to twist to thier original rotation too
Vector3 lCurrentUp = (mBones[i].Transform.rotation * mBones[i].ToBoneForward) * Vector3.up;
if (_BoneInfo[i].UntwistLerp > 0f)
{
Vector3 lBindUp = mBones[i].WorldBindRotation * Vector3.up;
lCurrentUp = Vector3.Lerp(lCurrentUp, lBindUp, _BoneInfo[i].UntwistLerp);
}
_BoneInfo[i].RotationTarget = Quaternion.LookRotation(lTargetForward, lCurrentUp);
//Debug.DrawLine(mBones[i].Transform.position, mBones[i].Transform.position + lTargetForward, Color.black);
//Debug.DrawLine(mBones[i].Transform.position, mBones[i].Transform.position + lCurrentUp, Color.magenta);
}
// In the case of a 0 length bone, we just use the current rotation
else
{
_BoneInfo[i].RotationTarget = mBones[i].Transform.rotation * mBones[i].ToBoneForward;
}
// Slowly move towards the rotation we determined
_BoneInfo[i].Rotation = Quaternion.Lerp(_BoneInfo[i].Rotation, _BoneInfo[i].RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? _BoneInfo[i].RotationLerp : 1f));
// Set the world rotation
mBones[i].SetWorldRotation(_BoneInfo[i].Rotation, Quaternion.identity, _BoneWeight);
// Clean up
_BoneInfo[i].PrevPosition = _BoneInfo[i].Position;
}
// Update that last extra bone info
_BoneInfo[_BoneInfo.Count - 1].PrevPosition = _BoneInfo[_BoneInfo.Count - 1].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)
{
continue;
}
lBone.SetWorldRotation(_BoneInfo[i].Rotation, Quaternion.identity, _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)
{
// Get out if there are no valid bones
if (mBones.Count == 0 || mBones[0] == null || mBones[0]._Transform == null)
{
return;
}
// If it's time to update, determine the positions we need to be
// at and lerp towards them.
if (rUpdate)
{
// Extract out the target info and resulting rotation
Vector3 lTargetPosition = _TargetPosition;
if (_TargetTransform != null)
{
if (_UseAsDirection)
{
lTargetPosition = mBones[0]._Transform.position + (_TargetTransform.forward * 2f);
}
else
{
lTargetPosition = _TargetTransform.position;
}
}
// Forward direction that we want to be looking from
Vector3 lAnchorPosition = mBones[0]._Transform.position;
// The target we want to rotate to
Vector3 lToTarget = (lTargetPosition - lAnchorPosition).normalized;
Quaternion lTargetRotation = Quaternion.LookRotation(lToTarget, (_InvertRotations ? -1 : 1) * (mSkeleton != null && mSkeleton.transform != null ? mSkeleton.transform.up : Vector3.up));
// Start rotating each of the bones independantly
for (int i = mBones.Count - 1; i >= 0; i--)
{
if (_BoneInfo.Count <= i)
{
continue;
}
BoneControllerBone lBone = mBones[i];
if (lBone == null)
{
continue;
}
// This swings around the bone around it's bone's current x and y axis and twists around
// the current bone direction
Quaternion lRotationOffset = lBone.BindRotation * lBone.ToBoneForward;
lRotationOffset = lRotationOffset * Quaternion.AngleAxis(_BoneInfo[i].RotationOffset.y, Vector3.up);
lRotationOffset = lRotationOffset * Quaternion.AngleAxis(_BoneInfo[i].RotationOffset.x, Vector3.right);
lRotationOffset = lRotationOffset * Quaternion.AngleAxis(_BoneInfo[i].RotationOffset.z, Vector3.forward);
// The current rotation we will lerp from. We remove the trailing rotation offset because we'll add it later
Quaternion lCurrentRotation = mBones[i].Transform.rotation * mBones[i].ToBoneForward * Quaternion.Inverse(lRotationOffset);
// Target rotation. We want to convert from "Bone Forward" to "Bone Up"-forward since it's the up direction
// that will point to the target. Then, we need to remove part of the offset since it will be re-added.
Quaternion lBoneTargetRotation = lTargetRotation * Quaternion.AngleAxis(180f, Vector3.up) * Quaternion.AngleAxis(-90f, Vector3.right) * Quaternion.Inverse(lBone.BindRotation * lBone.ToBoneForward);
// Based on the weight, use the current rotation or our target rotation
_BoneInfo[i].RotationTarget = Quaternion.Lerp(lCurrentRotation, lBoneTargetRotation, _Weight * _BoneInfo[i].Weight);
// Slowly move towards the targets we determined
float lLerp = _BoneInfo[i].RotationLerp;
#if UNITY_EDITOR
// If we're editing, don't lerp. Jut move
if (!EditorApplication.isPlaying)
{
lLerp = 1f;
}
#endif
// Slowly move towards the rotation we determined
_BoneInfo[i].Rotation = Quaternion.Lerp(_BoneInfo[i].Rotation, _BoneInfo[i].RotationTarget, (_IsFixedUpdateEnabled && !mIsFirstUpdate ? lLerp : 1f));
// Set the world rotation
lBone.SetWorldRotation(_BoneInfo[i].Rotation * lRotationOffset, _BoneWeight);
}
}
// If it's not on a consistant update, we just want to reset the
// last rotations that we found.
else
{
for (int i = mBones.Count - 1; i >= 0; i--)
{
if (_BoneInfo.Count <= i)
{
continue;
}
BoneControllerBone lBone = mBones[i];
if (lBone == null)
{
continue;
}
// This swings around the bone around it's bone's current x and y axis and twists around
// the current bone direction
Quaternion lRotationOffset = lBone.BindRotation * lBone.ToBoneForward;
lRotationOffset = lRotationOffset * Quaternion.AngleAxis(_BoneInfo[i].RotationOffset.y, Vector3.up);
lRotationOffset = lRotationOffset * Quaternion.AngleAxis(_BoneInfo[i].RotationOffset.x, Vector3.right);
lRotationOffset = lRotationOffset * Quaternion.AngleAxis(_BoneInfo[i].RotationOffset.z, Vector3.forward);
mBones[i].SetWorldRotation(_BoneInfo[i].Rotation * lRotationOffset, _BoneWeight);
}
}
}
