void _SolveArms( ref _UpperSolverArmsTemp armsTemp, int idx0 )
{
int idx1 = 1 - idx0;
float neckHeadPull = _solverCaches.neckHeadPull;
float[] armPull = _solverCaches.armPull;
float[] elbowPull = _solverCaches.elbowPull;
float[] wristPull = _solverCaches.wristPull;
float[] neckHeadToFullArmPull = _solverCaches.neckHeadToFullArmPull;
if( wristPull[idx0] > IKEpsilon || elbowPull[idx0] > IKEpsilon || armPull[idx0] > IKEpsilon || neckHeadPull > IKEpsilon ) {
if( armPull[idx0] > IKEpsilon ) {
_SolveArmsToArms( ref armsTemp, armPull[idx0], idx0 );
}
if( (wristPull[idx0] > IKEpsilon || elbowPull[idx0] > IKEpsilon) &&
arms.SolveTargetBeginPos( idx0, ref armsTemp.armPos[idx0] ) ) {
armsTemp.armPos[idx0] = arms.targetBeginPos[idx0]; // Update armPos
if( armsTemp.shoulderEnabled ) {
_KeepLength( ref armsTemp.shoulderPos[idx0], ref armsTemp.armPos[idx0], _solverCaches.shoulderToArmLength[idx0] );
if( neckHeadPull > IKEpsilon ) {
_SolveArmsToNeck( ref armsTemp, neckHeadToFullArmPull[idx0], idx0 ); // Contain wristPull/neckPull.
_KeepLength( ref armsTemp.armPos[idx0], ref armsTemp.shoulderPos[idx0], _solverCaches.shoulderToArmLength[idx0] );
}
_KeepLength( ref armsTemp.shoulderPos[idx1], ref armsTemp.shoulderPos[idx0], _solverCaches.nearArmToNearArmLen );
_KeepLength( ref armsTemp.armPos[idx1], ref armsTemp.shoulderPos[idx1], _solverCaches.shoulderToArmLength[idx1] );
} else {
if( neckHeadPull > IKEpsilon ) {
_SolveArmsToNeck( ref armsTemp, neckHeadToFullArmPull[idx0], idx0 );
}
_KeepLength( ref armsTemp.armPos[idx1], ref armsTemp.armPos[idx0], _solverCaches.armToArmLen );
}
} else if( armPull[idx0] > IKEpsilon || neckHeadPull > IKEpsilon ) {
if( armPull[idx0] > IKEpsilon ) {
if( armsTemp.shoulderEnabled ) {
_KeepLength( ref armsTemp.shoulderPos[idx0], ref armsTemp.armPos[idx0], _solverCaches.shoulderToArmLength[idx0] );
}
}
if( neckHeadPull > IKEpsilon ) {
_SolveArmsToNeck( ref armsTemp, neckHeadToFullArmPull[idx0], idx0 ); // Contain wristPull/neckPull.
if( armsTemp.shoulderEnabled ) {
_KeepLength( ref armsTemp.armPos[idx0], ref armsTemp.shoulderPos[idx0], _solverCaches.shoulderToArmLength[idx0] );
}
}
if( armsTemp.shoulderEnabled ) {
_KeepLength( ref armsTemp.shoulderPos[idx1], ref armsTemp.shoulderPos[idx0], _solverCaches.nearArmToNearArmLen );
_KeepLength( ref armsTemp.armPos[idx1], ref armsTemp.shoulderPos[idx1], _solverCaches.shoulderToArmLength[idx1] );
} else {
_KeepLength( ref armsTemp.armPos[idx1], ref armsTemp.armPos[idx0], _solverCaches.armToArmLen );
}
}
}
}
void _SolveArmsToArms( ref _UpperSolverArmsTemp armsTemp, float armPull, int idx0 )
{
Vector3 targetArmPos = _upperSolverTemp.targetArmPos[idx0];
armsTemp.armPos[idx0] = Vector3.Lerp( armsTemp.armPos[idx0], targetArmPos, armPull );
}
void _SolveArmsToNeck( ref _UpperSolverArmsTemp armsTemp, float neckToFullArmPull, int idx0 )
{
Vector3 nearArmPos0 = armsTemp.nearArmPos[idx0];
_KeepLength( ref nearArmPos0, ref _upperSolverTemp.targetNeckPos, _solverCaches.nearArmToNeckLength[idx0] );
armsTemp.nearArmPos[idx0] = Vector3.Lerp( nearArmPos0, armsTemp.nearArmPos[idx0], neckToFullArmPull );
}
public static _UpperSolverArmsTemp Alloc()
{
_UpperSolverArmsTemp r = new _UpperSolverArmsTemp();
r.shoulderPos = new Vector3[2];
r.armPos = new Vector3[2];
r.nearArmPos = null; // shoulderPos / armPos
r.shoulderEnabled = false;
r.centerArmPos = Vector3.zero;
r.centerArmDir = Vector3.zero;
return r;
}
