Ernesto Palacios / MatrixMath

This class provides with operations for Matrix Objects

Dependents:   Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more

MatrixMath.cpp@1:c74cdf14aea2, 2011-10-22 (annotated)

Committer:
Yo_Robot
Date:
Sat Oct 22 23:18:55 2011 +0000
Revision:
1:c74cdf14aea2
Parent:
0:eb69bbfb6486
Child:
2:d487bb616ec1
version 0.8 Inv() det()  defined

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yo_Robot 0:eb69bbfb6486 1 /**
Yo_Robot 1:c74cdf14aea2 2 * @brief Still under work version 0.2
Yo_Robot 0:eb69bbfb6486 3 * @file MatrixMath.cpp
Yo_Robot 0:eb69bbfb6486 4 * @author Erneseto Palacios
Yo_Robot 0:eb69bbfb6486 5 *
Yo_Robot 0:eb69bbfb6486 6 * Develop Under GPL v3.0 License
Yo_Robot 0:eb69bbfb6486 7 * http://www.gnu.org/licenses/gpl-3.0.html
Yo_Robot 0:eb69bbfb6486 8 */
Yo_Robot 0:eb69bbfb6486 9
Yo_Robot 0:eb69bbfb6486 10 #include "mbed.h"
Yo_Robot 0:eb69bbfb6486 11 #include "MatrixMath.h"
Yo_Robot 0:eb69bbfb6486 12
Yo_Robot 0:eb69bbfb6486 13 ///Transpose matrix
Yo_Robot 1:c74cdf14aea2 14 Matrix MatrixMath::Transpose(const Matrix& Mat)
Yo_Robot 0:eb69bbfb6486 15 {
Yo_Robot 1:c74cdf14aea2 16 Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
Yo_Robot 0:eb69bbfb6486 17
Yo_Robot 0:eb69bbfb6486 18 for( int i = 0; i < result._nRows; i++ )
Yo_Robot 0:eb69bbfb6486 19 for( int j = 0; j < result._nCols; j++ )
Yo_Robot 1:c74cdf14aea2 20 result._matrix[i][j] = Mat._matrix[j][i];
Yo_Robot 0:eb69bbfb6486 21
Yo_Robot 0:eb69bbfb6486 22 return result;
Yo_Robot 0:eb69bbfb6486 23 }
Yo_Robot 0:eb69bbfb6486 24
Yo_Robot 1:c74cdf14aea2 25 Matrix MatrixMath::Inv(const Matrix& Mat)
Yo_Robot 1:c74cdf14aea2 26 {
Yo_Robot 1:c74cdf14aea2 27 if( Mat._nRows == Mat._nCols )
Yo_Robot 1:c74cdf14aea2 28 {
Yo_Robot 1:c74cdf14aea2 29 if( Mat._nRows == 2 ) // 2x2 Matrices
Yo_Robot 1:c74cdf14aea2 30 {
Yo_Robot 1:c74cdf14aea2 31 float det = MatrixMath::det( Mat );
Yo_Robot 1:c74cdf14aea2 32 if( det != 0 )
Yo_Robot 1:c74cdf14aea2 33 {
Yo_Robot 1:c74cdf14aea2 34 Matrix Inv(2,2);
Yo_Robot 1:c74cdf14aea2 35 Inv._matrix[0][0] = Mat._matrix[1][1];
Yo_Robot 1:c74cdf14aea2 36 Inv._matrix[1][0] = -Mat._matrix[1][0];
Yo_Robot 1:c74cdf14aea2 37 Inv._matrix[0][1] = -Mat._matrix[0][1];
Yo_Robot 1:c74cdf14aea2 38 Inv._matrix[1][1] = Mat._matrix[0][0] ;
Yo_Robot 1:c74cdf14aea2 39
Yo_Robot 1:c74cdf14aea2 40 Inv *= 1/det;
Yo_Robot 1:c74cdf14aea2 41
Yo_Robot 1:c74cdf14aea2 42 return Inv;
Yo_Robot 1:c74cdf14aea2 43
Yo_Robot 1:c74cdf14aea2 44 }else{
Yo_Robot 1:c74cdf14aea2 45 printf( "\n\nWANRING: same matrix returned");
Yo_Robot 1:c74cdf14aea2 46 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
Yo_Robot 1:c74cdf14aea2 47 // Mat.print();
Yo_Robot 1:c74cdf14aea2 48 printf( "\n _____________\n" );
Yo_Robot 1:c74cdf14aea2 49
Yo_Robot 1:c74cdf14aea2 50 return Mat;
Yo_Robot 1:c74cdf14aea2 51 }
Yo_Robot 1:c74cdf14aea2 52
Yo_Robot 1:c74cdf14aea2 53 }else{ // nxn Matrices
Yo_Robot 1:c74cdf14aea2 54
Yo_Robot 1:c74cdf14aea2 55 float det = MatrixMath::det( Mat );
Yo_Robot 1:c74cdf14aea2 56 if( det!= 0 )
Yo_Robot 1:c74cdf14aea2 57 {
Yo_Robot 1:c74cdf14aea2 58 Matrix Inv( Mat ); //
Yo_Robot 1:c74cdf14aea2 59 Matrix SubMat;
Yo_Robot 1:c74cdf14aea2 60
Yo_Robot 1:c74cdf14aea2 61 // Matrix of Co-factors
Yo_Robot 1:c74cdf14aea2 62 for( int i = 0; i < Mat._nRows; i++ )
Yo_Robot 1:c74cdf14aea2 63 for( int j = 0; j < Mat._nCols; j++ )
Yo_Robot 1:c74cdf14aea2 64 {
Yo_Robot 1:c74cdf14aea2 65 SubMat = Mat ;
Yo_Robot 1:c74cdf14aea2 66
Yo_Robot 1:c74cdf14aea2 67 Matrix::DeleteRow( SubMat, i+1 );
Yo_Robot 1:c74cdf14aea2 68 Matrix::DeleteCol( SubMat, j+1 );
Yo_Robot 1:c74cdf14aea2 69
Yo_Robot 1:c74cdf14aea2 70 if( (i+j)%2 == 0 )
Yo_Robot 1:c74cdf14aea2 71 Inv._matrix[i][j] = MatrixMath::det( SubMat );
Yo_Robot 1:c74cdf14aea2 72 else
Yo_Robot 1:c74cdf14aea2 73 Inv._matrix[i][j] = -MatrixMath::det( SubMat );
Yo_Robot 1:c74cdf14aea2 74 }
Yo_Robot 1:c74cdf14aea2 75
Yo_Robot 1:c74cdf14aea2 76 // Adjugate Matrix
Yo_Robot 1:c74cdf14aea2 77 Inv = MatrixMath::Transpose( Inv );
Yo_Robot 1:c74cdf14aea2 78
Yo_Robot 1:c74cdf14aea2 79 // Inverse Matrix
Yo_Robot 1:c74cdf14aea2 80 Inv = 1/det * Inv;
Yo_Robot 1:c74cdf14aea2 81
Yo_Robot 1:c74cdf14aea2 82 return Inv;
Yo_Robot 1:c74cdf14aea2 83
Yo_Robot 1:c74cdf14aea2 84 }else{
Yo_Robot 1:c74cdf14aea2 85 printf( "\n\nWANRING: same matrix returned");
Yo_Robot 1:c74cdf14aea2 86 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
Yo_Robot 1:c74cdf14aea2 87 // Mat.print();
Yo_Robot 1:c74cdf14aea2 88 printf( "\n _____________\n" );
Yo_Robot 1:c74cdf14aea2 89
Yo_Robot 1:c74cdf14aea2 90 return Mat;
Yo_Robot 1:c74cdf14aea2 91 }
Yo_Robot 1:c74cdf14aea2 92
Yo_Robot 1:c74cdf14aea2 93 }
Yo_Robot 1:c74cdf14aea2 94
Yo_Robot 1:c74cdf14aea2 95 }else{
Yo_Robot 1:c74cdf14aea2 96 printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " );
Yo_Robot 1:c74cdf14aea2 97 }
Yo_Robot 1:c74cdf14aea2 98 }
Yo_Robot 1:c74cdf14aea2 99
Yo_Robot 1:c74cdf14aea2 100 float MatrixMath::det(const Matrix& Mat)
Yo_Robot 1:c74cdf14aea2 101 {
Yo_Robot 1:c74cdf14aea2 102 if( Mat._nRows == Mat._nCols )
Yo_Robot 1:c74cdf14aea2 103 {
Yo_Robot 1:c74cdf14aea2 104
Yo_Robot 1:c74cdf14aea2 105 if( Mat._nRows == 2 ) // 2x2 Matrix
Yo_Robot 1:c74cdf14aea2 106 {
Yo_Robot 1:c74cdf14aea2 107 float det;
Yo_Robot 1:c74cdf14aea2 108 det = Mat._matrix[0][0] * Mat._matrix[1][1] -
Yo_Robot 1:c74cdf14aea2 109 Mat._matrix[1][0] * Mat._matrix[0][1];
Yo_Robot 1:c74cdf14aea2 110 return det;
Yo_Robot 1:c74cdf14aea2 111 }
Yo_Robot 1:c74cdf14aea2 112 else if( Mat._nRows == 3 ) // 3x3 Matrix
Yo_Robot 1:c74cdf14aea2 113 {
Yo_Robot 1:c74cdf14aea2 114 float det;
Yo_Robot 1:c74cdf14aea2 115 MatrixMath dummy;
Yo_Robot 1:c74cdf14aea2 116
Yo_Robot 1:c74cdf14aea2 117 det = dummy.Det3x3( Mat );
Yo_Robot 1:c74cdf14aea2 118 return det;
Yo_Robot 1:c74cdf14aea2 119
Yo_Robot 1:c74cdf14aea2 120 } else {
Yo_Robot 1:c74cdf14aea2 121
Yo_Robot 1:c74cdf14aea2 122 float part1= 0;
Yo_Robot 1:c74cdf14aea2 123 float part2= 0;
Yo_Robot 1:c74cdf14aea2 124
Yo_Robot 1:c74cdf14aea2 125 //Find +/- on First Row
Yo_Robot 1:c74cdf14aea2 126 for( int i = 0; i < Mat._nCols; i++)
Yo_Robot 1:c74cdf14aea2 127 {
Yo_Robot 1:c74cdf14aea2 128 Matrix reduced( Mat ); // Copy Original Matrix
Yo_Robot 1:c74cdf14aea2 129 Matrix::DeleteRow( reduced, 1); // Delete First Row
Yo_Robot 1:c74cdf14aea2 130
Yo_Robot 1:c74cdf14aea2 131 if( i%2 == 0 ) //Odd Rows
Yo_Robot 1:c74cdf14aea2 132 {
Yo_Robot 1:c74cdf14aea2 133
Yo_Robot 1:c74cdf14aea2 134 Matrix::DeleteCol( reduced, i+1);
Yo_Robot 1:c74cdf14aea2 135 part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
Yo_Robot 1:c74cdf14aea2 136 }
Yo_Robot 1:c74cdf14aea2 137 else // Even Rows
Yo_Robot 1:c74cdf14aea2 138 {
Yo_Robot 1:c74cdf14aea2 139 Matrix::DeleteCol( reduced, i+1);
Yo_Robot 1:c74cdf14aea2 140 part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
Yo_Robot 1:c74cdf14aea2 141 }
Yo_Robot 1:c74cdf14aea2 142 }
Yo_Robot 1:c74cdf14aea2 143 return part1 - part2; //
Yo_Robot 1:c74cdf14aea2 144 }
Yo_Robot 1:c74cdf14aea2 145
Yo_Robot 1:c74cdf14aea2 146 }else{
Yo_Robot 1:c74cdf14aea2 147 printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det");
Yo_Robot 1:c74cdf14aea2 148 }
Yo_Robot 1:c74cdf14aea2 149 }
Yo_Robot 1:c74cdf14aea2 150
Yo_Robot 1:c74cdf14aea2 151
Yo_Robot 1:c74cdf14aea2 152 /************************************/
Yo_Robot 1:c74cdf14aea2 153
Yo_Robot 1:c74cdf14aea2 154 //Private Functions
Yo_Robot 1:c74cdf14aea2 155
Yo_Robot 1:c74cdf14aea2 156 /**@brief
Yo_Robot 1:c74cdf14aea2 157 * Expands the Matrix adding first and second column to the Matrix then
Yo_Robot 1:c74cdf14aea2 158 * performs the Algorithm.
Yo_Robot 1:c74cdf14aea2 159 * @param Mat
Yo_Robot 1:c74cdf14aea2 160 * @return Determinant
Yo_Robot 1:c74cdf14aea2 161 */
Yo_Robot 1:c74cdf14aea2 162 float MatrixMath::Det3x3(const Matrix& Mat)
Yo_Robot 1:c74cdf14aea2 163 {
Yo_Robot 1:c74cdf14aea2 164 Matrix D( Mat ); //Copy Initial matrix
Yo_Robot 1:c74cdf14aea2 165
Yo_Robot 1:c74cdf14aea2 166 Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
Yo_Robot 1:c74cdf14aea2 167 Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
Yo_Robot 1:c74cdf14aea2 168
Yo_Robot 1:c74cdf14aea2 169 float det = 0;
Yo_Robot 1:c74cdf14aea2 170 for( int i = 0; i < 3; i++ )
Yo_Robot 1:c74cdf14aea2 171 det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i]
Yo_Robot 1:c74cdf14aea2 172 - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i];
Yo_Robot 1:c74cdf14aea2 173
Yo_Robot 1:c74cdf14aea2 174 return det;
Yo_Robot 1:c74cdf14aea2 175 }