// 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));
}
}
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));
}
}
}
}
// 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));
}
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()
{
// 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 && 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;
}
// 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));
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, int rootIndex, int index)
{
Vector3 thighPos = positions[index];
Quaternion thighRot = rotations[index];
Vector3 calfPos = positions[index + 1];
Quaternion calfRot = rotations[index + 1];
Vector3 footPos = positions[index + 2];
Quaternion footRot = rotations[index + 2];
Vector3 toePos = positions[index + 3];
Quaternion toeRot = rotations[index + 3];
if (!initiated)
{
this.hasToes = hasToes;
bones = new VirtualBone[hasToes? 4: 3];
if (hasToes)
{
bones[0] = new VirtualBone(thighPos, thighRot);
bones[1] = new VirtualBone(calfPos, calfRot);
bones[2] = new VirtualBone(footPos, footRot);
bones[3] = new VirtualBone(toePos, toeRot);
IKPosition = toePos;
IKRotation = toeRot;
}
else
{
bones[0] = new VirtualBone(thighPos, thighRot);
bones[1] = new VirtualBone(calfPos, calfRot);
bones[2] = new VirtualBone(footPos, footRot);
IKPosition = footPos;
IKRotation = footRot;
}
bendNormal = Vector3.Cross(calfPos - thighPos, footPos - calfPos);
//bendNormal = rotations[0] * Vector3.right; // Use this to make the knees bend towards root.forward
bendNormalRelToPelvis = Quaternion.Inverse(rootRotation) * bendNormal;
bendNormalRelToTarget = Quaternion.Inverse(IKRotation) * bendNormal;
rotation = IKRotation;
}
if (hasToes)
{
bones[0].Read(thighPos, thighRot);
bones[1].Read(calfPos, calfRot);
bones[2].Read(footPos, footRot);
bones[3].Read(toePos, toeRot);
}
else
{
bones[0].Read(thighPos, thighRot);
bones[1].Read(calfPos, calfRot);
bones[2].Read(footPos, footRot);
}
}
private void FABRIKPass(Vector3 animatedPelvisPos, Vector3 rootUp)
{
Vector3 startPos = Vector3.Lerp(pelvis.solverPosition, animatedPelvisPos, maintainPelvisPosition) + pelvisPositionOffset - chestPositionOffset;
Vector3 endPos = headPosition - chestPositionOffset;
Vector3 startOffset = rootUp * (bones[bones.Length - 1].solverPosition - bones[0].solverPosition).magnitude;
VirtualBone.SolveFABRIK(bones, startPos, endPos, 1f, 1f, 1, mag, startOffset);
}
public void Solve(VirtualBone rootBone, Leg[] legs, Arm[] arms, float scale)
{
CalculateChestTargetRotation(rootBone, arms);
// Root rotation
if (maxRootAngle < 180f)
{
Vector3 f = faceDirection;
if (rootHeadingOffset != 0f)
{
f = Quaternion.AngleAxis(rootHeadingOffset, Vector3.up) * f;
}
Vector3 faceDirLocal = Quaternion.Inverse(rootBone.solverRotation) * f;
float angle = Mathf.Atan2(faceDirLocal.x, faceDirLocal.z) * Mathf.Rad2Deg;
float rotation = 0f;
float maxAngle = maxRootAngle;
if (angle > maxAngle)
{
rotation = angle - maxAngle;
}
if (angle < -maxAngle)
{
rotation = angle + maxAngle;
}
rootBone.solverRotation = Quaternion.AngleAxis(rotation, rootBone.readRotation * Vector3.up) * rootBone.solverRotation;
}
Vector3 animatedPelvisPos = pelvis.solverPosition;
Vector3 rootUp = rootBone.solverRotation * Vector3.up;
// Translate pelvis to make the head's position & rotation match with the head target
TranslatePelvis(legs, headDeltaPosition, pelvisDeltaRotation, scale);
FABRIKPass(animatedPelvisPos, rootUp, positionWeight);
// Bend the spine to look towards chest target rotation
Bend(bones, pelvisIndex, chestIndex, chestTargetRotation, chestRotationOffset, chestClampWeight, false, neckStiffness * rotationWeight);
if (LOD < 1 && chestGoalWeight > 0f)
{
Quaternion c = Quaternion.FromToRotation(bones[chestIndex].solverRotation * chestForward, goalPositionChest - bones[chestIndex].solverPosition) * bones[chestIndex].solverRotation;
Bend(bones, pelvisIndex, chestIndex, c, chestRotationOffset, chestClampWeight, false, chestGoalWeight * rotationWeight);
}
InverseTranslateToHead(legs, false, false, Vector3.zero, positionWeight);
if (LOD < 1)
{
FABRIKPass(animatedPelvisPos, rootUp, positionWeight);
}
Bend(bones, neckIndex, headIndex, headRotation, headClampWeight, true, rotationWeight);
SolvePelvis();
}
private void CalculateChestTargetRotation(VirtualBone rootBone, Arm[] arms)
{
chestTargetRotation = headRotation * chestRelativeRotation;
// Use hands to adjust c
AdjustChestByHands(ref chestTargetRotation, arms);
faceDirection = Vector3.Cross(anchorRotation * Vector3.right, rootBone.readRotation * Vector3.up) + anchorRotation * Vector3.forward;
}
private void Read(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, bool hasArms)
{
if (rootBone == null)
{
rootBone = new VirtualBone(positions [0], rotations [0]);
}
else
{
rootBone.Read(positions [0], rotations [0]);
}
spine.Read(positions, rotations, hasChest, hasNeck, hasShoulders, hasToes, hasLegs, 0, 1);
if (hasArms)
{
leftArm.Read(positions, rotations, hasChest, hasNeck, hasShoulders, hasToes, hasLegs, hasChest ? 3 : 2, 6);
rightArm.Read(positions, rotations, hasChest, hasNeck, hasShoulders, hasToes, hasLegs, hasChest ? 3 : 2, 10);
}
if (hasLegs)
{
leftLeg.Read(positions, rotations, hasChest, hasNeck, hasShoulders, hasToes, hasLegs, 1, 14);
rightLeg.Read(positions, rotations, hasChest, hasNeck, hasShoulders, hasToes, hasLegs, 1, 18);
}
for (int i = 0; i < rotations.Length; i++)
{
this.solvedPositions[i] = positions[i];
this.solvedRotations[i] = rotations[i];
}
if (!initiated)
{
if (hasLegs)
{
legs = new Leg[2] {
leftLeg, rightLeg
}
}
;
if (hasArms)
{
arms = new Arm[2] {
leftArm, rightArm
}
}
;
if (hasLegs)
{
locomotion.Initiate(animator, positions, rotations, hasToes, scale);
}
raycastOriginPelvis = spine.pelvis.readPosition;
spine.faceDirection = readRotations[0] * Vector3.forward;
}
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasNeck, bool hasShoulders, bool hasToes, int rootIndex, int index)
{
Vector3 pelvisPos = positions[index];
Quaternion pelvisRot = rotations[index];
Vector3 spinePos = positions[index + 1];
Quaternion spineRot = rotations[index + 1];
Vector3 chestPos = positions[index + 2];
Quaternion chestRot = rotations[index + 2];
Vector3 neckPos = positions[index + 3];
Quaternion neckRot = rotations[index + 3];
Vector3 headPos = positions[index + 4];
Quaternion headRot = rotations[index + 4];
if (!initiated)
{
this.hasNeck = hasNeck;
headHeight = headPos.y - positions[0].y;
bones = new VirtualBone[hasNeck? 5: 4];
headIndex = hasNeck? 4: 3;
bones[0] = new VirtualBone(pelvisPos, pelvisRot);
bones[1] = new VirtualBone(spinePos, spineRot);
bones[2] = new VirtualBone(chestPos, chestRot);
if (hasNeck)
{
bones[3] = new VirtualBone(neckPos, neckRot);
}
bones[headIndex] = new VirtualBone(headPos, headRot);
pelvisRotationOffset = Quaternion.identity;
chestRotationOffset = Quaternion.identity;
headRotationOffset = Quaternion.identity;
anchorRelativeToHead = Quaternion.Inverse(headRot) * rotations[0];
// Forward and up axes
pelvisRelativeRotation = Quaternion.Inverse(headRot) * pelvisRot;
chestRelativeRotation = Quaternion.Inverse(headRot) * chestRot;
faceDirection = rotations[0] * Vector3.forward;
IKPositionHead = headPos;
IKRotationHead = headRot;
IKPositionPelvis = pelvisPos;
}
bones[0].Read(pelvisPos, pelvisRot);
bones[1].Read(spinePos, spineRot);
bones[2].Read(chestPos, chestRot);
if (hasNeck)
{
bones[3].Read(neckPos, neckRot);
}
bones[headIndex].Read(headPos, headRot);
}
private void FABRIKPass(Vector3 animatedPelvisPos, Vector3 rootUp, float weight)
{
Vector3 startPos = Vector3.Lerp(pelvis.solverPosition, animatedPelvisPos, maintainPelvisPosition) + pelvisPositionOffset;// - chestPositionOffset;
Vector3 endPos = headPosition - chestPositionOffset;
//Vector3 startOffset = rootUp * (bones[bones.Length - 1].solverPosition - bones[0].solverPosition).magnitude;
Vector3 startOffset = Vector3.zero;// (bones[bones.Length - 1].solverPosition - bones[0].solverPosition) * weight;
float dist = Vector3.Distance(bones[0].solverPosition, bones[bones.Length - 1].solverPosition);
VirtualBone.SolveFABRIK(bones, startPos, endPos, weight, 1f, 1, dist, startOffset);
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, int rootIndex, int index)
{
Vector3 thighPos = positions[index];
Quaternion thighRot = rotations[index];
Vector3 calfPos = positions[index + 1];
Quaternion calfRot = rotations[index + 1];
Vector3 footPos = positions[index + 2];
Quaternion footRot = rotations[index + 2];
Vector3 toePos = positions[index + 3];
Quaternion toeRot = rotations[index + 3];
if (!initiated)
{
this.hasToes = hasToes;
bones = new VirtualBone[hasToes ? 4 : 3];
if (hasToes)
{
bones[0] = new VirtualBone(thighPos, thighRot);
bones[1] = new VirtualBone(calfPos, calfRot);
bones[2] = new VirtualBone(footPos, footRot);
bones[3] = new VirtualBone(toePos, toeRot);
IKPosition = toePos;
IKRotation = toeRot;
}
else
{
bones[0] = new VirtualBone(thighPos, thighRot);
bones[1] = new VirtualBone(calfPos, calfRot);
bones[2] = new VirtualBone(footPos, footRot);
IKPosition = footPos;
IKRotation = footRot;
}
rotation = IKRotation;
}
if (hasToes)
{
bones[0].Read(thighPos, thighRot);
bones[1].Read(calfPos, calfRot);
bones[2].Read(footPos, footRot);
bones[3].Read(toePos, toeRot);
}
else
{
bones[0].Read(thighPos, thighRot);
bones[1].Read(calfPos, calfRot);
bones[2].Read(footPos, footRot);
}
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, int rootIndex, int index)
{
Vector3 shoulderPosition = positions[index];
Quaternion shoulderRotation = rotations[index];
Vector3 upperArmPosition = positions[index + 1];
Quaternion upperArmRotation = rotations[index + 1];
Vector3 forearmPosition = positions[index + 2];
Quaternion forearmRotation = rotations[index + 2];
Vector3 handPosition = positions[index + 3];
Quaternion handRotation = rotations[index + 3];
if (!initiated)
{
IKPosition = handPosition;
IKRotation = handRotation;
rotation = IKRotation;
this.hasShoulder = hasShoulders;
bones = new VirtualBone[hasShoulder? 4: 3];
if (hasShoulder)
{
bones[0] = new VirtualBone(shoulderPosition, shoulderRotation);
bones[1] = new VirtualBone(upperArmPosition, upperArmRotation);
bones[2] = new VirtualBone(forearmPosition, forearmRotation);
bones[3] = new VirtualBone(handPosition, handRotation);
}
else
{
this.bones[0] = new VirtualBone(upperArmPosition, upperArmRotation);
this.bones[1] = new VirtualBone(forearmPosition, forearmRotation);
this.bones[2] = new VirtualBone(handPosition, handRotation);
}
chestForwardAxis = Quaternion.Inverse(rootRotation) * (rotations[0] * Vector3.forward);
chestUpAxis = Quaternion.Inverse(rootRotation) * (rotations[0] * Vector3.up);
}
if (hasShoulder)
{
bones[0].Read(shoulderPosition, shoulderRotation);
bones[1].Read(upperArmPosition, upperArmRotation);
bones[2].Read(forearmPosition, forearmRotation);
bones[3].Read(handPosition, handRotation);
}
else
{
bones[0].Read(upperArmPosition, upperArmRotation);
bones[1].Read(forearmPosition, forearmRotation);
bones[2].Read(handPosition, handRotation);
}
}
public void Read(Vector3[] positions, Quaternion[] rotations, bool hasNeck, bool hasShoulders, bool hasToes, int rootIndex, int index)
{
this.index = index;
rootPosition = positions[rootIndex];
rootRotation = rotations[rootIndex];
OnRead(positions, rotations, hasNeck, hasShoulders, hasToes, rootIndex, index);
mag = VirtualBone.PreSolve(ref bones);
sqrMag = mag * mag;
initiated = true;
}
public void Solve(VirtualBone rootBone, Leg[] legs, Arm[] arms)
{
CalculateChestTargetRotation(rootBone, arms);
// Root rotation
if (maxRootAngle < 180f)
{
Vector3 faceDirLocal = Quaternion.Inverse(rootBone.solverRotation) * faceDirection;
float angle = Mathf.Atan2(faceDirLocal.x, faceDirLocal.z) * Mathf.Rad2Deg;
float rotation = 0f;
float maxAngle = 25f;
if (angle > maxAngle)
{
rotation = angle - maxAngle;
}
if (angle < -maxAngle)
{
rotation = angle + maxAngle;
}
rootBone.solverRotation = Quaternion.AngleAxis(rotation, rootBone.readRotation * Vector3.up) * rootBone.solverRotation;
}
Vector3 animatedPelvisPos = pelvis.solverPosition;
// Translate pelvis to make the head's position & rotation match with the head target
TranslatePelvis(legs, headDeltaPosition, pelvisDeltaRotation);
// Solve a FABRIK pass to squash/stretch the spine
VirtualBone.SolveFABRIK(bones, Vector3.Lerp(pelvis.solverPosition, animatedPelvisPos, maintainPelvisPosition) + pelvisPositionOffset - chestPositionOffset, headPosition - chestPositionOffset, 1f, 1f, 1, mag);
// Bend the spine to look towards chest target rotation
Bend(bones, pelvisIndex, chestIndex, chestTargetRotation, chestRotationOffset, chestClampWeight, false, neckStiffness);
if (chestGoalWeight > 0f)
{
Quaternion c = Quaternion.FromToRotation(bones[chestIndex].solverRotation * chestForward, goalPositionChest - bones[chestIndex].solverPosition) * bones[chestIndex].solverRotation;
Bend(bones, pelvisIndex, chestIndex, c, chestRotationOffset, chestClampWeight, false, chestGoalWeight);
}
InverseTranslateToHead(legs, false, false, Vector3.zero, 1f);
VirtualBone.SolveFABRIK(bones, Vector3.Lerp(pelvis.solverPosition, animatedPelvisPos, maintainPelvisPosition) + pelvisPositionOffset - chestPositionOffset, headPosition - chestPositionOffset, 1f, 1f, 1, mag);
Bend(bones, neckIndex, headIndex, headRotation, headClampWeight, true, 1f);
SolvePelvis();
}
// TODO Move to IKSolverFABRIK
// Solves a simple FABRIK pass for a bone hierarchy, not using rotation limits or singularity breaking here
public static void SolveFABRIK(VirtualBone[] bones, Vector3 startPosition, Vector3 targetPosition, float weight, float minNormalizedTargetDistance, int iterations, float length, Vector3 startOffset)
{
if (weight <= 0f)
{
return;
}
if (minNormalizedTargetDistance > 0f)
{
Vector3 targetDirection = targetPosition - startPosition;
float targetLength = targetDirection.magnitude;
Vector3 tP = startPosition + (targetDirection / targetLength) * Mathf.Max(length * minNormalizedTargetDistance, targetLength);
targetPosition = Vector3.Lerp(targetPosition, tP, weight);
}
// Iterating the solver
for (int iteration = 0; iteration < iterations; iteration++)
{
// Stage 1
bones[bones.Length - 1].solverPosition = Vector3.Lerp(bones[bones.Length - 1].solverPosition, targetPosition, weight);
for (int i = bones.Length - 2; i > -1; i--)
{
// Finding joint positions
bones[i].solverPosition = SolveFABRIKJoint(bones[i].solverPosition, bones[i + 1].solverPosition, bones[i].length);
}
// Stage 2
if (iteration == 0)
{
foreach (VirtualBone bone in bones)
{
bone.solverPosition += startOffset;
}
}
bones[0].solverPosition = startPosition;
for (int i = 1; i < bones.Length; i++)
{
bones[i].solverPosition = SolveFABRIKJoint(bones[i].solverPosition, bones[i - 1].solverPosition, bones[i - 1].length);
}
}
for (int i = 0; i < bones.Length - 1; i++)
{
VirtualBone.SwingRotation(bones, i, bones[i + 1].solverPosition);
}
}
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 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 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 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;
}
public void Solve(VirtualBone rootBone, Spine spine, Leg leftLeg, Leg rightLeg, Arm leftArm, Arm rightArm, int supportLegIndex, out Vector3 leftFootPosition, out Vector3 rightFootPosition, out Quaternion leftFootRotation, out Quaternion rightFootRotation, out float leftFootOffset, out float rightFootOffset, out float leftHeelOffset, out float rightHeelOffset)
{
if (weight <= 0f)
{
leftFootPosition = Vector3.zero;
rightFootPosition = Vector3.zero;
leftFootRotation = Quaternion.identity;
rightFootRotation = Quaternion.identity;
leftFootOffset = 0f;
rightFootOffset = 0f;
leftHeelOffset = 0f;
rightHeelOffset = 0f;
return;
}
Vector3 rootUp = rootBone.solverRotation * Vector3.up;
Vector3 leftThighPosition = spine.pelvis.solverPosition + spine.pelvis.solverRotation * leftLeg.thighRelativeToPelvis;
Vector3 rightThighPosition = spine.pelvis.solverPosition + spine.pelvis.solverRotation * rightLeg.thighRelativeToPelvis;
footsteps[0].characterSpaceOffset = footDistance * Vector3.left;
footsteps[1].characterSpaceOffset = footDistance * Vector3.right;
Vector3 forward = spine.faceDirection;
Vector3 forwardY = V3Tools.ExtractVertical(forward, rootUp, 1f);
forward -= forwardY;
Quaternion forwardRotation = Quaternion.LookRotation(forward, rootUp);
if (spine.rootHeadingOffset != 0f)
{
forwardRotation = Quaternion.AngleAxis(spine.rootHeadingOffset, rootUp) * forwardRotation;
}
//centerOfMass = Vector3.Lerp(spine.pelvis.solverPosition, spine.head.solverPosition, 0.25f) + rootBone.solverRotation * offset;
float pelvisMass = 1f;
float headMass = 1f;
float armMass = 0.2f;
float totalMass = pelvisMass + headMass + 2f * armMass;
centerOfMass = Vector3.zero;
centerOfMass += spine.pelvis.solverPosition * pelvisMass;
centerOfMass += spine.head.solverPosition * headMass;
centerOfMass += leftArm.position * armMass;
centerOfMass += rightArm.position * armMass;
centerOfMass /= totalMass;
centerOfMass += rootBone.solverRotation * offset;
comVelocity = Time.deltaTime > 0f? (centerOfMass - lastComPosition) / Time.deltaTime: Vector3.zero;
lastComPosition = centerOfMass;
comVelocity = Vector3.ClampMagnitude(comVelocity, maxVelocity) * velocityFactor;
Vector3 centerOfMassV = centerOfMass + comVelocity;
Vector3 pelvisPositionGroundLevel = V3Tools.PointToPlane(spine.pelvis.solverPosition, rootBone.solverPosition, rootUp);
Vector3 centerOfMassVGroundLevel = V3Tools.PointToPlane(centerOfMassV, rootBone.solverPosition, rootUp);
Vector3 centerOfPressure = Vector3.Lerp(footsteps[0].position, footsteps[1].position, 0.5f);
Vector3 comDir = centerOfMassV - centerOfPressure;
float comAngle = Vector3.Angle(comDir, rootBone.solverRotation * Vector3.up) * comAngleMlp;
// Set support leg
for (int i = 0; i < footsteps.Length; i++)
{
footsteps[i].isSupportLeg = supportLegIndex == i;
}
// Update stepTo while stepping
for (int i = 0; i < footsteps.Length; i++)
{
if (footsteps[i].isStepping)
{
Vector3 stepTo = centerOfMassVGroundLevel + rootBone.solverRotation * footsteps[i].characterSpaceOffset;
if (!StepBlocked(footsteps[i].stepFrom, stepTo, rootBone.solverPosition))
{
footsteps[i].UpdateStepping(stepTo, forwardRotation, 10f);
}
}
else
{
footsteps[i].UpdateStanding(forwardRotation, relaxLegTwistMinAngle, relaxLegTwistSpeed);
}
}
// Triggering new footsteps
if (CanStep())
{
int stepLegIndex = -1;
float bestValue = -Mathf.Infinity;
for (int i = 0; i < footsteps.Length; i++)
{
if (!footsteps[i].isStepping)
{
Vector3 stepTo = centerOfMassVGroundLevel + rootBone.solverRotation * footsteps[i].characterSpaceOffset;
float legLength = i == 0? leftLeg.mag: rightLeg.mag;
Vector3 thighPos = i == 0? leftThighPosition: rightThighPosition;
float thighDistance = Vector3.Distance(footsteps[i].position, thighPos);
bool lengthStep = false;
if (thighDistance >= legLength * maxLegStretch) // * 0.95f) {
{
stepTo = pelvisPositionGroundLevel + rootBone.solverRotation * footsteps[i].characterSpaceOffset;
lengthStep = true;
}
bool collision = false;
for (int n = 0; n < footsteps.Length; n++)
{
if (n != i && !lengthStep)
{
if (Vector3.Distance(footsteps[i].position, footsteps[n].position) < 0.25f && (footsteps[i].position - stepTo).sqrMagnitude < (footsteps[n].position - stepTo).sqrMagnitude)
{
}
else
{
collision = GetLineSphereCollision(footsteps[i].position, stepTo, footsteps[n].position, 0.25f);
}
if (collision)
{
break;
}
}
}
float angle = Quaternion.Angle(forwardRotation, footsteps[i].stepToRootRot);
if (!collision || angle > angleThreshold)
{
float stepDistance = Vector3.Distance(footsteps[i].position, stepTo);
float sT = Mathf.Lerp(stepThreshold, stepThreshold * 0.1f, comAngle * 0.015f);
if (lengthStep)
{
sT *= 0.5f;
}
if (i == 0)
{
sT *= 0.9f;
}
if (!StepBlocked(footsteps[i].position, stepTo, rootBone.solverPosition))
{
if (stepDistance > sT || angle > angleThreshold)
{
float value = 0f;
value -= stepDistance;
if (value > bestValue)
{
stepLegIndex = i;
bestValue = value;
}
}
}
}
}
}
if (stepLegIndex != -1)
{
Vector3 stepTo = centerOfMassVGroundLevel + rootBone.solverRotation * footsteps[stepLegIndex].characterSpaceOffset;
footsteps[stepLegIndex].stepSpeed = UnityEngine.Random.Range(stepSpeed, stepSpeed * 1.5f);
footsteps[stepLegIndex].StepTo(stepTo, forwardRotation, stepThreshold);
}
}
footsteps[0].Update(stepInterpolation, onLeftFootstep);
footsteps[1].Update(stepInterpolation, onRightFootstep);
leftFootPosition = footsteps[0].position;
rightFootPosition = footsteps[1].position;
leftFootPosition = V3Tools.PointToPlane(leftFootPosition, leftLeg.lastBone.readPosition, rootUp);
rightFootPosition = V3Tools.PointToPlane(rightFootPosition, rightLeg.lastBone.readPosition, rootUp);
leftFootOffset = stepHeight.Evaluate(footsteps[0].stepProgress);
rightFootOffset = stepHeight.Evaluate(footsteps[1].stepProgress);
leftHeelOffset = heelHeight.Evaluate(footsteps[0].stepProgress);
rightHeelOffset = heelHeight.Evaluate(footsteps[1].stepProgress);
leftFootRotation = footsteps[0].rotation;
rightFootRotation = footsteps[1].rotation;
}
public void MoveRotation(Quaternion rotation)
{
Quaternion delta = QuaTools.FromToRotation(bones[0].solverRotation, rotation);
VirtualBone.RotateAroundPoint(bones, 0, bones[0].solverPosition, delta);
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, int rootIndex, int index)
{
Vector3 shoulderPosition = positions[index];
Quaternion shoulderRotation = rotations[index];
Vector3 upperArmPosition = positions[index + 1];
Quaternion upperArmRotation = rotations[index + 1];
Vector3 forearmPosition = positions[index + 2];
Quaternion forearmRotation = rotations[index + 2];
Vector3 handPosition = positions[index + 3];
Quaternion handRotation = rotations[index + 3];
if (!initiated)
{
IKPosition = handPosition;
IKRotation = handRotation;
rotation = IKRotation;
this.hasShoulder = hasShoulders;
bones = new VirtualBone[hasShoulder ? 4 : 3];
if (hasShoulder)
{
bones[0] = new VirtualBone(shoulderPosition, shoulderRotation);
bones[1] = new VirtualBone(upperArmPosition, upperArmRotation);
bones[2] = new VirtualBone(forearmPosition, forearmRotation);
bones[3] = new VirtualBone(handPosition, handRotation);
}
else
{
this.bones[0] = new VirtualBone(upperArmPosition, upperArmRotation);
this.bones[1] = new VirtualBone(forearmPosition, forearmRotation);
this.bones[2] = new VirtualBone(handPosition, handRotation);
}
Vector3 rootForward = rotations[0] * Vector3.forward;
chestForwardAxis = Quaternion.Inverse(rootRotation) * rootForward;
chestUpAxis = Quaternion.Inverse(rootRotation) * (rotations[0] * Vector3.up);
// Get the local axis of the upper arm pointing towards the bend normal
Vector3 upperArmForwardAxis = AxisTools.GetAxisVectorToDirection(upperArmRotation, rootForward);
if (Vector3.Dot(upperArmRotation * upperArmForwardAxis, rootForward) < 0f)
{
upperArmForwardAxis = -upperArmForwardAxis;
}
upperArmBendAxis = Vector3.Cross(Quaternion.Inverse(upperArmRotation) * (forearmPosition - upperArmPosition), upperArmForwardAxis);
if (upperArmBendAxis == Vector3.zero)
{
Debug.LogWarning("VRIK can not calculate which way to bend the arms because the arms are perfectly straight. Please rotate the elbow bones slightly in their natural bending direction in the Editor.");
}
}
if (hasShoulder)
{
bones[0].Read(shoulderPosition, shoulderRotation);
bones[1].Read(upperArmPosition, upperArmRotation);
bones[2].Read(forearmPosition, forearmRotation);
bones[3].Read(handPosition, handRotation);
}
else
{
bones[0].Read(upperArmPosition, upperArmRotation);
bones[1].Read(forearmPosition, forearmRotation);
bones[2].Read(handPosition, handRotation);
}
}
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 Visualize(VirtualBone bone1, VirtualBone bone2, VirtualBone bone3, Color color)
{
Debug.DrawLine(bone1.solverPosition, bone2.solverPosition, color);
Debug.DrawLine(bone2.solverPosition, bone3.solverPosition, color);
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, int rootIndex, int index)
{
Vector3 pelvisPos = positions[index];
Quaternion pelvisRot = rotations[index];
Vector3 spinePos = positions[index + 1];
Quaternion spineRot = rotations[index + 1];
Vector3 chestPos = positions[index + 2];
Quaternion chestRot = rotations[index + 2];
Vector3 neckPos = positions[index + 3];
Quaternion neckRot = rotations[index + 3];
Vector3 headPos = positions[index + 4];
Quaternion headRot = rotations[index + 4];
this.hasLegs = hasLegs;
if (!hasChest)
{
chestPos = spinePos;
chestRot = spineRot;
}
if (!initiated)
{
this.hasChest = hasChest;
this.hasNeck = hasNeck;
headHeight = V3Tools.ExtractVertical(headPos - positions[0], rotations[0] * Vector3.up, 1f).magnitude;
int boneCount = 3;
if (hasChest)
{
boneCount++;
}
if (hasNeck)
{
boneCount++;
}
bones = new VirtualBone[boneCount];
chestIndex = hasChest? 2: 1;
neckIndex = 1;
if (hasChest)
{
neckIndex++;
}
if (hasNeck)
{
neckIndex++;
}
headIndex = 2;
if (hasChest)
{
headIndex++;
}
if (hasNeck)
{
headIndex++;
}
bones[0] = new VirtualBone(pelvisPos, pelvisRot);
bones[1] = new VirtualBone(spinePos, spineRot);
if (hasChest)
{
bones[chestIndex] = new VirtualBone(chestPos, chestRot);
}
if (hasNeck)
{
bones[neckIndex] = new VirtualBone(neckPos, neckRot);
}
bones[headIndex] = new VirtualBone(headPos, headRot);
pelvisRotationOffset = Quaternion.identity;
chestRotationOffset = Quaternion.identity;
headRotationOffset = Quaternion.identity;
anchorRelativeToHead = Quaternion.Inverse(headRot) * rotations[0];
anchorRelativeToPelvis = Quaternion.Inverse(pelvisRot) * rotations[0];
faceDirection = rotations[0] * Vector3.forward;
IKPositionHead = headPos;
IKRotationHead = headRot;
IKPositionPelvis = pelvisPos;
IKRotationPelvis = pelvisRot;
goalPositionChest = chestPos + rotations[0] * Vector3.forward;
}
// Forward and up axes
pelvisRelativeRotation = Quaternion.Inverse(headRot) * pelvisRot;
chestRelativeRotation = Quaternion.Inverse(headRot) * chestRot;
chestForward = Quaternion.Inverse(chestRot) * (rotations[0] * Vector3.forward);
bones[0].Read(pelvisPos, pelvisRot);
bones[1].Read(spinePos, spineRot);
if (hasChest)
{
bones[chestIndex].Read(chestPos, chestRot);
}
if (hasNeck)
{
bones[neckIndex].Read(neckPos, neckRot);
}
bones[headIndex].Read(headPos, headRot);
float spineLength = Vector3.Distance(pelvisPos, headPos);
sizeMlp = spineLength / 0.7f;
}
