add MatrixCross

Committer:
NaotoMorita
Date:
Tue Aug 10 08:08:11 2021 +0000
Revision:
7:b22d56ac37aa
Parent:
6:aa5e94cddb3f
add Vector3;

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