// 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;
}
