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);
rootVelocity += (footPositionC - rootBone.solverPosition) * Time.deltaTime * 10f;
Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f);
rootVelocity -= rootVelocityV;
/*
* rootBone.solverPosition += rootVelocity * Time.deltaTime * 2f * locomotion.weight;
*
* //rootBone.solverPosition = Vector3.SmoothDamp(rootBone.solverPosition, footPositionC, ref rootV, locomotion.rootSDampTime);
*
* rootBone.solverPosition = Vector3.Lerp(rootBone.solverPosition, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight);
*/
Vector3 p = rootBone.solverPosition + rootVelocity * Time.deltaTime * 2f * locomotion.weight;
p = Vector3.Lerp(p, footPositionC, Time.deltaTime * locomotion.rootSpeed * locomotion.weight);
rootBone.solverPosition = p;
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();
}
}
}
// Stretching();
// Arms
for (int i = 0; i < arms.Length; i++)
{
arms[i].TranslateRoot(spine.chest.solverPosition, spine.chest.solverRotation);
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;
}
}
}
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;
}
private void Solve()
{
if (scale <= 0f)
{
Debug.LogError("VRIK solver scale <= 0, can not solve!");
return;
}
if (lastLocomotionWeight <= 0f && locomotion.weight > 0f)
{
locomotion.Reset(readPositions, readRotations);
}
spine.SetLOD(LOD);
if (hasArms)
{
foreach (Arm arm in arms)
{
arm.SetLOD(LOD);
}
}
if (hasLegs)
{
foreach (Leg leg in legs)
{
leg.SetLOD(LOD);
}
}
// Pre-Solving
spine.PreSolve(scale);
if (hasArms)
{
foreach (Arm arm in arms)
{
arm.PreSolve(scale);
}
}
if (hasLegs)
{
foreach (Leg leg in legs)
{
leg.PreSolve(scale);
}
}
// Applying spine and arm offsets
if (hasArms)
{
foreach (Arm arm in arms)
{
arm.ApplyOffsets(scale);
}
}
spine.ApplyOffsets(scale);
// Spine
spine.Solve(animator, rootBone, legs, arms, scale);
if (hasLegs && spine.pelvisPositionWeight > 0f && plantFeet)
{
Warning.Log("If VRIK 'Pelvis Position Weight' is > 0, 'Plant Feet' should be disabled to improve performance and stability.", root);
}
float deltaTime = Time.deltaTime;
// Locomotion
if (hasLegs)
{
if (locomotion.weight > 0f)
{
switch (locomotion.mode)
{
case Locomotion.Mode.Procedural:
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_Procedural(rootBone, spine, leftLeg, rightLeg, leftArm, rightArm, supportLegIndex, out leftFootPosition, out rightFootPosition, out leftFootRotation, out rightFootRotation, out leftFootOffset, out rightFootOffset, out leftHeelOffset, out rightHeelOffset, scale, deltaTime);
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 * deltaTime * 2f * locomotion.weight;
p = Vector3.Lerp(p, footPositionC, deltaTime * locomotion.rootSpeed * locomotion.weight);
rootBone.solverPosition = p;
rootVelocity += (footPositionC - rootBone.solverPosition) * deltaTime * 10f;
Vector3 rootVelocityV = V3Tools.ExtractVertical(rootVelocity, root.up, 1f);
rootVelocity -= rootVelocityV;
float bodyYOffset = Mathf.Min(leftFootOffset + rightFootOffset, locomotion.maxBodyYOffset * scale);
bodyOffset = Vector3.Lerp(bodyOffset, root.up * bodyYOffset, deltaTime * 3f);
bodyOffset = Vector3.Lerp(Vector3.zero, bodyOffset, locomotion.weight);
break;
case Locomotion.Mode.Animated:
if (lastLocomotionWeight <= 0f)
{
locomotion.Reset_Animated(readPositions);
}
locomotion.Solve_Animated(this, scale, deltaTime);
break;
}
}
else
{
if (lastLocomotionWeight > 0f)
{
locomotion.Reset_Animated(readPositions);
}
}
}
lastLocomotionWeight = locomotion.weight;
// Legs
if (hasLegs)
{
foreach (Leg leg in legs)
{
leg.ApplyOffsets(scale);
}
if (!plantFeet || LOD > 0)
{
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(true);
}
}
else
{
for (int i = 0; i < 2; i++)
{
spine.InverseTranslateToHead(legs, true, true, bodyOffset, 1f);
foreach (Leg leg in legs)
{
leg.TranslateRoot(spine.pelvis.solverPosition, spine.pelvis.solverRotation);
}
foreach (Leg leg in legs)
{
leg.Solve(i == 0);
}
}
}
}
else
{
spine.InverseTranslateToHead(legs, false, false, bodyOffset, 1f);
}
// Arms
if (hasArms)
{
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();
if (hasLegs)
{
foreach (Leg leg in legs)
{
leg.ResetOffsets();
}
}
if (hasArms)
{
foreach (Arm arm in arms)
{
arm.ResetOffsets();
}
}
if (hasLegs)
{
spine.pelvisPositionOffset += GetPelvisOffset(deltaTime);
spine.chestPositionOffset += spine.pelvisPositionOffset;
//spine.headPositionOffset += spine.pelvisPositionOffset;
}
Write();
// Find the support leg
if (hasLegs)
{
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;
}
}
}
}
