Ibn Hidayat
add kronecker product
Revision 6:aa5e94cddb3f, committed 2020-09-17
- Comitter:
- mori3rti
- Date:
- Thu Sep 17 17:51:22 2020 +0700
- Parent:
- 5:93948a9bbde2
- Commit message:
- update kronecker product operation
Changed in this revision
diff -r 93948a9bbde2 -r aa5e94cddb3f MatrixMath.cpp
--- a/MatrixMath.cpp Sun Oct 30 19:21:30 2011 +0000
+++ b/MatrixMath.cpp Thu Sep 17 17:51:22 2020 +0700
@@ -13,132 +13,137 @@
#include "MatrixMath.h"
///Transpose matrix
-Matrix MatrixMath::Transpose(const Matrix& Mat)
+Matrix MatrixMath::Transpose(const Matrix &Mat)
{
- Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
+ Matrix result(Mat._nCols, Mat._nRows); //Transpose Matrix
- for( int i = 0; i < result._nRows; i++ )
- for( int j = 0; j < result._nCols; j++ )
+ for (int i = 0; i < result._nRows; i++)
+ for (int j = 0; j < result._nCols; j++)
result._matrix[i][j] = Mat._matrix[j][i];
return result;
}
-Matrix MatrixMath::Inv(const Matrix& Mat)
+Matrix MatrixMath::Inv(const Matrix &Mat)
{
- if( Mat._nRows == Mat._nCols )
+ if (Mat._nRows == Mat._nCols)
{
- if( Mat._nRows == 2 ) // 2x2 Matrices
+ if (Mat._nRows == 2) // 2x2 Matrices
{
- float det = MatrixMath::det( Mat );
- if( det != 0 )
+ float det = MatrixMath::det(Mat);
+ if (det != 0)
{
- Matrix Inv(2,2);
- Inv._matrix[0][0] = Mat._matrix[1][1];
+ Matrix Inv(2, 2);
+ Inv._matrix[0][0] = Mat._matrix[1][1];
Inv._matrix[1][0] = -Mat._matrix[1][0];
Inv._matrix[0][1] = -Mat._matrix[0][1];
- Inv._matrix[1][1] = Mat._matrix[0][0] ;
+ Inv._matrix[1][1] = Mat._matrix[0][0];
- Inv *= 1/det;
+ Inv *= 1 / det;
return Inv;
-
- }else{
- printf( "\n\nWANRING: same matrix returned");
- printf( "\nSingular Matrix, cannot perform Invert @matrix\n " );
+ }
+ else
+ {
+ printf("\n\nWANRING: same matrix returned");
+ printf("\nSingular Matrix, cannot perform Invert @matrix\n ");
Mat.print();
- printf( "\n _____________\n" );
+ printf("\n _____________\n");
return Mat;
}
-
- }else{ // nxn Matrices
+ }
+ else
+ { // nxn Matrices
- float det = MatrixMath::det( Mat );
- if( det!= 0 )
+ float det = MatrixMath::det(Mat);
+ if (det != 0)
{
- Matrix Inv( Mat ); //
+ Matrix Inv(Mat); //
Matrix SubMat;
// Matrix of Co-factors
- for( int i = 0; i < Mat._nRows; i++ )
- for( int j = 0; j < Mat._nCols; j++ )
+ for (int i = 0; i < Mat._nRows; i++)
+ for (int j = 0; j < Mat._nCols; j++)
{
- SubMat = Mat ;
+ SubMat = Mat;
- Matrix::DeleteRow( SubMat, i+1 );
- Matrix::DeleteCol( SubMat, j+1 );
+ Matrix::DeleteRow(SubMat, i + 1);
+ Matrix::DeleteCol(SubMat, j + 1);
- if( (i+j)%2 == 0 )
- Inv._matrix[i][j] = MatrixMath::det( SubMat );
+ if ((i + j) % 2 == 0)
+ Inv._matrix[i][j] = MatrixMath::det(SubMat);
else
- Inv._matrix[i][j] = -MatrixMath::det( SubMat );
+ Inv._matrix[i][j] = -MatrixMath::det(SubMat);
}
// Adjugate Matrix
- Inv = MatrixMath::Transpose( Inv );
+ Inv = MatrixMath::Transpose(Inv);
// Inverse Matrix
- Inv = 1/det * Inv;
+ Inv = 1 / det * Inv;
return Inv;
-
- }else{
- printf( "\n\nWANRING: same matrix returned");
- printf( "\nSingular Matrix, cannot perform Invert @matrix\n" );
+ }
+ else
+ {
+ printf("\n\nWANRING: same matrix returned");
+ printf("\nSingular Matrix, cannot perform Invert @matrix\n");
Mat.print();
- printf( "\n _____________\n" );
+ printf("\n _____________\n");
return Mat;
}
-
}
-
- }else{
- printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant\n" );
+ }
+ else
+ {
+ printf("\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant\n");
}
}
-Matrix MatrixMath::Eye( int Rows )
+Matrix MatrixMath::Eye(int Rows)
{
- Matrix Identity( Rows, Rows ); //Square Matrix
+ Matrix Identity(Rows, Rows); //Square Matrix
- for( int i = 0; i < Rows; i++ )
+ for (int i = 0; i < Rows; i++)
Identity._matrix[i][i] = 1;
return Identity;
}
// Very Versitle Function. Accepts two Vector Matrices of any type:
-// Vector types may be: [n,1] dot [n,1]
+// Vector types may be: [n,1] dot [n,1]
// [n,1] dot [1,n] always same
// [1,n] dot [n,1] 'depth'
// [1,n] dot [1,n]
-float MatrixMath::dot(const Matrix& leftM, const Matrix& rightM)
+float MatrixMath::dot(const Matrix &leftM, const Matrix &rightM)
{
- if( leftM.isVector() && rightM.isVector() )
+ if (leftM.isVector() && rightM.isVector())
{
- if( leftM._nRows == 1 )
+ if (leftM._nRows == 1)
{
- if( rightM._nRows == 1 )
+ if (rightM._nRows == 1)
{
- if( leftM._nCols == rightM._nCols )
+ if (leftM._nCols == rightM._nCols)
{
// Calculate ( 1,n )( 1,n )
float dotP;
Matrix Cross;
- Cross = leftM * MatrixMath::Transpose( rightM );
+ Cross = leftM * MatrixMath::Transpose(rightM);
dotP = Cross.sum();
return dotP;
-
- }else{
- printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
+ }
+ else
+ {
+ printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
}
-
- }else{
- if( leftM._nCols == rightM._nRows )
+ }
+ else
+ {
+ if (leftM._nCols == rightM._nRows)
{
// Calculate (1, n)( n, 1 )
float dotP;
@@ -148,105 +153,132 @@
dotP = Cross.sum();
return dotP;
-
- }else{
- printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
+ }
+ else
+ {
+ printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
}
}
-
- }else{
- if( rightM._nRows == 1 )
+ }
+ else
+ {
+ if (rightM._nRows == 1)
{
- if( leftM._nRows == rightM._nCols )
+ if (leftM._nRows == rightM._nCols)
{
// Calculate ( n,1 )( 1,n )
float dotP;
Matrix Cross;
- Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM);
+ Cross = MatrixMath::Transpose(leftM) * MatrixMath::Transpose(rightM);
dotP = Cross.sum();
return dotP;
-
- }else{
- printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
+ }
+ else
+ {
+ printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
}
-
- }else{
- if( leftM._nRows == rightM._nRows )
+ }
+ else
+ {
+ if (leftM._nRows == rightM._nRows)
{
// Calculate (n, 1)( n, 1 )
float dotP;
Matrix Cross;
- Cross = MatrixMath::Transpose(leftM) * rightM ;
+ Cross = MatrixMath::Transpose(leftM) * rightM;
dotP = Cross.sum();
return dotP;
-
- }else{
- printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
+ }
+ else
+ {
+ printf("\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n");
}
}
}
-
- }else{
- printf( "\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n" );
+ }
+ else
+ {
+ printf("\n\nERROR:\n Matrix is not a Vector @ MatrixMath::dot()\n");
}
}
-
-float MatrixMath::det(const Matrix& Mat)
+float MatrixMath::det(const Matrix &Mat)
{
- if( Mat._nRows == Mat._nCols )
+ if (Mat._nRows == Mat._nCols)
{
- if( Mat._nRows == 2 ) // 2x2 Matrix
+ if (Mat._nRows == 2) // 2x2 Matrix
{
float det;
det = Mat._matrix[0][0] * Mat._matrix[1][1] -
- Mat._matrix[1][0] * Mat._matrix[0][1];
+ Mat._matrix[1][0] * Mat._matrix[0][1];
return det;
}
- else if( Mat._nRows == 3 ) // 3x3 Matrix
+ else if (Mat._nRows == 3) // 3x3 Matrix
{
float det;
MatrixMath dummy; //For Private Method.
- det = dummy.Det3x3( Mat );
+ det = dummy.Det3x3(Mat);
return det;
+ }
+ else
+ {
- } else {
-
- float part1= 0;
- float part2= 0;
+ float part1 = 0;
+ float part2 = 0;
//Find +/- on First Row
- for( int i = 0; i < Mat._nCols; i++)
+ for (int i = 0; i < Mat._nCols; i++)
{
- Matrix reduced( Mat ); // Copy Original Matrix
- Matrix::DeleteRow( reduced, 1); // Delete First Row
+ Matrix reduced(Mat); // Copy Original Matrix
+ Matrix::DeleteRow(reduced, 1); // Delete First Row
- if( i%2 == 0 ) //Even Rows
+ if (i % 2 == 0) //Even Rows
{
- Matrix::DeleteCol( reduced, i+1);
+ Matrix::DeleteCol(reduced, i + 1);
part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
}
- else // Odd Rows
+ else // Odd Rows
{
- Matrix::DeleteCol( reduced, i+1);
+ Matrix::DeleteCol(reduced, i + 1);
part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
}
}
- return part1 - part2;
+ return part1 - part2;
}
-
- }else{
+ }
+ else
+ {
printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::det");
}
}
+Matrix MatrixMath::kron(const Matrix &Mat_A, const Matrix &Mat_B)
+{
+ Matrix result(Mat_A._nRows * Mat_B._nRows, Mat_A._nCols * Mat_B._nCols);
+ for (unsigned int row_a = 0; row_a < Mat_A._nRows; row_a++)
+ {
+ for (unsigned int col_a = 0; col_a < Mat_A._nCols; col_a++)
+ {
+ Matrix block = Mat_A._matrix[row_a][col_a]*Mat_B;
+
+ for (unsigned int row_b = 0; row_b < Mat_B._nRows; row_b++)
+ {
+ for (unsigned int col_b = 0; col_b < Mat_B._nCols; col_b++)
+ {
+ result._matrix[row_b + (row_a * Mat_B._nRows)][col_b + (col_a * Mat_B._nCols)] = block._matrix[row_b][col_b];
+ }
+ }
+ }
+ }
+ return result;
+}
/************************************/
@@ -258,17 +290,16 @@
* @param Mat
* @return Determinant
*/
-float MatrixMath::Det3x3(const Matrix& Mat)
+float MatrixMath::Det3x3(const Matrix &Mat)
{
- Matrix D( Mat ); //Copy Initial matrix
+ Matrix D(Mat); //Copy Initial matrix
Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
float det = 0;
- for( int i = 0; i < 3; i++ )
- 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];
+ for (int i = 0; i < 3; i++)
+ 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];
return det;
}
\ No newline at end of file
diff -r 93948a9bbde2 -r aa5e94cddb3f MatrixMath.h
--- a/MatrixMath.h Sun Oct 30 19:21:30 2011 +0000
+++ b/MatrixMath.h Thu Sep 17 17:51:22 2020 +0700
@@ -67,6 +67,22 @@
*/
static float det( const Matrix& Mat );
+ /**@brief Calculates Kronecker product of two Matrix.
+ * Kronecker product is an operation on two matrices of arbitrary
+ * size resulting in a block matrix. If A is an m × n matrix
+ * and B is a p × q matrix, then the Kronecker product A ⊗ B is
+ * the pm × qn block matrix:
+ * - -
+ * | a_11 B ... a_1n B |
+ * A ⊗ B = | ... ... ... |
+ * | a_m1 B ... a_mn B |
+ * - -
+ * @param Mat_A Matrix m × n
+ * @param Mat_B Matrix p × q
+ * @return Kron Matrix pm × qn.
+ */
+ static Matrix kron(const Matrix& Mat_A, const Matrix& Mat_B);
+
//==== For Kinematics ====//
diff -r 93948a9bbde2 -r aa5e94cddb3f MatrixMath_Kinematics.cpp --- a/MatrixMath_Kinematics.cpp Sun Oct 30 19:21:30 2011 +0000 +++ b/MatrixMath_Kinematics.cpp Thu Sep 17 17:51:22 2020 +0700 @@ -12,6 +12,7 @@ #include "mbed.h" #include "Matrix.h" #include "MatrixMath.h" +#include "math.h" Matrix MatrixMath::RotX( float radians )