bool _PrepareSolverInternal()
{
if( _armBones == null || _legBones == null ) {
_solverInternal = null;
return false;
}
// Get pull values at SolverCaches.
if( _neckEffector != null ) {
_solverCaches.neckPull = _neckEffector.positionEnabled ? _neckEffector.pull : 0.0f;
}
if( _headEffector != null ) {
_solverCaches.headPull = _headEffector.positionEnabled ? _headEffector.pull : 0.0f;
}
if( _eyesEffector != null ) {
_solverCaches.eyesRate = _eyesEffector.positionEnabled ? (_eyesEffector.pull * _eyesEffector.positionWeight) : 0.0f;
}
for( int i = 0; i != 2; ++i ) {
if( _armEffectors[i] != null ) {
_solverCaches.armPull[i] = _armEffectors[i].positionEnabled ? _armEffectors[i].pull : 0.0f;
}
if( _elbowEffectors[i] != null ) {
_solverCaches.elbowPull[i] = _elbowEffectors[i].positionEnabled ? _elbowEffectors[i].pull : 0.0f;
}
if( _wristEffectors[i] != null ) {
_solverCaches.wristPull[i] = _wristEffectors[i].positionEnabled ? _wristEffectors[i].pull : 0.0f;
}
if( _kneeEffectors[i] != null ) {
_solverCaches.kneePull[i] = _kneeEffectors[i].positionEnabled ? _kneeEffectors[i].pull : 0.0f;
}
if( _footEffectors[i] != null ) {
_solverCaches.footPull[i] = _footEffectors[i].positionEnabled ? _footEffectors[i].pull : 0.0f;
}
}
_solverCaches.neckHeadPull = _ConcatPull( _solverCaches.neckPull, _solverCaches.headPull );
// Update pull values at SolverInternal.
float upperPull = _solverCaches.neckHeadPull;
float lowerPull = 0.0f;
for( int i = 0; i != 2; ++i ) {
float fullArmPull = _solverCaches.armPull[i];
if( fullArmPull == 0.0f ) { // Optimized for function calls.
fullArmPull = _solverCaches.elbowPull[i];
} else {
fullArmPull = _ConcatPull( fullArmPull, _solverCaches.elbowPull[i] );
}
if( fullArmPull == 0.0f ) { // Optimized for function calls.
fullArmPull = _solverCaches.wristPull[i];
} else {
fullArmPull = _ConcatPull( fullArmPull, _solverCaches.wristPull[i] );
}
float limbArmPull = _solverCaches.kneePull[i];
if( limbArmPull == 0.0f ) { // Optimized for function calls.
limbArmPull = _solverCaches.wristPull[i];
} else {
limbArmPull = _ConcatPull( limbArmPull, _solverCaches.wristPull[i] );
}
float legPull = _solverCaches.kneePull[i];
if( legPull == 0.0f ) { // Optimized for function calls.
legPull = _solverCaches.footPull[i];
} else {
legPull = _ConcatPull( legPull, _solverCaches.footPull[i] );
}
_solverCaches.fullArmPull[i] = fullArmPull;
_solverCaches.limbLegPull[i] = legPull;
_solverCaches.armToElbowPull[i] = _GetBalancedPullLockFrom( _solverCaches.armPull[i], _solverCaches.elbowPull[i] );
_solverCaches.armToWristPull[i] = _GetBalancedPullLockFrom( _solverCaches.armPull[i], _solverCaches.wristPull[i] );
_solverCaches.neckHeadToFullArmPull[i] = _GetBalancedPullLockTo( _solverCaches.neckHeadPull, fullArmPull );
upperPull += fullArmPull;
lowerPull += legPull;
}
_solverCaches.limbArmRate = _GetLerpRateFromPull2( _solverCaches.fullArmPull[0], _solverCaches.fullArmPull[1] );
_solverCaches.limbLegRate = _GetLerpRateFromPull2( _solverCaches.limbLegPull[0], _solverCaches.limbLegPull[1] );
_solverCaches.armToLegRate = _GetLerpRateFromPull2( upperPull, lowerPull );
// _spineDirXRate, _spineEnabled
if( _spineBones != null ) {
int spineLength = _spineBones.Length;
Assert( _spineDirXRate != null && _spineDirXRate.Length == spineLength );
float spineDirXRate = Mathf.Clamp01( _settings.bodyIK.spineDirXLegToArmRate );
float spineDirXToRate = Mathf.Max( _settings.bodyIK.spineDirXLegToArmToRate, spineDirXRate );
for( int i = 0; i != spineLength; ++i ) {
if( i == 0 ) {
_spineDirXRate[i] = spineDirXRate;
} else if( i + 1 == spineLength ) {
_spineDirXRate[i] = spineDirXToRate;
} else {
_spineDirXRate[i] = spineDirXRate + (spineDirXToRate - spineDirXRate) * ((float)i / (float)(spineLength - 1));
}
}
if( spineLength > 0 ) {
_spineEnabled[0] = _settings.bodyIK.upperSolveSpineEnabled;
}
if( spineLength > 1 ) {
_spineEnabled[1] = _settings.bodyIK.upperSolveSpine2Enabled;
}
if( spineLength > 2 ) {
_spineEnabled[2] = _settings.bodyIK.upperSolveSpine3Enabled;
}
if( spineLength > 3 ) {
_spineEnabled[3] = _settings.bodyIK.upperSolveSpine4Enabled;
}
}
//------------------------------------------------------------------------------------------------------------------------
// Allocate solverInternal.
if( _solverInternal == null ) {
_solverInternal = new SolverInternal();
_solverInternal.settings = _settings;
_solverInternal.internalValues = _internalValues;
_solverInternal._solverCaches = _solverCaches;
_solverInternal.arms._bendingPull = _solverCaches.armToElbowPull;
_solverInternal.arms._endPull = _solverCaches.armToWristPull;
_solverInternal.arms._beginToBendingLength = _elbowEffectorMaxLength;
_solverInternal.arms._beginToEndLength = _wristEffectorMaxLength;
_solverInternal.legs._bendingPull = _solverCaches.kneePull;
_solverInternal.legs._endPull = _solverCaches.footPull;
_solverInternal.legs._beginToBendingLength = _kneeEffectorMaxLength;
_solverInternal.legs._beginToEndLength = _footEffectorMaxLength;
_solverInternal._shouderLocalAxisYInv = _shouderLocalAxisYInv;
_solverInternal._armEffectors = _armEffectors;
_solverInternal._wristEffectors = _wristEffectors;
_solverInternal._neckEffector = _neckEffector;
_solverInternal._headEffector = _headEffector;
_solverInternal._spineBones = _spineBones;
_solverInternal._shoulderBones = _shoulderBones;
_solverInternal._armBones = _armBones;
_solverInternal._limbIK = _limbIK;
_solverInternal._centerLegBoneBasisInv = this._centerLegBoneBasisInv;
PrepareArray( ref _solverInternal.shoulderPos, _shoulderBones );
PrepareArray( ref _solverInternal.spinePos, _spineBones );
_solverInternal.nearArmPos = (_shoulderBones != null) ? _solverInternal.shoulderPos : _solverInternal.armPos;
if( _spineUBone != null ) {
if( _shoulderBones != null || _armBones != null ) {
var nearArmBones = (_shoulderBones != null) ? _shoulderBones : _armBones;
Vector3 dirY = nearArmBones[1]._defaultPosition + nearArmBones[0]._defaultPosition;
Vector3 dirX = nearArmBones[1]._defaultPosition - nearArmBones[0]._defaultPosition;
dirY = dirY * 0.5f - _spineUBone._defaultPosition;
Vector3 dirZ = Vector3.Cross( dirX, dirY );
dirX = Vector3.Cross( dirY, dirZ );
if( SAFBIKVecNormalize3( ref dirX, ref dirY, ref dirZ ) ) {
Matrix3x3 localBasis = Matrix3x3.FromColumn( ref dirX, ref dirY, ref dirZ );
_solverInternal._spineUBoneLocalAxisBasisInv = localBasis.transpose;
}
}
}
}
_solverInternal.headEnabled = _headBone != null && _solverCaches.headPull > IKEpsilon;
return true;
}