Revision 2:d487bb616ec1, committed 2011-10-30

Comitter:

Yo_Robot

Date:

Sun Oct 30 04:41:00 2011 +0000

Parent:

1:c74cdf14aea2

Child:

3:48754fe86e08

Commit message:

Kinematic and Dot functions version 0.9

Changed in this revision

--- a/MatrixMath.cpp	Sat Oct 22 23:18:55 2011 +0000
+++ b/MatrixMath.cpp	Sun Oct 30 04:41:00 2011 +0000
@@ -1,175 +1,269 @@
-/**
- * @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;
+/**
+ * @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 " );
+    }
+}
+
+Matrix MatrixMath::Eye( int Rows )
+{
+    Matrix Identity( Rows, Rows ); //Square Matrix
+
+    for( int i = 0; i < Rows; i++ )
+        Identity._matrix[i][i] = 1;
+
+    return Identity;
+}
+
+
+
+float MatrixMath::dot(const Matrix& leftM, const Matrix& rightM)
+{
+    if( leftM.isVector() && rightM.isVector() )
+    {
+        if( leftM._nRows == 1 )
+        {
+            if( rightM._nRows == 1 )
+            {
+                if( leftM._nCols == rightM._nCols )
+                {
+                    // Calculate ( 1,n )( 1,n )
+                    float dotP;
+                    Matrix Cross;
+
+                    Cross = leftM * MatrixMath::Transpose( rightM );
+                    dotP = Cross.sum();
+
+                    return dotP;
+
+                }else{
+                    printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
+                }
+
+            }else{
+                if( leftM._nCols == rightM._nRows )
+                {
+                    // Calculate (1, n)( n, 1 )
+                    float dotP;
+                    Matrix Cross;
+
+                    Cross = leftM * rightM;
+                    dotP = Cross.sum();
+
+                    return dotP;
+
+                }else{
+                    printf( "\n\nERROR:\n Matrices have diferent depths @ MatrixMath::dot()\n" );
+                }
+            }
+
+        }else{
+            if( rightM._nRows == 1 )
+            {
+                if( leftM._nRows == rightM._nCols )
+                {
+                    // Calculate ( n,1 )( 1,n )
+                    float dotP;
+                    Matrix Cross;
+
+                    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{
+                if( leftM._nRows == rightM._nRows )
+                {
+                    // Calculate (n, 1)( n, 1 )
+                    float dotP;
+                    Matrix Cross;
+
+                    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 Matrix is not a Vector @ MatrixMath::dot()\n" );
+    }
+}
+
+
+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	Sat Oct 22 23:18:55 2011 +0000
+++ b/MatrixMath.h	Sun Oct 30 04:41:00 2011 +0000
@@ -1,78 +1,116 @@
-/**
- * @file   MatrixMath.h
- * @author Ernesto Palacios
- *
- * Created on 15 de septiembre de 2011, 09:44 AM.
- *
- *  Develop Under  GPL v3.0 License
- *  http://www.gnu.org/licenses/gpl-3.0.html
- *
- */
-
-#ifndef    MATRIXMATH_H
-#define    MATRIXMATH_H
-
-#include "mbed.h"
-#include "Matrix.h"
-
-
-/**
- * @brief This class provides STATIC methods to preform operations over
- * Matrix Objects
- * version 0.8.
- *
- * Methods will be added as neccesary.
- *
- */
-class MatrixMath{
-public:
-
-
-    /**@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 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 );
-
-    static Matrix RotY( const Matrix& matrix, float radians );
-
-    static Matrix RotZ( const Matrix& matrix, float radians );
-
-    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 */
+/**
+ * @file   MatrixMath.h
+ * @author Ernesto Palacios
+ *
+ * Created on 15 de septiembre de 2011, 09:44 AM.
+ *
+ *  Develop Under  GPL v3.0 License
+ *  http://www.gnu.org/licenses/gpl-3.0.html
+ *
+ */
+
+#ifndef    MATRIXMATH_H
+#define    MATRIXMATH_H
+
+#include "mbed.h"
+#include "Matrix.h"
+
+
+/**
+ * @brief This class provides STATIC methods to preform operations over
+ * Matrix Objects
+ * version 0.8.
+ *
+ * Methods will be added as neccesary.
+ *
+ */
+class MatrixMath{
+public:
+
+
+    /**@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 );
+
+
+    /**@brief
+     * Creates an identity Matrix, n x n.
+     * @param Rows Number of Rowns and Columns
+     * @return Identity Matrix of dimensions Rows x Rows
+     */
+    static Matrix Eye( int Rows );
+
+    /**@brief
+     * Returns the dot Product of any two same leght vectors.
+     * In this case a vector is defined as a [1,n] Matrix.
+     * @param leftM First Vector
+     * @param rightM Second Vector
+     * @return Dot Product or Scalar Product.
+     */    
+    static float dot( 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 ====//
+
+    /**@brief
+     * Calculates the Rotation Matrix Transform along 'x' axis in radians.
+     * @param radians rotation angle.
+     * @return Rotation Matrix[4,4] along 'x' axis.
+     */
+    static Matrix RotX( float radians );
+
+
+    /**@brief
+     * Calculates the Rotation Matrix Transform along 'y' axis in radians.
+     * @param radians rotation angle.
+     * @return Rotation Matrix[4,4] along 'y' axis.
+     */
+    static Matrix RotY( float radians );
+
+
+    /**@brief
+     * Calculates the Rotation Matrix Transform along 'y' axis in radians.
+     * @param radians rotation angle.
+     * @return Rotation Matrix[4,4] along 'y' axis.
+     */
+    static Matrix RotZ( float radians );
+
+    /**@brief
+     * Calculates the Translation Matrix to coordenates, (x' y' z')
+     * @param x axis translation
+     * @param y axis translation
+     * @param z axis translation
+     * @return Translation Matrix[4,4] x'y'z'.
+     */
+    static Matrix Transl( 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 */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath_Kinematics.cpp	Sun Oct 30 04:41:00 2011 +0000
@@ -0,0 +1,74 @@
+/**@file  Kinematic_Operations
+ * @author Ernesto Palacios
+ *
+ * @brief Definition for Kinematic Operations.
+ *
+ *  Develop Under  GPL v3.0 License
+ *  http://www.gnu.org/licenses/gpl-3.0.html
+ *
+ */
+
+#include "mbed.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
+
+
+Matrix MatrixMath::RotX( float radians )
+{
+    float cs = cos( radians );
+    float sn = sin( radians );
+
+    Matrix rotate( 4, 4 );
+
+    rotate << 1 << 0  <<  0  << 0
+	       << 0 << cs << -sn << 0
+	       << 0 << sn <<  cs << 0
+	       << 0 << 0  <<  0  << 1;
+
+    return rotate;
+
+}
+
+Matrix MatrixMath::RotY( float radians )
+{
+    float cs = cos( radians );
+    float sn = sin( radians );
+
+    Matrix rotate( 4, 4 );
+
+    rotate << cs   << 0   <<  sn  << 0
+	       <<  0   << 1   <<  0   << 0
+	       << -sn  << 0   <<  cs  << 0
+	       <<  0   << 0   <<  0   << 1;
+
+    return rotate;
+}
+
+Matrix MatrixMath::RotZ( float radians )
+{
+    float cs = cos( radians );
+    float sn = sin( radians );
+
+    Matrix rotate( 4, 4 );
+
+    rotate << cs   << -sn  <<  0  << 0
+           << sn   <<  cs  <<  0  << 0
+	       <<  0   <<  0   <<  1  << 0
+	       <<  0   <<  0   <<  0  << 1;
+
+    return rotate;
+}
+
+
+Matrix MatrixMath::Transl( float x, float y, float z )
+{
+    Matrix Translation = MatrixMath::Eye( 3 );  //Identity Matrix
+    Matrix Position( 4, 1 );                   // Position Matrix
+
+    Position << x << y << z << 1;             // position @ x,y,z
+
+    Matrix::AddRow( Translation, 4 );             // Add Row
+    Matrix::AddCol( Translation, Position, 4 );  // Add Position Matrix
+
+    return Translation;
+}
\ No newline at end of file