add kronecker product

Files at this revision

API Documentation at this revision

Comitter:
mori3rti
Date:
Thu Sep 17 17:51:22 2020 +0700
Parent:
5:93948a9bbde2
Commit message:
update kronecker product operation

Changed in this revision

MatrixMath.cpp Show annotated file Show diff for this revision Revisions of this file
MatrixMath.h Show annotated file Show diff for this revision Revisions of this file
MatrixMath_Kinematics.cpp Show annotated file Show diff for this revision Revisions of this file
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 )