Ernesto Palacios / MatrixMath

This class provides with operations for Matrix Objects

Dependents:   Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more

MatrixMath.cpp@2:d487bb616ec1, 2011-10-30 (annotated)

Committer:
Yo_Robot
Date:
Sun Oct 30 04:41:00 2011 +0000
Revision:
2:d487bb616ec1
Parent:
1:c74cdf14aea2
Child:
3:48754fe86e08
Kinematic and Dot functions version 0.9

Who changed what in which revision?

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