Diff: MatrixMath/MatrixMath.cpp

Revision:

0:a0285293f6a6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath/MatrixMath.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,175 @@
+/**
+ * @brief  Still under work  version 0.2
+ * @file   MatrixMath.cpp
+ * @author Erneseto Palacios
+ *
+ *  Develop Under  GPL v3.0 License
+ * http://www.gnu.org/licenses/gpl-3.0.html
+ */
+
+#include "mbed.h"
+#include "MatrixMath.h"
+
+///Transpose matrix
+Matrix MatrixMath::Transpose(const Matrix& Mat)
+{
+    Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
+
+    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)
+{
+    if( Mat._nRows == Mat._nCols )
+    {
+        if( Mat._nRows == 2 )   // 2x2 Matrices
+        {
+            float det = MatrixMath::det( Mat );
+            if( det != 0 )
+            {
+                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 *= 1/det;
+
+                return Inv;
+
+            }else{
+                printf( "\n\nWANRING: same matrix returned");
+                printf( "\nSingular Matrix, cannot perform Invert @matrix " );
+//                Mat.print();
+                printf( "\n  _____________\n" );
+
+                return Mat;
+            }
+
+        }else{   // nxn Matrices
+
+            float det = MatrixMath::det( Mat );
+            if( det!= 0 )
+            {
+                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++ )
+                    {
+                        SubMat = Mat ;
+
+                        Matrix::DeleteRow( SubMat, i+1 );
+                        Matrix::DeleteCol( SubMat, j+1 );
+
+                        if( (i+j)%2 == 0 )
+                            Inv._matrix[i][j] = MatrixMath::det( SubMat );
+                        else
+                            Inv._matrix[i][j] = -MatrixMath::det( SubMat );
+                    }
+
+                // Adjugate Matrix
+                Inv = MatrixMath::Transpose( Inv );
+
+                // Inverse Matrix
+                Inv = 1/det * Inv;
+
+                return Inv;
+
+            }else{
+                printf( "\n\nWANRING: same matrix returned");
+                printf( "\nSingular Matrix, cannot perform Invert @matrix " );
+          //      Mat.print();
+                printf( "\n  _____________\n" );
+
+                return Mat;
+            }
+
+        }
+
+    }else{
+        printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " );
+    }
+}
+
+float MatrixMath::det(const Matrix& Mat)
+{
+    if( Mat._nRows == Mat._nCols  )
+    {
+
+        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];
+            return det;
+        }
+        else if( Mat._nRows == 3 ) // 3x3 Matrix
+        {
+            float det;
+            MatrixMath dummy;
+
+            det = dummy.Det3x3( Mat );
+            return det;
+
+        } else {
+
+            float part1= 0;
+            float part2= 0;
+
+            //Find +/- on First Row
+            for( int i = 0; i < Mat._nCols; i++)
+            {
+                Matrix reduced( Mat );           // Copy Original Matrix
+                Matrix::DeleteRow( reduced, 1); // Delete First Row
+
+                if( i%2 == 0 ) //Odd Rows
+                {
+
+                    Matrix::DeleteCol( reduced, i+1);
+                    part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
+                }
+                else  // Even Rows
+                {
+                    Matrix::DeleteCol( reduced, i+1);
+                    part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
+                }
+            }
+            return part1 - part2; //
+        }
+
+    }else{
+        printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det");
+    }
+}
+
+
+/************************************/
+
+//Private Functions
+
+/**@brief
+ * Expands the Matrix adding first and second column to the Matrix then
+ * performs the Algorithm.
+ * @param Mat
+ * @return Determinant
+ */
+float MatrixMath::Det3x3(const Matrix& Mat)
+{
+    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];
+
+    return det;
+}
\ No newline at end of file