/// <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);
                }
            }
        }
Example #2
0
        /// <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);
                }
            }
        }