Axiom.Math.Quaternion.FromRotationMatrix C# (CSharp) Method

		public static Quaternion FromRotationMatrix( Matrix3 matrix )
		{
			// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
			// article "Quaternion Calculus and Fast Animation".

			Quaternion result = Quaternion.Zero;

			Real trace = matrix.m00 + matrix.m11 + matrix.m22;

			Real root = 0.0f;

			if ( trace > 0.0f )
			{
				// |this.w| > 1/2, may as well choose this.w > 1/2
				root = Utility.Sqrt( trace + 1.0f );  // 2w
				result.w = 0.5f * root;

				root = 0.5f / root;  // 1/(4w)

				result.x = ( matrix.m21 - matrix.m12 ) * root;
				result.y = ( matrix.m02 - matrix.m20 ) * root;
				result.z = ( matrix.m10 - matrix.m01 ) * root;
			}
			else
			{
				// |result.w| <= 1/2

				int i = 0;
				if ( matrix.m11 > matrix.m00 )
					i = 1;
				if ( matrix.m22 > matrix[ i, i ] )
					i = 2;

				int j = next[ i ];
				int k = next[ j ];

				root = Utility.Sqrt( matrix[ i, i ] - matrix[ j, j ] - matrix[ k, k ] + 1.0f );

				unsafe
				{
					Real* apkQuat = &result.x;

					apkQuat[ i ] = 0.5f * root;
					root = 0.5f / root;

					result.w = ( matrix[ k, j ] - matrix[ j, k ] ) * root;

					apkQuat[ j ] = ( matrix[ j, i ] + matrix[ i, j ] ) * root;
					apkQuat[ k ] = ( matrix[ k, i ] + matrix[ i, k ] ) * root;
				}
			}

			return result;
		}

        /// <summary>
        ///    Decompose the matrix.
        /// </summary>
        /// <param name="translation"></param>
        /// <param name="scale"></param>
        /// <param name="orientation"></param>
        public void Decompose(out Vector3 translation, out Vector3 scale, out Quaternion orientation)
        {
            scale = Vector3.UnitScale;
            Matrix3 rotation = Matrix3.Identity;
            Vector3 axis     = Vector3.Zero;

            axis.x       = this.m00;
            axis.y       = this.m10;
            axis.z       = this.m20;
            scale.x      = axis.Normalize();        // Normalize() returns the vector's length before it was normalized
            rotation.m00 = axis.x;
            rotation.m10 = axis.y;
            rotation.m20 = axis.z;

            axis.x       = this.m01;
            axis.y       = this.m11;
            axis.z       = this.m21;
            scale.y      = axis.Normalize();
            rotation.m01 = axis.x;
            rotation.m11 = axis.y;
            rotation.m21 = axis.z;

            axis.x       = this.m02;
            axis.y       = this.m12;
            axis.z       = this.m22;
            scale.z      = axis.Normalize();
            rotation.m02 = axis.x;
            rotation.m12 = axis.y;
            rotation.m22 = axis.z;

            orientation = Quaternion.FromRotationMatrix(rotation);
            translation = this.Translation;
        }