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