Revision 1:c74cdf14aea2, committed 2011-10-22

Comitter:

Yo_Robot

Date:

Sat Oct 22 23:18:55 2011 +0000

Parent:

0:eb69bbfb6486

Child:

2:d487bb616ec1

Commit message:

version 0.8 Inv() det() defined

Changed in this revision

--- a/MatrixMath.cpp	Fri Oct 21 03:28:28 2011 +0000
+++ b/MatrixMath.cpp	Sat Oct 22 23:18:55 2011 +0000
@@ -1,5 +1,5 @@
 /**
- * @brief Still under work  version 0.1
+ * @brief  Still under work  version 0.2
  * @file   MatrixMath.cpp
  * @author Erneseto Palacios
  *
@@ -11,14 +11,165 @@
 #include "MatrixMath.h"
 
 ///Transpose matrix
-Matrix MatrixMath::Transpose(const Matrix& matrix)
+Matrix MatrixMath::Transpose(const Matrix& Mat)
 {
-    Matrix result( matrix._nCols, matrix._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++ )
-            result._matrix[i][j] = matrix._matrix[j][i];
+            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

--- a/MatrixMath.h	Fri Oct 21 03:28:28 2011 +0000
+++ b/MatrixMath.h	Sat Oct 22 23:18:55 2011 +0000
@@ -1,6 +1,6 @@
 /**
- * @file:   MatrixMath.h
- * @author: Ernesto Palacios
+ * @file   MatrixMath.h
+ * @author Ernesto Palacios
  *
  * Created on 15 de septiembre de 2011, 09:44 AM.
  *
@@ -9,8 +9,8 @@
  *
  */
 
-#ifndef MATRIXMATH_H
-#define	MATRIXMATH_H
+#ifndef    MATRIXMATH_H
+#define    MATRIXMATH_H
 
 #include "mbed.h"
 #include "Matrix.h"
@@ -18,7 +18,8 @@
 
 /**
  * @brief This class provides STATIC methods to preform operations over
- * Matrix Objects.
+ * Matrix Objects
+ * version 0.8.
  *
  * Methods will be added as neccesary.
  *
@@ -26,14 +27,34 @@
 class MatrixMath{
 public:
 
-    
-    static Matrix Transpose( const Matrix& matrix );
+
+    /**@brief
+     * Transposes Matrix, return new Object.
+     * @param Mat matrix to calculate
+     * @return the determinant
+     */
+    static Matrix Transpose( const Matrix& Mat );
+
+
+    /**@brief
+     * Calculate the inverse of a nxn Matrix BUT check first if the determinant
+     * is != 0. Same matrix will be return if Det( Mat ) == 0.
+     * @param Mat matrix to calcute inverse.
+     * @return Matrix Inverse
+     */
+    static Matrix Inv( const Matrix& Mat );
+
     
-    static Matrix Invert( const Matrix& matrix );
-    
-    static float DotProduct( const Matrix& leftM, const Matrix& rightM );
-    
-    static float Det( const Matrix& Mat );
+    static float dotProduct( const Matrix& leftM, const Matrix& rightM );
+
+    /**@brief Calculates the determinant of a Matrix.
+     * @param Mat matrix to calculate.
+     * @return the determinant.
+     */
+    static float det( const Matrix& Mat );
+
+
+    //**  For Kinematics **//
 
     static Matrix RotX( const Matrix& matrix, float radians );
 
@@ -43,6 +64,15 @@
 
     static Matrix Transl( const Matrix& matrix, float x, float y, float z );
 
+private:
+
+    /**@brief
+     * Calculates the Determinant of a 3x3 Matrix
+     * @param Mat Already made sure is a 3 by 3
+     * @return Float, determinant.
+     */
+    float Det3x3( const Matrix& Mat );
+
 };
 
-#endif	/* MATRIXMATH_H */
+#endif    /* MATRIXMATH_H */