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);
}
