Matrix class included
Dependencies: Matrix
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?
User | Revision | Line number | New 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 | } |