use from Ernesto Palacios
Dependents: RoboticArm DXL_SDK_Porting_Test
Fork of Matrix by
MatrixMath.cpp
- Committer:
- stanley1228
- Date:
- 2017-03-31
- Revision:
- 9:f19d5d6c3468
- Parent:
- 8:1774aa06ab99
File content as of revision 9:f19d5d6c3468:
/** * @brief version 0.9 * @file MatrixMath.cpp * @author Ernesto Palacios * * Created on 15 de septiembre de 2011, 09:44 AM. * * Develop Under GPL v3.0 License * http://www.gnu.org/licenses/gpl-3.0.html */ #include "mbed.h" #include "MatrixMath.h" #if (DEBUG) #define DBGMSG(x) pc.printf x; #else #define DBGMSG(x) #endif ///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{ DBGMSG(("\n\nWANRING: same matrix returned")) DBGMSG(("\nSingular Matrix, cannot perform Invert @matrix\n")) Mat.print(); DBGMSG(( "\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\n" ); Mat.print(); printf( "\n _____________\n" ); return Mat; } } }else{ printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant\n" ); } Matrix NULL_M( 1, 1 );//暫時這樣stanley return NULL_M; } 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; } // Very Versitle Function. Accepts two Vector Matrices of any type: // Vector types may be: [n,1] dot [n,1] // [n,1] dot [1,n] always same // [1,n] dot [n,1] 'depth' // [1,n] dot [1,n] 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" ); } return -1;//暫時這樣stanley } 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; //For Private Method. 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 ) //Even Rows { Matrix::DeleteCol( reduced, i+1); part1 += Mat._matrix[0][i] * MatrixMath::det(reduced); } else // Odd 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"); } return -1;//暫時這樣stanley } //cross stanley Matrix MatrixMath::cross(const Matrix& A,const Matrix& B) { Matrix result(3,1); if( (A._nRows == 3) && (B._nRows == 3)) { if( (A._nCols == 1) && (B._nCols == 1)) { result._matrix[0][0] = A._matrix[1][0]*B._matrix[2][0]-A._matrix[2][0]*B._matrix[1][0];//a1b2-a2b1 result._matrix[1][0] = A._matrix[2][0]*B._matrix[0][0]-A._matrix[0][0]*B._matrix[2][0];//a2b0-a0b2 result._matrix[2][0] = A._matrix[0][0]*B._matrix[1][0]-A._matrix[1][0]*B._matrix[0][0];//a0b1-a1b0 } } else { printf("\n\nERROR:\n cross product wtih error element"); } return result;//錯誤該如何避免 } /************************************/ //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; }