void _Solve(FullBodyIK fullBodyIK)
{
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Matrix3x3 parentBasis;
SAFBIKMatSetRotMultInv1(out parentBasis, ref parentWorldRotation, ref _neckBone.parentBone._defaultRotation);
Matrix3x3 parentBaseBasis;
SAFBIKMatMult(out parentBaseBasis, ref parentBasis, ref _internalValues.defaultRootBasis);
Quaternion parentBaseRotation;
SAFBIKQuatMult(out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation);
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
Quaternion neckBonePrevRotation = Quaternion.identity;
Quaternion headBonePrevRotation = Quaternion.identity;
Quaternion leftEyeBonePrevRotation = Quaternion.identity;
Quaternion rightEyeBonePrevRotation = Quaternion.identity;
if (!_internalValues.resetTransforms)
{
neckBonePrevRotation = _neckBone.worldRotation;
headBonePrevRotation = _headBone.worldRotation;
if (_leftEyeBone != null && _leftEyeBone.transformIsAlive)
{
leftEyeBonePrevRotation = _leftEyeBone.worldRotation;
}
if (_rightEyeBone != null && _rightEyeBone.transformIsAlive)
{
rightEyeBonePrevRotation = _rightEyeBone.worldRotation;
}
}
// for Neck
if (headPositionWeight > IKEpsilon)
{
Matrix3x3 neckBoneBasis;
SAFBIKMatMult(out neckBoneBasis, ref parentBasis, ref _neckBone._localAxisBasis);
Vector3 yDir = _headEffector.worldPosition - _neckBone.worldPosition; // Not use _hidden_worldPosition
if (SAFBIKVecNormalize(ref yDir))
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref neckBoneBasis, ref yDir);
if (_LimitXZ_Square(ref localDir,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitPitchDownTheta.sin,
_internalValues.headIK.neckLimitPitchUpTheta.sin))
{
SAFBIKMatMultVec(out yDir, ref neckBoneBasis, ref localDir);
}
Vector3 xDir = parentBaseBasis.column0;
Vector3 zDir = parentBaseBasis.column2;
if (SAFBIKComputeBasisLockY(out neckBoneBasis, ref xDir, ref yDir, ref zDir))
{
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref neckBoneBasis, ref _neckBone._boneToWorldBasis);
if (headPositionWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult(out fromRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
}
else
{
fromRotation = neckBonePrevRotation; // This is able to use _headBone.worldRotation directly.
}
_neckBone.worldRotation = Quaternion.Lerp(fromRotation, worldRotation, headPositionWeight);
}
else
{
_neckBone.worldRotation = worldRotation;
}
}
}
}
else if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
_neckBone.worldRotation = tempRotation;
}
// for Head / Eyes
if (eyesPositionWeight <= IKEpsilon)
{
float headRotationWeight = _headEffector.rotationEnabled ? _headEffector.rotationWeight : 0.0f;
if (headRotationWeight > IKEpsilon)
{
Quaternion headEffectorWorldRotation = _headEffector.worldRotation;
Quaternion toRotation;
SAFBIKQuatMult(out toRotation, ref headEffectorWorldRotation, ref _headEffectorToWorldRotation);
if (headRotationWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult3(out fromRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation);
}
else
{
// Not use _headBone.worldRotation.
SAFBIKQuatMultNorm3Inv1(out fromRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation);
}
_headBone.worldRotation = Quaternion.Lerp(fromRotation, toRotation, headRotationWeight);
}
else
{
_headBone.worldRotation = toRotation;
}
}
else
{
if (_internalValues.resetTransforms)
{
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headBoneWorldRotation;
SAFBIKQuatMult3(out headBoneWorldRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation);
_headBone.worldRotation = headBoneWorldRotation;
}
}
_HeadRotationLimit();
if (_internalValues.resetTransforms)
{
if (_isEnabledCustomEyes)
{
fullBodyIK._ResetCustomEyes();
}
else
{
_ResetEyes();
}
}
return;
}
{
Vector3 eyesPosition, parentBoneWorldPosition = _neckBone.parentBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref parentBasis, ref _eyesEffector._defaultPosition, ref _neckBone.parentBone._defaultPosition, ref parentBoneWorldPosition);
// Note: Not use _eyesEffector._hidden_worldPosition
Vector3 eyesDir = _eyesEffector.worldPosition - eyesPosition; // Memo: Not normalize yet.
Matrix3x3 neckBaseBasis = parentBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref parentBaseBasis, ref eyesDir);
localDir.y *= _settings.headIK.eyesToNeckPitchRate;
SAFBIKVecNormalize(ref localDir);
if (_ComputeEyesRange(ref localDir, _internalValues.headIK.eyesTraceTheta.cos))
{
if (localDir.y < -_internalValues.headIK.neckLimitPitchDownTheta.sin)
{
localDir.y = -_internalValues.headIK.neckLimitPitchDownTheta.sin;
}
else if (localDir.y > _internalValues.headIK.neckLimitPitchUpTheta.sin)
{
localDir.y = _internalValues.headIK.neckLimitPitchUpTheta.sin;
}
localDir.x = 0.0f;
localDir.z = SAFBIKSqrt(1.0f - localDir.y * localDir.y);
}
SAFBIKMatMultVec(out eyesDir, ref parentBaseBasis, ref localDir);
{
Vector3 xDir = parentBaseBasis.column0;
Vector3 yDir = parentBaseBasis.column1;
Vector3 zDir = eyesDir;
if (!SAFBIKComputeBasisLockZ(out neckBaseBasis, ref xDir, ref yDir, ref zDir))
{
neckBaseBasis = parentBaseBasis; // Failsafe.
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref neckBaseBasis, ref _neckBone._baseToWorldBasis);
if (_eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion neckWorldRotation = Quaternion.Lerp(_neckBone.worldRotation, worldRotation, _eyesEffector.positionWeight); // This is able to use _neckBone.worldRotation directly.
_neckBone.worldRotation = neckWorldRotation;
SAFBIKMatSetRotMult(out neckBaseBasis, ref neckWorldRotation, ref _neckBone._worldToBaseRotation);
}
else
{
_neckBone.worldRotation = worldRotation;
}
}
Matrix3x3 neckBasis;
SAFBIKMatMult(out neckBasis, ref neckBaseBasis, ref _internalValues.defaultRootBasisInv);
Vector3 neckBoneWorldPosition = _neckBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref neckBasis, ref _eyesEffector._defaultPosition, ref _neckBone._defaultPosition, ref neckBoneWorldPosition);
// Note: Not use _eyesEffector._hidden_worldPosition
eyesDir = _eyesEffector.worldPosition - eyesPosition;
Matrix3x3 headBaseBasis = neckBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref neckBaseBasis, ref eyesDir);
localDir.x *= _settings.headIK.eyesToHeadYawRate;
localDir.y *= _settings.headIK.eyesToHeadPitchRate;
SAFBIKVecNormalize(ref localDir);
if (_ComputeEyesRange(ref localDir, _internalValues.headIK.eyesTraceTheta.cos))
{
// Note: Not use _LimitXY() for Stability
_LimitXY_Square(ref localDir,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitPitchDownTheta.sin,
_internalValues.headIK.headLimitPitchUpTheta.sin);
}
SAFBIKMatMultVec(out eyesDir, ref neckBaseBasis, ref localDir);
{
Vector3 xDir = neckBaseBasis.column0;
Vector3 yDir = neckBaseBasis.column1;
Vector3 zDir = eyesDir;
if (!SAFBIKComputeBasisLockZ(out headBaseBasis, ref xDir, ref yDir, ref zDir))
{
headBaseBasis = neckBaseBasis;
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref headBaseBasis, ref _headBone._baseToWorldBasis);
if (_eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headFromWorldRotation;
SAFBIKQuatMultNorm3Inv1(out headFromWorldRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation);
Quaternion headWorldRotation = Quaternion.Lerp(headFromWorldRotation, worldRotation, _eyesEffector.positionWeight);
_headBone.worldRotation = headWorldRotation;
SAFBIKMatSetRotMult(out headBaseBasis, ref headWorldRotation, ref _headBone._worldToBaseRotation);
}
else
{
_headBone.worldRotation = worldRotation;
}
}
Matrix3x3 headBasis;
SAFBIKMatMult(out headBasis, ref headBaseBasis, ref _internalValues.defaultRootBasisInv);
if (_isEnabledCustomEyes)
{
fullBodyIK._SolveCustomEyes(ref neckBasis, ref headBasis, ref headBaseBasis);
}
else
{
_SolveEyes(ref neckBasis, ref headBasis, ref headBaseBasis, ref headBonePrevRotation, ref leftEyeBonePrevRotation, ref rightEyeBonePrevRotation);
}
}
}
