Matrix class included
Dependencies: Matrix
MatrixMath.cpp@8:7e41f6f1befa, 2021-09-07 (annotated)
- Committer:
- cocorlow
- Date:
- Tue Sep 07 06:35:37 2021 +0000
- Revision:
- 8:7e41f6f1befa
- Parent:
- 7:b22d56ac37aa
GPS/GNSS mounted
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 |
mori3rti | 6:aa5e94cddb3f | 16 | Matrix MatrixMath::Transpose(const Matrix &Mat) |
Yo_Robot | 3:48754fe86e08 | 17 | { |
mori3rti | 6:aa5e94cddb3f | 18 | Matrix result(Mat._nCols, Mat._nRows); //Transpose Matrix |
Yo_Robot | 3:48754fe86e08 | 19 | |
mori3rti | 6:aa5e94cddb3f | 20 | for (int i = 0; i < result._nRows; i++) |
mori3rti | 6:aa5e94cddb3f | 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 | |
NaotoMorita | 7:b22d56ac37aa | 27 | ///Transpose matrix |
NaotoMorita | 7:b22d56ac37aa | 28 | Matrix MatrixMath::Matrixcross(const float px, const float py, const float pz) |
NaotoMorita | 7:b22d56ac37aa | 29 | { |
NaotoMorita | 7:b22d56ac37aa | 30 | Matrix result(3,3); //Transpose Matrix |
NaotoMorita | 7:b22d56ac37aa | 31 | result(1,2) = -pz; |
NaotoMorita | 7:b22d56ac37aa | 32 | result(1,3) = py; |
NaotoMorita | 7:b22d56ac37aa | 33 | result(2,1) = pz; |
NaotoMorita | 7:b22d56ac37aa | 34 | result(2,3) = -px; |
NaotoMorita | 7:b22d56ac37aa | 35 | result(3,1) = -py; |
NaotoMorita | 7:b22d56ac37aa | 36 | result(3,2) = px; |
NaotoMorita | 7:b22d56ac37aa | 37 | return result; |
NaotoMorita | 7:b22d56ac37aa | 38 | } |
NaotoMorita | 7:b22d56ac37aa | 39 | Matrix MatrixMath::Vector2mat(const Vector3 vec) |
NaotoMorita | 7:b22d56ac37aa | 40 | { |
NaotoMorita | 7:b22d56ac37aa | 41 | Matrix result(3,1); //Transpose Matrix |
NaotoMorita | 7:b22d56ac37aa | 42 | result(1,1) = vec.x; |
NaotoMorita | 7:b22d56ac37aa | 43 | result(2,1) = vec.y; |
NaotoMorita | 7:b22d56ac37aa | 44 | result(3,1) = vec.z; |
NaotoMorita | 7:b22d56ac37aa | 45 | return result; |
NaotoMorita | 7:b22d56ac37aa | 46 | } |
mori3rti | 6:aa5e94cddb3f | 47 | Matrix MatrixMath::Inv(const Matrix &Mat) |
Yo_Robot | 3:48754fe86e08 | 48 | { |
mori3rti | 6:aa5e94cddb3f | 49 | if (Mat._nRows == Mat._nCols) |
Yo_Robot | 3:48754fe86e08 | 50 | { |
mori3rti | 6:aa5e94cddb3f | 51 | if (Mat._nRows == 2) // 2x2 Matrices |
Yo_Robot | 3:48754fe86e08 | 52 | { |
mori3rti | 6:aa5e94cddb3f | 53 | float det = MatrixMath::det(Mat); |
mori3rti | 6:aa5e94cddb3f | 54 | if (det != 0) |
Yo_Robot | 3:48754fe86e08 | 55 | { |
mori3rti | 6:aa5e94cddb3f | 56 | Matrix Inv(2, 2); |
mori3rti | 6:aa5e94cddb3f | 57 | Inv._matrix[0][0] = Mat._matrix[1][1]; |
Yo_Robot | 3:48754fe86e08 | 58 | Inv._matrix[1][0] = -Mat._matrix[1][0]; |
Yo_Robot | 3:48754fe86e08 | 59 | Inv._matrix[0][1] = -Mat._matrix[0][1]; |
mori3rti | 6:aa5e94cddb3f | 60 | Inv._matrix[1][1] = Mat._matrix[0][0]; |
Yo_Robot | 3:48754fe86e08 | 61 | |
mori3rti | 6:aa5e94cddb3f | 62 | Inv *= 1 / det; |
Yo_Robot | 3:48754fe86e08 | 63 | |
Yo_Robot | 3:48754fe86e08 | 64 | return Inv; |
mori3rti | 6:aa5e94cddb3f | 65 | } |
mori3rti | 6:aa5e94cddb3f | 66 | else |
mori3rti | 6:aa5e94cddb3f | 67 | { |
mori3rti | 6:aa5e94cddb3f | 68 | printf("\n\nWANRING: same matrix returned"); |
mori3rti | 6:aa5e94cddb3f | 69 | printf("\nSingular Matrix, cannot perform Invert @matrix\n "); |
Yo_Robot | 5:93948a9bbde2 | 70 | Mat.print(); |
mori3rti | 6:aa5e94cddb3f | 71 | printf("\n _____________\n"); |
Yo_Robot | 3:48754fe86e08 | 72 | |
Yo_Robot | 3:48754fe86e08 | 73 | return Mat; |
Yo_Robot | 3:48754fe86e08 | 74 | } |
mori3rti | 6:aa5e94cddb3f | 75 | } |
mori3rti | 6:aa5e94cddb3f | 76 | else |
mori3rti | 6:aa5e94cddb3f | 77 | { // nxn Matrices |
Yo_Robot | 3:48754fe86e08 | 78 | |
mori3rti | 6:aa5e94cddb3f | 79 | float det = MatrixMath::det(Mat); |
mori3rti | 6:aa5e94cddb3f | 80 | if (det != 0) |
Yo_Robot | 3:48754fe86e08 | 81 | { |
mori3rti | 6:aa5e94cddb3f | 82 | Matrix Inv(Mat); // |
Yo_Robot | 3:48754fe86e08 | 83 | Matrix SubMat; |
Yo_Robot | 3:48754fe86e08 | 84 | |
Yo_Robot | 3:48754fe86e08 | 85 | // Matrix of Co-factors |
mori3rti | 6:aa5e94cddb3f | 86 | for (int i = 0; i < Mat._nRows; i++) |
mori3rti | 6:aa5e94cddb3f | 87 | for (int j = 0; j < Mat._nCols; j++) |
Yo_Robot | 3:48754fe86e08 | 88 | { |
mori3rti | 6:aa5e94cddb3f | 89 | SubMat = Mat; |
Yo_Robot | 3:48754fe86e08 | 90 | |
mori3rti | 6:aa5e94cddb3f | 91 | Matrix::DeleteRow(SubMat, i + 1); |
mori3rti | 6:aa5e94cddb3f | 92 | Matrix::DeleteCol(SubMat, j + 1); |
Yo_Robot | 3:48754fe86e08 | 93 | |
mori3rti | 6:aa5e94cddb3f | 94 | if ((i + j) % 2 == 0) |
mori3rti | 6:aa5e94cddb3f | 95 | Inv._matrix[i][j] = MatrixMath::det(SubMat); |
Yo_Robot | 3:48754fe86e08 | 96 | else |
mori3rti | 6:aa5e94cddb3f | 97 | Inv._matrix[i][j] = -MatrixMath::det(SubMat); |
Yo_Robot | 3:48754fe86e08 | 98 | } |
Yo_Robot | 3:48754fe86e08 | 99 | |
Yo_Robot | 3:48754fe86e08 | 100 | // Adjugate Matrix |
mori3rti | 6:aa5e94cddb3f | 101 | Inv = MatrixMath::Transpose(Inv); |
Yo_Robot | 3:48754fe86e08 | 102 | |
Yo_Robot | 3:48754fe86e08 | 103 | // Inverse Matrix |
mori3rti | 6:aa5e94cddb3f | 104 | Inv = 1 / det * Inv; |
Yo_Robot | 3:48754fe86e08 | 105 | |
Yo_Robot | 3:48754fe86e08 | 106 | return Inv; |
mori3rti | 6:aa5e94cddb3f | 107 | } |
mori3rti | 6:aa5e94cddb3f | 108 | else |
mori3rti | 6:aa5e94cddb3f | 109 | { |
mori3rti | 6:aa5e94cddb3f | 110 | printf("\n\nWANRING: same matrix returned"); |
mori3rti | 6:aa5e94cddb3f | 111 | printf("\nSingular Matrix, cannot perform Invert @matrix\n"); |
Yo_Robot | 5:93948a9bbde2 | 112 | Mat.print(); |
mori3rti | 6:aa5e94cddb3f | 113 | printf("\n _____________\n"); |
Yo_Robot | 3:48754fe86e08 | 114 | |
Yo_Robot | 3:48754fe86e08 | 115 | return Mat; |
Yo_Robot | 3:48754fe86e08 | 116 | } |
Yo_Robot | 3:48754fe86e08 | 117 | } |
mori3rti | 6:aa5e94cddb3f | 118 | } |
mori3rti | 6:aa5e94cddb3f | 119 | else |
mori3rti | 6:aa5e94cddb3f | 120 | { |
mori3rti | 6:aa5e94cddb3f | 121 | printf("\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant\n"); |
Yo_Robot | 3:48754fe86e08 | 122 | } |
Yo_Robot | 3:48754fe86e08 | 123 | } |
Yo_Robot | 3:48754fe86e08 | 124 | |
mori3rti | 6:aa5e94cddb3f | 125 | Matrix MatrixMath::Eye(int Rows) |
Yo_Robot | 3:48754fe86e08 | 126 | { |
mori3rti | 6:aa5e94cddb3f | 127 | Matrix Identity(Rows, Rows); //Square Matrix |
Yo_Robot | 3:48754fe86e08 | 128 | |
mori3rti | 6:aa5e94cddb3f | 129 | for (int i = 0; i < Rows; i++) |
Yo_Robot | 3:48754fe86e08 | 130 | Identity._matrix[i][i] = 1; |
Yo_Robot | 3:48754fe86e08 | 131 | |
Yo_Robot | 3:48754fe86e08 | 132 | return Identity; |
Yo_Robot | 3:48754fe86e08 | 133 | } |
Yo_Robot | 3:48754fe86e08 | 134 | |
Yo_Robot | 5:93948a9bbde2 | 135 | // Very Versitle Function. Accepts two Vector Matrices of any type: |
mori3rti | 6:aa5e94cddb3f | 136 | // Vector types may be: [n,1] dot [n,1] |
Yo_Robot | 5:93948a9bbde2 | 137 | // [n,1] dot [1,n] always same |
Yo_Robot | 5:93948a9bbde2 | 138 | // [1,n] dot [n,1] 'depth' |
Yo_Robot | 5:93948a9bbde2 | 139 | // [1,n] dot [1,n] |
mori3rti | 6:aa5e94cddb3f | 140 | float MatrixMath::dot(const Matrix &leftM, const Matrix &rightM) |
Yo_Robot | 3:48754fe86e08 | 141 | { |
mori3rti | 6:aa5e94cddb3f | 142 | if (leftM.isVector() && rightM.isVector()) |
Yo_Robot | 3:48754fe86e08 | 143 | { |
mori3rti | 6:aa5e94cddb3f | 144 | if (leftM._nRows == 1) |
Yo_Robot | 3:48754fe86e08 | 145 | { |
mori3rti | 6:aa5e94cddb3f | 146 | if (rightM._nRows == 1) |
Yo_Robot | 3:48754fe86e08 | 147 | { |
mori3rti | 6:aa5e94cddb3f | 148 | if (leftM._nCols == rightM._nCols) |
Yo_Robot | 3:48754fe86e08 | 149 | { |
Yo_Robot | 3:48754fe86e08 | 150 | // Calculate ( 1,n )( 1,n ) |
Yo_Robot | 3:48754fe86e08 | 151 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 152 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 153 | |
mori3rti | 6:aa5e94cddb3f | 154 | Cross = leftM * MatrixMath::Transpose(rightM); |
Yo_Robot | 3:48754fe86e08 | 155 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 156 | |
Yo_Robot | 3:48754fe86e08 | 157 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 158 | } |
mori3rti | 6:aa5e94cddb3f | 159 | else |
mori3rti | 6:aa5e94cddb3f | 160 | { |
mori3rti | 6:aa5e94cddb3f | 161 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
Yo_Robot | 3:48754fe86e08 | 162 | } |
mori3rti | 6:aa5e94cddb3f | 163 | } |
mori3rti | 6:aa5e94cddb3f | 164 | else |
mori3rti | 6:aa5e94cddb3f | 165 | { |
mori3rti | 6:aa5e94cddb3f | 166 | if (leftM._nCols == rightM._nRows) |
Yo_Robot | 3:48754fe86e08 | 167 | { |
Yo_Robot | 3:48754fe86e08 | 168 | // Calculate (1, n)( n, 1 ) |
Yo_Robot | 3:48754fe86e08 | 169 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 170 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 171 | |
Yo_Robot | 3:48754fe86e08 | 172 | Cross = leftM * rightM; |
Yo_Robot | 3:48754fe86e08 | 173 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 174 | |
Yo_Robot | 3:48754fe86e08 | 175 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 176 | } |
mori3rti | 6:aa5e94cddb3f | 177 | else |
mori3rti | 6:aa5e94cddb3f | 178 | { |
mori3rti | 6:aa5e94cddb3f | 179 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
Yo_Robot | 3:48754fe86e08 | 180 | } |
Yo_Robot | 3:48754fe86e08 | 181 | } |
mori3rti | 6:aa5e94cddb3f | 182 | } |
mori3rti | 6:aa5e94cddb3f | 183 | else |
mori3rti | 6:aa5e94cddb3f | 184 | { |
mori3rti | 6:aa5e94cddb3f | 185 | if (rightM._nRows == 1) |
Yo_Robot | 3:48754fe86e08 | 186 | { |
mori3rti | 6:aa5e94cddb3f | 187 | if (leftM._nRows == rightM._nCols) |
Yo_Robot | 3:48754fe86e08 | 188 | { |
Yo_Robot | 3:48754fe86e08 | 189 | // Calculate ( n,1 )( 1,n ) |
Yo_Robot | 3:48754fe86e08 | 190 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 191 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 192 | |
mori3rti | 6:aa5e94cddb3f | 193 | Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM); |
Yo_Robot | 3:48754fe86e08 | 194 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 195 | |
Yo_Robot | 3:48754fe86e08 | 196 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 197 | } |
mori3rti | 6:aa5e94cddb3f | 198 | else |
mori3rti | 6:aa5e94cddb3f | 199 | { |
mori3rti | 6:aa5e94cddb3f | 200 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
Yo_Robot | 3:48754fe86e08 | 201 | } |
mori3rti | 6:aa5e94cddb3f | 202 | } |
mori3rti | 6:aa5e94cddb3f | 203 | else |
mori3rti | 6:aa5e94cddb3f | 204 | { |
mori3rti | 6:aa5e94cddb3f | 205 | if (leftM._nRows == rightM._nRows) |
Yo_Robot | 3:48754fe86e08 | 206 | { |
Yo_Robot | 3:48754fe86e08 | 207 | // Calculate (n, 1)( n, 1 ) |
Yo_Robot | 3:48754fe86e08 | 208 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 209 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 210 | |
mori3rti | 6:aa5e94cddb3f | 211 | Cross = MatrixMath::Transpose(leftM) * rightM; |
Yo_Robot | 3:48754fe86e08 | 212 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 213 | |
Yo_Robot | 3:48754fe86e08 | 214 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 215 | } |
mori3rti | 6:aa5e94cddb3f | 216 | else |
mori3rti | 6:aa5e94cddb3f | 217 | { |
mori3rti | 6:aa5e94cddb3f | 218 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
Yo_Robot | 3:48754fe86e08 | 219 | } |
Yo_Robot | 3:48754fe86e08 | 220 | } |
Yo_Robot | 3:48754fe86e08 | 221 | } |
mori3rti | 6:aa5e94cddb3f | 222 | } |
mori3rti | 6:aa5e94cddb3f | 223 | else |
mori3rti | 6:aa5e94cddb3f | 224 | { |
mori3rti | 6:aa5e94cddb3f | 225 | printf("\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n"); |
Yo_Robot | 3:48754fe86e08 | 226 | } |
Yo_Robot | 3:48754fe86e08 | 227 | } |
Yo_Robot | 3:48754fe86e08 | 228 | |
mori3rti | 6:aa5e94cddb3f | 229 | float MatrixMath::det(const Matrix &Mat) |
Yo_Robot | 3:48754fe86e08 | 230 | { |
mori3rti | 6:aa5e94cddb3f | 231 | if (Mat._nRows == Mat._nCols) |
Yo_Robot | 3:48754fe86e08 | 232 | { |
Yo_Robot | 3:48754fe86e08 | 233 | |
mori3rti | 6:aa5e94cddb3f | 234 | if (Mat._nRows == 2) // 2x2 Matrix |
Yo_Robot | 3:48754fe86e08 | 235 | { |
Yo_Robot | 3:48754fe86e08 | 236 | float det; |
Yo_Robot | 3:48754fe86e08 | 237 | det = Mat._matrix[0][0] * Mat._matrix[1][1] - |
mori3rti | 6:aa5e94cddb3f | 238 | Mat._matrix[1][0] * Mat._matrix[0][1]; |
Yo_Robot | 3:48754fe86e08 | 239 | return det; |
Yo_Robot | 3:48754fe86e08 | 240 | } |
mori3rti | 6:aa5e94cddb3f | 241 | else if (Mat._nRows == 3) // 3x3 Matrix |
Yo_Robot | 3:48754fe86e08 | 242 | { |
Yo_Robot | 3:48754fe86e08 | 243 | float det; |
Yo_Robot | 5:93948a9bbde2 | 244 | MatrixMath dummy; //For Private Method. |
Yo_Robot | 3:48754fe86e08 | 245 | |
mori3rti | 6:aa5e94cddb3f | 246 | det = dummy.Det3x3(Mat); |
Yo_Robot | 3:48754fe86e08 | 247 | return det; |
mori3rti | 6:aa5e94cddb3f | 248 | } |
mori3rti | 6:aa5e94cddb3f | 249 | else |
mori3rti | 6:aa5e94cddb3f | 250 | { |
Yo_Robot | 3:48754fe86e08 | 251 | |
mori3rti | 6:aa5e94cddb3f | 252 | float part1 = 0; |
mori3rti | 6:aa5e94cddb3f | 253 | float part2 = 0; |
Yo_Robot | 3:48754fe86e08 | 254 | |
Yo_Robot | 3:48754fe86e08 | 255 | //Find +/- on First Row |
mori3rti | 6:aa5e94cddb3f | 256 | for (int i = 0; i < Mat._nCols; i++) |
Yo_Robot | 3:48754fe86e08 | 257 | { |
mori3rti | 6:aa5e94cddb3f | 258 | Matrix reduced(Mat); // Copy Original Matrix |
mori3rti | 6:aa5e94cddb3f | 259 | Matrix::DeleteRow(reduced, 1); // Delete First Row |
Yo_Robot | 3:48754fe86e08 | 260 | |
mori3rti | 6:aa5e94cddb3f | 261 | if (i % 2 == 0) //Even Rows |
Yo_Robot | 3:48754fe86e08 | 262 | { |
Yo_Robot | 3:48754fe86e08 | 263 | |
mori3rti | 6:aa5e94cddb3f | 264 | Matrix::DeleteCol(reduced, i + 1); |
Yo_Robot | 3:48754fe86e08 | 265 | part1 += Mat._matrix[0][i] * MatrixMath::det(reduced); |
Yo_Robot | 3:48754fe86e08 | 266 | } |
mori3rti | 6:aa5e94cddb3f | 267 | else // Odd Rows |
Yo_Robot | 3:48754fe86e08 | 268 | { |
mori3rti | 6:aa5e94cddb3f | 269 | Matrix::DeleteCol(reduced, i + 1); |
Yo_Robot | 3:48754fe86e08 | 270 | part2 += Mat._matrix[0][i] * MatrixMath::det(reduced); |
Yo_Robot | 3:48754fe86e08 | 271 | } |
Yo_Robot | 3:48754fe86e08 | 272 | } |
mori3rti | 6:aa5e94cddb3f | 273 | return part1 - part2; |
Yo_Robot | 3:48754fe86e08 | 274 | } |
mori3rti | 6:aa5e94cddb3f | 275 | } |
mori3rti | 6:aa5e94cddb3f | 276 | else |
mori3rti | 6:aa5e94cddb3f | 277 | { |
Yo_Robot | 5:93948a9bbde2 | 278 | printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::det"); |
Yo_Robot | 3:48754fe86e08 | 279 | } |
Yo_Robot | 3:48754fe86e08 | 280 | } |
Yo_Robot | 3:48754fe86e08 | 281 | |
mori3rti | 6:aa5e94cddb3f | 282 | Matrix MatrixMath::kron(const Matrix &Mat_A, const Matrix &Mat_B) |
mori3rti | 6:aa5e94cddb3f | 283 | { |
mori3rti | 6:aa5e94cddb3f | 284 | Matrix result(Mat_A._nRows * Mat_B._nRows, Mat_A._nCols * Mat_B._nCols); |
mori3rti | 6:aa5e94cddb3f | 285 | for (unsigned int row_a = 0; row_a < Mat_A._nRows; row_a++) |
mori3rti | 6:aa5e94cddb3f | 286 | { |
mori3rti | 6:aa5e94cddb3f | 287 | for (unsigned int col_a = 0; col_a < Mat_A._nCols; col_a++) |
mori3rti | 6:aa5e94cddb3f | 288 | { |
mori3rti | 6:aa5e94cddb3f | 289 | Matrix block = Mat_A._matrix[row_a][col_a]*Mat_B; |
mori3rti | 6:aa5e94cddb3f | 290 | |
mori3rti | 6:aa5e94cddb3f | 291 | for (unsigned int row_b = 0; row_b < Mat_B._nRows; row_b++) |
mori3rti | 6:aa5e94cddb3f | 292 | { |
mori3rti | 6:aa5e94cddb3f | 293 | for (unsigned int col_b = 0; col_b < Mat_B._nCols; col_b++) |
mori3rti | 6:aa5e94cddb3f | 294 | { |
mori3rti | 6:aa5e94cddb3f | 295 | result._matrix[row_b + (row_a * Mat_B._nRows)][col_b + (col_a * Mat_B._nCols)] = block._matrix[row_b][col_b]; |
mori3rti | 6:aa5e94cddb3f | 296 | } |
mori3rti | 6:aa5e94cddb3f | 297 | } |
mori3rti | 6:aa5e94cddb3f | 298 | } |
mori3rti | 6:aa5e94cddb3f | 299 | } |
mori3rti | 6:aa5e94cddb3f | 300 | return result; |
mori3rti | 6:aa5e94cddb3f | 301 | } |
Yo_Robot | 3:48754fe86e08 | 302 | |
Yo_Robot | 3:48754fe86e08 | 303 | /************************************/ |
Yo_Robot | 3:48754fe86e08 | 304 | |
Yo_Robot | 3:48754fe86e08 | 305 | //Private Functions |
Yo_Robot | 3:48754fe86e08 | 306 | |
Yo_Robot | 3:48754fe86e08 | 307 | /**@brief |
Yo_Robot | 3:48754fe86e08 | 308 | * Expands the Matrix adding first and second column to the Matrix then |
Yo_Robot | 3:48754fe86e08 | 309 | * performs the Algorithm. |
Yo_Robot | 3:48754fe86e08 | 310 | * @param Mat |
Yo_Robot | 3:48754fe86e08 | 311 | * @return Determinant |
Yo_Robot | 3:48754fe86e08 | 312 | */ |
mori3rti | 6:aa5e94cddb3f | 313 | float MatrixMath::Det3x3(const Matrix &Mat) |
Yo_Robot | 3:48754fe86e08 | 314 | { |
mori3rti | 6:aa5e94cddb3f | 315 | Matrix D(Mat); //Copy Initial matrix |
Yo_Robot | 3:48754fe86e08 | 316 | |
Yo_Robot | 3:48754fe86e08 | 317 | Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column |
Yo_Robot | 3:48754fe86e08 | 318 | Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column |
Yo_Robot | 3:48754fe86e08 | 319 | |
Yo_Robot | 3:48754fe86e08 | 320 | float det = 0; |
mori3rti | 6:aa5e94cddb3f | 321 | for (int i = 0; i < 3; i++) |
mori3rti | 6:aa5e94cddb3f | 322 | det += D._matrix[0][i] * D._matrix[1][1 + i] * D._matrix[2][2 + i] - D._matrix[0][2 + i] * D._matrix[1][1 + i] * D._matrix[2][i]; |
Yo_Robot | 3:48754fe86e08 | 323 | |
Yo_Robot | 3:48754fe86e08 | 324 | return det; |
Yo_Robot | 1:c74cdf14aea2 | 325 | } |