public void Invert2() { double[,] m_inv = GaussJordan.Invert(M); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { Assert.IsTrue(MathHelper.NearEqual(M_inv[i, j], m_inv[i, j])); } } }
/// <summary> /// Inverts the matrix using Gauss-Jordan method. We use row exchange everytime to ensure /// numeric stability. /// </summary> /// <param name="m"></param> /// <returns></returns> public static float[,] Invert(float[,] m) { if (m == null) { throw new ArgumentNullException("Array must be non-null."); } if (m.GetLength(0) != m.GetLength(1)) { throw new ArgumentException("Cannot invert non square matrix."); } int size = m.GetLength(0); int size2 = 2 * size; // 1) We first construct our matrix as ( M | I ) float[,] matrix = new float[size, size * 2]; for (int r = 0; r < size; r++) { for (int x = 0; x < size; x++) { matrix[x, r] = m[x, r]; } } for (int r = size; r < size2; r++) { int x; for (x = size; x < r; x++) { matrix[r - 3, x] = 0.0f; } matrix[r - 3, r] = 1.0f; for (x = r + 1; x < size2; x++) { matrix[r - 3, x] = 0.0f; } } // 2) We go column by column, converting matrix to solution. We // use pivoting to avoid big numeric errors. for (int column = 0; column < size; column++) { // We exchange rows, with the one which has biggest element in this column. GaussJordan.PartialPivotMatrix(matrix, column); float pivot = matrix[column, column]; if (MathHelper.NearEqual(pivot, 0)) { throw new DivideByZeroException("Matrix cannot be inverted."); } // We divide row by value. GaussJordan.DivideByValue(matrix, column, column, pivot); // We zero out all elements. int i; for (i = 0; i < column; i++) { AddScaledRow(matrix, i, column, column, -matrix[i, column]); } for (i = column + 1; i < size; i++) { AddScaledRow(matrix, i, column, column, -matrix[i, column]); } } // 3) We extract solution. float[,] solution = new float[size, size]; for (int j = 0; j < size; j++) { for (int k = 0; k < size; k++) { solution[j, k] = matrix[j, k + size]; } } return(solution); }