Ernesto Palacios
This class provides with operations for Matrix Objects
Dependents: Matrix_class Wizardsneverdie mbed_multiplex_matrix Kinematics_Project_G5 ... more
Diff: MatrixMath.cpp
- Revision:
- 3:48754fe86e08
- Parent:
- 2:d487bb616ec1
- Child:
- 4:d360c068d55f
--- 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