Revision 3:48754fe86e08, committed 2011-10-30

Comitter:

Yo_Robot

Date:

Sun Oct 30 04:44:45 2011 +0000

Parent:

2:d487bb616ec1

Child:

4:d360c068d55f

Commit message:

Basic Kinematics & Basic Matrix version 0.9

Changed in this revision

--- a/MatrixMath.cpp	Sun Oct 30 04:41:00 2011 +0000
+++ b/MatrixMath.cpp	Sun Oct 30 04:44:45 2011 +0000
@@ -1,269 +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 " );
-    }
-}
-
-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;
+/**
+ * @brief  Almost there version 0.9
+ * @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	Sun Oct 30 04:41:00 2011 +0000
+++ b/MatrixMath.h	Sun Oct 30 04:44:45 2011 +0000
@@ -1,116 +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 );
-
-
-    /**@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 */
+/**
+ * @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 perform operations over
+ * Matrix Objects
+ * version 0.9.
+ *
+ * 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 */