This class provides with operations for Matrix Objects + some changes
MatrixMath.cpp@7:1c6ba87fa6a3, 2020-10-01 (annotated)
- Committer:
- saloutos
- Date:
- Thu Oct 01 04:08:00 2020 +0000
- Revision:
- 7:1c6ba87fa6a3
- Parent:
- 6:aa5e94cddb3f
Same changes as for Matrix library
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"); |
saloutos | 7:1c6ba87fa6a3 | 102 | return Mat; |
Yo_Robot | 3:48754fe86e08 | 103 | } |
Yo_Robot | 3:48754fe86e08 | 104 | } |
Yo_Robot | 3:48754fe86e08 | 105 | |
mori3rti | 6:aa5e94cddb3f | 106 | Matrix MatrixMath::Eye(int Rows) |
Yo_Robot | 3:48754fe86e08 | 107 | { |
mori3rti | 6:aa5e94cddb3f | 108 | Matrix Identity(Rows, Rows); //Square Matrix |
Yo_Robot | 3:48754fe86e08 | 109 | |
mori3rti | 6:aa5e94cddb3f | 110 | for (int i = 0; i < Rows; i++) |
Yo_Robot | 3:48754fe86e08 | 111 | Identity._matrix[i][i] = 1; |
Yo_Robot | 3:48754fe86e08 | 112 | |
Yo_Robot | 3:48754fe86e08 | 113 | return Identity; |
Yo_Robot | 3:48754fe86e08 | 114 | } |
Yo_Robot | 3:48754fe86e08 | 115 | |
Yo_Robot | 5:93948a9bbde2 | 116 | // Very Versitle Function. Accepts two Vector Matrices of any type: |
mori3rti | 6:aa5e94cddb3f | 117 | // Vector types may be: [n,1] dot [n,1] |
Yo_Robot | 5:93948a9bbde2 | 118 | // [n,1] dot [1,n] always same |
Yo_Robot | 5:93948a9bbde2 | 119 | // [1,n] dot [n,1] 'depth' |
Yo_Robot | 5:93948a9bbde2 | 120 | // [1,n] dot [1,n] |
mori3rti | 6:aa5e94cddb3f | 121 | float MatrixMath::dot(const Matrix &leftM, const Matrix &rightM) |
Yo_Robot | 3:48754fe86e08 | 122 | { |
mori3rti | 6:aa5e94cddb3f | 123 | if (leftM.isVector() && rightM.isVector()) |
Yo_Robot | 3:48754fe86e08 | 124 | { |
mori3rti | 6:aa5e94cddb3f | 125 | if (leftM._nRows == 1) |
Yo_Robot | 3:48754fe86e08 | 126 | { |
mori3rti | 6:aa5e94cddb3f | 127 | if (rightM._nRows == 1) |
Yo_Robot | 3:48754fe86e08 | 128 | { |
mori3rti | 6:aa5e94cddb3f | 129 | if (leftM._nCols == rightM._nCols) |
Yo_Robot | 3:48754fe86e08 | 130 | { |
Yo_Robot | 3:48754fe86e08 | 131 | // Calculate ( 1,n )( 1,n ) |
Yo_Robot | 3:48754fe86e08 | 132 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 133 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 134 | |
mori3rti | 6:aa5e94cddb3f | 135 | Cross = leftM * MatrixMath::Transpose(rightM); |
Yo_Robot | 3:48754fe86e08 | 136 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 137 | |
Yo_Robot | 3:48754fe86e08 | 138 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 139 | } |
mori3rti | 6:aa5e94cddb3f | 140 | else |
mori3rti | 6:aa5e94cddb3f | 141 | { |
mori3rti | 6:aa5e94cddb3f | 142 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
saloutos | 7:1c6ba87fa6a3 | 143 | return 0.0f; |
Yo_Robot | 3:48754fe86e08 | 144 | } |
mori3rti | 6:aa5e94cddb3f | 145 | } |
mori3rti | 6:aa5e94cddb3f | 146 | else |
mori3rti | 6:aa5e94cddb3f | 147 | { |
mori3rti | 6:aa5e94cddb3f | 148 | if (leftM._nCols == rightM._nRows) |
Yo_Robot | 3:48754fe86e08 | 149 | { |
Yo_Robot | 3:48754fe86e08 | 150 | // Calculate (1, n)( n, 1 ) |
Yo_Robot | 3:48754fe86e08 | 151 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 152 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 153 | |
Yo_Robot | 3:48754fe86e08 | 154 | Cross = leftM * 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"); |
saloutos | 7:1c6ba87fa6a3 | 162 | return 0.0f; |
Yo_Robot | 3:48754fe86e08 | 163 | } |
Yo_Robot | 3:48754fe86e08 | 164 | } |
mori3rti | 6:aa5e94cddb3f | 165 | } |
mori3rti | 6:aa5e94cddb3f | 166 | else |
mori3rti | 6:aa5e94cddb3f | 167 | { |
mori3rti | 6:aa5e94cddb3f | 168 | if (rightM._nRows == 1) |
Yo_Robot | 3:48754fe86e08 | 169 | { |
mori3rti | 6:aa5e94cddb3f | 170 | if (leftM._nRows == rightM._nCols) |
Yo_Robot | 3:48754fe86e08 | 171 | { |
Yo_Robot | 3:48754fe86e08 | 172 | // Calculate ( n,1 )( 1,n ) |
Yo_Robot | 3:48754fe86e08 | 173 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 174 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 175 | |
mori3rti | 6:aa5e94cddb3f | 176 | Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM); |
Yo_Robot | 3:48754fe86e08 | 177 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 178 | |
Yo_Robot | 3:48754fe86e08 | 179 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 180 | } |
mori3rti | 6:aa5e94cddb3f | 181 | else |
mori3rti | 6:aa5e94cddb3f | 182 | { |
mori3rti | 6:aa5e94cddb3f | 183 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
saloutos | 7:1c6ba87fa6a3 | 184 | return 0.0f; |
Yo_Robot | 3:48754fe86e08 | 185 | } |
mori3rti | 6:aa5e94cddb3f | 186 | } |
mori3rti | 6:aa5e94cddb3f | 187 | else |
mori3rti | 6:aa5e94cddb3f | 188 | { |
mori3rti | 6:aa5e94cddb3f | 189 | if (leftM._nRows == rightM._nRows) |
Yo_Robot | 3:48754fe86e08 | 190 | { |
Yo_Robot | 3:48754fe86e08 | 191 | // Calculate (n, 1)( n, 1 ) |
Yo_Robot | 3:48754fe86e08 | 192 | float dotP; |
Yo_Robot | 3:48754fe86e08 | 193 | Matrix Cross; |
Yo_Robot | 3:48754fe86e08 | 194 | |
mori3rti | 6:aa5e94cddb3f | 195 | Cross = MatrixMath::Transpose(leftM) * rightM; |
Yo_Robot | 3:48754fe86e08 | 196 | dotP = Cross.sum(); |
Yo_Robot | 3:48754fe86e08 | 197 | |
Yo_Robot | 3:48754fe86e08 | 198 | return dotP; |
mori3rti | 6:aa5e94cddb3f | 199 | } |
mori3rti | 6:aa5e94cddb3f | 200 | else |
mori3rti | 6:aa5e94cddb3f | 201 | { |
mori3rti | 6:aa5e94cddb3f | 202 | printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n"); |
saloutos | 7:1c6ba87fa6a3 | 203 | return 0.0f; |
Yo_Robot | 3:48754fe86e08 | 204 | } |
Yo_Robot | 3:48754fe86e08 | 205 | } |
Yo_Robot | 3:48754fe86e08 | 206 | } |
mori3rti | 6:aa5e94cddb3f | 207 | } |
mori3rti | 6:aa5e94cddb3f | 208 | else |
mori3rti | 6:aa5e94cddb3f | 209 | { |
mori3rti | 6:aa5e94cddb3f | 210 | printf("\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n"); |
saloutos | 7:1c6ba87fa6a3 | 211 | return 0.0f; |
Yo_Robot | 3:48754fe86e08 | 212 | } |
Yo_Robot | 3:48754fe86e08 | 213 | } |
Yo_Robot | 3:48754fe86e08 | 214 | |
mori3rti | 6:aa5e94cddb3f | 215 | float MatrixMath::det(const Matrix &Mat) |
Yo_Robot | 3:48754fe86e08 | 216 | { |
mori3rti | 6:aa5e94cddb3f | 217 | if (Mat._nRows == Mat._nCols) |
Yo_Robot | 3:48754fe86e08 | 218 | { |
Yo_Robot | 3:48754fe86e08 | 219 | |
mori3rti | 6:aa5e94cddb3f | 220 | if (Mat._nRows == 2) // 2x2 Matrix |
Yo_Robot | 3:48754fe86e08 | 221 | { |
Yo_Robot | 3:48754fe86e08 | 222 | float det; |
Yo_Robot | 3:48754fe86e08 | 223 | det = Mat._matrix[0][0] * Mat._matrix[1][1] - |
mori3rti | 6:aa5e94cddb3f | 224 | Mat._matrix[1][0] * Mat._matrix[0][1]; |
Yo_Robot | 3:48754fe86e08 | 225 | return det; |
Yo_Robot | 3:48754fe86e08 | 226 | } |
mori3rti | 6:aa5e94cddb3f | 227 | else if (Mat._nRows == 3) // 3x3 Matrix |
Yo_Robot | 3:48754fe86e08 | 228 | { |
Yo_Robot | 3:48754fe86e08 | 229 | float det; |
Yo_Robot | 5:93948a9bbde2 | 230 | MatrixMath dummy; //For Private Method. |
Yo_Robot | 3:48754fe86e08 | 231 | |
mori3rti | 6:aa5e94cddb3f | 232 | det = dummy.Det3x3(Mat); |
Yo_Robot | 3:48754fe86e08 | 233 | return det; |
mori3rti | 6:aa5e94cddb3f | 234 | } |
mori3rti | 6:aa5e94cddb3f | 235 | else |
mori3rti | 6:aa5e94cddb3f | 236 | { |
Yo_Robot | 3:48754fe86e08 | 237 | |
mori3rti | 6:aa5e94cddb3f | 238 | float part1 = 0; |
mori3rti | 6:aa5e94cddb3f | 239 | float part2 = 0; |
Yo_Robot | 3:48754fe86e08 | 240 | |
Yo_Robot | 3:48754fe86e08 | 241 | //Find +/- on First Row |
mori3rti | 6:aa5e94cddb3f | 242 | for (int i = 0; i < Mat._nCols; i++) |
Yo_Robot | 3:48754fe86e08 | 243 | { |
mori3rti | 6:aa5e94cddb3f | 244 | Matrix reduced(Mat); // Copy Original Matrix |
mori3rti | 6:aa5e94cddb3f | 245 | Matrix::DeleteRow(reduced, 1); // Delete First Row |
Yo_Robot | 3:48754fe86e08 | 246 | |
mori3rti | 6:aa5e94cddb3f | 247 | if (i % 2 == 0) //Even Rows |
Yo_Robot | 3:48754fe86e08 | 248 | { |
Yo_Robot | 3:48754fe86e08 | 249 | |
mori3rti | 6:aa5e94cddb3f | 250 | Matrix::DeleteCol(reduced, i + 1); |
Yo_Robot | 3:48754fe86e08 | 251 | part1 += Mat._matrix[0][i] * MatrixMath::det(reduced); |
Yo_Robot | 3:48754fe86e08 | 252 | } |
mori3rti | 6:aa5e94cddb3f | 253 | else // Odd Rows |
Yo_Robot | 3:48754fe86e08 | 254 | { |
mori3rti | 6:aa5e94cddb3f | 255 | Matrix::DeleteCol(reduced, i + 1); |
Yo_Robot | 3:48754fe86e08 | 256 | part2 += Mat._matrix[0][i] * MatrixMath::det(reduced); |
Yo_Robot | 3:48754fe86e08 | 257 | } |
Yo_Robot | 3:48754fe86e08 | 258 | } |
mori3rti | 6:aa5e94cddb3f | 259 | return part1 - part2; |
Yo_Robot | 3:48754fe86e08 | 260 | } |
mori3rti | 6:aa5e94cddb3f | 261 | } |
mori3rti | 6:aa5e94cddb3f | 262 | else |
mori3rti | 6:aa5e94cddb3f | 263 | { |
Yo_Robot | 5:93948a9bbde2 | 264 | printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::det"); |
saloutos | 7:1c6ba87fa6a3 | 265 | return 0.0f; |
Yo_Robot | 3:48754fe86e08 | 266 | } |
Yo_Robot | 3:48754fe86e08 | 267 | } |
Yo_Robot | 3:48754fe86e08 | 268 | |
mori3rti | 6:aa5e94cddb3f | 269 | Matrix MatrixMath::kron(const Matrix &Mat_A, const Matrix &Mat_B) |
mori3rti | 6:aa5e94cddb3f | 270 | { |
mori3rti | 6:aa5e94cddb3f | 271 | Matrix result(Mat_A._nRows * Mat_B._nRows, Mat_A._nCols * Mat_B._nCols); |
mori3rti | 6:aa5e94cddb3f | 272 | for (unsigned int row_a = 0; row_a < Mat_A._nRows; row_a++) |
mori3rti | 6:aa5e94cddb3f | 273 | { |
mori3rti | 6:aa5e94cddb3f | 274 | for (unsigned int col_a = 0; col_a < Mat_A._nCols; col_a++) |
mori3rti | 6:aa5e94cddb3f | 275 | { |
mori3rti | 6:aa5e94cddb3f | 276 | Matrix block = Mat_A._matrix[row_a][col_a]*Mat_B; |
mori3rti | 6:aa5e94cddb3f | 277 | |
mori3rti | 6:aa5e94cddb3f | 278 | for (unsigned int row_b = 0; row_b < Mat_B._nRows; row_b++) |
mori3rti | 6:aa5e94cddb3f | 279 | { |
mori3rti | 6:aa5e94cddb3f | 280 | for (unsigned int col_b = 0; col_b < Mat_B._nCols; col_b++) |
mori3rti | 6:aa5e94cddb3f | 281 | { |
mori3rti | 6:aa5e94cddb3f | 282 | 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 | 283 | } |
mori3rti | 6:aa5e94cddb3f | 284 | } |
mori3rti | 6:aa5e94cddb3f | 285 | } |
mori3rti | 6:aa5e94cddb3f | 286 | } |
mori3rti | 6:aa5e94cddb3f | 287 | return result; |
mori3rti | 6:aa5e94cddb3f | 288 | } |
Yo_Robot | 3:48754fe86e08 | 289 | |
Yo_Robot | 3:48754fe86e08 | 290 | /************************************/ |
Yo_Robot | 3:48754fe86e08 | 291 | |
Yo_Robot | 3:48754fe86e08 | 292 | //Private Functions |
Yo_Robot | 3:48754fe86e08 | 293 | |
Yo_Robot | 3:48754fe86e08 | 294 | /**@brief |
Yo_Robot | 3:48754fe86e08 | 295 | * Expands the Matrix adding first and second column to the Matrix then |
Yo_Robot | 3:48754fe86e08 | 296 | * performs the Algorithm. |
Yo_Robot | 3:48754fe86e08 | 297 | * @param Mat |
Yo_Robot | 3:48754fe86e08 | 298 | * @return Determinant |
Yo_Robot | 3:48754fe86e08 | 299 | */ |
mori3rti | 6:aa5e94cddb3f | 300 | float MatrixMath::Det3x3(const Matrix &Mat) |
Yo_Robot | 3:48754fe86e08 | 301 | { |
mori3rti | 6:aa5e94cddb3f | 302 | Matrix D(Mat); //Copy Initial matrix |
Yo_Robot | 3:48754fe86e08 | 303 | |
Yo_Robot | 3:48754fe86e08 | 304 | Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column |
Yo_Robot | 3:48754fe86e08 | 305 | Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column |
Yo_Robot | 3:48754fe86e08 | 306 | |
Yo_Robot | 3:48754fe86e08 | 307 | float det = 0; |
mori3rti | 6:aa5e94cddb3f | 308 | for (int i = 0; i < 3; i++) |
mori3rti | 6:aa5e94cddb3f | 309 | 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 | 310 | |
Yo_Robot | 3:48754fe86e08 | 311 | return det; |
Yo_Robot | 1:c74cdf14aea2 | 312 | } |