save loops

Dependencies:   mbed

Revision:
0:df6fdd9b99f0
diff -r 000000000000 -r df6fdd9b99f0 myVectorClass.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/myVectorClass.h	Tue Dec 02 04:39:15 2014 +0000
@@ -0,0 +1,431 @@
+// class vector2D (for the time being, only 2d):
+
+#include "mbed.h"
+
+#ifndef vector2D_H
+#define vector2D_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
+
+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;
+        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"
+    
+};
+
+/////////////////
+// Implementation
+/////////////////
+
+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;
+}
+
+
+//handy typedefs:
+typedef vector2D<short> vector2Dd;
+typedef vector2D<float> vector2Df;
+
+
+#endif