Beispiel #1
0
        private void Solve()
        {
            // Pre-Solving
            spine.PreSolve();
            foreach (Arm arm in arms)
            {
                arm.PreSolve();
            }
            foreach (Leg leg in legs)
            {
                leg.PreSolve();
            }

            // Applying spine and arm offsets
            foreach (Arm arm in arms)
            {
                arm.ApplyOffsets();
            }
            spine.ApplyOffsets();

            // Spine
            spine.Solve(rootBone, legs, arms);

            if (spine.pelvisPositionWeight > 0f && plantFeet)
            {
                Warning.Log("If VRIK 'Pelvis Position Weight' is > 0, 'Plant Feet' should be disabled to improve performance and stability.", root);
            }

            // Locomotion
            if (locomotion.weight > 0f)
            {
                Vector3    leftFootPosition  = Vector3.zero;
                Vector3    rightFootPosition = Vector3.zero;
                Quaternion leftFootRotation  = Quaternion.identity;
                Quaternion rightFootRotation = Quaternion.identity;
                float      leftFootOffset    = 0f;
                float      rightFootOffset   = 0f;
                float      leftHeelOffset    = 0f;
                float      rightHeelOffset   = 0f;

                locomotion.Solve(rootBone, spine, leftLeg, rightLeg, leftArm, rightArm, supportLegIndex, out leftFootPosition, out rightFootPosition, out leftFootRotation, out rightFootRotation, out leftFootOffset, out rightFootOffset, out leftHeelOffset, out rightHeelOffset);

                leftFootPosition  += root.up * leftFootOffset;
                rightFootPosition += root.up * rightFootOffset;

                leftLeg.footPositionOffset  += (leftFootPosition - leftLeg.lastBone.solverPosition) * IKPositionWeight * (1f - leftLeg.positionWeight) * locomotion.weight;
                rightLeg.footPositionOffset += (rightFootPosition - rightLeg.lastBone.solverPosition) * IKPositionWeight * (1f - rightLeg.positionWeight) * locomotion.weight;

                leftLeg.heelPositionOffset  += root.up * leftHeelOffset * locomotion.weight;
                rightLeg.heelPositionOffset += root.up * rightHeelOffset * locomotion.weight;

                Quaternion rotationOffsetLeft  = QuaTools.FromToRotation(leftLeg.lastBone.solverRotation, leftFootRotation);
                Quaternion rotationOffsetRight = QuaTools.FromToRotation(rightLeg.lastBone.solverRotation, rightFootRotation);

                rotationOffsetLeft  = Quaternion.Lerp(Quaternion.identity, rotationOffsetLeft, IKPositionWeight * (1f - leftLeg.rotationWeight) * locomotion.weight);
                rotationOffsetRight = Quaternion.Lerp(Quaternion.identity, rotationOffsetRight, IKPositionWeight * (1f - rightLeg.rotationWeight) * locomotion.weight);

                leftLeg.footRotationOffset  = rotationOffsetLeft * leftLeg.footRotationOffset;
                rightLeg.footRotationOffset = rotationOffsetRight * rightLeg.footRotationOffset;

                Vector3 footPositionC = Vector3.Lerp(leftLeg.position + leftLeg.footPositionOffset, rightLeg.position + rightLeg.footPositionOffset, 0.5f);
                footPositionC = V3Tools.PointToPlane(footPositionC, rootBone.solverPosition, root.up);

                Vector3 p = rootBone.solverPosition + rootVelocity * Time.deltaTime * 2f * locomotion.weight;
                p = Vector3.Lerp(p, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight);
                rootBone.solverPosition = p;

                rootVelocity += (footPositionC - rootBone.solverPosition) * Time.deltaTime * 10f;
                Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f);
                rootVelocity -= rootVelocityV;

                float bodyYOffset = leftFootOffset + rightFootOffset;
                bodyOffset = Vector3.Lerp(bodyOffset, root.up * bodyYOffset, Time.deltaTime * 3f);
                bodyOffset = Vector3.Lerp(Vector3.zero, bodyOffset, locomotion.weight);
            }

            // Legs
            foreach (Leg leg in legs)
            {
                leg.ApplyOffsets();
            }

            if (!plantFeet)
            {
                spine.InverseTranslateToHead(legs, false, false, bodyOffset, 1f);

                foreach (Leg leg in legs)
                {
                    leg.TranslateRoot(spine.pelvis.solverPosition, spine.pelvis.solverRotation);
                }
                foreach (Leg leg in legs)
                {
                    leg.Solve();
                }
            }
            else
            {
                for (int i = 0; i < 2; i++)
                {
                    spine.InverseTranslateToHead(legs, true, i == 0, bodyOffset, 1f);

                    foreach (Leg leg in legs)
                    {
                        leg.TranslateRoot(spine.pelvis.solverPosition, spine.pelvis.solverRotation);
                    }
                    foreach (Leg leg in legs)
                    {
                        leg.Solve();
                    }
                }
            }

            // Arms
            for (int i = 0; i < arms.Length; i++)
            {
                arms[i].TranslateRoot(spine.chest.solverPosition, spine.chest.solverRotation);
            }

            for (int i = 0; i < arms.Length; i++)
            {
                arms[i].Solve(i == 0);
            }

            // Reset offsets
            spine.ResetOffsets();
            foreach (Leg leg in legs)
            {
                leg.ResetOffsets();
            }
            foreach (Arm arm in arms)
            {
                arm.ResetOffsets();
            }

            spine.pelvisPositionOffset += GetPelvisOffset();
            spine.chestPositionOffset  += spine.pelvisPositionOffset;
            //spine.headPositionOffset += spine.pelvisPositionOffset;

            Write();

            // Find the support leg
            supportLegIndex = -1;
            float shortestMag = Mathf.Infinity;

            for (int i = 0; i < legs.Length; i++)
            {
                float mag = Vector3.SqrMagnitude(legs[i].lastBone.solverPosition - legs[i].bones[0].solverPosition);
                if (mag < shortestMag)
                {
                    supportLegIndex = i;
                    shortestMag     = mag;
                }
            }
        }
            public void Solve(VirtualBone rootBone, Spine spine, Leg leftLeg, Leg rightLeg, Arm leftArm, Arm rightArm, int supportLegIndex, out Vector3 leftFootPosition, out Vector3 rightFootPosition, out Quaternion leftFootRotation, out Quaternion rightFootRotation, out float leftFootOffset, out float rightFootOffset, out float leftHeelOffset, out float rightHeelOffset)
            {
                if (weight <= 0f)
                {
                    leftFootPosition  = Vector3.zero;
                    rightFootPosition = Vector3.zero;
                    leftFootRotation  = Quaternion.identity;
                    rightFootRotation = Quaternion.identity;
                    leftFootOffset    = 0f;
                    rightFootOffset   = 0f;
                    leftHeelOffset    = 0f;
                    rightHeelOffset   = 0f;
                    return;
                }

                Vector3 rootUp = rootBone.solverRotation * Vector3.up;

                Vector3 leftThighPosition  = spine.pelvis.solverPosition + spine.pelvis.solverRotation * leftLeg.thighRelativeToPelvis;
                Vector3 rightThighPosition = spine.pelvis.solverPosition + spine.pelvis.solverRotation * rightLeg.thighRelativeToPelvis;

                footsteps[0].characterSpaceOffset = footDistance * Vector3.left;
                footsteps[1].characterSpaceOffset = footDistance * Vector3.right;

                Vector3 forward  = spine.faceDirection;
                Vector3 forwardY = V3Tools.ExtractVertical(forward, rootUp, 1f);

                forward -= forwardY;
                Quaternion forwardRotation = Quaternion.LookRotation(forward, rootUp);

                //centerOfMass = Vector3.Lerp(spine.pelvis.solverPosition, spine.head.solverPosition, 0.25f) + rootBone.solverRotation * offset;

                float pelvisMass = 1f;
                float headMass   = 1f;
                float armMass    = 0.2f;
                float totalMass  = pelvisMass + headMass + 2f * armMass;

                centerOfMass  = Vector3.zero;
                centerOfMass += spine.pelvis.solverPosition * pelvisMass;
                centerOfMass += spine.head.solverPosition * headMass;
                centerOfMass += leftArm.position * armMass;
                centerOfMass += rightArm.position * armMass;
                centerOfMass /= totalMass;

                centerOfMass += rootBone.solverRotation * offset;

                comVelocity     = Time.deltaTime > 0f? (centerOfMass - lastComPosition) / Time.deltaTime: Vector3.zero;
                lastComPosition = centerOfMass;
                comVelocity     = Vector3.ClampMagnitude(comVelocity, maxVelocity) * velocityFactor;
                Vector3 centerOfMassV = centerOfMass + comVelocity;

                Vector3 pelvisPositionGroundLevel = V3Tools.PointToPlane(spine.pelvis.solverPosition, rootBone.solverPosition, rootUp);
                Vector3 centerOfMassVGroundLevel  = V3Tools.PointToPlane(centerOfMassV, rootBone.solverPosition, rootUp);

                Vector3 centerOfPressure = Vector3.Lerp(footsteps[0].position, footsteps[1].position, 0.5f);

                Vector3 comDir   = centerOfMassV - centerOfPressure;
                float   comAngle = Vector3.Angle(comDir, rootBone.solverRotation * Vector3.up) * comAngleMlp;

                // Set support leg
                for (int i = 0; i < footsteps.Length; i++)
                {
                    footsteps[i].isSupportLeg = supportLegIndex == i;
                }

                // Update stepTo while stepping
                for (int i = 0; i < footsteps.Length; i++)
                {
                    if (footsteps[i].isStepping)
                    {
                        Vector3 stepTo = centerOfMassVGroundLevel + rootBone.solverRotation * footsteps[i].characterSpaceOffset;

                        if (!StepBlocked(footsteps[i].stepFrom, stepTo, rootBone.solverPosition))
                        {
                            footsteps[i].UpdateStepping(stepTo, forwardRotation, 10f);
                        }
                    }
                    else
                    {
                        footsteps[i].UpdateStanding(forwardRotation, relaxLegTwistMinAngle, relaxLegTwistSpeed);
                    }
                }

                // Triggering new footsteps
                if (CanStep())
                {
                    int   stepLegIndex = -1;
                    float bestValue    = -Mathf.Infinity;

                    for (int i = 0; i < footsteps.Length; i++)
                    {
                        if (!footsteps[i].isStepping)
                        {
                            Vector3 stepTo = centerOfMassVGroundLevel + rootBone.solverRotation * footsteps[i].characterSpaceOffset;

                            float   legLength = i == 0? leftLeg.mag: rightLeg.mag;
                            Vector3 thighPos  = i == 0? leftThighPosition: rightThighPosition;

                            float thighDistance = Vector3.Distance(footsteps[i].position, thighPos);

                            bool lengthStep = false;
                            if (thighDistance >= legLength * maxLegStretch)                              // * 0.95f) {
                            {
                                stepTo     = pelvisPositionGroundLevel + rootBone.solverRotation * footsteps[i].characterSpaceOffset;
                                lengthStep = true;
                            }

                            bool collision = false;
                            for (int n = 0; n < footsteps.Length; n++)
                            {
                                if (n != i && !lengthStep)
                                {
                                    if (Vector3.Distance(footsteps[i].position, footsteps[n].position) < 0.25f && (footsteps[i].position - stepTo).sqrMagnitude < (footsteps[n].position - stepTo).sqrMagnitude)
                                    {
                                    }
                                    else
                                    {
                                        collision = GetLineSphereCollision(footsteps[i].position, stepTo, footsteps[n].position, 0.25f);
                                    }
                                    if (collision)
                                    {
                                        break;
                                    }
                                }
                            }

                            float angle = Quaternion.Angle(forwardRotation, footsteps[i].stepToRootRot);

                            if (!collision || angle > angleThreshold)
                            {
                                float stepDistance = Vector3.Distance(footsteps[i].position, stepTo);
                                float sT           = Mathf.Lerp(stepThreshold, stepThreshold * 0.1f, comAngle * 0.015f);
                                if (lengthStep)
                                {
                                    sT *= 0.5f;
                                }
                                if (i == 0)
                                {
                                    sT *= 0.9f;
                                }

                                if (!StepBlocked(footsteps[i].position, stepTo, rootBone.solverPosition))
                                {
                                    if (stepDistance > sT || angle > angleThreshold)
                                    {
                                        float value = 0f;

                                        value -= stepDistance;

                                        if (value > bestValue)
                                        {
                                            stepLegIndex = i;
                                            bestValue    = value;
                                        }
                                    }
                                }
                            }
                        }
                    }

                    if (stepLegIndex != -1)
                    {
                        Vector3 stepTo = centerOfMassVGroundLevel + rootBone.solverRotation * footsteps[stepLegIndex].characterSpaceOffset;
                        footsteps[stepLegIndex].stepSpeed = UnityEngine.Random.Range(stepSpeed, stepSpeed * 1.5f);
                        footsteps[stepLegIndex].StepTo(stepTo, forwardRotation);
                    }
                }

                footsteps[0].Update(stepInterpolation, onLeftFootstep);
                footsteps[1].Update(stepInterpolation, onRightFootstep);

                leftFootPosition  = footsteps[0].position;
                rightFootPosition = footsteps[1].position;

                leftFootPosition  = V3Tools.PointToPlane(leftFootPosition, leftLeg.lastBone.readPosition, rootUp);
                rightFootPosition = V3Tools.PointToPlane(rightFootPosition, rightLeg.lastBone.readPosition, rootUp);

                leftFootOffset  = stepHeight.Evaluate(footsteps[0].stepProgress);
                rightFootOffset = stepHeight.Evaluate(footsteps[1].stepProgress);

                leftHeelOffset  = heelHeight.Evaluate(footsteps[0].stepProgress);
                rightHeelOffset = heelHeight.Evaluate(footsteps[1].stepProgress);

                leftFootRotation  = footsteps[0].rotation;
                rightFootRotation = footsteps[1].rotation;
            }