Ernesto Palacios / MatrixMath

This class provides with operations for Matrix Objects

Dependents:   Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more

MatrixMath.cpp@4:d360c068d55f, 2011-10-30 (annotated)

Committer:
Yo_Robot
Date:
Sun Oct 30 04:48:15 2011 +0000
Revision:
4:d360c068d55f
Parent:
3:48754fe86e08
Child:
5:93948a9bbde2
Basic Kinematics & Basic Matrix, version 0.9

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yo_Robot 3:48754fe86e08 1 /**
Yo_Robot 4:d360c068d55f 2 * @brief version 0.9
Yo_Robot 3:48754fe86e08 3 * @file MatrixMath.cpp
Yo_Robot 3:48754fe86e08 4 * @author Erneseto Palacios
Yo_Robot 3:48754fe86e08 5 *
Yo_Robot 3:48754fe86e08 6 * Develop Under GPL v3.0 License
Yo_Robot 3:48754fe86e08 7 * http://www.gnu.org/licenses/gpl-3.0.html
Yo_Robot 3:48754fe86e08 8 */
Yo_Robot 3:48754fe86e08 9
Yo_Robot 3:48754fe86e08 10 #include "mbed.h"
Yo_Robot 3:48754fe86e08 11 #include "MatrixMath.h"
Yo_Robot 3:48754fe86e08 12
Yo_Robot 3:48754fe86e08 13 ///Transpose matrix
Yo_Robot 3:48754fe86e08 14 Matrix MatrixMath::Transpose(const Matrix& Mat)
Yo_Robot 3:48754fe86e08 15 {
Yo_Robot 3:48754fe86e08 16 Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
Yo_Robot 3:48754fe86e08 17
Yo_Robot 3:48754fe86e08 18 for( int i = 0; i < result._nRows; i++ )
Yo_Robot 3:48754fe86e08 19 for( int j = 0; j < result._nCols; j++ )
Yo_Robot 3:48754fe86e08 20 result._matrix[i][j] = Mat._matrix[j][i];
Yo_Robot 3:48754fe86e08 21
Yo_Robot 3:48754fe86e08 22 return result;
Yo_Robot 3:48754fe86e08 23 }
Yo_Robot 3:48754fe86e08 24
Yo_Robot 3:48754fe86e08 25 Matrix MatrixMath::Inv(const Matrix& Mat)
Yo_Robot 3:48754fe86e08 26 {
Yo_Robot 3:48754fe86e08 27 if( Mat._nRows == Mat._nCols )
Yo_Robot 3:48754fe86e08 28 {
Yo_Robot 3:48754fe86e08 29 if( Mat._nRows == 2 ) // 2x2 Matrices
Yo_Robot 3:48754fe86e08 30 {
Yo_Robot 3:48754fe86e08 31 float det = MatrixMath::det( Mat );
Yo_Robot 3:48754fe86e08 32 if( det != 0 )
Yo_Robot 3:48754fe86e08 33 {
Yo_Robot 3:48754fe86e08 34 Matrix Inv(2,2);
Yo_Robot 3:48754fe86e08 35 Inv._matrix[0][0] = Mat._matrix[1][1];
Yo_Robot 3:48754fe86e08 36 Inv._matrix[1][0] = -Mat._matrix[1][0];
Yo_Robot 3:48754fe86e08 37 Inv._matrix[0][1] = -Mat._matrix[0][1];
Yo_Robot 3:48754fe86e08 38 Inv._matrix[1][1] = Mat._matrix[0][0] ;
Yo_Robot 3:48754fe86e08 39
Yo_Robot 3:48754fe86e08 40 Inv *= 1/det;
Yo_Robot 3:48754fe86e08 41
Yo_Robot 3:48754fe86e08 42 return Inv;
Yo_Robot 3:48754fe86e08 43
Yo_Robot 3:48754fe86e08 44 }else{
Yo_Robot 3:48754fe86e08 45 printf( "\n\nWANRING: same matrix returned");
Yo_Robot 3:48754fe86e08 46 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
Yo_Robot 3:48754fe86e08 47 // Mat.print();
Yo_Robot 3:48754fe86e08 48 printf( "\n _____________\n" );
Yo_Robot 3:48754fe86e08 49
Yo_Robot 3:48754fe86e08 50 return Mat;
Yo_Robot 3:48754fe86e08 51 }
Yo_Robot 3:48754fe86e08 52
Yo_Robot 3:48754fe86e08 53 }else{ // nxn Matrices
Yo_Robot 3:48754fe86e08 54
Yo_Robot 3:48754fe86e08 55 float det = MatrixMath::det( Mat );
Yo_Robot 3:48754fe86e08 56 if( det!= 0 )
Yo_Robot 3:48754fe86e08 57 {
Yo_Robot 3:48754fe86e08 58 Matrix Inv( Mat ); //
Yo_Robot 3:48754fe86e08 59 Matrix SubMat;
Yo_Robot 3:48754fe86e08 60
Yo_Robot 3:48754fe86e08 61 // Matrix of Co-factors
Yo_Robot 3:48754fe86e08 62 for( int i = 0; i < Mat._nRows; i++ )
Yo_Robot 3:48754fe86e08 63 for( int j = 0; j < Mat._nCols; j++ )
Yo_Robot 3:48754fe86e08 64 {
Yo_Robot 3:48754fe86e08 65 SubMat = Mat ;
Yo_Robot 3:48754fe86e08 66
Yo_Robot 3:48754fe86e08 67 Matrix::DeleteRow( SubMat, i+1 );
Yo_Robot 3:48754fe86e08 68 Matrix::DeleteCol( SubMat, j+1 );
Yo_Robot 3:48754fe86e08 69
Yo_Robot 3:48754fe86e08 70 if( (i+j)%2 == 0 )
Yo_Robot 3:48754fe86e08 71 Inv._matrix[i][j] = MatrixMath::det( SubMat );
Yo_Robot 3:48754fe86e08 72 else
Yo_Robot 3:48754fe86e08 73 Inv._matrix[i][j] = -MatrixMath::det( SubMat );
Yo_Robot 3:48754fe86e08 74 }
Yo_Robot 3:48754fe86e08 75
Yo_Robot 3:48754fe86e08 76 // Adjugate Matrix
Yo_Robot 3:48754fe86e08 77 Inv = MatrixMath::Transpose( Inv );
Yo_Robot 3:48754fe86e08 78
Yo_Robot 3:48754fe86e08 79 // Inverse Matrix
Yo_Robot 3:48754fe86e08 80 Inv = 1/det * Inv;
Yo_Robot 3:48754fe86e08 81
Yo_Robot 3:48754fe86e08 82 return Inv;
Yo_Robot 3:48754fe86e08 83
Yo_Robot 3:48754fe86e08 84 }else{
Yo_Robot 3:48754fe86e08 85 printf( "\n\nWANRING: same matrix returned");
Yo_Robot 3:48754fe86e08 86 printf( "\nSingular Matrix, cannot perform Invert @matrix " );
Yo_Robot 3:48754fe86e08 87 // Mat.print();
Yo_Robot 3:48754fe86e08 88 printf( "\n _____________\n" );
Yo_Robot 3:48754fe86e08 89
Yo_Robot 3:48754fe86e08 90 return Mat;
Yo_Robot 3:48754fe86e08 91 }
Yo_Robot 3:48754fe86e08 92
Yo_Robot 3:48754fe86e08 93 }
Yo_Robot 3:48754fe86e08 94
Yo_Robot 3:48754fe86e08 95 }else{
Yo_Robot 3:48754fe86e08 96 printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " );
Yo_Robot 3:48754fe86e08 97 }
Yo_Robot 3:48754fe86e08 98 }
Yo_Robot 3:48754fe86e08 99
Yo_Robot 3:48754fe86e08 100 Matrix MatrixMath::Eye( int Rows )
Yo_Robot 3:48754fe86e08 101 {
Yo_Robot 3:48754fe86e08 102 Matrix Identity( Rows, Rows ); //Square Matrix
Yo_Robot 3:48754fe86e08 103
Yo_Robot 3:48754fe86e08 104 for( int i = 0; i < Rows; i++ )
Yo_Robot 3:48754fe86e08 105 Identity._matrix[i][i] = 1;
Yo_Robot 3:48754fe86e08 106
Yo_Robot 3:48754fe86e08 107 return Identity;
Yo_Robot 3:48754fe86e08 108 }
Yo_Robot 3:48754fe86e08 109
Yo_Robot 3:48754fe86e08 110
Yo_Robot 3:48754fe86e08 111
Yo_Robot 3:48754fe86e08 112 float MatrixMath::dot(const Matrix& leftM, const Matrix& rightM)
Yo_Robot 3:48754fe86e08 113 {
Yo_Robot 3:48754fe86e08 114 if( leftM.isVector() && rightM.isVector() )
Yo_Robot 3:48754fe86e08 115 {
Yo_Robot 3:48754fe86e08 116 if( leftM._nRows == 1 )
Yo_Robot 3:48754fe86e08 117 {
Yo_Robot 3:48754fe86e08 118 if( rightM._nRows == 1 )
Yo_Robot 3:48754fe86e08 119 {
Yo_Robot 3:48754fe86e08 120 if( leftM._nCols == rightM._nCols )
Yo_Robot 3:48754fe86e08 121 {
Yo_Robot 3:48754fe86e08 122 // Calculate ( 1,n )( 1,n )
Yo_Robot 3:48754fe86e08 123 float dotP;
Yo_Robot 3:48754fe86e08 124 Matrix Cross;
Yo_Robot 3:48754fe86e08 125
Yo_Robot 3:48754fe86e08 126 Cross = leftM * MatrixMath::Transpose( rightM );
Yo_Robot 3:48754fe86e08 127 dotP = Cross.sum();
Yo_Robot 3:48754fe86e08 128
Yo_Robot 3:48754fe86e08 129 return dotP;
Yo_Robot 3:48754fe86e08 130
Yo_Robot 3:48754fe86e08 131 }else{
Yo_Robot 3:48754fe86e08 132 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
Yo_Robot 3:48754fe86e08 133 }
Yo_Robot 3:48754fe86e08 134
Yo_Robot 3:48754fe86e08 135 }else{
Yo_Robot 3:48754fe86e08 136 if( leftM._nCols == rightM._nRows )
Yo_Robot 3:48754fe86e08 137 {
Yo_Robot 3:48754fe86e08 138 // Calculate (1, n)( n, 1 )
Yo_Robot 3:48754fe86e08 139 float dotP;
Yo_Robot 3:48754fe86e08 140 Matrix Cross;
Yo_Robot 3:48754fe86e08 141
Yo_Robot 3:48754fe86e08 142 Cross = leftM * rightM;
Yo_Robot 3:48754fe86e08 143 dotP = Cross.sum();
Yo_Robot 3:48754fe86e08 144
Yo_Robot 3:48754fe86e08 145 return dotP;
Yo_Robot 3:48754fe86e08 146
Yo_Robot 3:48754fe86e08 147 }else{
Yo_Robot 3:48754fe86e08 148 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
Yo_Robot 3:48754fe86e08 149 }
Yo_Robot 3:48754fe86e08 150 }
Yo_Robot 3:48754fe86e08 151
Yo_Robot 3:48754fe86e08 152 }else{
Yo_Robot 3:48754fe86e08 153 if( rightM._nRows == 1 )
Yo_Robot 3:48754fe86e08 154 {
Yo_Robot 3:48754fe86e08 155 if( leftM._nRows == rightM._nCols )
Yo_Robot 3:48754fe86e08 156 {
Yo_Robot 3:48754fe86e08 157 // Calculate ( n,1 )( 1,n )
Yo_Robot 3:48754fe86e08 158 float dotP;
Yo_Robot 3:48754fe86e08 159 Matrix Cross;
Yo_Robot 3:48754fe86e08 160
Yo_Robot 3:48754fe86e08 161 Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM);
Yo_Robot 3:48754fe86e08 162 dotP = Cross.sum();
Yo_Robot 3:48754fe86e08 163
Yo_Robot 3:48754fe86e08 164 return dotP;
Yo_Robot 3:48754fe86e08 165
Yo_Robot 3:48754fe86e08 166 }else{
Yo_Robot 3:48754fe86e08 167 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
Yo_Robot 3:48754fe86e08 168 }
Yo_Robot 3:48754fe86e08 169
Yo_Robot 3:48754fe86e08 170 }else{
Yo_Robot 3:48754fe86e08 171 if( leftM._nRows == rightM._nRows )
Yo_Robot 3:48754fe86e08 172 {
Yo_Robot 3:48754fe86e08 173 // Calculate (n, 1)( n, 1 )
Yo_Robot 3:48754fe86e08 174 float dotP;
Yo_Robot 3:48754fe86e08 175 Matrix Cross;
Yo_Robot 3:48754fe86e08 176
Yo_Robot 3:48754fe86e08 177 Cross = MatrixMath::Transpose(leftM) * rightM ;
Yo_Robot 3:48754fe86e08 178 dotP = Cross.sum();
Yo_Robot 3:48754fe86e08 179
Yo_Robot 3:48754fe86e08 180 return dotP;
Yo_Robot 3:48754fe86e08 181
Yo_Robot 3:48754fe86e08 182 }else{
Yo_Robot 3:48754fe86e08 183 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
Yo_Robot 3:48754fe86e08 184 }
Yo_Robot 3:48754fe86e08 185 }
Yo_Robot 3:48754fe86e08 186 }
Yo_Robot 3:48754fe86e08 187
Yo_Robot 3:48754fe86e08 188 }else{
Yo_Robot 3:48754fe86e08 189 printf( "\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n" );
Yo_Robot 3:48754fe86e08 190 }
Yo_Robot 3:48754fe86e08 191 }
Yo_Robot 3:48754fe86e08 192
Yo_Robot 3:48754fe86e08 193
Yo_Robot 3:48754fe86e08 194 float MatrixMath::det(const Matrix& Mat)
Yo_Robot 3:48754fe86e08 195 {
Yo_Robot 3:48754fe86e08 196 if( Mat._nRows == Mat._nCols )
Yo_Robot 3:48754fe86e08 197 {
Yo_Robot 3:48754fe86e08 198
Yo_Robot 3:48754fe86e08 199 if( Mat._nRows == 2 ) // 2x2 Matrix
Yo_Robot 3:48754fe86e08 200 {
Yo_Robot 3:48754fe86e08 201 float det;
Yo_Robot 3:48754fe86e08 202 det = Mat._matrix[0][0] * Mat._matrix[1][1] -
Yo_Robot 3:48754fe86e08 203 Mat._matrix[1][0] * Mat._matrix[0][1];
Yo_Robot 3:48754fe86e08 204 return det;
Yo_Robot 3:48754fe86e08 205 }
Yo_Robot 3:48754fe86e08 206 else if( Mat._nRows == 3 ) // 3x3 Matrix
Yo_Robot 3:48754fe86e08 207 {
Yo_Robot 3:48754fe86e08 208 float det;
Yo_Robot 3:48754fe86e08 209 MatrixMath dummy;
Yo_Robot 3:48754fe86e08 210
Yo_Robot 3:48754fe86e08 211 det = dummy.Det3x3( Mat );
Yo_Robot 3:48754fe86e08 212 return det;
Yo_Robot 3:48754fe86e08 213
Yo_Robot 3:48754fe86e08 214 } else {
Yo_Robot 3:48754fe86e08 215
Yo_Robot 3:48754fe86e08 216 float part1= 0;
Yo_Robot 3:48754fe86e08 217 float part2= 0;
Yo_Robot 3:48754fe86e08 218
Yo_Robot 3:48754fe86e08 219 //Find +/- on First Row
Yo_Robot 3:48754fe86e08 220 for( int i = 0; i < Mat._nCols; i++)
Yo_Robot 3:48754fe86e08 221 {
Yo_Robot 3:48754fe86e08 222 Matrix reduced( Mat ); // Copy Original Matrix
Yo_Robot 3:48754fe86e08 223 Matrix::DeleteRow( reduced, 1); // Delete First Row
Yo_Robot 3:48754fe86e08 224
Yo_Robot 3:48754fe86e08 225 if( i%2 == 0 ) //Odd Rows
Yo_Robot 3:48754fe86e08 226 {
Yo_Robot 3:48754fe86e08 227
Yo_Robot 3:48754fe86e08 228 Matrix::DeleteCol( reduced, i+1);
Yo_Robot 3:48754fe86e08 229 part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
Yo_Robot 3:48754fe86e08 230 }
Yo_Robot 3:48754fe86e08 231 else // Even Rows
Yo_Robot 3:48754fe86e08 232 {
Yo_Robot 3:48754fe86e08 233 Matrix::DeleteCol( reduced, i+1);
Yo_Robot 3:48754fe86e08 234 part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
Yo_Robot 3:48754fe86e08 235 }
Yo_Robot 3:48754fe86e08 236 }
Yo_Robot 3:48754fe86e08 237 return part1 - part2; //
Yo_Robot 3:48754fe86e08 238 }
Yo_Robot 3:48754fe86e08 239
Yo_Robot 3:48754fe86e08 240 }else{
Yo_Robot 3:48754fe86e08 241 printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det");
Yo_Robot 3:48754fe86e08 242 }
Yo_Robot 3:48754fe86e08 243 }
Yo_Robot 3:48754fe86e08 244
Yo_Robot 3:48754fe86e08 245
Yo_Robot 3:48754fe86e08 246 /************************************/
Yo_Robot 3:48754fe86e08 247
Yo_Robot 3:48754fe86e08 248 //Private Functions
Yo_Robot 3:48754fe86e08 249
Yo_Robot 3:48754fe86e08 250 /**@brief
Yo_Robot 3:48754fe86e08 251 * Expands the Matrix adding first and second column to the Matrix then
Yo_Robot 3:48754fe86e08 252 * performs the Algorithm.
Yo_Robot 3:48754fe86e08 253 * @param Mat
Yo_Robot 3:48754fe86e08 254 * @return Determinant
Yo_Robot 3:48754fe86e08 255 */
Yo_Robot 3:48754fe86e08 256 float MatrixMath::Det3x3(const Matrix& Mat)
Yo_Robot 3:48754fe86e08 257 {
Yo_Robot 3:48754fe86e08 258 Matrix D( Mat ); //Copy Initial matrix
Yo_Robot 3:48754fe86e08 259
Yo_Robot 3:48754fe86e08 260 Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
Yo_Robot 3:48754fe86e08 261 Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
Yo_Robot 3:48754fe86e08 262
Yo_Robot 3:48754fe86e08 263 float det = 0;
Yo_Robot 3:48754fe86e08 264 for( int i = 0; i < 3; i++ )
Yo_Robot 3:48754fe86e08 265 det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i]
Yo_Robot 3:48754fe86e08 266 - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i];
Yo_Robot 3:48754fe86e08 267
Yo_Robot 3:48754fe86e08 268 return det;
Yo_Robot 1:c74cdf14aea2 269 }