// Move and rotate the pelvis private void TranslatePelvis(Leg[] legs, Vector3 deltaPosition, Quaternion deltaRotation) { // Rotation Vector3 p = head.solverPosition; deltaRotation = QuaTools.ClampRotation(deltaRotation, chestClampWeight, 2); Quaternion r = Quaternion.Slerp(Quaternion.identity, deltaRotation, bodyRotStiffness); r = Quaternion.Slerp(r, QuaTools.FromToRotation(pelvis.solverRotation, IKRotationPelvis), pelvisRotationWeight); VirtualBone.RotateAroundPoint(bones, 0, pelvis.solverPosition, pelvisRotationOffset * r); deltaPosition -= head.solverPosition - p; // Position // Move the body back when head is moving down Vector3 m = rootRotation * Vector3.forward; m.y = 0f; float backOffset = deltaPosition.y * 0.35f * headHeight; deltaPosition += m * backOffset; /* * if (backOffset < 0f) { * foreach (Leg leg in legs) leg.heelPositionOffset += Vector3.up * backOffset * backOffset; // TODO Ignoring root rotation * } */ MovePosition(LimitPelvisPosition(legs, pelvis.solverPosition + deltaPosition * bodyPosStiffness, false)); }
// 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)); }
// Called by the FBBIK each time it is finished updating private void OnPostUpdate() { if (!enabled) { return; } if (!ik.enabled) { return; } if (!gameObject.activeInHierarchy) { return; } // Stretching the spine and neck PostStretching(); // Rotate the head bone Quaternion headRotation = QuaTools.FromToRotation(ik.references.head.rotation, transform.rotation); headRotation = QuaTools.ClampRotation(headRotation, headClampWeight, 2); ik.references.head.rotation = Quaternion.Lerp(Quaternion.identity, headRotation, rotationWeight * ik.solver.IKPositionWeight) * ik.references.head.rotation; }
/* * Stage 2 of FABRIK algorithm with limited rotations * */ private void BackwardReachLimited(Vector3 position) { // Move first bone to position bones[0].solverPosition = position; // Applying rotation limits bone by bone for (int i = 0; i < bones.Length - 1; i++) { // Rotating bone to look at the solved joint position Vector3 nextPosition = SolveJoint(bones[i + 1].solverPosition, bones[i].solverPosition, bones[i].length); Quaternion swing = Quaternion.FromToRotation(bones[i].solverRotation * bones[i].axis, nextPosition - bones[i].solverPosition); Quaternion targetRotation = swing * bones[i].solverRotation; // Rotation Constraints if (bones[i].rotationLimit != null) { bool changed = false; targetRotation = GetLimitedRotation(i, targetRotation, out changed); } Quaternion fromTo = QuaTools.FromToRotation(bones[i].solverRotation, targetRotation); bones[i].solverRotation = targetRotation; SolverRotateChildren(i, fromTo); // Positioning the next bone to its default local position bones[i + 1].solverPosition = bones[i].solverPosition + bones[i].solverRotation * solverLocalPositions[i + 1]; } // Reconstruct solver rotations to protect from invalid Quaternions for (int i = 0; i < bones.Length; i++) { bones[i].solverRotation = Quaternion.LookRotation(bones[i].solverRotation * Vector3.forward, bones[i].solverRotation * Vector3.up); } }
// Bending the spine to the head effector private void Bend(VirtualBone[] bones, int firstIndex, int lastIndex, Quaternion targetRotation, float clampWeight, bool uniformWeight, float w) { if (w <= 0f) { return; } if (bones.Length == 0) { return; } int bonesCount = (lastIndex + 1) - firstIndex; if (bonesCount < 1) { return; } Quaternion r = QuaTools.FromToRotation(bones[lastIndex].solverRotation, targetRotation); r = QuaTools.ClampRotation(r, clampWeight, 2); float step = uniformWeight? 1f / bonesCount: 0f; for (int i = firstIndex; i < lastIndex + 1; i++) { if (!uniformWeight) { step = Mathf.Clamp(((i - firstIndex) + 1) / bonesCount, 0, 1f); } VirtualBone.RotateAroundPoint(bones, i, bones[i].solverPosition, Quaternion.Slerp(Quaternion.identity, r, step * w)); } }
private void Iterate(int iteration) { if (!base.enabled) { return; } if (!this.ik.enabled) { return; } if (!base.gameObject.activeInHierarchy) { return; } if (this.ik.solver.iterations == 0) { return; } this.leftShoulderPos = base.transform.position + (this.leftShoulderPos - base.transform.position).normalized * this.leftShoulderDist; this.rightShoulderPos = base.transform.position + (this.rightShoulderPos - base.transform.position).normalized * this.rightShoulderDist; this.Solve(ref this.leftShoulderPos, ref this.rightShoulderPos, this.shoulderDist); this.LerpSolverPosition(this.ik.solver.leftShoulderEffector, this.leftShoulderPos, this.positionWeight * this.ik.solver.IKPositionWeight, this.ik.solver.leftShoulderEffector.positionOffset); this.LerpSolverPosition(this.ik.solver.rightShoulderEffector, this.rightShoulderPos, this.positionWeight * this.ik.solver.IKPositionWeight, this.ik.solver.rightShoulderEffector.positionOffset); Quaternion to = Quaternion.LookRotation(base.transform.position - this.leftShoulderPos, this.rightShoulderPos - this.leftShoulderPos); Quaternion quaternion = QuaTools.FromToRotation(this.chestRotation, to); Vector3 b = quaternion * this.headToBody; this.LerpSolverPosition(this.ik.solver.bodyEffector, base.transform.position + b, this.positionWeight * this.ik.solver.IKPositionWeight, this.ik.solver.bodyEffector.positionOffset - this.ik.solver.pullBodyOffset); Quaternion rotation = Quaternion.Lerp(Quaternion.identity, quaternion, this.thighWeight); Vector3 b2 = rotation * this.headToLeftThigh; Vector3 b3 = rotation * this.headToRightThigh; this.LerpSolverPosition(this.ik.solver.leftThighEffector, base.transform.position + b2, this.positionWeight * this.ik.solver.IKPositionWeight, this.ik.solver.bodyEffector.positionOffset - this.ik.solver.pullBodyOffset + this.ik.solver.leftThighEffector.positionOffset); this.LerpSolverPosition(this.ik.solver.rightThighEffector, base.transform.position + b3, this.positionWeight * this.ik.solver.IKPositionWeight, this.ik.solver.bodyEffector.positionOffset - this.ik.solver.pullBodyOffset + this.ik.solver.rightThighEffector.positionOffset); }
// Called by the FBBIK before each solver iteration private void Iterate(int iteration) { if (ik.solver.iterations == 0) { return; } // Shoulders leftShoulderPos = transform.position + (leftShoulderPos - transform.position).normalized * leftShoulderDist; rightShoulderPos = transform.position + (rightShoulderPos - transform.position).normalized * rightShoulderDist; Solve(ref leftShoulderPos, ref rightShoulderPos, shoulderDist); LerpSolverPosition(ik.solver.leftShoulderEffector, leftShoulderPos, positionWeight); LerpSolverPosition(ik.solver.rightShoulderEffector, rightShoulderPos, positionWeight); // Body Quaternion chestRotationSolved = Quaternion.LookRotation(transform.position - leftShoulderPos, rightShoulderPos - leftShoulderPos); Quaternion rBody = QuaTools.FromToRotation(chestRotation, chestRotationSolved); Vector3 headToBodySolved = rBody * headToBody; LerpSolverPosition(ik.solver.bodyEffector, transform.position + headToBodySolved, positionWeight); // Thighs Quaternion rThighs = Quaternion.Lerp(Quaternion.identity, rBody, thighWeight); Vector3 headToLeftThighSolved = rThighs * headToLeftThigh; Vector3 headToRightThighSolved = rThighs * headToRightThigh; LerpSolverPosition(ik.solver.leftThighEffector, transform.position + headToLeftThighSolved, positionWeight); LerpSolverPosition(ik.solver.rightThighEffector, transform.position + headToRightThighSolved, positionWeight); }
public void RecordVelocity() { this.deltaPosition = this.t.position - this.lastPosition; this.lastPosition = this.t.position; this.deltaRotation = QuaTools.FromToRotation(this.lastRotation, this.t.rotation); this.lastRotation = this.t.rotation; this.deltaTime = Time.deltaTime; }
/* * Applying rotation limit to a bone in stage 1 in a more stable way * */ private void LimitForward(int rotateBone, int limitBone) { if (!useRotationLimits) { return; } if (bones[limitBone].rotationLimit == null) { return; } // Storing last bone's position before applying the limit Vector3 lastBoneBeforeLimit = bones[bones.Length - 1].solverPosition; // Moving and rotating this bone and all its children to their solver positions for (int i = rotateBone; i < bones.Length - 1; i++) { if (limitedBones[i]) { break; } Quaternion fromTo = Quaternion.FromToRotation(bones[i].solverRotation * bones[i].axis, bones[i + 1].solverPosition - bones[i].solverPosition); SolverRotate(i, fromTo, false); } // Limit the bone's rotation bool changed = false; Quaternion afterLimit = GetLimitedRotation(limitBone, bones[limitBone].solverRotation, out changed); if (changed) { // Rotating and positioning the hierarchy so that the last bone's position is maintained if (limitBone < bones.Length - 1) { Quaternion change = QuaTools.FromToRotation(bones[limitBone].solverRotation, afterLimit); bones[limitBone].solverRotation = afterLimit; SolverRotateChildren(limitBone, change); SolverMoveChildrenAroundPoint(limitBone, change); // Rotating to compensate for the limit Quaternion fromTo = Quaternion.FromToRotation(bones[bones.Length - 1].solverPosition - bones[rotateBone].solverPosition, lastBoneBeforeLimit - bones[rotateBone].solverPosition); SolverRotate(rotateBone, fromTo, true); SolverMoveChildrenAroundPoint(rotateBone, fromTo); // Moving the bone so that last bone maintains it's initial position SolverMove(rotateBone, lastBoneBeforeLimit - bones[bones.Length - 1].solverPosition); } else { // last bone bones[limitBone].solverRotation = afterLimit; } } limitedBones[limitBone] = true; }
public override void ApplyOffsets() { headPosition += headPositionOffset; headPosition.y = Math.Max(rootPosition.y + 0.8f, headPosition.y); headRotation = headRotationOffset * headRotation; headDeltaPosition = headPosition - head.solverPosition; pelvisDeltaRotation = QuaTools.FromToRotation(pelvis.solverRotation, headRotation * pelvisRelativeRotation); anchorRotation = headRotation * anchorRelativeToHead; }
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; } }
// How fast the mapped target is moving? Will be used to set rigidbody velocities when puppet is killed. // Rigidbody velocities otherwise might be close to 0 when FixedUpdate called more than once per frame or velocity wrongully changing when mapping weights not 1. public void CalculateMappedVelocity() { float writeDeltaTime = Time.time - lastWriteTime; if (writeDeltaTime > 0f) { mappedVelocity = (target.position - lastMappedPosition) / writeDeltaTime; mappedAngularVelocity = QuaTools.FromToRotation(lastMappedRotation, target.rotation).eulerAngles / writeDeltaTime; lastWriteTime = Time.time; } lastMappedPosition = target.position; lastMappedRotation = target.rotation; }
public void TranslateRoot(Vector3 newRootPos, Quaternion newRootRot) { Vector3 deltaPosition = newRootPos - rootPosition; rootPosition = newRootPos; foreach (VirtualBone bone in bones) { bone.solverPosition += deltaPosition; } Quaternion deltaRotation = QuaTools.FromToRotation(rootRotation, newRootRot); rootRotation = newRootRot; VirtualBone.RotateAroundPoint(bones, 0, newRootPos, deltaRotation); }
private void LimitForward(int rotateBone, int limitBone) { if (!this.useRotationLimits) { return; } if (this.bones[limitBone].rotationLimit == null) { return; } Vector3 solverPosition = this.bones[this.bones.Length - 1].solverPosition; for (int i = rotateBone; i < this.bones.Length - 1; i++) { if (this.limitedBones[i]) { break; } Quaternion rotation = Quaternion.FromToRotation(this.bones[i].solverRotation * this.bones[i].axis, this.bones[i + 1].solverPosition - this.bones[i].solverPosition); this.SolverRotate(i, rotation, false); } bool flag = false; Quaternion limitedRotation = this.GetLimitedRotation(limitBone, this.bones[limitBone].solverRotation, out flag); if (flag) { if (limitBone < this.bones.Length - 1) { Quaternion rotation2 = QuaTools.FromToRotation(this.bones[limitBone].solverRotation, limitedRotation); this.bones[limitBone].solverRotation = limitedRotation; this.SolverRotateChildren(limitBone, rotation2); this.SolverMoveChildrenAroundPoint(limitBone, rotation2); Quaternion rotation3 = Quaternion.FromToRotation(this.bones[this.bones.Length - 1].solverPosition - this.bones[rotateBone].solverPosition, solverPosition - this.bones[rotateBone].solverPosition); this.SolverRotate(rotateBone, rotation3, true); this.SolverMoveChildrenAroundPoint(rotateBone, rotation3); this.SolverMove(rotateBone, solverPosition - this.bones[this.bones.Length - 1].solverPosition); } else { this.bones[limitBone].solverRotation = limitedRotation; } } this.limitedBones[limitBone] = true; }
private void OnPostUpdate() { if (!base.enabled) { return; } if (!this.ik.enabled) { return; } if (!base.gameObject.activeInHierarchy) { return; } this.PostStretching(); Quaternion quaternion = QuaTools.FromToRotation(this.ik.references.head.rotation, base.transform.rotation); quaternion = QuaTools.ClampRotation(quaternion, this.headClampWeight, 2); this.ik.references.head.rotation = Quaternion.Lerp(Quaternion.identity, quaternion, this.rotationWeight * this.ik.solver.IKPositionWeight) * this.ik.references.head.rotation; }
// Read the target public void Read() { float readDeltaTime = Time.time - lastReadTime; lastReadTime = Time.time; if (readDeltaTime > 0f) { targetVelocity = (target.position - targetAnimatedPosition) / readDeltaTime; targetAngularVelocity = QuaTools.FromToRotation(targetAnimatedWorldRotation, target.rotation).eulerAngles / readDeltaTime; } //if (props.mapPosition) targetLocalPosition = target.localPosition; targetAnimatedPosition = target.position; targetAnimatedWorldRotation = target.rotation; if (joint.connectedBody != null) { targetAnimatedRotation = targetLocalRotation * localRotationConvert; } }
private void BackwardReachLimited(Vector3 position) { this.bones[0].solverPosition = position; for (int i = 0; i < this.bones.Length - 1; i++) { Vector3 a = this.SolveJoint(this.bones[i + 1].solverPosition, this.bones[i].solverPosition, this.bones[i].length); Quaternion quaternion = Quaternion.FromToRotation(this.bones[i].solverRotation * this.bones[i].axis, a - this.bones[i].solverPosition) * this.bones[i].solverRotation; if (this.bones[i].rotationLimit != null) { bool flag = false; quaternion = this.GetLimitedRotation(i, quaternion, out flag); } Quaternion rotation = QuaTools.FromToRotation(this.bones[i].solverRotation, quaternion); this.bones[i].solverRotation = quaternion; this.SolverRotateChildren(i, rotation); this.bones[i + 1].solverPosition = this.bones[i].solverPosition + this.bones[i].solverRotation * this.solverLocalPositions[i + 1]; } for (int j = 0; j < this.bones.Length; j++) { this.bones[j].solverRotation = Quaternion.LookRotation(this.bones[j].solverRotation * Vector3.forward, this.bones[j].solverRotation * Vector3.up); } }
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; }
public void RotateTo(VirtualBone bone, Quaternion rotation, float weight = 1f) { if (weight <= 0f) { return; } Quaternion q = QuaTools.FromToRotation(bone.solverRotation, rotation); if (weight < 1f) { q = Quaternion.Slerp(Quaternion.identity, q, weight); } for (int i = 0; i < bones.Length; i++) { if (bones[i] == bone) { VirtualBone.RotateAroundPoint(bones, i, bones[i].solverPosition, q); return; } } }
public static void SolverRotate(Bone[] bones, int index, Quaternion rotation) { Quaternion q = QuaTools.FromToRotation(bones[index].solverRotation, rotation); SolverRotateBonesAroundPoint(bones, index, bones[index].solverPosition, q); }
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; } } } }
public void MoveRotation(Quaternion rotation) { Quaternion delta = QuaTools.FromToRotation(bones[0].solverRotation, rotation); VirtualBone.RotateAroundPoint(bones, 0, bones[0].solverPosition, delta); }
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; } } }
// Rotate this target towards a position public void RotateTo(Transform bone) { if (pivot == null) { return; } if (pivot != lastPivot) { defaultLocalRotation = pivot.localRotation; lastPivot = pivot; } // Rotate to the default local rotation pivot.localRotation = defaultLocalRotation; switch (rotationMode) { case RotationMode.TwoDOF: // Twisting around the twist axis if (twistWeight > 0f) { Vector3 targetTangent = transform.position - pivot.position; Vector3 n = pivot.rotation * twistAxis; Vector3 normal = n; Vector3.OrthoNormalize(ref normal, ref targetTangent); normal = n; Vector3 direction = bone.position - pivot.position; Vector3.OrthoNormalize(ref normal, ref direction); Quaternion q = QuaTools.FromToAroundAxis(targetTangent, direction, n); pivot.rotation = Quaternion.Lerp(Quaternion.identity, q, twistWeight) * pivot.rotation; } // Swinging freely if (swingWeight > 0f) { Quaternion s = Quaternion.FromToRotation(transform.position - pivot.position, bone.position - pivot.position); pivot.rotation = Quaternion.Lerp(Quaternion.identity, s, swingWeight) * pivot.rotation; } break; case RotationMode.ThreeDOF: // Free rotation around all axes if (threeDOFWeight <= 0f) { break; } Quaternion fromTo = QuaTools.FromToRotation(transform.rotation, bone.rotation); if (threeDOFWeight >= 1f) { pivot.rotation = fromTo * pivot.rotation; } else { pivot.rotation = Quaternion.Slerp(Quaternion.identity, fromTo, threeDOFWeight) * pivot.rotation; } break; } }
public static void RotateTo(VirtualBone[] bones, int index, Quaternion rotation) { Quaternion q = QuaTools.FromToRotation(bones[index].solverRotation, rotation); RotateAroundPoint(bones, index, bones[index].solverPosition, q); }