void _LowerSolve( bool firstPass )
{
var temp = _solverInternal;
var cache = _solverCaches;
Assert( temp != null );
Assert( cache != null );
if( temp.spinePos == null || temp.spinePos.Length == 0 ) {
return;
}
float limbRate = firstPass ? 1.0f : cache.armToLegRate;
if( temp.PrepareLowerRotation( 0 ) ) {
Vector3 centerLegBoneY = temp.centerLegBasis.column1;
for( int i = 0; i < 2; ++i ) {
if( temp.legs.endPosEnabled[i] && temp.legs.targetBeginPosEnabled[i] ) {
Vector3 legDir = temp.legs.targetBeginPos[i] - temp.legs.beginPos[i];
if( SAFBIKVecNormalize( ref legDir ) ) {
float legDirFeedbackRate = Vector3.Dot( centerLegBoneY, -legDir );
#if false
legDirFeedbackRate = Mathf.Asin( Mathf.Clamp( legDirFeedbackRate, -1.0f, 1.0f ) );
legDirFeedbackRate *= 1.0f / (90.0f * Mathf.Deg2Rad);
legDirFeedbackRate = Mathf.Clamp01( legDirFeedbackRate );
#else // Faster
legDirFeedbackRate = Mathf.Clamp01( legDirFeedbackRate );
#endif
legDirFeedbackRate = 1.0f - legDirFeedbackRate;
temp.SetSolveFeedbackRate( i, legDirFeedbackRate * limbRate );
}
}
}
Quaternion origLowerRotation;
if( temp.SolveLowerRotation( 0, out origLowerRotation ) ) {
temp.LowerRotation( 0, ref origLowerRotation, false );
}
}
if( _hipsEffector.positionEnabled &&
_hipsEffector.positionWeight <= IKEpsilon &&
_hipsEffector.pull >= 1.0f - IKEpsilon ) {
// Nothing.(Always locked.)
} else {
if( temp.PrepareLowerTranslate() ) {
Vector3 origLowerTranslate;
if( temp.SolveLowerTranslate( out origLowerTranslate ) ) {
if( limbRate < 1.0f - IKEpsilon ) {
origLowerTranslate *= limbRate;
}
if( _hipsEffector.positionEnabled && _hipsEffector.pull > IKEpsilon ) {
Vector3 extraTranslate = _hipsEffector._hidden_worldPosition - temp.centerLegPos;
origLowerTranslate = Vector3.Lerp( origLowerTranslate, extraTranslate, _hipsEffector.pull );
}
temp.Translate( ref origLowerTranslate );
}
}
}
}