This class provides with operations for Matrix Objects
Dependents: Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more
Diff: MatrixMath.cpp
- Revision:
- 1:c74cdf14aea2
- Parent:
- 0:eb69bbfb6486
- Child:
- 2:d487bb616ec1
--- a/MatrixMath.cpp Fri Oct 21 03:28:28 2011 +0000 +++ b/MatrixMath.cpp Sat Oct 22 23:18:55 2011 +0000 @@ -1,5 +1,5 @@ /** - * @brief Still under work version 0.1 + * @brief Still under work version 0.2 * @file MatrixMath.cpp * @author Erneseto Palacios * @@ -11,14 +11,165 @@ #include "MatrixMath.h" ///Transpose matrix -Matrix MatrixMath::Transpose(const Matrix& matrix) +Matrix MatrixMath::Transpose(const Matrix& Mat) { - Matrix result( matrix._nCols, matrix._nRows ); //Transpose Matrix + Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix for( int i = 0; i < result._nRows; i++ ) for( int j = 0; j < result._nCols; j++ ) - result._matrix[i][j] = matrix._matrix[j][i]; + result._matrix[i][j] = Mat._matrix[j][i]; return result; } +Matrix MatrixMath::Inv(const Matrix& Mat) +{ + if( Mat._nRows == Mat._nCols ) + { + if( Mat._nRows == 2 ) // 2x2 Matrices + { + float det = MatrixMath::det( Mat ); + if( det != 0 ) + { + Matrix Inv(2,2); + Inv._matrix[0][0] = Mat._matrix[1][1]; + Inv._matrix[1][0] = -Mat._matrix[1][0]; + Inv._matrix[0][1] = -Mat._matrix[0][1]; + Inv._matrix[1][1] = Mat._matrix[0][0] ; + + Inv *= 1/det; + + return Inv; + + }else{ + printf( "\n\nWANRING: same matrix returned"); + printf( "\nSingular Matrix, cannot perform Invert @matrix " ); +// Mat.print(); + printf( "\n _____________\n" ); + + return Mat; + } + + }else{ // nxn Matrices + + float det = MatrixMath::det( Mat ); + if( det!= 0 ) + { + Matrix Inv( Mat ); // + Matrix SubMat; + + // Matrix of Co-factors + for( int i = 0; i < Mat._nRows; i++ ) + for( int j = 0; j < Mat._nCols; j++ ) + { + SubMat = Mat ; + + Matrix::DeleteRow( SubMat, i+1 ); + Matrix::DeleteCol( SubMat, j+1 ); + + if( (i+j)%2 == 0 ) + Inv._matrix[i][j] = MatrixMath::det( SubMat ); + else + Inv._matrix[i][j] = -MatrixMath::det( SubMat ); + } + + // Adjugate Matrix + Inv = MatrixMath::Transpose( Inv ); + + // Inverse Matrix + Inv = 1/det * Inv; + + return Inv; + + }else{ + printf( "\n\nWANRING: same matrix returned"); + printf( "\nSingular Matrix, cannot perform Invert @matrix " ); + // Mat.print(); + printf( "\n _____________\n" ); + + return Mat; + } + + } + + }else{ + printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " ); + } +} + +float MatrixMath::det(const Matrix& Mat) +{ + if( Mat._nRows == Mat._nCols ) + { + + if( Mat._nRows == 2 ) // 2x2 Matrix + { + float det; + det = Mat._matrix[0][0] * Mat._matrix[1][1] - + Mat._matrix[1][0] * Mat._matrix[0][1]; + return det; + } + else if( Mat._nRows == 3 ) // 3x3 Matrix + { + float det; + MatrixMath dummy; + + det = dummy.Det3x3( Mat ); + return det; + + } else { + + float part1= 0; + float part2= 0; + + //Find +/- on First Row + for( int i = 0; i < Mat._nCols; i++) + { + Matrix reduced( Mat ); // Copy Original Matrix + Matrix::DeleteRow( reduced, 1); // Delete First Row + + if( i%2 == 0 ) //Odd Rows + { + + Matrix::DeleteCol( reduced, i+1); + part1 += Mat._matrix[0][i] * MatrixMath::det(reduced); + } + else // Even Rows + { + Matrix::DeleteCol( reduced, i+1); + part2 += Mat._matrix[0][i] * MatrixMath::det(reduced); + } + } + return part1 - part2; // + } + + }else{ + printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det"); + } +} + + +/************************************/ + +//Private Functions + +/**@brief + * Expands the Matrix adding first and second column to the Matrix then + * performs the Algorithm. + * @param Mat + * @return Determinant + */ +float MatrixMath::Det3x3(const Matrix& Mat) +{ + Matrix D( Mat ); //Copy Initial matrix + + Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column + Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column + + float det = 0; + for( int i = 0; i < 3; i++ ) + det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i] + - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i]; + + return det; +} \ No newline at end of file