use from Ernesto Palacios
Dependents: RoboticArm DXL_SDK_Porting_Test
Fork of Matrix by
MatrixMath.cpp@9:f19d5d6c3468, 2017-03-31 (annotated)
- Committer:
- stanley1228
- Date:
- Fri Mar 31 09:10:27 2017 +0800
- Revision:
- 9:f19d5d6c3468
- Parent:
- 8:1774aa06ab99
no
Who changed what in which revision?
User | Revision | Line number | New 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 | } |