public bool Solve(FullBodyIK fullBodyIK)
{
if (_neckBone == null || !_neckBone.transformIsAlive ||
_headBone == null || !_headBone.transformIsAlive ||
_headBone.parentBone == null || !_headBone.parentBone.transformIsAlive)
{
return(false);
}
_SyncDisplacement(fullBodyIK);
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
if (headPositionWeight <= IKEpsilon && eyesPositionWeight <= IKEpsilon)
{
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Quaternion parentBaseRotation;
SAFBIKQuatMult(out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation);
if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
_neckBone.worldRotation = tempRotation;
}
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;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult(out fromRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation);
}
else
{
fromRotation = _headBone.worldRotation; // This is able to use _headBone.worldRotation directly.
}
_headBone.worldRotation = Quaternion.Lerp(fromRotation, toRotation, headRotationWeight);
}
else
{
_headBone.worldRotation = toRotation;
}
_HeadRotationLimit();
}
else
{
if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation);
_headBone.worldRotation = tempRotation;
}
}
if (_internalValues.resetTransforms)
{
if (_isEnabledCustomEyes)
{
fullBodyIK._ResetCustomEyes();
}
else
{
_ResetEyes();
}
}
return(_internalValues.resetTransforms || (headRotationWeight > IKEpsilon));
}
_Solve(fullBodyIK);
return(true);
}
public bool Solve( FullBodyIK fullBodyIK )
{
if( _neckBone == null || !_neckBone.transformIsAlive ||
_headBone == null || !_headBone.transformIsAlive ||
_headBone.parentBone == null || !_headBone.parentBone.transformIsAlive ) {
return false;
}
_SyncDisplacement( fullBodyIK );
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
if( headPositionWeight <= IKEpsilon && eyesPositionWeight <= IKEpsilon ) {
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Quaternion parentBaseRotation;
SAFBIKQuatMult( out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation );
if( _internalValues.resetTransforms ) {
Quaternion tempRotation;
SAFBIKQuatMult( out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation );
_neckBone.worldRotation = tempRotation;
}
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;
if( _internalValues.resetTransforms ) {
SAFBIKQuatMult( out fromRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation );
} else {
fromRotation = _headBone.worldRotation; // This is able to use _headBone.worldRotation directly.
}
_headBone.worldRotation = Quaternion.Lerp( fromRotation, toRotation, headRotationWeight );
} else {
_headBone.worldRotation = toRotation;
}
_HeadRotationLimit();
} else {
if( _internalValues.resetTransforms ) {
Quaternion tempRotation;
SAFBIKQuatMult( out tempRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation );
_headBone.worldRotation = tempRotation;
}
}
if( _internalValues.resetTransforms ) {
if( _isEnabledCustomEyes ) {
fullBodyIK._ResetCustomEyes();
} else {
_ResetEyes();
}
}
return _internalValues.resetTransforms || (headRotationWeight > IKEpsilon);
}
_Solve( fullBodyIK );
return true;
}
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);
}
}
}
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 );
}
}
}