File content as of revision 0:958e1e10e536:

/**
 * @file   TrackVector2D.cpp
 * @author Ernesto Palacios
 *
 * @brief
 * Develop Under  GPL v3.0 License
 * http://www.gnu.org/licenses/gpl-3.0.html .
 *
 * Created on 22 de octubre de 2011, 11:26 PM
 */

#include "TrackVector2D.h"

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

#define   PI 3.1416

TrackVector2D::TrackVector2D():_nMoves(0), _angRad(0), _angDeg(0), _distance(0)
{
    _TrnsfM = MatrixMath::Eye( 4 );  //Identity [4,4] Matrix
    _InvM   = MatrixMath::Eye( 4 );  
    _RotM   = MatrixMath::Eye( 4 ); 
    
    _Moves.push_back( complex <float>( 0.0, 0.0) ); //First Position is Origin
}


///Copy Constructor
TrackVector2D::TrackVector2D(const TrackVector2D& orig)
{
    _TrnsfM = orig._TrnsfM;
    _InvM   = orig._InvM;
    _RotM   = orig._RotM;
    _Moves = orig._Moves;

    _angRad = orig._angRad;
    _angDeg = orig._angDeg;
    _distance = orig._distance;
    
}



const Matrix TrackVector2D::moveto(float x, float y)
{
    _InvM = MatrixMath::Inv( _TrnsfM );         // Inverse of Last 
                                               //  Transformation Matrix
    
    Matrix Position(4,1);                    // Position Matrix &
    Position << x                           //  Position Data
             << y
             << 0
             << 1;            
    
    Matrix Pos_uv = _InvM * Position;       // Target Position.
                                           //   P[x; y; z; 1 ]
    Matrix Pos_rect( 1,2 );
    Pos_rect << Pos_uv(1,1) << Pos_uv( 2,1 );

    printf( "\n\nPosition in Rect Coordenates is\n" );
    Pos_rect.print();
    
    Matrix Pos_ma;
    Pos_ma = TrackVector2D::Rect2Polar( Pos_rect );  //To Polar Coordenates
    
    printf( "\n\nPosition in Polar Coordenates is\n" );
    Pos_ma.print();



    //***  SEPARATE  ROTATION FROM TRANSLATION ***// 
    
    // Origin is not Recursive. Only one origin
    Matrix M_Transl = MatrixMath::Transl( x, y, 0 );   
    
    //  Rotation is Recursive
    float rotAngle = TrackVector2D::deg2rad( Pos_ma(1,2) );
    _RotM *= MatrixMath::RotZ( rotAngle );
    
    // Updated Transformation Matrix
    _TrnsfM =  M_Transl * _RotM;  
    
       
    //***  VALUES FOR THE ROBOT TO TRAVEL  ***//
    
    _angRad += Pos_ma(1,2);               // Previous + New Angle
    _angDeg = TrackVector2D::rad2deg(_angRad);
    _distance = Pos_ma(1,1);
    
    Pos_ma(1,2) = TrackVector2D::rad2deg( Pos_ma(1,2) ); // To Degrees
    
    return Pos_ma;
    // Matrix with the info for the "vector" to move.
    // Pos_ma = [ magnitude, angle ]
    
}

// This function does not return anything because if you're callin' it
// you already know the angle the 'vector' rotated.
void TrackVector2D::rotate(float degrees)
{
    float rad = TrackVector2D::deg2rad( degrees ) ;
    _RotM *= MatrixMath::RotZ( rad );
}

const Matrix TrackVector2D::GetRotM() { return this->_RotM; }


const Matrix TrackVector2D::GetTrasfM() { return this->_TrnsfM; }


const Matrix TrackVector2D::GetLastMove()
{
    Matrix last(1,2); 
    last( 1, 1 ) = _Moves[_Moves.size()].real(); // Parte Real "X"
    last( 1, 2 ) = _Moves[_Moves.size()].imag(); // Parte Imaginaria "Y"
    return last;
}

const Matrix TrackVector2D::GetMove( int index)
{
    --index;
    Matrix last(1,2); 
    last( 1, 1 ) = _Moves[ index ].real(); // Parte Real "X"
    last( 1, 2 ) = _Moves[ index ].imag(); // Parte Imaginaria "Y"
    return last;
}


float TrackVector2D::getX()
{
    return _Moves[_Moves.size()].real();
}

float TrackVector2D::getY()
{
    return _Moves[_Moves.size()].imag();
}

float TrackVector2D::getDeg() { return _angDeg;  }

float TrackVector2D::getRad() { return _angRad;  }


float TrackVector2D::rad2deg(float angle)
{
    if( angle != 0 )
        return ( angle * 180 ) / PI;
    else
        return 0;
}


float TrackVector2D::deg2rad(float angle)
{
    if( angle != 0 )
        return ( angle / 180 ) * PI;
    else
        return 0;
}


const Matrix TrackVector2D::Rect2Polar(const Matrix& Coord) 
{
    Matrix Polar( 1, 2 );
    
    float ang, mag, X, Y;
    
    X = Coord(1,1);
    Y = Coord(1,2);
    
    if( X < 0 && X > -0.01 )         // Discard error caused by PI
      X = 0;
      
    if( Y < 0 && Y > -0.01 )       // Discard error
      Y = 0;
    
    if( X == 0 && Y == 0 )       // Vector at Origin
        ang = 0;
    
    else if( X < 0 && Y == 0)  // Along "-X" axis
        ang = PI;     //180º
    
    else
        ang = atan( Y / X );         // a = atan( y/x )
    
    if( X < 0 && Y > 0 )           // Quad II
        ang = PI + ang;           // Correction for second Quadrant 
    
    if( X < 0 && Y < 0 )        // Quad III
      ang = -PI + ang;     // Correction for Third Quadrant
    
    
    mag = sqrt(  pow(X, 2)  +  pow(Y,2)  );
    ang = TrackVector2D::rad2deg( ang );
    
    Polar(1,1) = mag;     // Polar[mag, ang]
    Polar(1,2) = ang;
    
    return Polar;
}

const Matrix TrackVector2D::Polar2Rect(const Matrix& Coord)
{
    Matrix Rect( 1,2 );

    float angle = TrackVector2D::deg2rad( Coord( 1,2 ) );
    
    Rect(1,1)  = cos( angle ) * Coord(1,1);
    Rect(1,2)  = sin( angle ) * Coord(1,1);
    
    return Rect;
}