hsu han-lin / Matrix

use from Ernesto Palacios

Dependents:   RoboticArm DXL_SDK_Porting_Test

Fork of Matrix by Ernesto Palacios

MatrixMath.cpp@8:1774aa06ab99, 2017-02-11 (annotated)

Committer:
stanley1228
Date:
Sat Feb 11 13:07:13 2017 +0000
Revision:
8:1774aa06ab99
1.fix some temporary return

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 8:1774aa06ab99 1 /**
stanley1228 8:1774aa06ab99 2 * @brief version 0.9
stanley1228 8:1774aa06ab99 3 * @file MatrixMath.cpp
stanley1228 8:1774aa06ab99 4 * @author Ernesto Palacios
stanley1228 8:1774aa06ab99 5 *
stanley1228 8:1774aa06ab99 6 * Created on 15 de septiembre de 2011, 09:44 AM.
stanley1228 8:1774aa06ab99 7 *
stanley1228 8:1774aa06ab99 8 * Develop Under GPL v3.0 License
stanley1228 8:1774aa06ab99 9 * http://www.gnu.org/licenses/gpl-3.0.html
stanley1228 8:1774aa06ab99 10 */
stanley1228 8:1774aa06ab99 11
stanley1228 8:1774aa06ab99 12 #include "mbed.h"
stanley1228 8:1774aa06ab99 13 #include "MatrixMath.h"
stanley1228 8:1774aa06ab99 14
stanley1228 8:1774aa06ab99 15
stanley1228 8:1774aa06ab99 16 #if (DEBUG)
stanley1228 8:1774aa06ab99 17 #define DBGMSG(x) pc.printf x;
stanley1228 8:1774aa06ab99 18 #else
stanley1228 8:1774aa06ab99 19 #define DBGMSG(x)
stanley1228 8:1774aa06ab99 20 #endif
stanley1228 8:1774aa06ab99 21
stanley1228 8:1774aa06ab99 22
stanley1228 8:1774aa06ab99 23
stanley1228 8:1774aa06ab99 24 ///Transpose matrix
stanley1228 8:1774aa06ab99 25 Matrix MatrixMath::Transpose(const Matrix& Mat)
stanley1228 8:1774aa06ab99 26 {
stanley1228 8:1774aa06ab99 27 Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
stanley1228 8:1774aa06ab99 28
stanley1228 8:1774aa06ab99 29 for( int i = 0; i < result._nRows; i++ )
stanley1228 8:1774aa06ab99 30 for( int j = 0; j < result._nCols; j++ )
stanley1228 8:1774aa06ab99 31 result._matrix[i][j] = Mat._matrix[j][i];
stanley1228 8:1774aa06ab99 32
stanley1228 8:1774aa06ab99 33 return result;
stanley1228 8:1774aa06ab99 34 }
stanley1228 8:1774aa06ab99 35
stanley1228 8:1774aa06ab99 36 Matrix MatrixMath::Inv(const Matrix& Mat)
stanley1228 8:1774aa06ab99 37 {
stanley1228 8:1774aa06ab99 38 if( Mat._nRows == Mat._nCols )
stanley1228 8:1774aa06ab99 39 {
stanley1228 8:1774aa06ab99 40 if( Mat._nRows == 2 ) // 2x2 Matrices
stanley1228 8:1774aa06ab99 41 {
stanley1228 8:1774aa06ab99 42 float det = MatrixMath::det( Mat );
stanley1228 8:1774aa06ab99 43 if( det != 0 )
stanley1228 8:1774aa06ab99 44 {
stanley1228 8:1774aa06ab99 45 Matrix Inv(2,2);
stanley1228 8:1774aa06ab99 46 Inv._matrix[0][0] = Mat._matrix[1][1];
stanley1228 8:1774aa06ab99 47 Inv._matrix[1][0] = -Mat._matrix[1][0];
stanley1228 8:1774aa06ab99 48 Inv._matrix[0][1] = -Mat._matrix[0][1];
stanley1228 8:1774aa06ab99 49 Inv._matrix[1][1] = Mat._matrix[0][0] ;
stanley1228 8:1774aa06ab99 50
stanley1228 8:1774aa06ab99 51 Inv *= 1/det;
stanley1228 8:1774aa06ab99 52
stanley1228 8:1774aa06ab99 53 return Inv;
stanley1228 8:1774aa06ab99 54
stanley1228 8:1774aa06ab99 55 }else{
stanley1228 8:1774aa06ab99 56 DBGMSG(("\n\nWANRING: same matrix returned"))
stanley1228 8:1774aa06ab99 57 DBGMSG(("\nSingular Matrix, cannot perform Invert @matrix\n"))
stanley1228 8:1774aa06ab99 58 Mat.print();
stanley1228 8:1774aa06ab99 59 DBGMSG(( "\n _____________\n"))
stanley1228 8:1774aa06ab99 60
stanley1228 8:1774aa06ab99 61 return Mat;
stanley1228 8:1774aa06ab99 62 }
stanley1228 8:1774aa06ab99 63
stanley1228 8:1774aa06ab99 64 }else{ // nxn Matrices
stanley1228 8:1774aa06ab99 65
stanley1228 8:1774aa06ab99 66 float det = MatrixMath::det( Mat );
stanley1228 8:1774aa06ab99 67 if( det!= 0 )
stanley1228 8:1774aa06ab99 68 {
stanley1228 8:1774aa06ab99 69 Matrix Inv( Mat ); //
stanley1228 8:1774aa06ab99 70 Matrix SubMat;
stanley1228 8:1774aa06ab99 71
stanley1228 8:1774aa06ab99 72 // Matrix of Co-factors
stanley1228 8:1774aa06ab99 73 for( int i = 0; i < Mat._nRows; i++ )
stanley1228 8:1774aa06ab99 74 for( int j = 0; j < Mat._nCols; j++ )
stanley1228 8:1774aa06ab99 75 {
stanley1228 8:1774aa06ab99 76 SubMat = Mat ;
stanley1228 8:1774aa06ab99 77
stanley1228 8:1774aa06ab99 78 Matrix::DeleteRow( SubMat, i+1 );
stanley1228 8:1774aa06ab99 79 Matrix::DeleteCol( SubMat, j+1 );
stanley1228 8:1774aa06ab99 80
stanley1228 8:1774aa06ab99 81 if( (i+j)%2 == 0 )
stanley1228 8:1774aa06ab99 82 Inv._matrix[i][j] = MatrixMath::det( SubMat );
stanley1228 8:1774aa06ab99 83 else
stanley1228 8:1774aa06ab99 84 Inv._matrix[i][j] = -MatrixMath::det( SubMat );
stanley1228 8:1774aa06ab99 85 }
stanley1228 8:1774aa06ab99 86
stanley1228 8:1774aa06ab99 87 // Adjugate Matrix
stanley1228 8:1774aa06ab99 88 Inv = MatrixMath::Transpose( Inv );
stanley1228 8:1774aa06ab99 89
stanley1228 8:1774aa06ab99 90 // Inverse Matrix
stanley1228 8:1774aa06ab99 91 Inv = 1/det * Inv;
stanley1228 8:1774aa06ab99 92
stanley1228 8:1774aa06ab99 93 return Inv;
stanley1228 8:1774aa06ab99 94
stanley1228 8:1774aa06ab99 95 }else{
stanley1228 8:1774aa06ab99 96 printf( "\n\nWANRING: same matrix returned");
stanley1228 8:1774aa06ab99 97 printf( "\nSingular Matrix, cannot perform Invert @matrix\n" );
stanley1228 8:1774aa06ab99 98 Mat.print();
stanley1228 8:1774aa06ab99 99 printf( "\n _____________\n" );
stanley1228 8:1774aa06ab99 100
stanley1228 8:1774aa06ab99 101 return Mat;
stanley1228 8:1774aa06ab99 102 }
stanley1228 8:1774aa06ab99 103
stanley1228 8:1774aa06ab99 104 }
stanley1228 8:1774aa06ab99 105
stanley1228 8:1774aa06ab99 106 }else{
stanley1228 8:1774aa06ab99 107 printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant\n" );
stanley1228 8:1774aa06ab99 108 }
stanley1228 8:1774aa06ab99 109
stanley1228 8:1774aa06ab99 110 Matrix NULL_M( 1, 1 );//暫時這樣stanley
stanley1228 8:1774aa06ab99 111 return NULL_M;
stanley1228 8:1774aa06ab99 112 }
stanley1228 8:1774aa06ab99 113
stanley1228 8:1774aa06ab99 114 Matrix MatrixMath::Eye( int Rows )
stanley1228 8:1774aa06ab99 115 {
stanley1228 8:1774aa06ab99 116 Matrix Identity( Rows, Rows ); //Square Matrix
stanley1228 8:1774aa06ab99 117
stanley1228 8:1774aa06ab99 118 for( int i = 0; i < Rows; i++ )
stanley1228 8:1774aa06ab99 119 Identity._matrix[i][i] = 1;
stanley1228 8:1774aa06ab99 120
stanley1228 8:1774aa06ab99 121 return Identity;
stanley1228 8:1774aa06ab99 122 }
stanley1228 8:1774aa06ab99 123
stanley1228 8:1774aa06ab99 124 // Very Versitle Function. Accepts two Vector Matrices of any type:
stanley1228 8:1774aa06ab99 125 // Vector types may be: [n,1] dot [n,1]
stanley1228 8:1774aa06ab99 126 // [n,1] dot [1,n] always same
stanley1228 8:1774aa06ab99 127 // [1,n] dot [n,1] 'depth'
stanley1228 8:1774aa06ab99 128 // [1,n] dot [1,n]
stanley1228 8:1774aa06ab99 129 float MatrixMath::dot(const Matrix& leftM, const Matrix& rightM)
stanley1228 8:1774aa06ab99 130 {
stanley1228 8:1774aa06ab99 131 if( leftM.isVector() && rightM.isVector() )
stanley1228 8:1774aa06ab99 132 {
stanley1228 8:1774aa06ab99 133 if( leftM._nRows == 1 )
stanley1228 8:1774aa06ab99 134 {
stanley1228 8:1774aa06ab99 135 if( rightM._nRows == 1 )
stanley1228 8:1774aa06ab99 136 {
stanley1228 8:1774aa06ab99 137 if( leftM._nCols == rightM._nCols )
stanley1228 8:1774aa06ab99 138 {
stanley1228 8:1774aa06ab99 139 // Calculate ( 1,n )( 1,n )
stanley1228 8:1774aa06ab99 140 float dotP;
stanley1228 8:1774aa06ab99 141 Matrix Cross;
stanley1228 8:1774aa06ab99 142
stanley1228 8:1774aa06ab99 143 Cross = leftM * MatrixMath::Transpose( rightM );
stanley1228 8:1774aa06ab99 144 dotP = Cross.sum();
stanley1228 8:1774aa06ab99 145
stanley1228 8:1774aa06ab99 146 return dotP;
stanley1228 8:1774aa06ab99 147
stanley1228 8:1774aa06ab99 148 }else{
stanley1228 8:1774aa06ab99 149 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
stanley1228 8:1774aa06ab99 150
stanley1228 8:1774aa06ab99 151 }
stanley1228 8:1774aa06ab99 152
stanley1228 8:1774aa06ab99 153 }else{
stanley1228 8:1774aa06ab99 154 if( leftM._nCols == rightM._nRows )
stanley1228 8:1774aa06ab99 155 {
stanley1228 8:1774aa06ab99 156 // Calculate (1, n)( n, 1 )
stanley1228 8:1774aa06ab99 157 float dotP;
stanley1228 8:1774aa06ab99 158 Matrix Cross;
stanley1228 8:1774aa06ab99 159
stanley1228 8:1774aa06ab99 160 Cross = leftM * rightM;
stanley1228 8:1774aa06ab99 161 dotP = Cross.sum();
stanley1228 8:1774aa06ab99 162
stanley1228 8:1774aa06ab99 163 return dotP;
stanley1228 8:1774aa06ab99 164
stanley1228 8:1774aa06ab99 165 }else{
stanley1228 8:1774aa06ab99 166 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
stanley1228 8:1774aa06ab99 167 }
stanley1228 8:1774aa06ab99 168 }
stanley1228 8:1774aa06ab99 169
stanley1228 8:1774aa06ab99 170 }else{
stanley1228 8:1774aa06ab99 171 if( rightM._nRows == 1 )
stanley1228 8:1774aa06ab99 172 {
stanley1228 8:1774aa06ab99 173 if( leftM._nRows == rightM._nCols )
stanley1228 8:1774aa06ab99 174 {
stanley1228 8:1774aa06ab99 175 // Calculate ( n,1 )( 1,n )
stanley1228 8:1774aa06ab99 176 float dotP;
stanley1228 8:1774aa06ab99 177 Matrix Cross;
stanley1228 8:1774aa06ab99 178
stanley1228 8:1774aa06ab99 179 Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM);
stanley1228 8:1774aa06ab99 180 dotP = Cross.sum();
stanley1228 8:1774aa06ab99 181
stanley1228 8:1774aa06ab99 182 return dotP;
stanley1228 8:1774aa06ab99 183
stanley1228 8:1774aa06ab99 184 }else{
stanley1228 8:1774aa06ab99 185 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
stanley1228 8:1774aa06ab99 186 }
stanley1228 8:1774aa06ab99 187
stanley1228 8:1774aa06ab99 188 }else{
stanley1228 8:1774aa06ab99 189 if( leftM._nRows == rightM._nRows )
stanley1228 8:1774aa06ab99 190 {
stanley1228 8:1774aa06ab99 191 // Calculate (n, 1)( n, 1 )
stanley1228 8:1774aa06ab99 192 float dotP;
stanley1228 8:1774aa06ab99 193 Matrix Cross;
stanley1228 8:1774aa06ab99 194
stanley1228 8:1774aa06ab99 195 Cross = MatrixMath::Transpose(leftM) * rightM ;
stanley1228 8:1774aa06ab99 196 dotP = Cross.sum();
stanley1228 8:1774aa06ab99 197
stanley1228 8:1774aa06ab99 198 return dotP;
stanley1228 8:1774aa06ab99 199
stanley1228 8:1774aa06ab99 200 }else{
stanley1228 8:1774aa06ab99 201 printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
stanley1228 8:1774aa06ab99 202 }
stanley1228 8:1774aa06ab99 203 }
stanley1228 8:1774aa06ab99 204 }
stanley1228 8:1774aa06ab99 205
stanley1228 8:1774aa06ab99 206 }else{
stanley1228 8:1774aa06ab99 207 printf( "\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n" );
stanley1228 8:1774aa06ab99 208
stanley1228 8:1774aa06ab99 209 }
stanley1228 8:1774aa06ab99 210
stanley1228 8:1774aa06ab99 211 return -1;//暫時這樣stanley
stanley1228 8:1774aa06ab99 212 }
stanley1228 8:1774aa06ab99 213
stanley1228 8:1774aa06ab99 214
stanley1228 8:1774aa06ab99 215 float MatrixMath::det(const Matrix& Mat)
stanley1228 8:1774aa06ab99 216 {
stanley1228 8:1774aa06ab99 217
stanley1228 8:1774aa06ab99 218 if( Mat._nRows == Mat._nCols )
stanley1228 8:1774aa06ab99 219 {
stanley1228 8:1774aa06ab99 220
stanley1228 8:1774aa06ab99 221 if( Mat._nRows == 2 ) // 2x2 Matrix
stanley1228 8:1774aa06ab99 222 {
stanley1228 8:1774aa06ab99 223 float det;
stanley1228 8:1774aa06ab99 224 det = Mat._matrix[0][0] * Mat._matrix[1][1] -
stanley1228 8:1774aa06ab99 225 Mat._matrix[1][0] * Mat._matrix[0][1];
stanley1228 8:1774aa06ab99 226 return det;
stanley1228 8:1774aa06ab99 227 }
stanley1228 8:1774aa06ab99 228 else if( Mat._nRows == 3 ) // 3x3 Matrix
stanley1228 8:1774aa06ab99 229 {
stanley1228 8:1774aa06ab99 230 float det;
stanley1228 8:1774aa06ab99 231 MatrixMath dummy; //For Private Method.
stanley1228 8:1774aa06ab99 232
stanley1228 8:1774aa06ab99 233 det = dummy.Det3x3( Mat );
stanley1228 8:1774aa06ab99 234 return det;
stanley1228 8:1774aa06ab99 235
stanley1228 8:1774aa06ab99 236 } else {
stanley1228 8:1774aa06ab99 237
stanley1228 8:1774aa06ab99 238 float part1= 0;
stanley1228 8:1774aa06ab99 239 float part2= 0;
stanley1228 8:1774aa06ab99 240
stanley1228 8:1774aa06ab99 241 //Find +/- on First Row
stanley1228 8:1774aa06ab99 242 for( int i = 0; i < Mat._nCols; i++)
stanley1228 8:1774aa06ab99 243 {
stanley1228 8:1774aa06ab99 244 Matrix reduced(Mat); // Copy Original Matrix
stanley1228 8:1774aa06ab99 245 Matrix::DeleteRow( reduced, 1); // Delete First Row
stanley1228 8:1774aa06ab99 246
stanley1228 8:1774aa06ab99 247 if( i%2 == 0 ) //Even Rows
stanley1228 8:1774aa06ab99 248 {
stanley1228 8:1774aa06ab99 249
stanley1228 8:1774aa06ab99 250 Matrix::DeleteCol( reduced, i+1);
stanley1228 8:1774aa06ab99 251 part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
stanley1228 8:1774aa06ab99 252 }
stanley1228 8:1774aa06ab99 253 else // Odd Rows
stanley1228 8:1774aa06ab99 254 {
stanley1228 8:1774aa06ab99 255 Matrix::DeleteCol( reduced, i+1);
stanley1228 8:1774aa06ab99 256 part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
stanley1228 8:1774aa06ab99 257 }
stanley1228 8:1774aa06ab99 258 }
stanley1228 8:1774aa06ab99 259 return part1 - part2;
stanley1228 8:1774aa06ab99 260 }
stanley1228 8:1774aa06ab99 261
stanley1228 8:1774aa06ab99 262 }else{
stanley1228 8:1774aa06ab99 263 printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::det");
stanley1228 8:1774aa06ab99 264
stanley1228 8:1774aa06ab99 265 }
stanley1228 8:1774aa06ab99 266
stanley1228 8:1774aa06ab99 267 return -1;//暫時這樣stanley
stanley1228 8:1774aa06ab99 268 }
stanley1228 8:1774aa06ab99 269
stanley1228 8:1774aa06ab99 270
stanley1228 8:1774aa06ab99 271 //cross stanley
stanley1228 8:1774aa06ab99 272 Matrix MatrixMath::cross(const Matrix& A,const Matrix& B)
stanley1228 8:1774aa06ab99 273 {
stanley1228 8:1774aa06ab99 274 Matrix result(3,1);
stanley1228 8:1774aa06ab99 275
stanley1228 8:1774aa06ab99 276 if( (A._nRows == 3) && (B._nRows == 3))
stanley1228 8:1774aa06ab99 277 {
stanley1228 8:1774aa06ab99 278 if( (A._nCols == 1) && (B._nCols == 1))
stanley1228 8:1774aa06ab99 279 {
stanley1228 8:1774aa06ab99 280
stanley1228 8:1774aa06ab99 281
stanley1228 8:1774aa06ab99 282 result._matrix[0][0] = A._matrix[1][0]*B._matrix[2][0]-A._matrix[2][0]*B._matrix[1][0];//a1b2-a2b1
stanley1228 8:1774aa06ab99 283 result._matrix[1][0] = A._matrix[2][0]*B._matrix[0][0]-A._matrix[0][0]*B._matrix[2][0];//a2b0-a0b2
stanley1228 8:1774aa06ab99 284 result._matrix[2][0] = A._matrix[0][0]*B._matrix[1][0]-A._matrix[1][0]*B._matrix[0][0];//a0b1-a1b0
stanley1228 8:1774aa06ab99 285 }
stanley1228 8:1774aa06ab99 286 }
stanley1228 8:1774aa06ab99 287 else
stanley1228 8:1774aa06ab99 288 {
stanley1228 8:1774aa06ab99 289 printf("\n\nERROR:\n cross product wtih error element");
stanley1228 8:1774aa06ab99 290 }
stanley1228 8:1774aa06ab99 291
stanley1228 8:1774aa06ab99 292 return result;//錯誤該如何避免
stanley1228 8:1774aa06ab99 293 }
stanley1228 8:1774aa06ab99 294 /************************************/
stanley1228 8:1774aa06ab99 295
stanley1228 8:1774aa06ab99 296 //Private Functions
stanley1228 8:1774aa06ab99 297
stanley1228 8:1774aa06ab99 298 /**@brief
stanley1228 8:1774aa06ab99 299 * Expands the Matrix adding first and second column to the Matrix then
stanley1228 8:1774aa06ab99 300 * performs the Algorithm.
stanley1228 8:1774aa06ab99 301 * @param Mat
stanley1228 8:1774aa06ab99 302 * @return Determinant
stanley1228 8:1774aa06ab99 303 */
stanley1228 8:1774aa06ab99 304 float MatrixMath::Det3x3(const Matrix& Mat)
stanley1228 8:1774aa06ab99 305 {
stanley1228 8:1774aa06ab99 306 Matrix D( Mat ); //Copy Initial matrix
stanley1228 8:1774aa06ab99 307
stanley1228 8:1774aa06ab99 308 Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
stanley1228 8:1774aa06ab99 309 Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
stanley1228 8:1774aa06ab99 310
stanley1228 8:1774aa06ab99 311 float det = 0;
stanley1228 8:1774aa06ab99 312 for( int i = 0; i < 3; i++ )
stanley1228 8:1774aa06ab99 313 det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i]
stanley1228 8:1774aa06ab99 314 - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i];
stanley1228 8:1774aa06ab99 315
stanley1228 8:1774aa06ab99 316 return det;
stanley1228 8:1774aa06ab99 317 }