File content as of revision 44:2432c218f191:

// classes for 2d and 3d vectors 

#ifndef vectors_H
#define vectors_H

#include "mbed.h"

#ifndef DEG_TO_RAD
#define DEG_TO_RAD (PI/180.0)
#endif

#ifndef RAD_TO_DEG
#define RAD_TO_DEG (180.0/PI)
#endif

#ifndef CW
#define CW 1.0
#endif

#ifndef CCW
#define CCW -1.0
#endif

#ifndef PI
#define PI 3.14159265889
#endif

// =================  2D VECTORS ===================== 
template <class T>
class vector2D {

    public:
        
        // Overloaded constructor with parameters:
        vector2D( T _x=0.0f, T _y=0.0f );
        // note: we use the default copy constructor
        
        // Explicit setting:
        void set( T _x, T _y );
        void set( const vector2D& vec );
        
        // Comparison:
        bool operator==( const vector2D& vec );
        bool operator!=( const vector2D& vec );
        bool match( const vector2D& vec, float tollerance=0.0001 );
    
        // Overloaded operators:
        //
        void    operator=( const vector2D& vec );    // I cannot declare this if we want also operator chaining?
        //vector2D & operator=( const vector2D& vec ); // this is to enable operator chaining (vec1=vec2=vec3). 
        vector2D  operator+( const vector2D& vec ) const; // note: "const" here means that the member function will not alter any member variable
        vector2D& operator+=( const vector2D& vec );      // why it has an output? for doing vec1=vec2+=vec3? YES!!! (operator chaining). 
        vector2D  operator-( const vector2D& vec ) const;
        vector2D& operator-=( const vector2D& vec );
        vector2D  operator*( const vector2D& vec ) const;
        vector2D& operator*=( const vector2D& vec );
        vector2D  operator/( const vector2D& vec ) const;
        vector2D& operator/=( const vector2D& vec );
    
    //operator overloading for float:
    void       operator=( const T f);  // I cannot declare this if we want also operator chaining?
    //vector2D & operator=( const float& val ); // to allow operator chaining
    vector2D  operator+( const T f ) const;
    vector2D& operator+=( const T f );
    vector2D  operator-( const T f ) const;
    vector2D& operator-=( const T f );
    vector2D  operator-() const;

    
    // multiplication by a scalar:
    vector2D  operator*( const T f ) const;
    vector2D& operator*=( const T f );
    vector2D  operator/( const T f ) const;
    vector2D& operator/=( const T f );    
    
    // Distance (between end points of two vector2Ds):
    float distance( const vector2D& pnt) const;
    float squareDistance( const vector2D& pnt ) const;
    
    // Length of vector2D (norm):
    float length() const;
    float squareLength() const; // faster, no sqrt
    
    // Scaling:
    vector2D  getScaled( const float length ) const;
    vector2D& scale( const float length );
    
    // Normalization:
    vector2D  getNormalized() const;
    vector2D& normalize();
    
    // Perpendicular normalized vector2D.
    vector2D  getPerpendicularNormed(int orientation) const;
    vector2D& perpendicular(int orientation);
    
    // Rotation
    vector2D  getRotatedDeg( float angle ) const;
    vector2D  getRotatedRad( float angle ) const;
    vector2D& rotateDeg( float angle );
    vector2D& rotateRad( float angle );
    
    //vector2D product (for 3d vector2Ds - for 2d vector2Ds, something like this is just the "angle" between them):
    //vector2D getvector2DProduct(const vector2D& vec) const;
    //vector2D& vector2DProduct(const vector2D& vec) const;
    
    //Angle (deg) between two vector2Ds (using atan2, so between -180 and 180)
    float angleDeg( const vector2D& vec ) const;
    float angleRad( const vector2D& vec ) const;
    float angleDegHoriz( ) const; // particular case when the second vector is just (1,0)
    
    //Dot Product:
    float dot( const vector2D& vec ) const;
    
    // =================================================================
    
    // Actual variables:
    T x, y; // or make a class "point"
    
};

// 3D VECTORS: 
template <class T>
class vector3D {

    public:
        
        // Overloaded constructor with parameters:
        vector3D( T _x=0.0f, T _y=0.0f ,  T _z=0.0f );
        // note: we use the default copy constructor
        
        // Explicit setting:
        void set( T _x, T _y, T _z );
        void set( const vector3D& vec );
        
        // Comparison:
        bool operator==( const vector3D& vec );
        bool operator!=( const vector3D& vec );
        bool match( const vector3D& vec, float tollerance=0.0001 );
    
        // Overloaded operators:
        //
        void    operator=( const vector3D& vec );    // I cannot declare this if we want also operator chaining?
        //vector3D & operator=( const vector2D& vec ); // this is to enable operator chaining (vec1=vec2=vec3). 
        vector3D  operator+( const vector3D& vec ) const;
        vector3D& operator+=( const vector3D& vec );      // why it has an output? for doing vec1=vec2+=vec3? YES!!! (operator chaining). 
        vector3D  operator-( const vector3D& vec ) const;
        vector3D& operator-=( const vector3D& vec );
        vector3D  operator*( const vector3D& vec ) const;
        vector3D& operator*=( const vector3D& vec );
        
        // division "dimension by dimension" (like the matlab "./"):
        vector3D  operator/( const vector3D& vec ) const;
        vector3D& operator/=( const vector3D& vec );
        // note: division by a vector is properly defined for 2d vectors: norm is divided and argument is substracted (complex division). 
    
    //operator overloading for float:
    void       operator=( const T f);  // I cannot declare this if we want also operator chaining?
    //vector3D & operator=( const float& val ); // to allow operator chaining
    vector3D  operator+( const T f ) const;
    vector3D& operator+=( const T f );
    vector3D  operator-( const T f ) const;
    vector3D& operator-=( const T f );
    vector3D  operator-() const;

    
    // multiplication by a scalar:
    vector3D  operator*( const T f ) const;
    vector3D& operator*=( const T f );
    vector3D  operator/( const T f ) const;
    vector3D& operator/=( const T f );    
    
    // Distance (between end points of two vector3Ds):
    float distance( const vector3D& pnt) const;
    float squareDistance( const vector3D& pnt ) const;
    
    // Length of vector3D (norm):
    float length() const;
    float squareLength() const; // faster, no sqrt
    
    // Scaling:
    vector3D  getScaled( const float length ) const;
    vector3D& scale( const float length );
    
    // Normalization:
    vector3D  getNormalized() const;
    vector3D& normalize();
    
    
    //Angle (deg) between two vector2Ds (using atan2, so between -180 and 180)
    float angleDeg( const vector3D& vec ) const;
    float angleRad( const vector3D& vec ) const;
    
    //Dot Product:
    float dot( const vector3D& vec ) const;
    
    //Cross product: 
    vector3D  cross( const vector3D& vec ) const;
    vector3D& cross( const vector3D& vec );
    
    // =================================================================
    
    // Actual variables:
    T x, y, z; // or make a class "point"
    
};

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Implementation
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// ============================================  2D vectors ============================================

template <class T>
inline vector2D<T>::vector2D( T _x, T _y ) {
    x = _x; y = _y;
}

template <class T>
inline void vector2D<T>::set( T _x, T _y ) {
    x = _x; y = _y;
}

template <class T>
inline void vector2D<T>::set( const vector2D<T>& vec ) {
    x=vec.x; y=vec.y;
}

template <class T>
inline bool vector2D<T>::operator==( const vector2D<T>& vec ) {
    return (x == vec.x) && (y == vec.y);
}

template <class T>
inline bool vector2D<T>::operator!=( const vector2D<T>& vec ) {
    return (x != vec.x) || (y != vec.y);
}

template <class T>
inline bool vector2D<T>::match( const vector2D<T>& vec, float tolerance ) {
    return (abs(x - vec.x) < tolerance)&& (abs(y - vec.y) < tolerance);
}


/*
inline vector2D & operator=( const vector2D& vec ){ // returning a reference to the vector2D object for allowing operator chaining
    x = vec.x;
    y = vec.y;
    return *this;
}
 */

template <class T>
inline void vector2D<T>::operator=( const vector2D<T>& vec ){
    x = vec.x;  y = vec.y;
}

template <class T>
inline vector2D<T> vector2D<T>::operator+( const vector2D<T>& vec ) const {
    return vector2D<T>( x+vec.x, y+vec.y);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator+=( const vector2D<T>& vec ) {
    x += vec.x;
    y += vec.y;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::operator-( const vector2D<T>& vec ) const {  
    return vector2D<T>(x-vec.x, y-vec.y);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator-=( const vector2D<T>& vec ) {
    x -= vec.x;
    y -= vec.y;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::operator*( const vector2D<T>& vec ) const {
    return vector2D<T>(x*vec.x, y*vec.y);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator*=( const vector2D<T>& vec ) {
    x*=vec.x;
    y*=vec.y;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::operator/( const vector2D<T>& vec ) const {
    return vector2D<T>( vec.x!=0 ? x/vec.x : x , vec.y!=0 ? y/vec.y : y);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator/=( const vector2D<T>& vec ) {
    vec.x!=0 ? x/=vec.x : x;
    vec.y!=0 ? y/=vec.y : y;
    return *this;
}

//operator overloading for float:
/*
inline vector2D<T> & operator=( const float& val ){
    x = val;
    y = val;
    return *this;
}
 */

template <class T>
inline void vector2D<T>::operator=( const T f){
    x = f;  y = f;
}

template <class T>
inline vector2D<T> vector2D<T>::operator+( const T f ) const {
    return vector2D<T>( x+f, y+f);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator+=( const T f ) {
    x += f; y += f;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::operator-( const T f ) const {
    return vector2D<T>( x-f, y-f);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator-=( const T f ) {
    x -= f; y -= f;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::operator-() const {
    return vector2D<T>(-x, -y);
}

template <class T>
inline vector2D<T> vector2D<T>::operator*( const T f ) const {
    return vector2D<T>(x*f, y*f);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator*=( const T f ) {
    x*=f; y*=f;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::operator/( const T f ) const {
    //cout << "here" << endl;
    if(f == 0) return vector2D<T>(x, y);
    return vector2D<T>(x/f, y/f);
}

template <class T>
inline vector2D<T>& vector2D<T>::operator/=( const T f ) {
    if(f == 0) return *this;
    x/=f; y/=f;
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::getScaled( const float length ) const {
    float l = (float)sqrt(x*x + y*y);
    if( l > 0 )
        return vector2D<T>( (x/l)*length, (y/l)*length );
    else
        return vector2D<T>();
}

template <class T>
inline vector2D<T>& vector2D<T>::scale( const float length ) {
    float l = (float)sqrt(x*x + y*y);
    if (l > 0) {
        x = (x/l)*length;
        y = (y/l)*length;
    }
    return *this;
}

// Rotation
//
//

template <class T>
inline vector2D<T> vector2D<T>::getRotatedDeg( float angle ) const {
    float a = (float)(angle*DEG_TO_RAD);
    return vector2D<T>( x*cos(a) - y*sin(a),
                    x*sin(a) + y*cos(a) );
}

template <class T> 
inline vector2D<T> vector2D<T>::getRotatedRad( float angle ) const {
    float a = angle;
    return vector2D<T>( x*cos(a) - y*sin(a),
                    x*sin(a) + y*cos(a) );
}

template <class T>
inline vector2D<T>& vector2D<T>::rotateDeg( float angle ) {
    float a = (float)(angle * DEG_TO_RAD);
    float xrot = x*cos(a) - y*sin(a);
    y = x*sin(a) + y*cos(a);
    x = xrot;
    return *this;
}

template <class T>
inline vector2D<T>& vector2D<T>::rotateRad( float angle ) {
    float a = angle;
    float xrot = x*cos(a) - y*sin(a);
    y = x*sin(a) + y*cos(a);
    x = xrot;
    return *this;
}

template <class T>
inline float vector2D<T>::distance( const vector2D<T>& pnt) const {
    float vx = x-pnt.x;
    float vy = y-pnt.y;
    return (float)sqrt(vx*vx + vy*vy);
}

template <class T>
inline float vector2D<T>::squareDistance( const vector2D<T>& pnt ) const {
    float vx = x-pnt.x;
    float vy = y-pnt.y;
    return vx*vx + vy*vy;
}

// Normalization:
template <class T>
inline vector2D<T> vector2D<T>::getNormalized() const {
    float length = (float)sqrt(x*x + y*y);
    if( length > 0 ) {
        return vector2D<T>( x/length, y/length );
    } else {
        return vector2D<T>();
    }
}

template <class T>
inline vector2D<T>& vector2D<T>::normalize() {
    float length = (float)sqrt(x*x + y*y);
    if( length > 0 ) {
        x /= length;
        y /= length;
    }
    return *this;
}

template <class T>
inline vector2D<T> vector2D<T>::getPerpendicularNormed(int orientation) const {
    float length = (float)sqrt( x*x + y*y );
    if( length > 0 )
        return vector2D<T>( -orientation*(y/length), orientation*x/length );
    else
        return vector2D<T>(0.0, 0.0); // something very small (will be used to compute a force)
}

template <class T>
inline vector2D<T>& vector2D<T>::perpendicular(int orientation) {
    float length = (float)sqrt( x*x + y*y );
    if( length > 0 ) {
        float _x = x;
        x = -(y/length)*orientation;
        y = _x/length*orientation;
    }
    return *this;
}

// Length (norm of vector2D<T>):
template <class T>
inline float vector2D<T>::length() const {
    return (float)sqrt( x*x + y*y );
}

template <class T>
inline float vector2D<T>::squareLength() const {
    return (float)(x*x + y*y);
}

// Angle between two vector2Ds:
template <class T>
inline float vector2D<T>::angleDeg( const vector2D<T>& vec ) const {
    return (float)(atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y )*RAD_TO_DEG); 
}

template <class T>
inline float vector2D<T>::angleRad( const vector2D<T>& vec ) const {
    return atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y );
}

template <class T>
inline float vector2D<T>::angleDegHoriz( ) const {
    return (float)(atan2( y, x )*RAD_TO_DEG+180); // by adding 180, we get values between 0 and 360 (CCW, with 0 on the left quadrant)
}

//Dot Product:
template <class T>
inline float vector2D<T>::dot( const vector2D<T>& vec ) const {
    return x*vec.x + y*vec.y;
}

// ============================================  3D vectors ============================================

template <class T>
inline vector3D<T>::vector3D( T _x, T _y ,T _z ) {
    x = _x; y = _y; z = _z;
}

template <class T>
inline void vector3D<T>::set( T _x, T _y ,T _z ) {
    x = _x; y = _y; z = _z;
}

template <class T>
inline void vector3D<T>::set( const vector3D<T>& vec ) {
    x=vec.x; y=vec.y; z = vec.z;
}

template <class T>
inline bool vector3D<T>::operator==( const vector3D<T>& vec ) {
    return (x == vec.x) && (y == vec.y)&& (z == vec.z);
}

template <class T>
inline bool vector3D<T>::operator!=( const vector3D<T>& vec ) {
    return (x != vec.x) || (y != vec.y)|| (z != vec.z);
}

template <class T>
inline bool vector3D<T>::match( const vector3D<T>& vec, float tolerance ) {
    return (abs(x - vec.x) < tolerance)&& (abs(y - vec.y) < tolerance)&& (abs(z - vec.z) < tolerance);
}


/*
inline vector3D & operator=( const vector3D& vec ){ // returning a reference to the vector3D object for allowing operator chaining
    x = vec.x;
    y = vec.y;
    return *this;
}
 */

template <class T>
inline void vector3D<T>::operator=( const vector3D<T>& vec ){
    x = vec.x;  y = vec.y; z = vec.z;
}

template <class T>
inline vector3D<T> vector3D<T>::operator+( const vector3D<T>& vec ) const {
    return vector3D<T>( x+vec.x, y+vec.y, z+vec.z);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator+=( const vector3D<T>& vec ) {
    x += vec.x;
    y += vec.y;
    z += vec.z;
    return *this;
}

template <class T>
inline vector3D<T> vector3D<T>::operator-( const vector3D<T>& vec ) const {  
    return vector3D<T>(x-vec.x, y-vec.y, z-vec.z);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator-=( const vector3D<T>& vec ) {
    x -= vec.x;
    y -= vec.y;
    z -= vec.z;
    return *this;
}

template <class T>
inline vector3D<T> vector3D<T>::operator*( const vector3D<T>& vec ) const {
    return vector3D<T>(x*vec.x, y*vec.y, z*vec.z);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator*=( const vector3D<T>& vec ) {
    x*=vec.x;
    y*=vec.y;
    z*=vec.z;
    return *this;
}

//operator overloading for float:
/*
inline vector3D<T> & operator=( const float& val ){
    x = val;
    y = val;
    return *this;
}
 */

template <class T>
inline void vector3D<T>::operator=( const T f){
    x = f;  y = f;  z = f;
}

template <class T>
inline vector3D<T> vector3D<T>::operator+( const T f ) const {
    return vector3D<T>( x+f, y+f, z+f);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator+=( const T f ) {
    x += f; y += f; z+=f;
    return *this;
}

template <class T>
inline vector3D<T> vector3D<T>::operator-( const T f ) const {
    return vector3D<T>( x-f, y-f, z-f);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator-=( const T f ) {
    x -= f; y -= f; z-=f;
    return *this;
}

template <class T>
inline vector3D<T> vector3D<T>::operator-() const {
    return vector3D<T>(-x, -y, -z);
}

template <class T>
inline vector3D<T> vector3D<T>::operator*( const T f ) const {
    return vector3D<T>(x*f, y*f, z*f);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator*=( const T f ) {
    x*=f; y*=f; z*=f;
    return *this;
}

template <class T>
inline vector3D<T> vector3D<T>::operator/( const T f ) const {
    //cout << "here" << endl;
    if(f == 0) return vector3D<T>(x, y);
    return vector3D<T>(x/f, y/f, z/f);
}

template <class T>
inline vector3D<T>& vector3D<T>::operator/=( const T f ) {
    if(f == 0) return *this;
    x/=f; y/=f; z/=f;
    return *this;
}

template <class T>
inline vector3D<T> vector3D<T>::getScaled( const float length ) const {
    float l = (float)sqrt(x*x + y*y + z*z);
    if( l > 0 )
        return vector3D<T>( (x/l)*length, (y/l)*length , (z/l)*length);
    else
        return vector3D<T>();
}

template <class T>
inline vector3D<T>& vector3D<T>::scale( const float length ) {
    float l = (float)sqrt(x*x + y*y + z*z);
    if (l > 0) {
        x = (x/l)*length;
        y = (y/l)*length;
        z = (z/l)*length;
    }
    return *this;
}


template <class T>
inline float vector3D<T>::distance( const vector3D<T>& pnt) const {
    float vx = x-pnt.x;
    float vy = y-pnt.y;
    float vz = z-pnt.z;
    return (float)sqrt(vx*vx + vy*vy + vz*vz);
}

template <class T>
inline float vector3D<T>::squareDistance( const vector3D<T>& pnt ) const {
    float vx = x-pnt.x;
    float vy = y-pnt.y;
    float vz = z-pnt.z;
    return vx*vx + vy*vy+ vz*vz;
}

// Normalization:
template <class T>
inline vector3D<T> vector3D<T>::getNormalized() const {
    float length = (float)sqrt(x*x + y*y + z*z);
    if( length > 0 ) {
        return vector3D<T>( x/length, y/length , z/length);
    } else {
        return vector3D<T>();
    }
}

template <class T>
inline vector3D<T>& vector3D<T>::normalize() {
    float length = (float)sqrt(x*x + y*y + z*z);
    if( length > 0 ) {
        x /= length;
        y /= length;
        z /= length;
    }
    return *this;
}

// Length (norm of vector3D<T>):
template <class T>
inline float vector3D<T>::length() const {
    return (float)sqrt( x*x + y*y + z*z);
}

template <class T>
inline float vector3D<T>::squareLength() const {
    return (float)(x*x + y*y+ z*z);
}

// Angle between two vector3Ds:
template <class T>
inline float vector3D<T>::angleDeg( const vector3D<T>& vec ) const {
//atan2(norm(cross(a,b)), dot(a,b))
    return (float)(atan2( (this->cross(vec)).length(),this->dot(vec))*RAD_TO_DEG); 
}

template <class T>
inline float vector3D<T>::angleRad( const vector3D<T>& vec ) const {
    return atan2((this->cross(vec)).length(),this->dot(vec));
}

//Dot Product:
template <class T>
inline float vector3D<T>::dot( const vector3D<T>& vec ) const {
    return x*vec.x + y*vec.y +  z*vec.z;
}

// Cross product: 
template <class T>
inline vector3D<T> vector3D<T>::cross( const vector3D<T>& vec ) const {
    return vector3D<T>( y*vec.z - vec.y*z, -x*vec.z + vec.x * z, x.vec.y - vec.x* y);
}

template <class T>
inline vector3D<T>& vector3D<T>::cross( const vector3D<T>& vec ) {
    x = y*vec.z - vec.y*z;
    y = -x*vec.z + vec.x * z;
    z = x.vec.y - vec.x* y;
    return *this;
}

// ======================================================================================

//handy typedefs:
typedef vector2D<short> vector2Dd;
typedef vector2D<float> vector2Df;
typedef vector3D<short> vector3Dd;
typedef vector3D<float> vector3Df;



// ================= other auxiliary structures ============
struct Box3d {
    float minX, maxX; 
    float minY, maxY; 
    float minZ, maxZ; 
};

#endif