public static void SolveCCD(VirtualBone[] bones, Vector3 targetPosition, float weight, int iterations)
{
if (weight <= 0f)
{
return;
}
// Iterating the solver
for (int iteration = 0; iteration < iterations; iteration++)
{
for (int i = bones.Length - 2; i > -1; i--)
{
Vector3 toLastBone = bones[bones.Length - 1].solverPosition - bones[i].solverPosition;
Vector3 toTarget = targetPosition - bones[i].solverPosition;
Quaternion rotation = Quaternion.FromToRotation(toLastBone, toTarget);
if (weight >= 1)
{
//bones[i].transform.rotation = targetRotation;
VirtualBone.RotateBy(bones, i, rotation);
}
else
{
VirtualBone.RotateBy(bones, i, Quaternion.Lerp(Quaternion.identity, rotation, weight));
}
}
}
}
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);
}
}
private void SolvePelvis()
{
if (pelvisPositionWeight <= 0f)
{
return;
}
var headSolverRotation = head.solverRotation;
var headSolverPosition = head.solverPosition;
var middleLegPosition = (mySolver.legs[0].thigh.readPosition + mySolver.legs[1].thigh.readPosition) / 2;
var hipLocalOffset = Quaternion.Inverse(pelvis.readRotation) * (middleLegPosition - pelvis.readPosition);
if (IkTweaksSettings.DoHipShifting)
{
pelvis.solverPosition += pelvis.solverRotation * hipLocalOffset;
IKPositionPelvis += IKRotationPelvis * hipLocalOffset;
}
if (IkTweaksSettings.PreStraightenSpine)
{
for (var i = 1; i < bones.Length - 1; i++)
{
var rotation = Quaternion.FromToRotation(bones[i + 1].solverPosition - bones[i].solverPosition,
bones[i].solverPosition - bones[i - 1].solverPosition);
VirtualBone.RotateBy(bones, i, rotation);
}
}
if (IkTweaksSettings.StraightenNeck)
{
if (neckIndex >= 0)
{
var rotation = Quaternion.FromToRotation(bones[neckIndex + 1].solverPosition - bones[neckIndex].solverPosition,
bones[neckIndex].solverPosition - bones[neckIndex - 1].solverPosition);
VirtualBone.RotateBy(bones, neckIndex, rotation);
}
}
var minAngle = 0f;
var maxAngle = 1f;
var currentAngle = 0f;
for (var i = 0; i < bones.Length; i++)
{
bonesShadow[i].solverPosition = bones[i].solverPosition;
bonesShadow[i].solverRotation = bones[i].solverRotation;
}
var targetDistance = (IKPositionPelvis - headSolverPosition).magnitude;
for (var i = 0; i < relaxationIterations; i++)
{
for (var j = 0; j < bones.Length; j++)
{
bones[j].solverPosition = bonesShadow[j].solverPosition;
bones[j].solverRotation = bonesShadow[j].solverRotation;
}
var currentIkTargetPos = IKPositionPelvis;
Vector3 delta = ((currentIkTargetPos + pelvisPositionOffset) - pelvis.solverPosition) *
pelvisPositionWeight;
foreach (VirtualBone bone in bones)
{
bone.solverPosition += delta;
}
VirtualBone.RotateTo(bones, pelvisIndex, IKRotationPelvis);
var targetToHead = (headSolverPosition - currentIkTargetPos).normalized;
// var currentToHead = head.solverPosition - pelvis.solverPosition;
var currentToHead = (pelvis.solverRotation * Quaternion.Inverse(pelvis.readRotation) *
(head.readPosition - pelvis.readPosition)).normalized;
var rotationNormal = Vector3.Cross(currentToHead, targetToHead);
var rotationForward = Vector3.ProjectOnPlane(anchorRotation * Vector3.forward, currentToHead).normalized;
var bendDirection = Vector3.ProjectOnPlane(targetToHead, currentToHead).normalized;
var bendForwardness = (Vector3.Dot(rotationForward, bendDirection) + 1) / 2;
var maxBendTotal = Mathf.Pow(Mathf.Clamp01(Mathf.Acos(Vector3.Dot(currentToHead, targetToHead)) * Mathf2.Rad2Deg / IkTweaksSettings.StraightSpineAngle), IkTweaksSettings.StraightSpinePower);
var maxSpineAngle = Mathf.Lerp(maxSpineAngleBack, maxSpineAngleFwd, bendForwardness) * maxBendTotal;
var maxNeckAngle = Mathf.Lerp(maxNeckAngleBack, maxNeckAngleFwd, bendForwardness) * maxBendTotal;
var lastBoneToRotate = hipRotationPinning ? 1 : 0;
for (var j = bones.Length - 2; j > lastBoneToRotate; j--)
{
// var rotationNormal = Vector3.Cross(bones[j + 1].solverPosition - bones[j].solverPosition, headSolverPosition - bones[j].solverPosition);
var targetAngle = j == neckIndex
? Mathf.Clamp01(currentAngle *neckBendPriority) * maxNeckAngle
: currentAngle * maxSpineAngle;
VirtualBone.RotateBy(bones, j, Quaternion.AngleAxis(targetAngle, rotationNormal));
}
if (hipRotationPinning)
{
var od = pelvis.solverPosition - bones[1].solverPosition;
var p = Vector3.Dot(od, targetToHead);
var q = od.sqrMagnitude - (bones[1].solverPosition - head.solverPosition).sqrMagnitude;
var t = -p + Mathf.Sqrt(p * p - q);
var headRotateToTarget = pelvis.solverPosition + targetToHead * t;
VirtualBone.RotateBy(bones, 1,
Quaternion.FromToRotation(head.solverPosition - bones[1].solverPosition,
headRotateToTarget - bones[1].solverPosition));
}
else
{
VirtualBone.RotateBy(bones,
Quaternion.FromToRotation(head.solverPosition - pelvis.solverPosition,
headSolverPosition - IKPositionPelvis));
}
delta = headSolverPosition - head.solverPosition;
foreach (VirtualBone bone in bones)
{
bone.solverPosition += delta;
}
var currentDistance = (head.solverPosition - pelvis.solverPosition).magnitude;
if (currentDistance > targetDistance)
{
minAngle = currentAngle;
}
else
{
maxAngle = currentAngle;
}
currentAngle = (minAngle + maxAngle) / 2;
}
if (IkTweaksSettings.DoHipShifting)
{
pelvis.solverPosition -= pelvis.solverRotation * hipLocalOffset;
IKPositionPelvis -= IKRotationPelvis * hipLocalOffset;
}
head.solverRotation = headSolverRotation;
}
