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