// 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 LimitBend(float solverWeight, float positionWeight)
{
if (!this.initiated)
{
return;
}
Vector3 vector = this.bone1.rotation * -this.defaultLocalDirection;
Vector3 fromDirection = this.bone3.position - this.bone2.position;
bool flag = false;
Vector3 toDirection = V3Tools.ClampDirection(fromDirection, vector, this.clampF * solverWeight, 0, out flag);
Quaternion rotation = this.bone3.rotation;
if (flag)
{
Quaternion lhs = Quaternion.FromToRotation(fromDirection, toDirection);
this.bone2.rotation = lhs * this.bone2.rotation;
}
if (positionWeight > 0f)
{
Vector3 vector2 = this.bone2.position - this.bone1.position;
Vector3 fromDirection2 = this.bone3.position - this.bone2.position;
Vector3.OrthoNormalize(ref vector2, ref fromDirection2);
Quaternion lhs2 = Quaternion.FromToRotation(fromDirection2, vector);
this.bone2.rotation = Quaternion.Lerp(this.bone2.rotation, lhs2 * this.bone2.rotation, positionWeight * solverWeight);
}
if (flag || positionWeight > 0f)
{
this.bone3.rotation = rotation;
}
}
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;
bool isLeg = i >= 14;
if (isRootOrPelvis)
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
if (isArm || isLeg)
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
solverTransforms[i].rotation = QuaTools.Lerp(solverTransforms[i].rotation, GetRotation(i), IKPositionWeight);
}
}
}
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;
}
// Is the character grounded?
private void GroundCheck()
{
Vector3 platformVelocityTarget = Vector3.zero;
platformAngularVelocity = Vector3.zero;
float stickyForceTarget = 0f;
// Spherecasting
hit = GetSpherecastHit();
//normal = hit.normal;
normal = transform.up;
//groundDistance = r.position.y - hit.point.y;
groundDistance = Vector3.Project(r.position - hit.point, transform.up).magnitude;
// if not jumping...
bool findGround = Time.time > jumpEndTime && velocityY < jumpPower * 0.5f;
if (findGround)
{
bool g = onGround;
onGround = false;
// The distance of considering the character grounded
float groundHeight = !g? airborneThreshold * 0.5f: airborneThreshold;
//Vector3 horizontalVelocity = r.velocity;
Vector3 horizontalVelocity = V3Tools.ExtractHorizontal(r.velocity, gravity, 1f);
float velocityF = horizontalVelocity.magnitude;
if (groundDistance < groundHeight)
{
// Force the character on the ground
stickyForceTarget = groundStickyEffect * velocityF * groundHeight;
// On moving platforms
if (hit.rigidbody != null)
{
platformVelocityTarget = hit.rigidbody.GetPointVelocity(hit.point);
platformAngularVelocity = Vector3.Project(hit.rigidbody.angularVelocity, transform.up);
}
// Flag the character grounded
onGround = true;
}
}
// Interpolate the additive velocity of the platform the character might be standing on
platformVelocity = Vector3.Lerp(platformVelocity, platformVelocityTarget, Time.deltaTime * platformFriction);
stickyForce = stickyForceTarget; //Mathf.Lerp(stickyForce, stickyForceTarget, Time.deltaTime * 5f);
// remember when we were last in air, for jump delay
if (!onGround)
{
lastAirTime = Time.time;
}
}
public void ClearVelocities()
{
rigidbody.velocity = Vector3.zero;
rigidbody.angularVelocity = Vector3.zero;
targetVelocity = Vector3.zero;
targetAnimatedCenterOfMass = V3Tools.TransformPointUnscaled(target, rigidbody.centerOfMass);
}
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 SetFootToPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 heelHitPoint)
{
planeNormal = this.RotateNormal(planeNormal);
this.toHitNormal = Quaternion.FromToRotation(this.up, planeNormal);
Vector3 hitPoint = V3Tools.LineToPlane(this.transform.position + this.up * this.grounding.maxStep, -this.up, planeNormal, planePoint);
this.heightFromGround = this.GetHeightFromGround(hitPoint);
float heightFromGround = this.GetHeightFromGround(heelHitPoint);
this.heightFromGround = Mathf.Clamp(this.heightFromGround, float.NegativeInfinity, heightFromGround);
}
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);
}
}
public override void ApplyOffsets(float scale)
{
headPosition += headPositionOffset;
float mHH = minHeadHeight * scale;
Vector3 rootUp = rootRotation * Vector3.up;
if (rootUp == Vector3.up)
{
headPosition.y = Math.Max(rootPosition.y + mHH, 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 < mHH)
{
ver = ver.normalized * mHH;
}
}
else
{
ver = -ver.normalized * mHH;
}
headPosition = rootPosition + hor + ver;
}
headRotation = headRotationOffset * headRotation;
headDeltaPosition = headPosition - head.solverPosition;
pelvisDeltaRotation = QuaTools.FromToRotation(pelvis.solverRotation, headRotation * pelvisRelativeRotation);
if (pelvisRotationWeight <= 0f)
{
anchorRotation = headRotation * anchorRelativeToHead;
}
else if (pelvisRotationWeight > 0f && pelvisRotationWeight < 1f)
{
anchorRotation = Quaternion.Lerp(headRotation * anchorRelativeToHead, pelvisRotation * anchorRelativeToPelvis, pelvisRotationWeight);
}
else if (pelvisRotationWeight >= 1f)
{
anchorRotation = pelvisRotation * anchorRelativeToPelvis;
}
}
public void Read()
{
Vector3 tAM = V3Tools.TransformPointUnscaled(target, rigidbody.centerOfMass);
targetVelocity = (tAM - targetAnimatedCenterOfMass) / Time.deltaTime;
targetAnimatedCenterOfMass = tAM;
if (joint.connectedBody != null)
{
targetAnimatedRotation = targetLocalRotation * localRotationConvert;
}
targetAnimatedWorldRotation = target.rotation;
}
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;
}
private void WriteTransforms()
{
for (int i = 0; i < solverTransforms.Length; i++)
{
if (solverTransforms[i] != null)
{
if (i < 2)
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
solverTransforms[i].rotation = QuaTools.Lerp(solverTransforms[i].rotation, GetRotation(i), IKPositionWeight);
}
}
}
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);
}
// Set foot height from ground relative to a plane
private void SetFootToPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 heelHitPoint)
{
planeNormal = RotateNormal(planeNormal);
toHitNormal = Quaternion.FromToRotation(up, planeNormal);
Vector3 pointOnPlane = V3Tools.LineToPlane(transform.position + up * grounding.maxStep, -up, planeNormal, planePoint);
// Get the height offset of the point on the plane
heightFromGround = GetHeightFromGround(pointOnPlane);
// Making sure the heel doesn't penetrate the ground
float heelHeight = GetHeightFromGround(heelHitPoint);
heightFromGround = Mathf.Clamp(heightFromGround, -Mathf.Infinity, heelHeight);
}
public override void PreSolve()
{
if (headTarget != null)
{
IKPositionHead = headTarget.position;
IKRotationHead = headTarget.rotation;
}
if (pelvisTarget != null)
{
IKPositionPelvis = pelvisTarget.position;
}
headPosition = V3Tools.Lerp(head.solverPosition, IKPositionHead, positionWeight);
headRotation = QuaTools.Lerp(head.solverRotation, IKRotationHead, rotationWeight);
}
// Processing horizontal wall running
private void WallRun() {
bool canWallRun = CanWallRun();
// Remove flickering in and out of wall-running
if (wallRunWeight > 0f && !canWallRun) wallRunEndTime = Time.time;
if (Time.time < wallRunEndTime + 0.5f) canWallRun = false;
wallRunWeight = Mathf.MoveTowards(wallRunWeight, (canWallRun? 1f: 0f), Time.deltaTime * wallRunWeightSpeed);
if (wallRunWeight <= 0f) {
// Reset
if (lastWallRunWeight > 0f) {
Vector3 frw = V3Tools.ExtractHorizontal(transform.forward, gravity, 1f);
transform.rotation = Quaternion.LookRotation(frw, -gravity);
wallNormal = -gravity.normalized;
}
}
lastWallRunWeight = wallRunWeight;
if (wallRunWeight <= 0f) return;
// Make sure the character won't fall down
if (onGround && velocityY < 0f) r.velocity = V3Tools.ExtractHorizontal(r.velocity, gravity, 1f);
// transform.forward flattened
Vector3 f = V3Tools.ExtractHorizontal(transform.forward, gravity, 1f);
// Raycasting to find a walkable wall
RaycastHit velocityHit = new RaycastHit();
velocityHit.normal = -gravity.normalized;
Physics.Raycast(onGround? transform.position: capsule.bounds.center, f, out velocityHit, 3f, wallRunLayers);
// Finding the normal to rotate to
wallNormal = Vector3.Lerp(wallNormal, velocityHit.normal, Time.deltaTime * wallRunRotationSpeed);
// Clamping wall normal to max rotation angle
wallNormal = Vector3.RotateTowards(-gravity.normalized, wallNormal, wallRunMaxRotationAngle * Mathf.Deg2Rad, 0f);
// Get transform.forward ortho-normalized to the wall normal
Vector3 fW = transform.forward;
Vector3 nW = wallNormal;
Vector3.OrthoNormalize(ref nW, ref fW);
// Rotate from upright to wall normal
transform.rotation = Quaternion.Slerp(Quaternion.LookRotation(f, -gravity), Quaternion.LookRotation(fW, wallNormal), wallRunWeight);
}
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);
}
private void FixedUpdate()
{
// Add torque to the Rigidbody to make it follow the rotation target
Vector3 angularAcc = PhysXTools.GetAngularAcceleration(_rigidbody.rotation, rotationTarget.rotation);
_rigidbody.AddTorque(angularAcc * torque);
// Add snowboard-like skid drag
if (_isGrounded)
{
Vector3 velocity = _rigidbody.velocity;
Vector3 skid = V3Tools.ExtractHorizontal(velocity, _rigidbody.rotation * Vector3.up, 1f);
skid = Vector3.Project(velocity, _rigidbody.rotation * Vector3.right);
_rigidbody.velocity = velocity - Vector3.ClampMagnitude(skid * skidDrag * Time.deltaTime, skid.magnitude);
}
}
/*
* Limits the bending joint of the limb to 90 degrees from the default 90 degrees of bend direction
* */
public void LimitBend(float solverWeight, float positionWeight)
{
if (!initiated)
{
return;
}
Vector3 normalDirection = bone1.rotation * -defaultLocalDirection;
Vector3 axis2 = bone3.position - bone2.position;
// Clamp the direction from knee/elbow to foot/hand to valid range (90 degrees from right-angledly bent limb)
bool changed = false;
Vector3 clampedAxis2 = V3Tools.ClampDirection(axis2, normalDirection, clampF * solverWeight, 0, out changed);
Quaternion bone3Rotation = bone3.rotation;
if (changed)
{
Quaternion f = Quaternion.FromToRotation(axis2, clampedAxis2);
bone2.rotation = f * bone2.rotation;
}
// Rotating bend direction to normal when the limb is stretched out
if (positionWeight > 0f)
{
Vector3 normal = bone2.position - bone1.position;
Vector3 tangent = bone3.position - bone2.position;
Vector3.OrthoNormalize(ref normal, ref tangent);
Quaternion q = Quaternion.FromToRotation(tangent, normalDirection);
bone2.rotation = Quaternion.Lerp(bone2.rotation, q * bone2.rotation, positionWeight * solverWeight);
}
if (changed || positionWeight > 0f)
{
bone3.rotation = bone3Rotation;
}
}
private void WriteTransforms()
{
for (int i = 0; i < solverTransforms.Length; i++)
{
if (solverTransforms[i] != null)
{
bool isRootOrPelvis = i < 2;
bool isArmStretchable = i == 8 || i == 9 || i == 12 || i == 13;
bool isLegStretchable = (i >= 15 && i <= 17) || (i >= 19 && i <= 21);
if (LOD > 0)
{
isArmStretchable = false;
isLegStretchable = false;
}
if (isRootOrPelvis)
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
if (isArmStretchable || isLegStretchable)
{
if (IKPositionWeight < 1f)
{
Vector3 localPosition = solverTransforms[i].localPosition;
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
solverTransforms[i].localPosition = Vector3.Project(solverTransforms[i].localPosition, localPosition);
}
else
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
}
solverTransforms[i].rotation = QuaTools.Lerp(solverTransforms[i].rotation, GetRotation(i), IKPositionWeight);
}
}
}
/*
* Limits the bending joint of the limb to 90 degrees from the default 90 degrees of bend direction
* */
public void LimitBend(float solverWeight)
{
Vector3 normalDirection = bone1.rotation * -defaultLocalDirection;
Vector3 axis2 = bone3.position - bone2.position;
bool changed = false;
Vector3 clampedAxis2 = V3Tools.ClampDirection(axis2, normalDirection, 0.505f * solverWeight, 0, out changed);
if (!changed)
{
return;
}
Quaternion bone3Rotation = bone3.rotation;
Quaternion f = Quaternion.FromToRotation(axis2, clampedAxis2);
bone2.rotation = f * bone2.rotation;
bone3.rotation = bone3Rotation;
}
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;
}
// 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);
}
private void ChestDirection()
{
float num = this.chestDirectionWeight * this.ik.solver.IKPositionWeight;
if (num <= 0f)
{
return;
}
bool flag = false;
this.chestDirection = V3Tools.ClampDirection(this.chestDirection, this.ik.references.root.forward, 0.45f, 2, out flag);
if (this.chestDirection == Vector3.zero)
{
return;
}
Quaternion quaternion = Quaternion.FromToRotation(this.ik.references.root.forward, this.chestDirection);
quaternion = Quaternion.Lerp(Quaternion.identity, quaternion, num * (1f / (float)this.chestBones.Length));
foreach (Transform transform in this.chestBones)
{
transform.rotation = quaternion * transform.rotation;
}
}
// 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;
}
