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 );
}
}
}