SA.FullBodyIK.BodyIK._ComputeWorldTransform C# (CSharp) Method

			void _ComputeWorldTransform()
			{
				var temp = _solverInternal;
				if( temp == null || temp.spinePos == null || temp.spinePos.Length == 0 ) {
					return;
				}

				// Compute worldPosition / worldRotation.
				if( _hipsBone != null && _hipsBone.transformIsAlive && temp.spinePos != null && temp.spinePos.Length > 0 && _neckBone != null && _neckBone.transformIsAlive ) {
					Vector3 hipsToSpineDirX = new Vector3( 1.0f, 0.0f, 0.0f );

					Vector3 dirX = temp.legs.beginPos[1] - temp.legs.beginPos[0];
					Vector3 dirY = temp.spinePos[0] - (temp.legs.beginPos[1] + temp.legs.beginPos[0]) * 0.5f;

					Matrix3x3 boneBasis = new Matrix3x3();

					if( SAFBIKVecNormalize( ref dirY ) && SAFBIKComputeBasisFromXYLockY( out boneBasis, ref dirX, ref dirY ) ) {
						Matrix3x3 tempBasis;
						SAFBIKMatMult( out tempBasis, ref boneBasis, ref _centerLegBoneBasisInv );

						hipsToSpineDirX = boneBasis.column0; // Counts as baseBasis.

						Quaternion worldRotation;
						SAFBIKMatMultGetRot( out worldRotation, ref tempBasis, ref _hipsBone._defaultBasis );
						_hipsBone.worldRotation = worldRotation;

						if( _hipsBone.isWritebackWorldPosition ) {
							Vector3 worldPosition;
							Vector3 inv_defaultLocalTranslate = -_spineBone._defaultLocalTranslate;
							SAFBIKMatMultVecAdd( out worldPosition, ref tempBasis, ref inv_defaultLocalTranslate, ref temp.spinePos[0] );
							_hipsBone.worldPosition = worldPosition;
						}
					} else { // Failsafe.
						if( SAFBIKVecNormalize( ref dirX ) ) {
							hipsToSpineDirX = dirX;
                        }
					}

					int spineLength = temp.spinePos.Length;
                    for( int i = 0; i != spineLength; ++i ) {
						if( !_spineEnabled[i] ) {
							continue;
						}

						if( i + 1 == spineLength ) {
							dirY = temp.neckPos - temp.spinePos[i];
							if( temp.nearArmPos != null ) {
								dirX = temp.nearArmPos[1] - temp.nearArmPos[0];
							} else { // Failsafe.
								dirX = hipsToSpineDirX;
							}
						} else {
							dirY = temp.spinePos[i + 1] - temp.spinePos[i];
							dirX = hipsToSpineDirX;
							if( temp.nearArmPos != null ) {
								Vector3 dirX0 = temp.nearArmPos[1] - temp.nearArmPos[0];
								if( SAFBIKVecNormalize( ref dirX0 ) ) {
									dirX = Vector3.Lerp( dirX, dirX0, _settings.bodyIK.spineDirXLegToArmRate );
								}
							}
						}

						if( SAFBIKVecNormalize( ref dirY ) && SAFBIKComputeBasisFromXYLockY( out boneBasis, ref dirX, ref dirY ) ) {
							hipsToSpineDirX = boneBasis.column0;
							Quaternion worldRotation;
							SAFBIKMatMultGetRot( out worldRotation, ref boneBasis, ref _spineBones[i]._boneToWorldBasis );
							_spineBones[i].worldRotation = worldRotation;
							if( _spineBones[i].isWritebackWorldPosition ) {
								_spineBones[i].worldPosition = temp.spinePos[i];
							}
						}
					}

					if( _shoulderBones != null ) {
						for( int i = 0; i != 2; ++i ) {
							Vector3 xDir, yDir, zDir;
							xDir = temp.armPos[i] - temp.shoulderPos[i];
							if( _internalValues.shoulderDirYAsNeck != 0 ) {
								yDir = temp.neckPos - temp.shoulderPos[i];
							} else {
								yDir = temp.shoulderPos[i] - temp.spineUPos;
							}
							xDir = (i == 0) ? -xDir : xDir;
							zDir = Vector3.Cross( xDir, yDir );
							yDir = Vector3.Cross( zDir, xDir );

							if( SAFBIKVecNormalize3( ref xDir, ref yDir, ref zDir ) ) {
								boneBasis.SetColumn( ref xDir, ref yDir, ref zDir );
								Quaternion worldRotation;
								SAFBIKMatMultGetRot( out worldRotation, ref boneBasis, ref _shoulderBones[i]._boneToWorldBasis );
								_shoulderBones[i].worldRotation = worldRotation;
							}
						}
					}
				}
			}