This is a test Program for the Class Matrix version 1.6.4

Dependencies:   Matrix Kinematics mbed TrackVector2D MatrixMath

main.cpp

Committer:
Yo_Robot
Date:
2011-12-03
Revision:
2:e3b963c560d8
Parent:
1:9e4cb305fb24

File content as of revision 2:e3b963c560d8:

#include "mbed.h"
#include "Matrix.h"
#include "MatrixMath.h"

int main() {

    DigitalOut myled(LED1);
    Timer t,t2;
    
    t.start();
 
//---
    Matrix myMatrix(3,3);
    Matrix anotherMatrix;

    // Fill Matrix with data.
    myMatrix << 1  << 2  << 3
             << 4  << 5  << 6
             << 7  << 8  << 9;

    printf( "\nmyMatrix:\n\n");
    myMatrix.print();
    printf( "\n" );

    
    // Matrix operations //

    // Add 5 to negative Matrix 
    anotherMatrix = - myMatrix + 5;

    printf( "Result Matrix: anotherMatrix = - myMatrix + 5\n\n" );
    anotherMatrix.print();
    printf( "\n" );
    
    // Matrix Multiplication *
    anotherMatrix *=  myMatrix;

    printf( "\nanotherMatrix = anotherMatrix * myMatrix\n\n" );
    anotherMatrix.print();
    printf( "\n" );
    
    // Scalar Matrix Multiplication anotherMatrix *= 0.5
    anotherMatrix *= 0.5;

    printf( "\nResult Matrix *= 0.5:\n\n" );
    anotherMatrix.print();
    printf( "    _______________________________ \n" );


    printf("\n\n *** MEMBER OPERATIONS *** \n\n");

    //Copy myMatrix
    Matrix temp( myMatrix );

    // Resize Matrix
    temp.Resize(4,4);
    printf("\nAdded one Column, one Row to the limits of myMatrix saved in temp Matrix\n");
    temp.print();

    //Delete those new elements, we don't need them anyway.
    Matrix::DeleteRow( temp, 4 );
    Matrix::DeleteCol( temp, 4 );

    printf("\nBack to normal\n");
    temp.print();

    
    // Make room at the begining of Matrix
    Matrix::AddRow( temp, 1 );
    Matrix::AddCol( temp, 1 );
    
    printf("\nAdded Just one Row and column to the beginning\n");
    temp.print();

    // Take the second Row as a new Matrix
    anotherMatrix = Matrix::ExportRow( temp, 2 );
    printf("\nExport Second Row \n");
    anotherMatrix.print();

    // The second Column as a new Matrix, then transpose it to make it a Row
    anotherMatrix = Matrix::ExportCol( temp, 2 );
    anotherMatrix = MatrixMath::Transpose( anotherMatrix );
    printf("\nAnd Export Second Column and Transpose it \n");
    anotherMatrix.print();

    // This will Check to see if your are reduce to a single Row or Column
    temp = Matrix::ToPackedVector( myMatrix );
    printf("\nInitial Matrix turned into a single Row\n");
    temp.print();
           
    //  Matrix Math  //
    printf("\n\n *** Matrix Inverse and Determinant ***\n");
    
    Matrix BigMat( 8, 8 );
    
    BigMat   << 1 << 0.3 << 1.0 << 1 << 3 << 0.5 << 7.12 << 899
             << 2 << 3.2 << 4.1 << 0 << 4 << 0.8 << 9.26 << 321
             << 5 << 6.0 << 1   << 1 << 2 << 7.4 << 3.87 << 562
             << 1 << 0.0 << 2.7 << 1 << 1 << 4.6 << 1.21 << 478
             << 2 << 3.7 << 48  << 2 << 0 << 77  << 0.19 << 147
             << 1 << 1.0 << 3.8 << 7 << 1 << 9.9 << 7.25 << 365
             << 9 << 0.9 << 2.7 << 8 << 0 << 13  << 4.16 << 145
             << 7 << 23  << 28  << 9 << 9 << 1.7 << 9.16 << 156;

    printf( "\nBigMat:\n");
    BigMat.print();
    printf( "\n" );

    t2.start();
    float determ = MatrixMath::det( BigMat );

    Matrix myInv = MatrixMath::Inv( BigMat );
    t2.stop();

    printf( "\nBigMat's Determinant is: %f \n", determ);
    printf( "\n" );
    
    printf( "\nBigMat's Inverse is:\n");
    myInv.print();
    printf( "\n" );

    //***  Homogenous Transformations **//
    
    printf( "\n\n *** TRANSFORMATIONS *** \n\n");

    Matrix rot;

    printf( " RotX  0.5 rad \n" );
    rot = MatrixMath::RotX(0.5);
    rot.print();
    printf( "    _______________________________ \n\n" );

    printf( " RotY  0.5 rad \n" );
    rot = MatrixMath::RotY(0.5);
    rot.print();
    printf( "    _______________________________ \n\n" );

    printf( " RotZ  0.5 rad \n" );
    rot = MatrixMath::RotZ(0.5);
    rot.print();
    printf( "    _______________________________ \n\n" );

    printf( " Transl  5x 3y 4z\n" );
    rot = MatrixMath::Transl( 5, 3, 4 );
    rot.print();
    printf( "    _______________________________ \n\n" );

 //---
 
    t.stop();
    
    float bigtime = t2.read();
    float average = 12.149647 - bigtime;
        
    printf( "\n\nThe time for all those operations in mbed was : %f seconds\n", t.read() );
    printf( "\nOnly operations witout any print takes: 12.149647 seconds\n" );
    printf( "\nDue to the 8x8 matrix alone takes: %f \n",bigtime );
    printf( "\nSo normal 4x4 matrix ops: %f\n", average );
           
    while(1) {
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
    }
}