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