private void SolvePelvis()
{
// Pelvis target
if (pelvisPositionWeight > 0f)
{
Quaternion headSolverRotation = head.solverRotation;
Vector3 delta = ((IKPositionPelvis + pelvisPositionOffset) - pelvis.solverPosition) * pelvisPositionWeight;
foreach (VirtualBone bone in bones)
{
bone.solverPosition += delta;
}
Vector3 bendNormal = anchorRotation * Vector3.right;
if (hasNeck)
{
VirtualBone.SolveTrigonometric(bones, 0, 1, bones.Length - 1, headPosition, bendNormal, pelvisPositionWeight * 0.6f);
VirtualBone.SolveTrigonometric(bones, 1, 2, bones.Length - 1, headPosition, bendNormal, pelvisPositionWeight * 0.6f);
VirtualBone.SolveTrigonometric(bones, 2, 3, bones.Length - 1, headPosition, bendNormal, pelvisPositionWeight * 1f);
}
else
{
VirtualBone.SolveTrigonometric(bones, 0, 1, bones.Length - 1, headPosition, bendNormal, pelvisPositionWeight * 0.75f);
VirtualBone.SolveTrigonometric(bones, 1, 2, bones.Length - 1, headPosition, bendNormal, pelvisPositionWeight * 1f);
}
head.solverRotation = headSolverRotation;
}
}
public void Solve(bool stretch)
{
if (stretch && LOD < 1)
{
Stretching();
}
// Foot pass
VirtualBone.SolveTrigonometric(bones, 0, 1, 2, footPosition, bendNormal, 1f);
// Rotate foot back to where it was before the last solving
RotateTo(foot, footRotation);
// Toes pass
if (!hasToes)
{
FixTwistRotations();
return;
}
Vector3 b = Vector3.Cross(foot.solverPosition - thigh.solverPosition, toes.solverPosition - foot.solverPosition).normalized;
VirtualBone.SolveTrigonometric(bones, 0, 2, 3, position, b, 1f);
// Fix thigh twist relative to target rotation
FixTwistRotations();
// Keep toe rotation fixed
toes.solverRotation = rotation;
}
public void Solve()
{
// Foot pass
VirtualBone.SolveTrigonometric(bones, 0, 1, 2, footPosition, bendNormal, 1f);
// Rotate foot back to where it was before the last solving
RotateTo(foot, footRotation);
// Toes pass
if (!hasToes)
{
return;
}
Vector3 b = Vector3.Cross(foot.solverPosition - thigh.solverPosition, toes.solverPosition - foot.solverPosition);
VirtualBone.SolveTrigonometric(bones, 0, 2, 3, position, b, 1f);
// Fix calf twist relative to thigh
Quaternion calfRotation = thigh.solverRotation * calfRelToThigh;
Quaternion fromTo = Quaternion.FromToRotation(calfRotation * calf.axis, foot.solverPosition - calf.solverPosition);
RotateTo(calf, fromTo * calfRotation, 1f);
// Keep toe rotation fixed
toes.solverRotation = rotation;
}
public void Solve(bool stretch)
{
if (stretch)
{
Stretching();
}
// Foot pass
VirtualBone.SolveTrigonometric(bones, 0, 1, 2, footPosition, bendNormal, 1f);
// Rotate foot back to where it was before the last solving
RotateTo(foot, footRotation);
// Toes pass
if (!hasToes)
{
return;
}
Vector3 b = Vector3.Cross(foot.solverPosition - thigh.solverPosition, toes.solverPosition - foot.solverPosition);
VirtualBone.SolveTrigonometric(bones, 0, 2, 3, position, b, 1f);
// Fix thigh twist relative to target rotation
if (bendToTargetWeight > 0f)
{
Quaternion thighRotation = rotation * thighRelToFoot;
Quaternion f = Quaternion.FromToRotation(thighRotation * thigh.axis, calf.solverPosition - thigh.solverPosition);
if (bendToTargetWeight < 1f)
{
thigh.solverRotation = Quaternion.Slerp(thigh.solverRotation, f * thighRotation, bendToTargetWeight);
}
else
{
thigh.solverRotation = f * thighRotation;
}
}
// Fix calf twist relative to thigh
Quaternion calfRotation = thigh.solverRotation * calfRelToThigh;
Quaternion fromTo = Quaternion.FromToRotation(calfRotation * calf.axis, foot.solverPosition - calf.solverPosition);
calf.solverRotation = fromTo * calfRotation;
// Keep toe rotation fixed
toes.solverRotation = rotation;
}
private void SolvePelvis()
{
// Pelvis target
if (pelvisPositionWeight > 0f)
{
Quaternion headSolverRotation = head.solverRotation;
Vector3 delta = ((IKPositionPelvis + pelvisPositionOffset) - pelvis.solverPosition) * pelvisPositionWeight;
foreach (VirtualBone bone in bones)
{
bone.solverPosition += delta;
}
Vector3 bendNormal = anchorRotation * Vector3.right;
if (hasChest && hasNeck)
{
VirtualBone.SolveTrigonometric(bones, pelvisIndex, spineIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 0.6f);
VirtualBone.SolveTrigonometric(bones, spineIndex, chestIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 0.6f);
VirtualBone.SolveTrigonometric(bones, chestIndex, neckIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 1f);
}
else if (hasChest && !hasNeck)
{
VirtualBone.SolveTrigonometric(bones, pelvisIndex, spineIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 0.75f);
VirtualBone.SolveTrigonometric(bones, spineIndex, chestIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 1f);
}
else if (!hasChest && hasNeck)
{
VirtualBone.SolveTrigonometric(bones, pelvisIndex, spineIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 0.75f);
VirtualBone.SolveTrigonometric(bones, spineIndex, neckIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight * 1f);
}
else if (!hasNeck && !hasChest)
{
VirtualBone.SolveTrigonometric(bones, pelvisIndex, spineIndex, headIndex, headPosition, bendNormal, pelvisPositionWeight);
}
head.solverRotation = headSolverRotation;
}
}
public void Solve(bool isLeft)
{
chestRotation = Quaternion.LookRotation(rootRotation * chestForwardAxis, rootRotation * chestUpAxis);
chestForward = chestRotation * Vector3.forward;
chestUp = chestRotation * Vector3.up;
//Debug.DrawRay (Vector3.up * 2f, chestForward);
//Debug.DrawRay (Vector3.up * 2f, chestUp);
if (hasShoulder && shoulderRotationWeight > 0f)
{
switch (shoulderRotationMode)
{
case ShoulderRotationMode.YawPitch:
Vector3 sDir = position - shoulder.solverPosition;
sDir = sDir.normalized;
// Shoulder Yaw
float yOA = isLeft? yawOffsetAngle: -yawOffsetAngle;
Quaternion yawOffset = Quaternion.AngleAxis((isLeft? -90f: 90f) + yOA, chestUp);
Quaternion workingSpace = yawOffset * chestRotation;
//Debug.DrawRay(Vector3.up * 2f, workingSpace * Vector3.forward);
//Debug.DrawRay(Vector3.up * 2f, workingSpace * Vector3.up);
Vector3 sDirWorking = Quaternion.Inverse(workingSpace) * sDir;
//Debug.DrawRay(Vector3.up * 2f, sDirWorking);
float yaw = Mathf.Atan2(sDirWorking.x, sDirWorking.z) * Mathf.Rad2Deg;
float dotY = Vector3.Dot(sDirWorking, Vector3.up);
dotY = 1f - Mathf.Abs(dotY);
yaw *= dotY;
yaw -= yOA;
float yawLimitMin = isLeft? -20f: -50f;
float yawLimitMax = isLeft? 50f: 20f;
yaw = DamperValue(yaw, yawLimitMin - yOA, yawLimitMax - yOA, 0.7f); // back, forward
Vector3 f = shoulder.solverRotation * shoulder.axis;
Vector3 t = workingSpace * (Quaternion.AngleAxis(yaw, Vector3.up) * Vector3.forward);
Quaternion yawRotation = Quaternion.FromToRotation(f, t);
//Debug.DrawRay(Vector3.up * 2f, f, Color.red);
//Debug.DrawRay(Vector3.up * 2f, t, Color.green);
//Debug.DrawRay(Vector3.up * 2f, yawRotation * Vector3.forward, Color.blue);
//Debug.DrawRay(Vector3.up * 2f, yawRotation * Vector3.up, Color.green);
//Debug.DrawRay(Vector3.up * 2f, yawRotation * Vector3.right, Color.red);
// Shoulder Pitch
Quaternion pitchOffset = Quaternion.AngleAxis(isLeft? -90f: 90f, chestUp);
workingSpace = pitchOffset * chestRotation;
workingSpace = Quaternion.AngleAxis(isLeft? pitchOffsetAngle: -pitchOffsetAngle, chestForward) * workingSpace;
//Debug.DrawRay(Vector3.up * 2f, workingSpace * Vector3.forward);
//Debug.DrawRay(Vector3.up * 2f, workingSpace * Vector3.up);
sDir = position - (shoulder.solverPosition + chestRotation * (isLeft? Vector3.right: Vector3.left) * mag);
sDirWorking = Quaternion.Inverse(workingSpace) * sDir;
//Debug.DrawRay(Vector3.up * 2f, sDirWorking);
float pitch = Mathf.Atan2(sDirWorking.y, sDirWorking.z) * Mathf.Rad2Deg;
pitch -= pitchOffsetAngle;
pitch = DamperValue(pitch, -45f - pitchOffsetAngle, 45f - pitchOffsetAngle);
Quaternion pitchRotation = Quaternion.AngleAxis(-pitch, workingSpace * Vector3.right);
//Debug.DrawRay(Vector3.up * 2f, pitchRotation * Vector3.forward, Color.green);
//Debug.DrawRay(Vector3.up * 2f, pitchRotation * Vector3.up, Color.green);
// Rotate bones
Quaternion sR = pitchRotation * yawRotation;
if (shoulderRotationWeight * positionWeight < 1f)
{
sR = Quaternion.Lerp(Quaternion.identity, sR, shoulderRotationWeight * positionWeight);
}
VirtualBone.RotateBy(bones, sR);
Stretching();
// Solve trigonometric
VirtualBone.SolveTrigonometric(bones, 1, 2, 3, position, GetBendNormal(position - upperArm.solverPosition), positionWeight);
float p = Mathf.Clamp(pitch * positionWeight * shoulderRotationWeight * shoulderTwistWeight * 2f, 0f, 180f);
shoulder.solverRotation = Quaternion.AngleAxis(p, shoulder.solverRotation * (isLeft? shoulder.axis: -shoulder.axis)) * shoulder.solverRotation;
upperArm.solverRotation = Quaternion.AngleAxis(p, upperArm.solverRotation * (isLeft? upperArm.axis: -upperArm.axis)) * upperArm.solverRotation;
// Additional pass to reach with the shoulders
//VirtualBone.SolveTrigonometric(bones, 0, 1, 3, position, Vector3.Cross(upperArm.solverPosition - shoulder.solverPosition, hand.solverPosition - shoulder.solverPosition), positionWeight * 0.5f);
break;
case ShoulderRotationMode.FromTo:
Quaternion shoulderRotation = shoulder.solverRotation;
Quaternion r = Quaternion.FromToRotation((upperArm.solverPosition - shoulder.solverPosition).normalized + chestForward, position - shoulder.solverPosition);
r = Quaternion.Slerp(Quaternion.identity, r, 0.5f * shoulderRotationWeight * positionWeight);
VirtualBone.RotateBy(bones, r);
Stretching();
VirtualBone.SolveTrigonometric(bones, 0, 2, 3, position, Vector3.Cross(forearm.solverPosition - shoulder.solverPosition, hand.solverPosition - shoulder.solverPosition), 0.5f * shoulderRotationWeight * positionWeight);
VirtualBone.SolveTrigonometric(bones, 1, 2, 3, position, GetBendNormal(position - upperArm.solverPosition), positionWeight);
// Twist shoulder and upper arm bones when holding hands up
Quaternion q = Quaternion.Inverse(Quaternion.LookRotation(chestUp, chestForward));
Vector3 vBefore = q * (shoulderRotation * shoulder.axis);
Vector3 vAfter = q * (shoulder.solverRotation * shoulder.axis);
float angleBefore = Mathf.Atan2(vBefore.x, vBefore.z) * Mathf.Rad2Deg;
float angleAfter = Mathf.Atan2(vAfter.x, vAfter.z) * Mathf.Rad2Deg;
float pitchAngle = Mathf.DeltaAngle(angleBefore, angleAfter);
if (isLeft)
{
pitchAngle = -pitchAngle;
}
pitchAngle = Mathf.Clamp(pitchAngle * shoulderRotationWeight * shoulderTwistWeight * 2f * positionWeight, 0f, 180f);
shoulder.solverRotation = Quaternion.AngleAxis(pitchAngle, shoulder.solverRotation * (isLeft? shoulder.axis: -shoulder.axis)) * shoulder.solverRotation;
upperArm.solverRotation = Quaternion.AngleAxis(pitchAngle, upperArm.solverRotation * (isLeft? upperArm.axis: -upperArm.axis)) * upperArm.solverRotation;
break;
}
}
else
{
Stretching();
// Solve arm trigonometric
if (hasShoulder)
{
VirtualBone.SolveTrigonometric(bones, 1, 2, 3, position, GetBendNormal(position - upperArm.solverPosition), positionWeight);
}
else
{
VirtualBone.SolveTrigonometric(bones, 0, 1, 2, position, GetBendNormal(position - upperArm.solverPosition), positionWeight);
}
}
// Fix forearm twist relative to upper arm
Quaternion forearmFixed = upperArm.solverRotation * forearmRelToUpperArm;
Quaternion fromTo = Quaternion.FromToRotation(forearmFixed * forearm.axis, hand.solverPosition - forearm.solverPosition);
RotateTo(forearm, fromTo * forearmFixed, positionWeight);
// Set hand rotation
if (rotationWeight >= 1f)
{
hand.solverRotation = rotation;
}
else if (rotationWeight > 0f)
{
hand.solverRotation = Quaternion.Lerp(hand.solverRotation, rotation, rotationWeight);
}
}
