Ernesto Palacios / Mbed 2 deprecated Matrix_class

This is a test Program for the Class Matrix version 1.6.4

Dependencies:   Matrix Kinematics mbed TrackVector2D MatrixMath

main.cpp@2:e3b963c560d8, 2011-12-03 (annotated)

Committer:
Yo_Robot
Date:
Sat Dec 03 17:57:25 2011 +0000
Revision:
2:e3b963c560d8
Parent:
1:9e4cb305fb24 
Homogeneus transformations included !  Big 8x8 matrix inverse and determinant

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yo_Robot 0:63af78c5943c 1 #include "mbed.h"
Yo_Robot 0:63af78c5943c 2 #include "Matrix.h"
Yo_Robot 0:63af78c5943c 3 #include "MatrixMath.h"
Yo_Robot 0:63af78c5943c 4
Yo_Robot 0:63af78c5943c 5 int main() {
Yo_Robot 0:63af78c5943c 6
Yo_Robot 0:63af78c5943c 7 DigitalOut myled(LED1);
Yo_Robot 2:e3b963c560d8 8 Timer t,t2;
Yo_Robot 0:63af78c5943c 9
Yo_Robot 1:9e4cb305fb24 10 t.start();
Yo_Robot 1:9e4cb305fb24 11
Yo_Robot 1:9e4cb305fb24 12 //---
Yo_Robot 0:63af78c5943c 13 Matrix myMatrix(3,3);
Yo_Robot 0:63af78c5943c 14 Matrix anotherMatrix;
Yo_Robot 0:63af78c5943c 15
Yo_Robot 0:63af78c5943c 16 // Fill Matrix with data.
Yo_Robot 0:63af78c5943c 17 myMatrix << 1 << 2 << 3
Yo_Robot 0:63af78c5943c 18 << 4 << 5 << 6
Yo_Robot 0:63af78c5943c 19 << 7 << 8 << 9;
Yo_Robot 0:63af78c5943c 20
Yo_Robot 2:e3b963c560d8 21 printf( "\nmyMatrix:\n\n");
Yo_Robot 0:63af78c5943c 22 myMatrix.print();
Yo_Robot 0:63af78c5943c 23 printf( "\n" );
Yo_Robot 0:63af78c5943c 24
Yo_Robot 0:63af78c5943c 25
Yo_Robot 2:e3b963c560d8 26 // Matrix operations //
Yo_Robot 0:63af78c5943c 27
Yo_Robot 0:63af78c5943c 28 // Add 5 to negative Matrix
Yo_Robot 0:63af78c5943c 29 anotherMatrix = - myMatrix + 5;
Yo_Robot 0:63af78c5943c 30
Yo_Robot 2:e3b963c560d8 31 printf( "Result Matrix: anotherMatrix = - myMatrix + 5\n\n" );
Yo_Robot 0:63af78c5943c 32 anotherMatrix.print();
Yo_Robot 0:63af78c5943c 33 printf( "\n" );
Yo_Robot 0:63af78c5943c 34
Yo_Robot 0:63af78c5943c 35 // Matrix Multiplication *
Yo_Robot 0:63af78c5943c 36 anotherMatrix *= myMatrix;
Yo_Robot 0:63af78c5943c 37
Yo_Robot 2:e3b963c560d8 38 printf( "\nanotherMatrix = anotherMatrix * myMatrix\n\n" );
Yo_Robot 0:63af78c5943c 39 anotherMatrix.print();
Yo_Robot 0:63af78c5943c 40 printf( "\n" );
Yo_Robot 0:63af78c5943c 41
Yo_Robot 0:63af78c5943c 42 // Scalar Matrix Multiplication anotherMatrix *= 0.5
Yo_Robot 0:63af78c5943c 43 anotherMatrix *= 0.5;
Yo_Robot 0:63af78c5943c 44
Yo_Robot 2:e3b963c560d8 45 printf( "\nResult Matrix *= 0.5:\n\n" );
Yo_Robot 0:63af78c5943c 46 anotherMatrix.print();
Yo_Robot 0:63af78c5943c 47 printf( " _______________________________ \n" );
Yo_Robot 0:63af78c5943c 48
Yo_Robot 0:63af78c5943c 49
Yo_Robot 2:e3b963c560d8 50 printf("\n\n *** MEMBER OPERATIONS *** \n\n");
Yo_Robot 0:63af78c5943c 51
Yo_Robot 0:63af78c5943c 52 //Copy myMatrix
Yo_Robot 0:63af78c5943c 53 Matrix temp( myMatrix );
Yo_Robot 0:63af78c5943c 54
Yo_Robot 0:63af78c5943c 55 // Resize Matrix
Yo_Robot 0:63af78c5943c 56 temp.Resize(4,4);
Yo_Robot 2:e3b963c560d8 57 printf("\nAdded one Column, one Row to the limits of myMatrix saved in temp Matrix\n");
Yo_Robot 0:63af78c5943c 58 temp.print();
Yo_Robot 0:63af78c5943c 59
Yo_Robot 0:63af78c5943c 60 //Delete those new elements, we don't need them anyway.
Yo_Robot 0:63af78c5943c 61 Matrix::DeleteRow( temp, 4 );
Yo_Robot 0:63af78c5943c 62 Matrix::DeleteCol( temp, 4 );
Yo_Robot 0:63af78c5943c 63
Yo_Robot 0:63af78c5943c 64 printf("\nBack to normal\n");
Yo_Robot 0:63af78c5943c 65 temp.print();
Yo_Robot 0:63af78c5943c 66
Yo_Robot 0:63af78c5943c 67
Yo_Robot 0:63af78c5943c 68 // Make room at the begining of Matrix
Yo_Robot 0:63af78c5943c 69 Matrix::AddRow( temp, 1 );
Yo_Robot 1:9e4cb305fb24 70 Matrix::AddCol( temp, 1 );
Yo_Robot 1:9e4cb305fb24 71
Yo_Robot 0:63af78c5943c 72 printf("\nAdded Just one Row and column to the beginning\n");
Yo_Robot 0:63af78c5943c 73 temp.print();
Yo_Robot 0:63af78c5943c 74
Yo_Robot 2:e3b963c560d8 75 // Take the second Row as a new Matrix
Yo_Robot 0:63af78c5943c 76 anotherMatrix = Matrix::ExportRow( temp, 2 );
Yo_Robot 0:63af78c5943c 77 printf("\nExport Second Row \n");
Yo_Robot 0:63af78c5943c 78 anotherMatrix.print();
Yo_Robot 0:63af78c5943c 79
Yo_Robot 2:e3b963c560d8 80 // The second Column as a new Matrix, then transpose it to make it a Row
Yo_Robot 0:63af78c5943c 81 anotherMatrix = Matrix::ExportCol( temp, 2 );
Yo_Robot 0:63af78c5943c 82 anotherMatrix = MatrixMath::Transpose( anotherMatrix );
Yo_Robot 0:63af78c5943c 83 printf("\nAnd Export Second Column and Transpose it \n");
Yo_Robot 0:63af78c5943c 84 anotherMatrix.print();
Yo_Robot 0:63af78c5943c 85
Yo_Robot 0:63af78c5943c 86 // This will Check to see if your are reduce to a single Row or Column
Yo_Robot 0:63af78c5943c 87 temp = Matrix::ToPackedVector( myMatrix );
Yo_Robot 0:63af78c5943c 88 printf("\nInitial Matrix turned into a single Row\n");
Yo_Robot 0:63af78c5943c 89 temp.print();
Yo_Robot 0:63af78c5943c 90
Yo_Robot 2:e3b963c560d8 91 // Matrix Math //
Yo_Robot 2:e3b963c560d8 92 printf("\n\n *** Matrix Inverse and Determinant ***\n");
Yo_Robot 2:e3b963c560d8 93
Yo_Robot 2:e3b963c560d8 94 Matrix BigMat( 8, 8 );
Yo_Robot 1:9e4cb305fb24 95
Yo_Robot 2:e3b963c560d8 96 BigMat << 1 << 0.3 << 1.0 << 1 << 3 << 0.5 << 7.12 << 899
Yo_Robot 2:e3b963c560d8 97 << 2 << 3.2 << 4.1 << 0 << 4 << 0.8 << 9.26 << 321
Yo_Robot 2:e3b963c560d8 98 << 5 << 6.0 << 1 << 1 << 2 << 7.4 << 3.87 << 562
Yo_Robot 2:e3b963c560d8 99 << 1 << 0.0 << 2.7 << 1 << 1 << 4.6 << 1.21 << 478
Yo_Robot 2:e3b963c560d8 100 << 2 << 3.7 << 48 << 2 << 0 << 77 << 0.19 << 147
Yo_Robot 2:e3b963c560d8 101 << 1 << 1.0 << 3.8 << 7 << 1 << 9.9 << 7.25 << 365
Yo_Robot 2:e3b963c560d8 102 << 9 << 0.9 << 2.7 << 8 << 0 << 13 << 4.16 << 145
Yo_Robot 2:e3b963c560d8 103 << 7 << 23 << 28 << 9 << 9 << 1.7 << 9.16 << 156;
Yo_Robot 1:9e4cb305fb24 104
Yo_Robot 1:9e4cb305fb24 105 printf( "\nBigMat:\n");
Yo_Robot 1:9e4cb305fb24 106 BigMat.print();
Yo_Robot 1:9e4cb305fb24 107 printf( "\n" );
Yo_Robot 1:9e4cb305fb24 108
Yo_Robot 2:e3b963c560d8 109 t2.start();
Yo_Robot 1:9e4cb305fb24 110 float determ = MatrixMath::det( BigMat );
Yo_Robot 1:9e4cb305fb24 111
Yo_Robot 2:e3b963c560d8 112 Matrix myInv = MatrixMath::Inv( BigMat );
Yo_Robot 2:e3b963c560d8 113 t2.stop();
Yo_Robot 2:e3b963c560d8 114
Yo_Robot 1:9e4cb305fb24 115 printf( "\nBigMat's Determinant is: %f \n", determ);
Yo_Robot 1:9e4cb305fb24 116 printf( "\n" );
Yo_Robot 2:e3b963c560d8 117
Yo_Robot 1:9e4cb305fb24 118 printf( "\nBigMat's Inverse is:\n");
Yo_Robot 1:9e4cb305fb24 119 myInv.print();
Yo_Robot 1:9e4cb305fb24 120 printf( "\n" );
Yo_Robot 1:9e4cb305fb24 121
Yo_Robot 2:e3b963c560d8 122 //*** Homogenous Transformations **//
Yo_Robot 2:e3b963c560d8 123
Yo_Robot 2:e3b963c560d8 124 printf( "\n\n *** TRANSFORMATIONS *** \n\n");
Yo_Robot 2:e3b963c560d8 125
Yo_Robot 2:e3b963c560d8 126 Matrix rot;
Yo_Robot 2:e3b963c560d8 127
Yo_Robot 2:e3b963c560d8 128 printf( " RotX 0.5 rad \n" );
Yo_Robot 2:e3b963c560d8 129 rot = MatrixMath::RotX(0.5);
Yo_Robot 2:e3b963c560d8 130 rot.print();
Yo_Robot 2:e3b963c560d8 131 printf( " _______________________________ \n\n" );
Yo_Robot 1:9e4cb305fb24 132
Yo_Robot 2:e3b963c560d8 133 printf( " RotY 0.5 rad \n" );
Yo_Robot 2:e3b963c560d8 134 rot = MatrixMath::RotY(0.5);
Yo_Robot 2:e3b963c560d8 135 rot.print();
Yo_Robot 2:e3b963c560d8 136 printf( " _______________________________ \n\n" );
Yo_Robot 1:9e4cb305fb24 137
Yo_Robot 2:e3b963c560d8 138 printf( " RotZ 0.5 rad \n" );
Yo_Robot 2:e3b963c560d8 139 rot = MatrixMath::RotZ(0.5);
Yo_Robot 2:e3b963c560d8 140 rot.print();
Yo_Robot 2:e3b963c560d8 141 printf( " _______________________________ \n\n" );
Yo_Robot 2:e3b963c560d8 142
Yo_Robot 2:e3b963c560d8 143 printf( " Transl 5x 3y 4z\n" );
Yo_Robot 2:e3b963c560d8 144 rot = MatrixMath::Transl( 5, 3, 4 );
Yo_Robot 2:e3b963c560d8 145 rot.print();
Yo_Robot 2:e3b963c560d8 146 printf( " _______________________________ \n\n" );
Yo_Robot 2:e3b963c560d8 147
Yo_Robot 2:e3b963c560d8 148 //---
Yo_Robot 2:e3b963c560d8 149
Yo_Robot 1:9e4cb305fb24 150 t.stop();
Yo_Robot 1:9e4cb305fb24 151
Yo_Robot 2:e3b963c560d8 152 float bigtime = t2.read();
Yo_Robot 2:e3b963c560d8 153 float average = 12.149647 - bigtime;
Yo_Robot 2:e3b963c560d8 154
Yo_Robot 2:e3b963c560d8 155 printf( "\n\nThe time for all those operations in mbed was : %f seconds\n", t.read() );
Yo_Robot 2:e3b963c560d8 156 printf( "\nOnly operations witout any print takes: 12.149647 seconds\n" );
Yo_Robot 2:e3b963c560d8 157 printf( "\nDue to the 8x8 matrix alone takes: %f \n",bigtime );
Yo_Robot 2:e3b963c560d8 158 printf( "\nSo normal 4x4 matrix ops: %f\n", average );
Yo_Robot 0:63af78c5943c 159
Yo_Robot 0:63af78c5943c 160 while(1) {
Yo_Robot 0:63af78c5943c 161 myled = 1;
Yo_Robot 0:63af78c5943c 162 wait(0.2);
Yo_Robot 0:63af78c5943c 163 myled = 0;
Yo_Robot 0:63af78c5943c 164 wait(0.2);
Yo_Robot 0:63af78c5943c 165 }
Yo_Robot 0:63af78c5943c 166 }