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