コード例 #1
0
        /// <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);
        }
コード例 #2
0
        /// <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);
        }
コード例 #3
0
        /// <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);
            }
        }
コード例 #4
0
        /// <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;
        }
コード例 #5
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)
        {
            // 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);
                }
            }
        }
コード例 #6
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);
                }
            }
        }