This class provides with operations for Matrix Objects
Dependents: Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more
Diff: MatrixMath.cpp
- Revision:
- 3:48754fe86e08
- Parent:
- 2:d487bb616ec1
- Child:
- 4:d360c068d55f
diff -r d487bb616ec1 -r 48754fe86e08 MatrixMath.cpp --- a/MatrixMath.cpp Sun Oct 30 04:41:00 2011 +0000 +++ b/MatrixMath.cpp Sun Oct 30 04:44:45 2011 +0000 @@ -1,269 +1,269 @@ -/** - * @brief Still under work version 0.2 - * @file MatrixMath.cpp - * @author Erneseto Palacios - * - * Develop Under GPL v3.0 License - * http://www.gnu.org/licenses/gpl-3.0.html - */ - -#include "mbed.h" -#include "MatrixMath.h" - -///Transpose matrix -Matrix MatrixMath::Transpose(const Matrix& Mat) -{ - 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] = 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 " ); - } -} - -Matrix MatrixMath::Eye( int Rows ) -{ - Matrix Identity( Rows, Rows ); //Square Matrix - - for( int i = 0; i < Rows; i++ ) - Identity._matrix[i][i] = 1; - - return Identity; -} - - - -float MatrixMath::dot(const Matrix& leftM, const Matrix& rightM) -{ - if( leftM.isVector() && rightM.isVector() ) - { - if( leftM._nRows == 1 ) - { - if( rightM._nRows == 1 ) - { - if( leftM._nCols == rightM._nCols ) - { - // Calculate ( 1,n )( 1,n ) - float dotP; - Matrix Cross; - - Cross = leftM * MatrixMath::Transpose( rightM ); - dotP = Cross.sum(); - - return dotP; - - }else{ - printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); - } - - }else{ - if( leftM._nCols == rightM._nRows ) - { - // Calculate (1, n)( n, 1 ) - float dotP; - Matrix Cross; - - Cross = leftM * rightM; - dotP = Cross.sum(); - - return dotP; - - }else{ - printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); - } - } - - }else{ - if( rightM._nRows == 1 ) - { - if( leftM._nRows == rightM._nCols ) - { - // Calculate ( n,1 )( 1,n ) - float dotP; - Matrix Cross; - - Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM); - dotP = Cross.sum(); - - return dotP; - - }else{ - printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); - } - - }else{ - if( leftM._nRows == rightM._nRows ) - { - // Calculate (n, 1)( n, 1 ) - float dotP; - Matrix Cross; - - Cross = MatrixMath::Transpose(leftM) * rightM ; - dotP = Cross.sum(); - - return dotP; - - }else{ - printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); - } - } - } - - }else{ - printf( "\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n" ); - } -} - - -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; +/** + * @brief Almost there version 0.9 + * @file MatrixMath.cpp + * @author Erneseto Palacios + * + * Develop Under GPL v3.0 License + * http://www.gnu.org/licenses/gpl-3.0.html + */ + +#include "mbed.h" +#include "MatrixMath.h" + +///Transpose matrix +Matrix MatrixMath::Transpose(const Matrix& Mat) +{ + 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] = 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 " ); + } +} + +Matrix MatrixMath::Eye( int Rows ) +{ + Matrix Identity( Rows, Rows ); //Square Matrix + + for( int i = 0; i < Rows; i++ ) + Identity._matrix[i][i] = 1; + + return Identity; +} + + + +float MatrixMath::dot(const Matrix& leftM, const Matrix& rightM) +{ + if( leftM.isVector() && rightM.isVector() ) + { + if( leftM._nRows == 1 ) + { + if( rightM._nRows == 1 ) + { + if( leftM._nCols == rightM._nCols ) + { + // Calculate ( 1,n )( 1,n ) + float dotP; + Matrix Cross; + + Cross = leftM * MatrixMath::Transpose( rightM ); + dotP = Cross.sum(); + + return dotP; + + }else{ + printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); + } + + }else{ + if( leftM._nCols == rightM._nRows ) + { + // Calculate (1, n)( n, 1 ) + float dotP; + Matrix Cross; + + Cross = leftM * rightM; + dotP = Cross.sum(); + + return dotP; + + }else{ + printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); + } + } + + }else{ + if( rightM._nRows == 1 ) + { + if( leftM._nRows == rightM._nCols ) + { + // Calculate ( n,1 )( 1,n ) + float dotP; + Matrix Cross; + + Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM); + dotP = Cross.sum(); + + return dotP; + + }else{ + printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); + } + + }else{ + if( leftM._nRows == rightM._nRows ) + { + // Calculate (n, 1)( n, 1 ) + float dotP; + Matrix Cross; + + Cross = MatrixMath::Transpose(leftM) * rightM ; + dotP = Cross.sum(); + + return dotP; + + }else{ + printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" ); + } + } + } + + }else{ + printf( "\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n" ); + } +} + + +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