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