Ernesto Palacios / MatrixMath

This class provides with operations for Matrix Objects

Dependents:   Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more

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 
update kronecker product operation

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");
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 }