public bool UpperSolve()
{
targetCenterArmEnabled = false;
float neckPull = _solverCaches.neckPull;
float headPull = _solverCaches.headPull;
float[] armPull = _solverCaches.armPull;
float[] elbowPull = _solverCaches.elbowPull;
float[] wristPull = _solverCaches.wristPull;
if( wristPull[0] <= IKEpsilon && wristPull[1] <= IKEpsilon &&
elbowPull[0] <= IKEpsilon && elbowPull[1] <= IKEpsilon &&
armPull[0] <= IKEpsilon && armPull[1] <= IKEpsilon &&
neckPull <= IKEpsilon && headPull <= IKEpsilon ) {
targetCenterArmPos = this.currentCenterArmPos;
targetCenterArmDir = this.currentCenterArmDir;
return false;
}
// Prepare _upperSolverTemp
_upperSolverTemp.targetNeckPos = (_neckEffector != null) ? _neckEffector._hidden_worldPosition : neckPos;
_upperSolverTemp.targetHeadPos = (_headEffector != null) ? _headEffector._hidden_worldPosition : headPos;
_upperSolverTemp.targetArmPos[0] = (_armEffectors != null) ? _armEffectors[0]._hidden_worldPosition : armPos[0];
_upperSolverTemp.targetArmPos[1] = (_armEffectors != null) ? _armEffectors[1]._hidden_worldPosition : armPos[1];
// Prepare _upperSolverPreArmsTemp
_upperSolverPreArmsTemp.neckPos = neckPos;
_upperSolverPreArmsTemp.armPos[0] = armPos[0];
_upperSolverPreArmsTemp.armPos[1] = armPos[1];
_upperSolverPreArmsTemp.shoulderEnabled = (shoulderPos != null);
if( _upperSolverPreArmsTemp.shoulderEnabled ) {
_upperSolverPreArmsTemp.shoulderPos[0] = shoulderPos[0];
_upperSolverPreArmsTemp.shoulderPos[1] = shoulderPos[1];
_upperSolverPreArmsTemp.nearArmPos = _upperSolverPreArmsTemp.shoulderPos;
} else {
_upperSolverPreArmsTemp.nearArmPos = _upperSolverPreArmsTemp.armPos;
}
// Moving fix.
float bodyMovingfixRate = settings.bodyIK.upperBodyMovingfixRate;
float headMovingfixRate = settings.bodyIK.upperHeadMovingfixRate;
if( bodyMovingfixRate > IKEpsilon || headMovingfixRate > IKEpsilon ) {
Vector3 headMove = Vector3.zero;
Vector3 bodyMove = Vector3.zero;
if( headPull > IKEpsilon ) {
headMove = _upperSolverTemp.targetHeadPos - headPos;
if( headMovingfixRate < 1.0f - IKEpsilon ) {
headMove *= headPull * headMovingfixRate;
} else {
headMove *= headPull;
}
}
float bodyPull = 0.0f;
float bodyPullInv = 0.0f;
if( neckPull > IKEpsilon || armPull[0] > IKEpsilon || armPull[1] > IKEpsilon ) {
bodyPull = (neckPull + armPull[0] + armPull[1]);
bodyPullInv = 1.0f / bodyPull;
if( neckPull > IKEpsilon ) {
bodyMove = (_upperSolverTemp.targetNeckPos - neckPos) * (neckPull * neckPull);
}
if( armPull[0] > IKEpsilon ) {
bodyMove += (_upperSolverTemp.targetArmPos[0] - armPos[0]) * (armPull[0] * armPull[0]);
}
if( armPull[1] > IKEpsilon ) {
bodyMove += (_upperSolverTemp.targetArmPos[1] - armPos[1]) * (armPull[1] * armPull[1]);
}
if( bodyMovingfixRate < 1.0f - IKEpsilon ) {
bodyMove *= bodyPullInv * bodyMovingfixRate;
} else {
bodyMove *= bodyPullInv;
}
}
Vector3 totalMove = new Vector3();
if( headPull > IKEpsilon && bodyPull > IKEpsilon ) {
totalMove = (headMove * headPull) + (bodyMove * bodyPull);
totalMove *= 1.0f / (headPull + bodyPull);
} else if( headPull > IKEpsilon ) {
totalMove = headMove;
} else {
totalMove = bodyMove;
}
_upperSolverPreArmsTemp.neckPos += totalMove;
_upperSolverPreArmsTemp.armPos[0] += totalMove;
_upperSolverPreArmsTemp.armPos[1] += totalMove;
if( _upperSolverPreArmsTemp.shoulderEnabled ) {
_upperSolverPreArmsTemp.shoulderPos[0] += totalMove;
_upperSolverPreArmsTemp.shoulderPos[1] += totalMove;
}
}
// Preprocess neckSolver.
if( headMovingfixRate < 1.0f - IKEpsilon || bodyMovingfixRate < 1.0f - IKEpsilon ) {
if( headPull > IKEpsilon || neckPull > IKEpsilon ) {
if( headMovingfixRate < 1.0f - IKEpsilon && headPull > IKEpsilon ) {
Vector3 tempNeckPos = _upperSolverPreArmsTemp.neckPos;
if( _KeepMaxLength( ref tempNeckPos, ref _upperSolverTemp.targetHeadPos, _solverCaches.neckToHeadLength ) ) { // Not KeepLength
_upperSolverPreArmsTemp.neckPos = Vector3.Lerp( _upperSolverPreArmsTemp.neckPos, tempNeckPos, headPull );
}
}
for( int i = 0; i != 2; ++i ) {
if( bodyMovingfixRate < 1.0f - IKEpsilon && neckPull > IKEpsilon ) {
Vector3 tempNearArmPos = _upperSolverPreArmsTemp.nearArmPos[i];
_KeepLength( ref tempNearArmPos, ref _upperSolverTemp.targetNeckPos, _solverCaches.nearArmToNeckLength[i] );
_upperSolverPreArmsTemp.nearArmPos[i] = Vector3.Lerp( _upperSolverPreArmsTemp.nearArmPos[i], tempNearArmPos, neckPull ); // Not use neckToFullArmPull in Presolve.
} else {
_KeepLength(
ref _upperSolverPreArmsTemp.nearArmPos[i],
ref _upperSolverPreArmsTemp.neckPos,
_solverCaches.nearArmToNeckLength[i] );
}
if( _upperSolverPreArmsTemp.shoulderEnabled ) {
_KeepLength(
ref _upperSolverPreArmsTemp.armPos[i],
ref _upperSolverPreArmsTemp.shoulderPos[i],
_solverCaches.shoulderToArmLength[i] );
}
//internalValues.AddDebugPoint( nearArmPos, Color.black, 0.1f );
}
}
}
// Update targetNeckPos using presolved. (Contain neckPull / headPull)
_upperSolverTemp.targetNeckPos = _upperSolverPreArmsTemp.neckPos;
// Prepare _upperSolverArmsTemps
for( int i = 0; i != 2; ++i ) {
_upperSolverArmsTemps[i].armPos[0] = _upperSolverPreArmsTemp.armPos[0];
_upperSolverArmsTemps[i].armPos[1] = _upperSolverPreArmsTemp.armPos[1];
_upperSolverArmsTemps[i].shoulderEnabled = _upperSolverPreArmsTemp.shoulderEnabled;
if( _upperSolverArmsTemps[i].shoulderEnabled ) {
_upperSolverArmsTemps[i].shoulderPos[0] = _upperSolverPreArmsTemp.shoulderPos[0];
_upperSolverArmsTemps[i].shoulderPos[1] = _upperSolverPreArmsTemp.shoulderPos[1];
_upperSolverArmsTemps[i].nearArmPos = _upperSolverArmsTemps[i].shoulderPos;
} else {
_upperSolverArmsTemps[i].nearArmPos = _upperSolverArmsTemps[i].armPos;
}
}
// Check enabled by side.
bool enabled0 = (wristPull[0] > IKEpsilon || elbowPull[0] > IKEpsilon || armPull[0] > IKEpsilon);
bool enabled1 = (wristPull[1] > IKEpsilon || elbowPull[1] > IKEpsilon || armPull[1] > IKEpsilon);
float neckHeadPull = _solverCaches.neckHeadPull;
if( (enabled0 && enabled1) || neckHeadPull > IKEpsilon ) {
for( int i = 0; i != 2; ++i ) {
int idx0 = i;
int idx1 = 1 - i;
_SolveArms( ref _upperSolverArmsTemps[idx0], idx0 );
_SolveArms( ref _upperSolverArmsTemps[idx0], idx1 );
_SolveArms( ref _upperSolverArmsTemps[idx0], idx0 );
if( _upperSolverArmsTemps[idx0].shoulderEnabled ) {
_upperSolverArmsTemps[idx0].centerArmPos = (_upperSolverArmsTemps[idx0].shoulderPos[0] + _upperSolverArmsTemps[idx0].shoulderPos[1]) * 0.5f;
_upperSolverArmsTemps[idx0].centerArmDir = _upperSolverArmsTemps[idx0].shoulderPos[1] - _upperSolverArmsTemps[idx0].shoulderPos[0];
} else {
_upperSolverArmsTemps[idx0].centerArmPos = (_upperSolverArmsTemps[idx0].armPos[0] + _upperSolverArmsTemps[idx0].armPos[1]) * 0.5f;
_upperSolverArmsTemps[idx0].centerArmDir = _upperSolverArmsTemps[idx0].armPos[1] - _upperSolverArmsTemps[idx0].armPos[0];
}
}
if( !SAFBIKVecNormalize2( ref _upperSolverArmsTemps[0].centerArmDir, ref _upperSolverArmsTemps[1].centerArmDir ) ) {
return false;
}
float limbArmRate = _solverCaches.limbArmRate;
targetCenterArmEnabled = true;
targetCenterArmPos = Vector3.Lerp( _upperSolverArmsTemps[0].centerArmPos, _upperSolverArmsTemps[1].centerArmPos, limbArmRate );
targetCenterArmDir = _LerpDir( ref _upperSolverArmsTemps[0].centerArmDir, ref _upperSolverArmsTemps[1].centerArmDir, limbArmRate );
} else {
int idx0 = enabled0 ? 0 : 1;
_SolveArms( ref _upperSolverArmsTemps[idx0], idx0 );
if( _upperSolverArmsTemps[idx0].shoulderEnabled ) {
_upperSolverArmsTemps[idx0].centerArmPos = (_upperSolverArmsTemps[idx0].shoulderPos[0] + _upperSolverArmsTemps[idx0].shoulderPos[1]) * 0.5f;
_upperSolverArmsTemps[idx0].centerArmDir = _upperSolverArmsTemps[idx0].shoulderPos[1] - _upperSolverArmsTemps[idx0].shoulderPos[0];
//internalValues.AddDebugPoint( _upperSolverArmsTemps[idx0].shoulderPos[0], Color.black, 0.1f );
//internalValues.AddDebugPoint( _upperSolverArmsTemps[idx0].shoulderPos[1], Color.black, 0.1f );
} else {
_upperSolverArmsTemps[idx0].centerArmPos = (_upperSolverArmsTemps[idx0].armPos[0] + _upperSolverArmsTemps[idx0].armPos[1]) * 0.5f;
_upperSolverArmsTemps[idx0].centerArmDir = _upperSolverArmsTemps[idx0].armPos[1] - _upperSolverArmsTemps[idx0].armPos[0];
//internalValues.AddDebugPoint( _upperSolverArmsTemps[idx0].armPos[0], Color.black, 0.1f );
//internalValues.AddDebugPoint( _upperSolverArmsTemps[idx0].armPos[1], Color.black, 0.1f );
}
if( !SAFBIKVecNormalize( ref _upperSolverArmsTemps[idx0].centerArmDir ) ) {
return false;
}
targetCenterArmEnabled = true;
targetCenterArmPos = _upperSolverArmsTemps[idx0].centerArmPos;
targetCenterArmDir = _upperSolverArmsTemps[idx0].centerArmDir;
}
return true;
}