SA.FullBodyIK.BodyIK.SolverInternal.ShoulderResolve C# (CSharp) Method

				public bool ShoulderResolve()
				{
					Bone[] armBones = _solverCaches.armBones;
					Bone[] shoulderBones = _solverCaches.shoulderBones;
					float[] shoulderToArmLength = _solverCaches.shoulderToArmLength;
					if( armBones == null || shoulderBones == null ) {
						return false;
					}

					Assert( shoulderToArmLength != null );
					Assert( _limbIK != null );

					if( !_limbIK[(int)LimbIKLocation.LeftArm].IsSolverEnabled() &&
						!_limbIK[(int)LimbIKLocation.RightArm].IsSolverEnabled() ) {
						return false; // Not required.
					}

					for( int i = 0; i != 2; ++i ) {
						int limbIKIndex = (i == 0) ? (int)LimbIKLocation.LeftArm : (int)LimbIKLocation.RightArm;

						if( _limbIK[limbIKIndex].IsSolverEnabled() ) {
							Vector3 xDir, yDir, zDir;
							xDir = this.armPos[i] - this.shoulderPos[i];
							if( internalValues.shoulderDirYAsNeck != 0 ) {
								yDir = this.neckPos - this.shoulderPos[i];
							} else {
								yDir = this.shoulderPos[i] - this.spineUPos;
							}
							xDir = (i == 0) ? -xDir : xDir;
							zDir = Vector3.Cross( xDir, yDir );
							yDir = Vector3.Cross( zDir, xDir );
							if( SAFBIKVecNormalize3( ref xDir, ref yDir, ref zDir ) ) {
								Matrix3x3 boneBasis = Matrix3x3.FromColumn( ref xDir, ref yDir, ref zDir );
								SAFBIKMatMult( out _tempParentBasis[i], ref boneBasis, ref _shoulderBones[i]._boneToBaseBasis );
							}

							_tempArmPos[i] = this.armPos[i];
							_tempElbowPosEnabled[i] = _limbIK[limbIKIndex].Presolve(
								ref _tempParentBasis[i],
								ref _tempArmPos[i],
								out _tempArmToElbowDir[i],
								out _tempElbowToWristDir[i] );

							if( _tempElbowPosEnabled[i] ) {
								SAFBIKMatMultCol0( out _tempArmToElbowDefaultDir[i], ref _tempParentBasis[i], ref _armBones[i]._baseToBoneBasis );
								if( i == 0 ) {
									_tempArmToElbowDefaultDir[i] = -_tempArmToElbowDefaultDir[i];
								}
							}
						}
					}

					if( !_tempElbowPosEnabled[0] && !_tempElbowPosEnabled[1] ) {
						return false; // Not required.
					}

					float feedbackRate = settings.bodyIK.shoulderSolveBendingRate;

					bool updateAnything = false;
					for( int i = 0; i != 2; ++i ) {
						if( _tempElbowPosEnabled[i] ) {
							float theta;
							Vector3 axis;
							_ComputeThetaAxis( ref _tempArmToElbowDefaultDir[i], ref _tempArmToElbowDir[i], out theta, out axis );
							if( theta >= -FLOAT_EPSILON && theta <= FLOAT_EPSILON ) {
								// Nothing.
							} else {
								updateAnything = true;
								theta = SAFBIKCos( SAFBIKAcos( theta ) * feedbackRate );
								Matrix3x3 m = new Matrix3x3();
								SAFBIKMatSetAxisAngle( out m, ref axis, theta );

								Vector3 tempShoulderPos = this.shoulderPos[i];
								Vector3 tempDir = _tempArmPos[i] - tempShoulderPos;
								SAFBIKVecNormalize( ref tempDir );
								Vector3 resultDir;
								SAFBIKMatMultVec( out resultDir, ref m, ref tempDir );
								Vector3 destArmPos = tempShoulderPos + resultDir * shoulderToArmLength[i];

								SolveShoulderToArmInternal( i, ref destArmPos );
							}
						}
					}

					return updateAnything;
				}