/// <summary>
/// Allows the motor to process any specific bone logic after
/// a bone has been added
/// </summary>
/// <param name="rIndex">Index position of the new bone</param>
/// <param name="rBone">New bone that was added</param>
public override void AddBone(BoneControllerBone rBone, bool rIncludeChildren)
{
base.AddBone(rBone, rIncludeChildren);
while (_BoneInfo.Count < mBones.Count)
{
FootPlacementMotorBone lBoneInfo = new FootPlacementMotorBone();
_BoneInfo.Insert(mBones.IndexOf(rBone), lBoneInfo);
}
}
/// <summary>
/// Renders out bone details specific to the motor
/// </summary>
/// <param name="rIndex"></param>
/// <param name="rBone"></param>
/// <returns></returns>
protected override bool RenderBone(int rIndex, BoneControllerBone rBone)
{
bool lIsDirty = false;
#if UNITY_EDITOR
while (rIndex >= _BoneInfo.Count)
{
FootPlacementMotorBone lBoneInfo = new FootPlacementMotorBone();
_BoneInfo.Insert(mBones.IndexOf(rBone), lBoneInfo);
}
// Set the bone weight
float lNewWeight = EditorGUILayout.FloatField(new GUIContent("Weight", "Determines how much the motor effects vs. currently animated rotation."), _BoneInfo[rIndex].Weight);
if (lNewWeight != _BoneInfo[rIndex].Weight)
{
lIsDirty = true;
_BoneInfo[rIndex].Weight = lNewWeight;
}
// Set the bone yaw
float lNewRotationLerp = EditorGUILayout.FloatField(new GUIContent("Rotation Lerp", "Determines how quickly we rotate to the target."), _BoneInfo[rIndex].RotationLerp);
if (lNewRotationLerp != _BoneInfo[rIndex].RotationLerp)
{
lIsDirty = true;
_BoneInfo[rIndex].RotationLerp = lNewRotationLerp;
}
// Axis to rotate around
Vector3 lNewBendAxis = EditorGUILayout.Vector3Field(new GUIContent("Support Axis", "Axis used for rotating the bone."), _BoneInfo[rIndex].BendAxis);
if (lNewBendAxis != _BoneInfo[rIndex].BendAxis)
{
lIsDirty = true;
_BoneInfo[rIndex].BendAxis = lNewBendAxis;
}
// Final twist to apply to the bone after it's rotated
float lNewTwist = EditorGUILayout.FloatField(new GUIContent("Final Twist Adjust", "Twist applied to the bone after movement."), _BoneInfo[rIndex].Twist);
if (lNewTwist != _BoneInfo[rIndex].Twist)
{
lIsDirty = true;
_BoneInfo[rIndex].Twist = lNewTwist;
}
#endif
return(lIsDirty);
}
/// <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);
}
}
}
