Naoto Morita / MatrixMath

add MatrixCross

MatrixMath.cpp@5:93948a9bbde2, 2011-10-30 (annotated)

Committer:
Yo_Robot
Date:
Sun Oct 30 19:21:30 2011 +0000
Revision:
5:93948a9bbde2
Parent:
4:d360c068d55f
Child:
6:aa5e94cddb3f
Version 0.9 Simple Kinematic Operations. View Log.c on Matrix Class for details.

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