// 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));
}
public override void PreSolve()
{
if (target != null)
{
IKPosition = target.position;
IKRotation = target.rotation;
}
position = V3Tools.Lerp(hand.solverPosition, IKPosition, positionWeight);
rotation = QuaTools.Lerp(hand.solverRotation, IKRotation, rotationWeight);
shoulder.axis = shoulder.axis.normalized;
forearmRelToUpperArm = Quaternion.Inverse(upperArm.solverRotation) * forearm.solverRotation;
}
private void WriteTransforms()
{
for (int i = 0; i < solverTransforms.Length; i++)
{
if (solverTransforms[i] != null)
{
bool isRootOrPelvis = i < 2;
bool isArm = i > 5 && i < 14;
if (isRootOrPelvis || isArm)
{
solverTransforms[i].position = V3Tools.Lerp(solverTransforms[i].position, GetPosition(i), IKPositionWeight);
}
solverTransforms[i].rotation = QuaTools.Lerp(solverTransforms[i].rotation, GetRotation(i), IKPositionWeight);
}
}
}
public override void PreSolve()
{
if (headTarget != null)
{
IKPositionHead = headTarget.position;
IKRotationHead = headTarget.rotation;
}
if (chestGoal != null)
{
goalPositionChest = chestGoal.position;
}
if (pelvisTarget != null)
{
IKPositionPelvis = pelvisTarget.position;
IKRotationPelvis = pelvisTarget.rotation;
}
headPosition = V3Tools.Lerp(head.solverPosition, IKPositionHead, positionWeight);
headRotation = QuaTools.Lerp(head.solverRotation, IKRotationHead, rotationWeight);
}
public override void ApplyOffsets()
{
headPosition += headPositionOffset;
Vector3 rootUp = rootRotation * Vector3.up;
if (rootUp == Vector3.up)
{
headPosition.y = Math.Max(rootPosition.y + minHeadHeight, headPosition.y);
}
else
{
Vector3 toHead = headPosition - rootPosition;
Vector3 hor = V3Tools.ExtractHorizontal(toHead, rootUp, 1f);
Vector3 ver = toHead - hor;
float dot = Vector3.Dot(ver, rootUp);
if (dot > 0f)
{
if (ver.magnitude < minHeadHeight)
{
ver = ver.normalized * minHeadHeight;
}
}
else
{
ver = -ver.normalized * minHeadHeight;
}
headPosition = rootPosition + hor + ver;
}
headRotation = headRotationOffset * headRotation;
headDeltaPosition = headPosition - head.solverPosition;
pelvisDeltaRotation = QuaTools.FromToRotation(pelvis.solverRotation, headRotation * pelvisRelativeRotation);
anchorRotation = headRotation * anchorRelativeToHead;
}
private void Solve()
{
// Pre-Solving
spine.PreSolve();
foreach (Arm arm in arms)
{
arm.PreSolve();
}
foreach (Leg leg in legs)
{
leg.PreSolve();
}
// Applying spine and arm offsets
foreach (Arm arm in arms)
{
arm.ApplyOffsets();
}
spine.ApplyOffsets();
// Spine
spine.Solve(rootBone, legs, arms);
if (spine.pelvisPositionWeight > 0f && plantFeet)
{
Warning.Log("If VRIK 'Pelvis Position Weight' is > 0, 'Plant Feet' should be disabled to improve performance and stability.", root);
}
// Locomotion
if (locomotion.weight > 0f)
{
Vector3 leftFootPosition = Vector3.zero;
Vector3 rightFootPosition = Vector3.zero;
Quaternion leftFootRotation = Quaternion.identity;
Quaternion rightFootRotation = Quaternion.identity;
float leftFootOffset = 0f;
float rightFootOffset = 0f;
float leftHeelOffset = 0f;
float rightHeelOffset = 0f;
locomotion.Solve(rootBone, spine, leftLeg, rightLeg, leftArm, rightArm, supportLegIndex, out leftFootPosition, out rightFootPosition, out leftFootRotation, out rightFootRotation, out leftFootOffset, out rightFootOffset, out leftHeelOffset, out rightHeelOffset);
leftFootPosition += root.up * leftFootOffset;
rightFootPosition += root.up * rightFootOffset;
leftLeg.footPositionOffset += (leftFootPosition - leftLeg.lastBone.solverPosition) * IKPositionWeight * (1f - leftLeg.positionWeight) * locomotion.weight;
rightLeg.footPositionOffset += (rightFootPosition - rightLeg.lastBone.solverPosition) * IKPositionWeight * (1f - rightLeg.positionWeight) * locomotion.weight;
leftLeg.heelPositionOffset += root.up * leftHeelOffset * locomotion.weight;
rightLeg.heelPositionOffset += root.up * rightHeelOffset * locomotion.weight;
Quaternion rotationOffsetLeft = QuaTools.FromToRotation(leftLeg.lastBone.solverRotation, leftFootRotation);
Quaternion rotationOffsetRight = QuaTools.FromToRotation(rightLeg.lastBone.solverRotation, rightFootRotation);
rotationOffsetLeft = Quaternion.Lerp(Quaternion.identity, rotationOffsetLeft, IKPositionWeight * (1f - leftLeg.rotationWeight) * locomotion.weight);
rotationOffsetRight = Quaternion.Lerp(Quaternion.identity, rotationOffsetRight, IKPositionWeight * (1f - rightLeg.rotationWeight) * locomotion.weight);
leftLeg.footRotationOffset = rotationOffsetLeft * leftLeg.footRotationOffset;
rightLeg.footRotationOffset = rotationOffsetRight * rightLeg.footRotationOffset;
Vector3 footPositionC = Vector3.Lerp(leftLeg.position + leftLeg.footPositionOffset, rightLeg.position + rightLeg.footPositionOffset, 0.5f);
footPositionC = V3Tools.PointToPlane(footPositionC, rootBone.solverPosition, root.up);
Vector3 p = rootBone.solverPosition + rootVelocity * Time.deltaTime * 2f * locomotion.weight;
p = Vector3.Lerp(p, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight);
rootBone.solverPosition = p;
rootVelocity += (footPositionC - rootBone.solverPosition) * Time.deltaTime * 10f;
Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f);
rootVelocity -= rootVelocityV;
float bodyYOffset = leftFootOffset + rightFootOffset;
bodyOffset = Vector3.Lerp(bodyOffset, root.up * bodyYOffset, Time.deltaTime * 3f);
bodyOffset = Vector3.Lerp(Vector3.zero, bodyOffset, locomotion.weight);
}
// Legs
foreach (Leg leg in legs)
{
leg.ApplyOffsets();
}
if (!plantFeet)
{
spine.InverseTranslateToHead(legs, false, false, bodyOffset, 1f);
foreach (Leg leg in legs)
{
leg.TranslateRoot(spine.pelvis.solverPosition, spine.pelvis.solverRotation);
}
foreach (Leg leg in legs)
{
leg.Solve();
}
}
else
{
for (int i = 0; i < 2; i++)
{
spine.InverseTranslateToHead(legs, true, i == 0, bodyOffset, 1f);
foreach (Leg leg in legs)
{
leg.TranslateRoot(spine.pelvis.solverPosition, spine.pelvis.solverRotation);
}
foreach (Leg leg in legs)
{
leg.Solve();
}
}
}
// Arms
for (int i = 0; i < arms.Length; i++)
{
arms[i].TranslateRoot(spine.chest.solverPosition, spine.chest.solverRotation);
}
for (int i = 0; i < arms.Length; i++)
{
arms[i].Solve(i == 0);
}
// Reset offsets
spine.ResetOffsets();
foreach (Leg leg in legs)
{
leg.ResetOffsets();
}
foreach (Arm arm in arms)
{
arm.ResetOffsets();
}
spine.pelvisPositionOffset += GetPelvisOffset();
spine.chestPositionOffset += spine.pelvisPositionOffset;
//spine.headPositionOffset += spine.pelvisPositionOffset;
Write();
// Find the support leg
supportLegIndex = -1;
float shortestMag = Mathf.Infinity;
for (int i = 0; i < legs.Length; i++)
{
float mag = Vector3.SqrMagnitude(legs[i].lastBone.solverPosition - legs[i].bones[0].solverPosition);
if (mag < shortestMag)
{
supportLegIndex = i;
shortestMag = mag;
}
}
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, 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 (!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];
// Forward and up axes
pelvisRelativeRotation = Quaternion.Inverse(headRot) * pelvisRot;
chestRelativeRotation = Quaternion.Inverse(headRot) * chestRot;
chestForward = Quaternion.Inverse(chestRot) * (rotations[0] * Vector3.forward);
faceDirection = rotations[0] * Vector3.forward;
IKPositionHead = headPos;
IKRotationHead = headRot;
IKPositionPelvis = pelvisPos;
IKRotationPelvis = pelvisRot;
goalPositionChest = chestPos + 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;
}
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);
//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);
}
}
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;
}
