Esempio n. 1
0
        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
        }
Esempio n. 2
0
        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);
        }
Esempio n. 3
0
        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);
        }
Esempio n. 4
0
        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