This class provides with operations for Matrix Objects + some changes

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?

UserRevisionLine numberNew 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 }