// Move and rotate the pelvis private void TranslatePelvis(Leg[] legs, Vector3 deltaPosition, Quaternion deltaRotation, float scale) { // Rotation Vector3 p = head.solverPosition; deltaRotation = QuaTools.ClampRotation(deltaRotation, chestClampWeight, 2); Quaternion r = Quaternion.Slerp(Quaternion.identity, deltaRotation, bodyRotStiffness * rotationWeight); 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; if (scale > 0f) { deltaY /= scale; } float backOffset = deltaY * -moveBodyBackWhenCrouching * headHeight; deltaPosition += m * backOffset; MovePosition(LimitPelvisPosition(legs, pelvis.solverPosition + deltaPosition * bodyPosStiffness * positionWeight, false)); }
public void Update(IKSolverFullBodyBiped solver, float w, float deltaTime) { if (this.transform == null || this.relativeTo == null) { return; } Vector3 a = this.relativeTo.InverseTransformDirection(this.transform.position - this.relativeTo.position); if (this.firstUpdate) { this.lastRelativePos = a; this.firstUpdate = false; } Vector3 vector = (a - this.lastRelativePos) / deltaTime; this.smoothDelta = ((this.speed > 0f) ? Vector3.Lerp(this.smoothDelta, vector, deltaTime * this.speed) : vector); Vector3 v = this.relativeTo.TransformDirection(this.smoothDelta); Vector3 a2 = V3Tools.ExtractVertical(v, solver.GetRoot().up, this.verticalWeight) + V3Tools.ExtractHorizontal(v, solver.GetRoot().up, this.horizontalWeight); for (int i = 0; i < this.effectorLinks.Length; i++) { solver.GetEffector(this.effectorLinks[i].effector).positionOffset += a2 * w * this.effectorLinks[i].weight; } this.lastRelativePos = a; }
void FixedUpdate() { gravity = GetGravity(); verticalVelocity = V3Tools.ExtractVertical(_rigidbody.velocity, gravity, 1f); velocityY = verticalVelocity.magnitude; if (Vector3.Dot(verticalVelocity, gravity) > 0f) { velocityY = -velocityY; } // Smoothing out the fixed time step _rigidbody.interpolation = smoothPhysics ? RigidbodyInterpolation.Interpolate : RigidbodyInterpolation.None; MoveFixed(blackboard.fixedDeltaPosition); Rotate(); GroundCheck(); // detect and stick to ground // Friction if (blackboard.input == Vector2.zero && groundDistance < airborneThreshold * 0.5f) { HighFriction(); } else { ZeroFriction(); } bool stopSlide = onGround && groundDistance < airborneThreshold * 0.5f; // Individual gravity if (gravityTarget != null) { _rigidbody.useGravity = false; if (!stopSlide) { _rigidbody.AddForce(gravity); } } if (stopSlide) { _rigidbody.useGravity = false; _rigidbody.velocity = Vector3.zero; } else if (gravityTarget == null) { _rigidbody.useGravity = true; } // Scale the capsule colllider while crouching ScaleCapsule(blackboard.isCrouching ? crouchCapsuleScaleMlp : 1f); fixedFrame = true; }
private void MoveFixed(Vector3 deltaPosition) { // Process horizontal wall-running WallRun(); Vector3 velocity = fixedDeltaTime > 0f? deltaPosition / fixedDeltaTime: Vector3.zero; // Add velocity of the rigidbody the character is standing on if (!fullRootMotion) { velocity += V3Tools.ExtractHorizontal(platformVelocity, gravity, 1f); if (onGround) { // Rotate velocity to ground tangent if (velocityToGroundTangentWeight > 0f) { Quaternion rotation = Quaternion.FromToRotation(transform.up, normal); velocity = Quaternion.Lerp(Quaternion.identity, rotation, velocityToGroundTangentWeight) * velocity; } } else { // Air move //Vector3 airMove = new Vector3 (userControl.state.move.x * airSpeed, 0f, userControl.state.move.z * airSpeed); Vector3 airMove = V3Tools.ExtractHorizontal(userControl.state.move * airSpeed, gravity, 1f); velocity = Vector3.Lerp(r.velocity, airMove, Time.deltaTime * airControl); } if (onGround && Time.time > jumpEndTime) { r.velocity = r.velocity - transform.up * stickyForce * Time.deltaTime; } // Vertical velocity Vector3 verticalVelocity = V3Tools.ExtractVertical(r.velocity, gravity, 1f); Vector3 horizontalVelocity = V3Tools.ExtractHorizontal(velocity, gravity, 1f); if (onGround) { if (Vector3.Dot(verticalVelocity, gravity) < 0f) { verticalVelocity = Vector3.ClampMagnitude(verticalVelocity, maxVerticalVelocityOnGround); } } r.velocity = horizontalVelocity + verticalVelocity; } else { r.velocity = velocity; } // Dampering forward speed on the slopes (Not working since Unity 2017.2) //float slopeDamper = !onGround? 1f: GetSlopeDamper(-deltaPosition / Time.deltaTime, normal); //forwardMlp = Mathf.Lerp(forwardMlp, slopeDamper, Time.deltaTime * 5f); forwardMlp = 1f; }
/* * Pulling the body with the hands * */ private void PullBody() { // Getting the body positionOffset if (pullBodyVertical != 0f || pullBodyHorizontal != 0f) { Vector3 offset = GetBodyOffset(); bodyEffector.positionOffset += V3Tools.ExtractVertical(offset, root.up, pullBodyVertical) + V3Tools.ExtractHorizontal(offset, root.up, pullBodyHorizontal); } }
private void PullBody() { if (this.iterations < 1) { return; } if (this.pullBodyVertical != 0f || this.pullBodyHorizontal != 0f) { Vector3 bodyOffset = this.GetBodyOffset(); this.bodyEffector.positionOffset += V3Tools.ExtractVertical(bodyOffset, this.root.up, this.pullBodyVertical) + V3Tools.ExtractHorizontal(bodyOffset, this.root.up, this.pullBodyHorizontal); } }
private void MoveFixed(Vector3 deltaPosition) { // Process horizontal wall-running WallRun(); Vector3 velocity = deltaPosition / Time.deltaTime; // Add velocity of the rigidbody the character is standing on velocity += V3Tools.ExtractHorizontal(platformVelocity, gravity, 1f); if (onGround) { // Rotate velocity to ground tangent if (velocityToGroundTangentWeight > 0f) { Quaternion rotation = Quaternion.FromToRotation(transform.up, normal); velocity = Quaternion.Lerp(Quaternion.identity, rotation, velocityToGroundTangentWeight) * velocity; } } else { // Air move Vector3 airMove = V3Tools.ExtractHorizontal(blackboard.input * airSpeed, gravity, 1f); velocity = Vector3.Lerp(_rigidbody.velocity, airMove, Time.deltaTime * airControl); } if (onGround && Time.time > jumpEndTime) { _rigidbody.velocity = _rigidbody.velocity - transform.up * stickyForce * Time.deltaTime; } // Vertical velocity Vector3 verticalVelocity = V3Tools.ExtractVertical(_rigidbody.velocity, gravity, 1f); Vector3 horizontalVelocity = V3Tools.ExtractHorizontal(velocity, gravity, 1f); if (onGround) { if (Vector3.Dot(verticalVelocity, gravity) < 0f) { verticalVelocity = Vector3.ClampMagnitude(verticalVelocity, maxVerticalVelocityOnGround); } } //rb.velocity = horizontalVelocity + verticalVelocity; // Dampering forward speed on the slopes float slopeDamper = !onGround ? 1f : GetSlopeDamper(-deltaPosition / Time.deltaTime, normal); forwardMlp = Mathf.Lerp(forwardMlp, slopeDamper, Time.deltaTime * 5f); }
public override void PreSolve(float scale) { if (headTarget != null) { IKPositionHead = headTarget.position; IKRotationHead = headTarget.rotation; } if (chestGoal != null) { goalPositionChest = chestGoal.position; } if (pelvisTarget != null) { IKPositionPelvis = pelvisTarget.position; IKRotationPelvis = pelvisTarget.rotation; } // Use animated head height range if (useAnimatedHeadHeightWeight > 0f && useAnimatedHeadHeightRange > 0f) { Vector3 rootUp = rootRotation * Vector3.up; if (animatedHeadHeightBlend > 0f) { float headTargetVOffset = V3Tools.ExtractVertical(IKPositionHead - head.solverPosition, rootUp, 1f).magnitude; float abs = Mathf.Abs(headTargetVOffset); abs = Mathf.Max(abs - useAnimatedHeadHeightRange * scale, 0f); float f = Mathf.Lerp(0f, 1f, abs / (animatedHeadHeightBlend * scale)); f = Interp.Float(1f - f, InterpolationMode.InOutSine); Vector3 toHeadPos = head.solverPosition - IKPositionHead; IKPositionHead += V3Tools.ExtractVertical(toHeadPos, rootUp, f * useAnimatedHeadHeightWeight); } else { IKPositionHead += V3Tools.ExtractVertical(head.solverPosition - IKPositionHead, rootUp, useAnimatedHeadHeightWeight); } } headPosition = V3Tools.Lerp(head.solverPosition, IKPositionHead, positionWeight); headRotation = QuaTools.Lerp(head.solverRotation, IKRotationHead, rotationWeight); pelvisRotation = QuaTools.Lerp(pelvis.solverRotation, IKRotationPelvis, rotationWeight); }
// 1 is writhe animation, 0 is catch fall private float GetBlendTarget(float groundHeight) { if (groundHeight > writheHeight) { return(1f); } Vector3 verticalVelocity = V3Tools.ExtractVertical(puppetMaster.muscles[0].rigidbody.velocity, puppetMaster.targetRoot.up, 1f); float velocityY = verticalVelocity.magnitude; if (Vector3.Dot(verticalVelocity, puppetMaster.targetRoot.up) < 0f) { velocityY = -velocityY; } if (velocityY > writheYVelocity) { return(1f); } //if (puppetMaster.muscles[0].rigidbody.velocity.y > writheYVelocity) return 1f; return(0f); }
// Update the Body public void Update(IKSolverFullBodyBiped solver, float w, float deltaTime) { if (transform == null || relativeTo == null) { return; } // Find the relative position of the transform Vector3 relativePos = relativeTo.InverseTransformDirection(transform.position - relativeTo.position); // Initiating if (firstUpdate) { lastRelativePos = relativePos; firstUpdate = false; } // Find how much the relative position has changed Vector3 delta = (relativePos - lastRelativePos) / deltaTime; // Smooth the change smoothDelta = speed <= 0f? delta: Vector3.Lerp(smoothDelta, delta, deltaTime * speed); // Convert to world space Vector3 worldDelta = relativeTo.TransformDirection(smoothDelta); // Extract horizontal and vertical offset Vector3 offset = V3Tools.ExtractVertical(worldDelta, solver.GetRoot().up, verticalWeight) + V3Tools.ExtractHorizontal(worldDelta, solver.GetRoot().up, horizontalWeight); // Apply the amplitude to the effector links for (int i = 0; i < effectorLinks.Length; i++) { solver.GetEffector(effectorLinks[i].effector).positionOffset += offset * w * effectorLinks[i].weight; } lastRelativePos = relativePos; }
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, 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]; this.hasLegs = hasLegs; 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]; anchorRelativeToPelvis = Quaternion.Inverse(pelvisRot) * rotations[0]; faceDirection = rotations[0] * Vector3.forward; IKPositionHead = headPos; IKRotationHead = headRot; IKPositionPelvis = pelvisPos; IKRotationPelvis = pelvisRot; goalPositionChest = chestPos + rotations[0] * Vector3.forward; } // Forward and up axes pelvisRelativeRotation = Quaternion.Inverse(headRot) * pelvisRot; chestRelativeRotation = Quaternion.Inverse(headRot) * chestRot; chestForward = Quaternion.Inverse(chestRot) * (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); if (spine.rootHeadingOffset != 0f) { forwardRotation = Quaternion.AngleAxis(spine.rootHeadingOffset, rootUp) * forwardRotation; } //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, stepThreshold); } } 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; }
void FixedUpdate() { if (initialised) { gravity = GetGravity(); verticalVelocity = V3Tools.ExtractVertical(r.velocity, gravity, 1f); velocityY = verticalVelocity.magnitude; if (Vector3.Dot(verticalVelocity, gravity) > 0f) { velocityY = -velocityY; } /* * if (animator != null && animator.updateMode == AnimatorUpdateMode.AnimatePhysics) { * smoothPhysics = false; * characterAnimation.smoothFollow = false; * } */ // Smoothing out the fixed time step r.interpolation = smoothPhysics ? RigidbodyInterpolation.Interpolate : RigidbodyInterpolation.None; characterAnimation.smoothFollow = smoothPhysics; // Move MoveFixed(fixedDeltaPosition); fixedDeltaPosition = Vector3.zero; //r.MoveRotation(transform.rotation * fixedDeltaRotation); fixedDeltaRotation = Quaternion.identity; //Rotate(); GroundCheck(); // detect and stick to ground // Friction if (userControl.state.move == Vector3.zero && groundDistance < airborneThreshold * 0.5f) { HighFriction(); } else { ZeroFriction(); } bool stopSlide = onGround && userControl.state.move == Vector3.zero && r.velocity.magnitude < 0.5f && groundDistance < airborneThreshold * 0.5f; // Individual gravity if (gravityTarget != null) { r.useGravity = false; if (!stopSlide) { r.AddForce(gravity); } } if (stopSlide) { r.useGravity = false; r.velocity = Vector3.zero; } else if (gravityTarget == null) { r.useGravity = true; } if (onGround) { // Jumping animState.jump = Jump(); jumpReleased = false; doubleJumped = false; } else { if (!userControl.state.jump) { jumpReleased = true; } //r.AddForce(gravity * gravityMultiplier); if (jumpReleased && userControl.state.jump && !doubleJumped && doubleJumpEnabled) { jumpEndTime = Time.time + 0.1f; animState.doubleJump = true; Vector3 jumpVelocity = userControl.state.move * airSpeed; r.velocity = jumpVelocity; r.velocity += transform.up * jumpPower * doubleJumpPowerMlp; doubleJumped = true; } } // Scale the capsule colllider while crouching ScaleCapsule(userControl.state.crouch ? crouchCapsuleScaleMlp : 1f); fixedFrame = true; } }
void FixedUpdate() { //获取重力向量 gravity = GetGravity(); //将刚体速度投影到重力上去 verticalVelocity = V3Tools.ExtractVertical(r.velocity, gravity, 1f); Debug.Log("verticalVelocity = " + verticalVelocity); velocityY = verticalVelocity.magnitude; //速度与重力同向, Y轴速度 velocityY 为负 if (Vector3.Dot(verticalVelocity, gravity) > 0f) { velocityY = -velocityY; } // if (animator != null && animator.updateMode == AnimatorUpdateMode.AnimatePhysics) { smoothPhysics = false; characterAnimation.smoothFollow = false; } // Smoothing out the fixed time step r.interpolation = smoothPhysics? RigidbodyInterpolation.Interpolate: RigidbodyInterpolation.None; characterAnimation.smoothFollow = smoothPhysics; // Move MoveFixed(fixedDeltaPosition); fixedDeltaPosition = Vector3.zero; transform.rotation *= fixedDeltaRotation; fixedDeltaRotation = Quaternion.identity; Rotate(); GroundCheck(); // detect and stick to ground // Friction if (userControl.state.move == Vector3.zero && groundDistance < airborneThreshold * 0.5f) { HighFriction(); } else { ZeroFriction(); } // Individual gravity if (gravityTarget != null) { r.useGravity = false; r.AddForce(gravity); } if (onGround) { // Jumping animState.jump = Jump(); } else { r.AddForce(gravity * gravityMultiplier); } // Scale the capsule colllider while crouching ScaleCapsule(userControl.state.crouch? crouchCapsuleScaleMlp: 1f); fixedFrame = true; }
private void Solve() { if (scale <= 0f) { Debug.LogError("VRIK solver scale <= 0, can not solve!"); return; } if (lastLocomotionWeight <= 0f && locomotion.weight > 0f) { locomotion.Reset(readPositions, readRotations); } spine.SetLOD(LOD); if (hasArms) { foreach (Arm arm in arms) { arm.SetLOD(LOD); } } if (hasLegs) { foreach (Leg leg in legs) { leg.SetLOD(LOD); } } // Pre-Solving spine.PreSolve(scale); if (hasArms) { foreach (Arm arm in arms) { arm.PreSolve(scale); } } if (hasLegs) { foreach (Leg leg in legs) { leg.PreSolve(scale); } } // Applying spine and arm offsets if (hasArms) { foreach (Arm arm in arms) { arm.ApplyOffsets(scale); } } spine.ApplyOffsets(scale); // Spine spine.Solve(animator, rootBone, legs, arms, scale); if (hasLegs && spine.pelvisPositionWeight > 0f && plantFeet) { Warning.Log("If VRIK 'Pelvis Position Weight' is > 0, 'Plant Feet' should be disabled to improve performance and stability.", root); } float deltaTime = Time.deltaTime; // Locomotion if (hasLegs) { if (locomotion.weight > 0f) { switch (locomotion.mode) { case Locomotion.Mode.Procedural: 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_Procedural(rootBone, spine, leftLeg, rightLeg, leftArm, rightArm, supportLegIndex, out leftFootPosition, out rightFootPosition, out leftFootRotation, out rightFootRotation, out leftFootOffset, out rightFootOffset, out leftHeelOffset, out rightHeelOffset, scale, deltaTime); 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 * deltaTime * 2f * locomotion.weight; p = Vector3.Lerp(p, footPositionC, deltaTime * locomotion.rootSpeed * locomotion.weight); rootBone.solverPosition = p; rootVelocity += (footPositionC - rootBone.solverPosition) * deltaTime * 10f; Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f); rootVelocity -= rootVelocityV; float bodyYOffset = Mathf.Min(leftFootOffset + rightFootOffset, locomotion.maxBodyYOffset * scale); bodyOffset = Vector3.Lerp(bodyOffset, root.up * bodyYOffset, deltaTime * 3f); bodyOffset = Vector3.Lerp(Vector3.zero, bodyOffset, locomotion.weight); break; case Locomotion.Mode.Animated: if (lastLocomotionWeight <= 0f) { locomotion.Reset_Animated(readPositions); } locomotion.Solve_Animated(this, scale, deltaTime); break; } } else { if (lastLocomotionWeight > 0f) { locomotion.Reset_Animated(readPositions); } } } lastLocomotionWeight = locomotion.weight; // Legs if (hasLegs) { foreach (Leg leg in legs) { leg.ApplyOffsets(scale); } if (!plantFeet || LOD > 0) { 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(true); } } else { for (int i = 0; i < 2; i++) { spine.InverseTranslateToHead(legs, true, true, bodyOffset, 1f); foreach (Leg leg in legs) { leg.TranslateRoot(spine.pelvis.solverPosition, spine.pelvis.solverRotation); } foreach (Leg leg in legs) { leg.Solve(i == 0); } } } } else { spine.InverseTranslateToHead(legs, false, false, bodyOffset, 1f); } // Arms if (hasArms) { 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(); if (hasLegs) { foreach (Leg leg in legs) { leg.ResetOffsets(); } } if (hasArms) { foreach (Arm arm in arms) { arm.ResetOffsets(); } } if (hasLegs) { spine.pelvisPositionOffset += GetPelvisOffset(deltaTime); spine.chestPositionOffset += spine.pelvisPositionOffset; //spine.headPositionOffset += spine.pelvisPositionOffset; } Write(); // Find the support leg if (hasLegs) { 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; } } } }
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); rootVelocity += (footPositionC - rootBone.solverPosition) * Time.deltaTime * 10f; Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f); rootVelocity -= rootVelocityV; /* * rootBone.solverPosition += rootVelocity * Time.deltaTime * 2f * locomotion.weight; * * //rootBone.solverPosition = Vector3.SmoothDamp(rootBone.solverPosition, footPositionC, ref rootV, locomotion.rootSDampTime); * * rootBone.solverPosition = Vector3.Lerp(rootBone.solverPosition, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight); */ Vector3 p = rootBone.solverPosition + rootVelocity * Time.deltaTime * 2f * locomotion.weight; p = Vector3.Lerp(p, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight); rootBone.solverPosition = p; 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(); } } } // Stretching(); // Arms for (int i = 0; i < arms.Length; i++) { arms[i].TranslateRoot(spine.chest.solverPosition, spine.chest.solverRotation); 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_Animated(IKSolverVR solver, float scale, float deltaTime) { if (animator == null) { Debug.LogError("VRIK cannot find Animator on the VRIK root gameobject.", solver.root); return; } if (deltaTime <= 0f) { return; } if (!animator.enabled) { Debug.LogWarning("Trying to use VRIK animated locomotion with a disabled animator!", solver.root); return; } // Root up vector Vector3 rootUp = solver.rootBone.solverRotation * Vector3.up; // Substract any motion from parent transforms Vector3 externalDelta = solver.rootBone.solverPosition - lastEndRootPos; externalDelta -= animator.deltaPosition; // Head target position Vector3 headTargetPos = solver.spine.headPosition; Vector3 standOffsetWorld = solver.rootBone.solverRotation * new Vector3(standOffset.x, 0f, standOffset.y) * scale; headTargetPos += standOffsetWorld; if (firstFrame) { lastHeadTargetPos = headTargetPos; firstFrame = false; } // Head target velocity Vector3 headTargetVelocity = (headTargetPos - lastHeadTargetPos) / deltaTime; lastHeadTargetPos = headTargetPos; headTargetVelocity = V3Tools.Flatten(headTargetVelocity, rootUp); // Head target offset Vector3 offset = headTargetPos - solver.rootBone.solverPosition; offset -= externalDelta; offset -= lastCorrection; offset = V3Tools.Flatten(offset, rootUp); // Turning Vector3 headForward = (solver.spine.IKRotationHead * solver.spine.anchorRelativeToHead) * Vector3.forward; headForward.y = 0f; Vector3 headForwardLocal = Quaternion.Inverse(solver.rootBone.solverRotation) * headForward; float angle = Mathf.Atan2(headForwardLocal.x, headForwardLocal.z) * Mathf.Rad2Deg; angle += solver.spine.rootHeadingOffset; float turnTarget = angle / 90f; bool isTurning = true; if (Mathf.Abs(turnTarget) < 0.2f) { turnTarget = 0f; isTurning = false; } turn = Mathf.Lerp(turn, turnTarget, Time.deltaTime * 3f); animator.SetFloat(VRIK_Turn, turn * 2f); // Local Velocity, animation smoothing Vector3 velocityLocalTarget = Quaternion.Inverse(solver.readRotations[0]) * (headTargetVelocity + offset); velocityLocalTarget *= weight * stepLengthMlp; float animationSmoothTimeTarget = isTurning && !isMoving ? 0.2f : animationSmoothTime; currentAnimationSmoothTime = Mathf.Lerp(currentAnimationSmoothTime, animationSmoothTimeTarget, deltaTime * 20f); velocityLocal = Vector3.SmoothDamp(velocityLocal, velocityLocalTarget, ref velocityLocalV, currentAnimationSmoothTime, Mathf.Infinity, deltaTime); float velLocalMag = velocityLocal.magnitude / stepLengthMlp; //animator.SetBool("VRIK_StartWithRightFoot", velocityLocal.x >= 0f); animator.SetFloat(VRIK_Horizontal, velocityLocal.x / scale); animator.SetFloat(VRIK_Vertical, velocityLocal.z / scale); // Is Moving float m = moveThreshold * scale; if (isMoving) { m *= 0.9f; } bool isMovingRaw = velocityLocal.sqrMagnitude > m * m; if (isMovingRaw) { stopMoveTimer = 0f; } else { stopMoveTimer += deltaTime; } isMoving = stopMoveTimer < 0.05f; // Max root angle float maxRootAngleTarget = isMoving ? maxRootAngleMoving : maxRootAngleStanding; solver.spine.maxRootAngle = Mathf.SmoothDamp(solver.spine.maxRootAngle, maxRootAngleTarget, ref maxRootAngleV, 0.2f, Mathf.Infinity, deltaTime); animator.SetBool(VRIK_IsMoving, isMoving); // Animation speed Vector3 currentRootPos = solver.rootBone.solverPosition; currentRootPos -= externalDelta; currentRootPos -= lastCorrection; Vector3 rootVelocity = (currentRootPos - lastSpeedRootPos) / deltaTime; lastSpeedRootPos = solver.rootBone.solverPosition; float rootVelocityMag = rootVelocity.magnitude; float animSpeedTarget = minAnimationSpeed; if (rootVelocityMag > 0f && isMovingRaw) { animSpeedTarget = animSpeed * (velLocalMag / rootVelocityMag); } animSpeedTarget = Mathf.Clamp(animSpeedTarget, minAnimationSpeed, maxAnimationSpeed); animSpeed = Mathf.SmoothDamp(animSpeed, animSpeedTarget, ref animSpeedV, 0.05f, Mathf.Infinity, deltaTime); animSpeed = Mathf.Lerp(1f, animSpeed, weight); animator.SetFloat(VRIK_Speed, animSpeed); // Is Stopping AnimatorTransitionInfo transInfo = animator.GetAnimatorTransitionInfo(0); bool isStopping = transInfo.IsUserName("VRIK_Stop"); // Root lerp speed float rootLerpSpeedTarget = 0; if (isMoving) { rootLerpSpeedTarget = rootLerpSpeedWhileMoving; } if (isStopping) { rootLerpSpeedTarget = rootLerpSpeedWhileStopping; } if (isTurning) { rootLerpSpeedTarget = rootLerpSpeedWhileTurning; } rootLerpSpeedTarget *= Mathf.Max(headTargetVelocity.magnitude, 0.2f); rootLerpSpeed = Mathf.Lerp(rootLerpSpeed, rootLerpSpeedTarget, deltaTime * 20f); // Root lerp and limits headTargetPos += V3Tools.ExtractVertical(solver.rootBone.solverPosition - headTargetPos, rootUp, 1f); if (maxRootOffset > 0f) { // Lerp towards head target position Vector3 p = solver.rootBone.solverPosition; if (rootLerpSpeed > 0f) { solver.rootBone.solverPosition = Vector3.Lerp(solver.rootBone.solverPosition, headTargetPos, rootLerpSpeed * deltaTime * weight); } lastCorrection = solver.rootBone.solverPosition - p; // Max offset offset = headTargetPos - solver.rootBone.solverPosition; offset = V3Tools.Flatten(offset, rootUp); float offsetMag = offset.magnitude; if (offsetMag > maxRootOffset) { lastCorrection += (offset - (offset / offsetMag) * maxRootOffset) * weight; solver.rootBone.solverPosition += lastCorrection; } } else { // Snap to head target position lastCorrection = (headTargetPos - solver.rootBone.solverPosition) * weight; solver.rootBone.solverPosition += lastCorrection; } lastEndRootPos = solver.rootBone.solverPosition; }