// 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;
float deltaY = V3Tools.ExtractVertical(deltaPosition, rootRotation * Vector3.up, 1f).magnitude;
float backOffset = deltaY * -moveBodyBackWhenCrouching * 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));
}
// Bending the spine to the head effector
private void Bend(VirtualBone[] bones, int firstIndex, int lastIndex, Quaternion targetRotation, Quaternion rotationOffset, 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.Slerp(Quaternion.identity, rotationOffset, step), r, step * w));
}
}
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;
}
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 WriteTransforms()
{
for (int i = 0; i < solverTransforms.Length; i++)
{
if (solverTransforms[i] != null)
{
bool isRootOrPelvis = i < 2;
bool isArm = i > 5 && i < 14;
if (isRootOrPelvis || isArm)
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
solverTransforms[i].rotation = QuaTools.Lerp(solverTransforms[i].rotation, GetRotation(i), IKPositionWeight);
}
}
}
public override void PreSolve()
{
if (headTarget != null)
{
IKPositionHead = headTarget.position;
IKRotationHead = headTarget.rotation;
}
if (chestGoal != null)
{
goalPositionChest = chestGoal.position;
}
if (pelvisTarget != null)
{
IKPositionPelvis = pelvisTarget.position;
IKRotationPelvis = pelvisTarget.rotation;
}
headPosition = V3Tools.Lerp(head.solverPosition, IKPositionHead, positionWeight);
headRotation = QuaTools.Lerp(head.solverRotation, IKRotationHead, rotationWeight);
}
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;
}
}
}
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);
Vector3 p = rootBone.solverPosition + rootVelocity * Time.deltaTime * 2f * locomotion.weight;
p = Vector3.Lerp(p, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight);
rootBone.solverPosition = p;
rootVelocity += (footPositionC - rootBone.solverPosition) * Time.deltaTime * 10f;
Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f);
rootVelocity -= rootVelocityV;
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();
}
}
}
// Arms
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();
foreach (Leg leg in legs)
{
leg.ResetOffsets();
}
foreach (Arm arm in arms)
{
arm.ResetOffsets();
}
spine.pelvisPositionOffset += GetPelvisOffset();
spine.chestPositionOffset += spine.pelvisPositionOffset;
//spine.headPositionOffset += spine.pelvisPositionOffset;
Write();
// Find the support leg
supportLegIndex = -1;
float shortestMag = Mathf.Infinity;
for (int i = 0; i < legs.Length; i++)
{
float mag = Vector3.SqrMagnitude(legs[i].lastBone.solverPosition - legs[i].bones[0].solverPosition);
if (mag < shortestMag)
{
supportLegIndex = i;
shortestMag = mag;
}
}
}
public void MoveRotation(Quaternion rotation)
{
Quaternion delta = QuaTools.FromToRotation(bones[0].solverRotation, rotation);
VirtualBone.RotateAroundPoint(bones, 0, bones[0].solverPosition, delta);
}
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);
}
