Nu/Nu.Math/Matrix3x3.cs
/// <summary>
/// Calculate the inverse of the given matrix
/// </summary>
/// <param name="mat">The matrix to invert</param>
/// <param name="result">The inverse of the given matrix if it has one, or the input if it is singular</param>
/// <exception cref="InvalidOperationException">Thrown if the Matrix3 is singular.</exception>
Calculate the inverse of the given matrix
Calculate the inverse of the given matrix
public static void Invert(ref Matrix3x3 mat, out Matrix3x3 result)
        {
            int[] colIdx = { 0, 0, 0 };
            int[] rowIdx = { 0, 0, 0 };
            int[] pivotIdx = { -1, -1, -1 };
            float[,] inverse = {{mat.Row0.X, mat.Row0.Y, mat.Row0.Z},
                {mat.Row1.X, mat.Row1.Y, mat.Row1.Z},
                {mat.Row2.X, mat.Row2.Y, mat.Row2.Z}};
            int icol = 0;
            int irow = 0;
            for (int i = 0; i < 3; i++)
            {
                float maxPivot = 0.0f;
                for (int j = 0; j < 3; j++)
                {
                    if (pivotIdx[j] != 0)
                    {
                        for (int k = 0; k < 3; ++k)
                        {
                            if (pivotIdx[k] == -1)
                            {
                                float absVal = System.Math.Abs(inverse[j, k]);
                                if (absVal > maxPivot)
                                {
                                    maxPivot = absVal;
                                    irow = j;
                                    icol = k;
                                }
                            }
                            else if (pivotIdx[k] > 0)
                            {
                                result = mat;
                                return;
                            }
                        }
                    }
                }
                ++(pivotIdx[icol]);
                if (irow != icol)
                {
                    for (int k = 0; k < 3; ++k)
                    {
                        float f = inverse[irow, k];
                        inverse[irow, k] = inverse[icol, k];
                        inverse[icol, k] = f;
                    }
                }
                rowIdx[i] = irow;
                colIdx[i] = icol;
                float pivot = inverse[icol, icol];
                if (pivot == 0.0f)
                {
                    throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
                }
                float oneOverPivot = 1.0f / pivot;
                inverse[icol, icol] = 1.0f;
                for (int k = 0; k < 3; ++k)
                {
                    inverse[icol, k] *= oneOverPivot;
                }
                for (int j = 0; j < 3; ++j)
                {
                    if (icol != j)
                    {
                        float f = inverse[j, icol];
                        inverse[j, icol] = 0.0f;
                        for (int k = 0; k < 3; ++k)
                        {
                            inverse[j, k] -= inverse[icol, k] * f;
                        }
                    }
                }
            }
            for (int j = 2; j >= 0; --j)
            {
                int ir = rowIdx[j];
                int ic = colIdx[j];
                for (int k = 0; k < 3; ++k)
                {
                    float f = inverse[k, ir];
                    inverse[k, ir] = inverse[k, ic];
                    inverse[k, ic] = f;
                }
            }
            result.Row0.X = inverse[0, 0];
            result.Row0.Y = inverse[0, 1];
            result.Row0.Z = inverse[0, 2];
            result.Row1.X = inverse[1, 0];
            result.Row1.Y = inverse[1, 1];
            result.Row1.Z = inverse[1, 2];
            result.Row2.X = inverse[2, 0];
            result.Row2.Y = inverse[2, 1];
            result.Row2.Z = inverse[2, 2];
        }
public static void Invert ( ref Matrix3x3 mat , out Matrix3x3 result ) { int [ ] colIdx = { 0 , 0 , 0 } ; int [ ] rowIdx = { 0 , 0 , 0 } ; int [ ] pivotIdx = { - 1 , - 1 , - 1 } ; float [ , ] inverse = { { mat . Row0 . X , mat . Row0 . Y , mat . Row0 . Z } , { mat . Row1 . X , mat . Row1 . Y , mat . Row1 . Z } , { mat . Row2 . X , mat . Row2 . Y , mat . Row2 . Z } } ; int icol = 0 ; int irow = 0 ; for ( int i = 0 ; i < 3 ; i ++ ) { float maxPivot = 0.0f ; for ( int j = 0 ; j < 3 ; j ++ ) { if ( pivotIdx [ j ] != 0 ) { for ( int k = 0 ; k < 3 ; ++ k ) { if ( pivotIdx [ k ] == - 1 ) { float absVal = System . Math . Abs ( inverse [ j , k ] ) ; if ( absVal > maxPivot ) { maxPivot = absVal ; irow = j ; icol = k ; } } else if ( pivotIdx [ k ] > 0 ) { result = mat ; return ; } } } } ++ ( pivotIdx [ icol ] ) ; if ( irow != icol ) { for ( int k = 0 ; k < 3 ; ++ k ) { float f = inverse [ irow , k ] ; inverse [ irow , k ] = inverse [ icol , k ] ; inverse [ icol , k ] = f ; } } rowIdx [ i ] = irow ; colIdx [ i ] = icol ; float pivot = inverse [ icol , icol ] ; if ( pivot == 0.0f ) { throw new InvalidOperationException ( " " ) ; } float oneOverPivot = 1.0f / pivot ; inverse [ icol , icol ] = 1.0f ; for ( int k = 0 ; k < 3 ; ++ k ) { inverse [ icol , k ] *= oneOverPivot ; } for ( int j = 0 ; j < 3 ; ++ j ) { if ( icol != j ) { float f = inverse [ j , icol ] ; inverse [ j , icol ] = 0.0f ; for ( int k = 0 ; k < 3 ; ++ k ) { inverse [ j , k ] -= inverse [ icol , k ] * f ; } } } } for ( int j = 2 ; j >= 0 ; -- j ) { int ir = rowIdx [ j ] ; int ic = colIdx [ j ] ; for ( int k = 0 ; k < 3 ; ++ k ) { float f = inverse [ k , ir ] ; inverse [ k , ir ] = inverse [ k , ic ] ; inverse [ k , ic ] = f ; } } result . Row0 . X = inverse [ 0 , 0 ] ; result . Row0 . Y = inverse [ 0 , 1 ] ; result . Row0 . Z = inverse [ 0 , 2 ] ; result . Row1 . X = inverse [ 1 , 0 ] ; result . Row1 . Y = inverse [ 1 , 1 ] ; result . Row1 . Z = inverse [ 1 , 2 ] ; result . Row2 . X = inverse [ 2 , 0 ] ; result . Row2 . Y = inverse [ 2 , 1 ] ; result . Row2 . Z = inverse [ 2 , 2 ] ; }
Calculate the inverse of the given matrix
Calculate the inverse of the given matrix
Thrown if the Matrix3 is singular.
Thrown if the Matrix3 is singular .
Matrix3x3
out
mat
The matrix to invert
The matrix to invert
result
The inverse of the given matrix if it has one, or the input if it is singular
The inverse of the given matrix if it has one or the input if it is singular