// 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));
}
}
// Move and rotate the pelvis
private void TranslatePelvis(Leg[] legs, Vector3 deltaPosition, Quaternion deltaRotation, float w)
{
// Rotation
Vector3 p = head.solverPosition;
deltaRotation = QuaTools.ClampRotation(deltaRotation, chestClampWeight, 2);
Quaternion f = w >= 1f? pelvisRotationOffset: Quaternion.Slerp(Quaternion.identity, pelvisRotationOffset, w);
VirtualBone.RotateAroundPoint(bones, 0, pelvis.solverPosition, f * Quaternion.Slerp(Quaternion.identity, deltaRotation, w * bodyRotStiffness));
deltaPosition -= head.solverPosition - p;
// Position
// Move the body back when head is moving down
Vector3 m = anchorRotation * 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 * w * 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));
}
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);
}
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 void MoveRotation(Quaternion rotation)
{
Quaternion delta = QuaTools.FromToRotation(bones[0].solverRotation, rotation);
VirtualBone.RotateAroundPoint(bones, 0, bones[0].solverPosition, delta);
}
