示例#1
0
            // Move and rotate the pelvis
            private void TranslatePelvis(Leg[] legs, Vector3 deltaPosition, Quaternion deltaRotation)
            {
                // Rotation
                Vector3 p = head.solverPosition;

                deltaRotation = QuaTools.ClampRotation(deltaRotation, chestClampWeight, 2);

                Quaternion r = Quaternion.Slerp(Quaternion.identity, deltaRotation, bodyRotStiffness);

                r = Quaternion.Slerp(r, QuaTools.FromToRotation(pelvis.solverRotation, IKRotationPelvis), pelvisRotationWeight);
                VirtualBone.RotateAroundPoint(bones, 0, pelvis.solverPosition, pelvisRotationOffset * r);

                deltaPosition -= head.solverPosition - p;

                // Position
                // Move the body back when head is moving down
                Vector3 m          = rootRotation * Vector3.forward;
                float   deltaY     = V3Tools.ExtractVertical(deltaPosition, rootRotation * Vector3.up, 1f).magnitude;
                float   backOffset = deltaY * -moveBodyBackWhenCrouching * headHeight;

                deltaPosition += m * backOffset;

                /*
                 * if (backOffset < 0f) {
                 *      foreach (Leg leg in legs) leg.heelPositionOffset += Vector3.up * backOffset * backOffset; // TODO Ignoring root rotation
                 * }
                 */

                MovePosition(LimitPelvisPosition(legs, pelvis.solverPosition + deltaPosition * bodyPosStiffness, false));
            }
            public override void PreSolve()
            {
                if (target != null)
                {
                    IKPosition = target.position;
                    IKRotation = target.rotation;
                }

                position = V3Tools.Lerp(hand.solverPosition, IKPosition, positionWeight);
                rotation = QuaTools.Lerp(hand.solverRotation, IKRotation, rotationWeight);

                shoulder.axis        = shoulder.axis.normalized;
                forearmRelToUpperArm = Quaternion.Inverse(upperArm.solverRotation) * forearm.solverRotation;
            }
示例#3
0
        private void WriteTransforms()
        {
            for (int i = 0; i < solverTransforms.Length; i++)
            {
                if (solverTransforms[i] != null)
                {
                    bool isRootOrPelvis = i < 2;
                    bool isArm          = i > 5 && i < 14;
                    if (isRootOrPelvis || isArm)
                    {
                        solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
                    }

                    solverTransforms[i].rotation = QuaTools.Lerp(solverTransforms[i].rotation, GetRotation(i), IKPositionWeight);
                }
            }
        }
示例#4
0
            public override void PreSolve()
            {
                if (headTarget != null)
                {
                    IKPositionHead = headTarget.position;
                    IKRotationHead = headTarget.rotation;
                }

                if (chestGoal != null)
                {
                    goalPositionChest = chestGoal.position;
                }

                if (pelvisTarget != null)
                {
                    IKPositionPelvis = pelvisTarget.position;
                    IKRotationPelvis = pelvisTarget.rotation;
                }

                headPosition = V3Tools.Lerp(head.solverPosition, IKPositionHead, positionWeight);
                headRotation = QuaTools.Lerp(head.solverRotation, IKRotationHead, rotationWeight);
            }
示例#5
0
            public override void ApplyOffsets()
            {
                headPosition += headPositionOffset;

                Vector3 rootUp = rootRotation * Vector3.up;

                if (rootUp == Vector3.up)
                {
                    headPosition.y = Math.Max(rootPosition.y + minHeadHeight, headPosition.y);
                }
                else
                {
                    Vector3 toHead = headPosition - rootPosition;
                    Vector3 hor    = V3Tools.ExtractHorizontal(toHead, rootUp, 1f);
                    Vector3 ver    = toHead - hor;
                    float   dot    = Vector3.Dot(ver, rootUp);
                    if (dot > 0f)
                    {
                        if (ver.magnitude < minHeadHeight)
                        {
                            ver = ver.normalized * minHeadHeight;
                        }
                    }
                    else
                    {
                        ver = -ver.normalized * minHeadHeight;
                    }

                    headPosition = rootPosition + hor + ver;
                }

                headRotation = headRotationOffset * headRotation;

                headDeltaPosition   = headPosition - head.solverPosition;
                pelvisDeltaRotation = QuaTools.FromToRotation(pelvis.solverRotation, headRotation * pelvisRelativeRotation);

                anchorRotation = headRotation * anchorRelativeToHead;
            }
示例#6
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;
                }
            }
        }
示例#7
0
            protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, int rootIndex, int index)
            {
                Vector3    pelvisPos = positions[index];
                Quaternion pelvisRot = rotations[index];
                Vector3    spinePos  = positions[index + 1];
                Quaternion spineRot  = rotations[index + 1];
                Vector3    chestPos  = positions[index + 2];
                Quaternion chestRot  = rotations[index + 2];
                Vector3    neckPos   = positions[index + 3];
                Quaternion neckRot   = rotations[index + 3];
                Vector3    headPos   = positions[index + 4];
                Quaternion headRot   = rotations[index + 4];

                if (!hasChest)
                {
                    chestPos = spinePos;
                    chestRot = spineRot;
                }

                if (!initiated)
                {
                    this.hasChest = hasChest;
                    this.hasNeck  = hasNeck;
                    headHeight    = V3Tools.ExtractVertical(headPos - positions[0], rotations[0] * Vector3.up, 1f).magnitude;

                    int boneCount = 3;
                    if (hasChest)
                    {
                        boneCount++;
                    }
                    if (hasNeck)
                    {
                        boneCount++;
                    }
                    bones = new VirtualBone[boneCount];

                    chestIndex = hasChest? 2: 1;

                    neckIndex = 1;
                    if (hasChest)
                    {
                        neckIndex++;
                    }
                    if (hasNeck)
                    {
                        neckIndex++;
                    }

                    headIndex = 2;
                    if (hasChest)
                    {
                        headIndex++;
                    }
                    if (hasNeck)
                    {
                        headIndex++;
                    }

                    bones[0] = new VirtualBone(pelvisPos, pelvisRot);
                    bones[1] = new VirtualBone(spinePos, spineRot);
                    if (hasChest)
                    {
                        bones[chestIndex] = new VirtualBone(chestPos, chestRot);
                    }
                    if (hasNeck)
                    {
                        bones[neckIndex] = new VirtualBone(neckPos, neckRot);
                    }
                    bones[headIndex] = new VirtualBone(headPos, headRot);

                    pelvisRotationOffset = Quaternion.identity;
                    chestRotationOffset  = Quaternion.identity;
                    headRotationOffset   = Quaternion.identity;

                    anchorRelativeToHead = Quaternion.Inverse(headRot) * rotations[0];

                    // Forward and up axes
                    pelvisRelativeRotation = Quaternion.Inverse(headRot) * pelvisRot;
                    chestRelativeRotation  = Quaternion.Inverse(headRot) * chestRot;

                    chestForward = Quaternion.Inverse(chestRot) * (rotations[0] * Vector3.forward);

                    faceDirection = rotations[0] * Vector3.forward;

                    IKPositionHead    = headPos;
                    IKRotationHead    = headRot;
                    IKPositionPelvis  = pelvisPos;
                    IKRotationPelvis  = pelvisRot;
                    goalPositionChest = chestPos + rotations[0] * Vector3.forward;
                }

                bones[0].Read(pelvisPos, pelvisRot);
                bones[1].Read(spinePos, spineRot);
                if (hasChest)
                {
                    bones[chestIndex].Read(chestPos, chestRot);
                }
                if (hasNeck)
                {
                    bones[neckIndex].Read(neckPos, neckRot);
                }
                bones[headIndex].Read(headPos, headRot);

                float spineLength = Vector3.Distance(pelvisPos, headPos);

                sizeMlp = spineLength / 0.7f;
            }
            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;
            }