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