MatrixMath.cpp

00001 /**
00002  * @brief  Still under work  version 0.2
00003  * @file   MatrixMath.cpp
00004  * @author Erneseto Palacios
00005  *
00006  *  Develop Under  GPL v3.0 License
00007  * http://www.gnu.org/licenses/gpl-3.0.html
00008  */
00009 
00010 #include "mbed.h"
00011 #include "MatrixMath.h "
00012 
00013 ///Transpose matrix
00014 Matrix MatrixMath::Transpose(const Matrix& Mat)
00015 {
00016     Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
00017 
00018     for( int i = 0; i < result._nRows; i++ )
00019         for( int j = 0; j < result._nCols; j++ )
00020             result._matrix[i][j] = Mat._matrix[j][i];
00021 
00022     return result;
00023 }
00024 
00025 Matrix MatrixMath::Inv(const Matrix& Mat)
00026 {
00027     if( Mat._nRows == Mat._nCols )
00028     {
00029         if( Mat._nRows == 2 )   // 2x2 Matrices
00030         {
00031             float det = MatrixMath::det( Mat );
00032             if( det != 0 )
00033             {
00034                 Matrix Inv(2,2);
00035                 Inv._matrix[0][0] =  Mat._matrix[1][1];
00036                 Inv._matrix[1][0] = -Mat._matrix[1][0];
00037                 Inv._matrix[0][1] = -Mat._matrix[0][1];
00038                 Inv._matrix[1][1] =  Mat._matrix[0][0] ;
00039 
00040                 Inv *= 1/det;
00041 
00042                 return Inv;
00043 
00044             }else{
00045                 printf( "\n\nWANRING: same matrix returned");
00046                 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
00047 //                Mat.print();
00048                 printf( "\n  _____________\n" );
00049 
00050                 return Mat;
00051             }
00052 
00053         }else{   // nxn Matrices
00054 
00055             float det = MatrixMath::det( Mat );
00056             if( det!= 0 )
00057             {
00058                 Matrix Inv( Mat ); //
00059                 Matrix SubMat;
00060 
00061                 // Matrix of Co-factors
00062                 for( int i = 0; i < Mat._nRows; i++ )
00063                     for( int j = 0; j < Mat._nCols; j++ )
00064                     {
00065                         SubMat = Mat ;
00066 
00067                         Matrix::DeleteRow( SubMat, i+1 );
00068                         Matrix::DeleteCol( SubMat, j+1 );
00069 
00070                         if( (i+j)%2 == 0 )
00071                             Inv._matrix[i][j] = MatrixMath::det( SubMat );
00072                         else
00073                             Inv._matrix[i][j] = -MatrixMath::det( SubMat );
00074                     }
00075 
00076                 // Adjugate Matrix
00077                 Inv = MatrixMath::Transpose( Inv );
00078 
00079                 // Inverse Matrix
00080                 Inv = 1/det * Inv;
00081 
00082                 return Inv;
00083 
00084             }else{
00085                 printf( "\n\nWANRING: same matrix returned");
00086                 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
00087           //      Mat.print();
00088                 printf( "\n  _____________\n" );
00089 
00090                 return Mat;
00091             }
00092 
00093         }
00094 
00095     }else{
00096         printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " );
00097     }
00098 }
00099 
00100 float MatrixMath::det(const Matrix& Mat)
00101 {
00102     if( Mat._nRows == Mat._nCols  )
00103     {
00104 
00105         if( Mat._nRows == 2 )  // 2x2 Matrix
00106         {
00107             float det;
00108             det = Mat._matrix[0][0] * Mat._matrix[1][1] -
00109                     Mat._matrix[1][0] * Mat._matrix[0][1];
00110             return det;
00111         }
00112         else if( Mat._nRows == 3 ) // 3x3 Matrix
00113         {
00114             float det;
00115             MatrixMath dummy;
00116 
00117             det = dummy.Det3x3( Mat );
00118             return det;
00119 
00120         } else {
00121 
00122             float part1= 0;
00123             float part2= 0;
00124 
00125             //Find +/- on First Row
00126             for( int i = 0; i < Mat._nCols; i++)
00127             {
00128                 Matrix reduced( Mat );           // Copy Original Matrix
00129                 Matrix::DeleteRow( reduced, 1); // Delete First Row
00130 
00131                 if( i%2 == 0 ) //Odd Rows
00132                 {
00133 
00134                     Matrix::DeleteCol( reduced, i+1);
00135                     part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
00136                 }
00137                 else  // Even Rows
00138                 {
00139                     Matrix::DeleteCol( reduced, i+1);
00140                     part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
00141                 }
00142             }
00143             return part1 - part2; //
00144         }
00145 
00146     }else{
00147         printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det");
00148     }
00149 }
00150 
00151 
00152 /************************************/
00153 
00154 //Private Functions
00155 
00156 /**@brief
00157  * Expands the Matrix adding first and second column to the Matrix then
00158  * performs the Algorithm.
00159  * @param Mat
00160  * @return Determinant
00161  */
00162 float MatrixMath::Det3x3(const Matrix& Mat)
00163 {
00164     Matrix D( Mat );  //Copy Initial matrix
00165 
00166     Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
00167     Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
00168 
00169     float det = 0;
00170     for( int i = 0; i < 3; i++ )
00171         det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i]
00172                 - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i];
00173 
00174     return det;
00175 }