public static bool CheckRotationMatrix(this Matrix3d R)
{
//test
Matrix3d RT = R.Copy();
RT.Transpose();
RT = Matrix3d.Mult(R, RT);
return(RT.CompareMatrices(Matrix3d.Identity, 1e-10f));
//RT should be unit matrix
}
private Matrix3d ExtractMatrixRowN(int N, Matrix3d W)
{
Matrix3d Vnew = new Matrix3d();
Vnew = W.Copy();
for (int i = 0; i < 3; i++)
{
if (i != N)
{
for (int j = 0; j < 3; j++)
{
Vnew[j, i] = 0;
}
}
}
return(Vnew);
}
public static Matrix3d ReplaceRow(this Matrix3d W, int iSource, int iTarget)
{
Matrix3d Vnew = new Matrix3d();
Vnew = W.Copy();
double[] col = new double[3];
for (int k = 0; k < 3; k++)
{
col[k] = Vnew[iTarget, k];
}
for (int k = 0; k < 3; k++)
{
Vnew[iTarget, k] = Vnew[iSource, k];
}
for (int k = 0; k < 3; k++)
{
Vnew[iSource, k] = col[k];
}
return(Vnew);
}
public static Matrix3d MatrixDecompose(this Matrix3d matrix, out int[] perm, out int toggle)
{
// Doolittle LUP decomposition with partial pivoting.
// rerturns: result is L (with 1s on diagonal) and U; perm holds row permutations; toggle is +1 or -1 (even or odd)
int rows = 3; // matrix.Length;
int cols = 3; // matrix[0].Length; // assume all rows have the same number of columns so just use row [0].
if (rows != cols)
{
throw new Exception("Attempt to MatrixDecompose a non-square mattrix");
}
int n = rows; // convenience
Matrix3d result = matrix.Copy(); // make a copy of the input matrix
perm = new int[n]; // set up row permutation result
for (int i = 0; i < n; ++i)
{
perm[i] = i;
}
toggle = 1; // toggle tracks row swaps. +1 -> even, -1 -> odd. used by MatrixDeterminant
for (int j = 0; j < n - 1; ++j) // each column
{
double colMax = Math.Abs(result[j, j]); // find largest value in col j
int pRow = j;
for (int i = j + 1; i < n; ++i)
{
if (result[i, j] > colMax)
{
colMax = result[i, j];
pRow = i;
}
}
if (pRow != j) // if largest value not on pivot, swap rows
{
Vector3d rowPtr = result.Row(pRow);
result.RowSet(pRow, result.Row(j));
result.RowSet(j, rowPtr);
int tmp = perm[pRow]; // and swap perm info
perm[pRow] = perm[j];
perm[j] = tmp;
toggle = -toggle; // adjust the row-swap toggle
}
if (Math.Abs(result[j, j]) < 1.0E-20) // if diagonal after swap is zero . . .
{
throw new Exception("Matric Decomposition error"); // consider a throw
}
for (int i = j + 1; i < n; ++i)
{
result[i, j] /= result[j, j];
for (int k = j + 1; k < n; ++k)
{
result[i, k] -= result[i, j] * result[j, k];
}
}
} // main j column loop
return(result);
} // MatrixDecompose
