myVectorClass.h

00001 // class vector2D (for the time being, only 2d):
00002 
00003 #include "mbed.h"
00004 
00005 #ifndef vector2D_H
00006 #define vector2D_H
00007 
00008 #ifndef DEG_TO_RAD
00009 #define DEG_TO_RAD (PI/180.0)
00010 #endif
00011 
00012 #ifndef RAD_TO_DEG
00013 #define RAD_TO_DEG (180.0/PI)
00014 #endif
00015 
00016 #ifndef CW
00017 #define CW 1.0
00018 #endif
00019 
00020 #ifndef CCW
00021 #define CCW -1.0
00022 #endif
00023 
00024 #ifndef PI
00025 #define PI 3.14159265889
00026 #endif
00027 
00028 template <class T>
00029 class vector2D {
00030 
00031     public:
00032         
00033         // Overloaded constructor with parameters:
00034         vector2D( T _x=0.0f, T _y=0.0f );
00035         // note: we use the default copy constructor
00036         
00037         // Explicit setting:
00038         void set( T _x, T _y );
00039         void set( const vector2D& vec );
00040         
00041         // Comparison:
00042         bool operator==( const vector2D& vec );
00043         bool operator!=( const vector2D& vec );
00044         bool match( const vector2D& vec, float tollerance=0.0001 );
00045     
00046         // Overloaded operators:
00047         //
00048         void    operator=( const vector2D& vec );    // I cannot declare this if we want also operator chaining?
00049         //vector2D & operator=( const vector2D& vec ); // this is to enable operator chaining (vec1=vec2=vec3). 
00050         vector2D  operator+( const vector2D& vec ) const;
00051         vector2D& operator+=( const vector2D& vec );      // why it has an output? for doing vec1=vec2+=vec3? YES!!! (operator chaining). 
00052         vector2D  operator-( const vector2D& vec ) const;
00053         vector2D& operator-=( const vector2D& vec );
00054         vector2D  operator*( const vector2D& vec ) const;
00055         vector2D& operator*=( const vector2D& vec );
00056         vector2D  operator/( const vector2D& vec ) const;
00057         vector2D& operator/=( const vector2D& vec );
00058     
00059     //operator overloading for float:
00060     void       operator=( const T f);  // I cannot declare this if we want also operator chaining?
00061     //vector2D & operator=( const float& val ); // to allow operator chaining
00062     vector2D  operator+( const T f ) const;
00063     vector2D& operator+=( const T f );
00064     vector2D  operator-( const T f ) const;
00065     vector2D& operator-=( const T f );
00066     vector2D  operator-() const;
00067 
00068     
00069     // multiplication by a scalar:
00070     vector2D  operator*( const T f ) const;
00071     vector2D& operator*=( const T f );
00072     vector2D  operator/( const T f ) const;
00073     vector2D& operator/=( const T f );    
00074     
00075     // Distance (between end points of two vector2Ds):
00076     float distance( const vector2D& pnt) const;
00077     float squareDistance( const vector2D& pnt ) const;
00078     
00079     // Length of vector2D (norm):
00080     float length() const;
00081     float squareLength() const; // faster, no sqrt
00082     
00083     // Scaling:
00084     vector2D  getScaled( const float length ) const;
00085     vector2D& scale( const float length );
00086     
00087     // Normalization:
00088     vector2D  getNormalized() const;
00089     vector2D& normalize();
00090     
00091     // Perpendicular normalized vector2D.
00092     vector2D  getPerpendicularNormed(int orientation) const;
00093     vector2D& perpendicular(int orientation);
00094     
00095     // Rotation
00096     vector2D  getRotatedDeg( float angle ) const;
00097     vector2D  getRotatedRad( float angle ) const;
00098     vector2D& rotateDeg( float angle );
00099     vector2D& rotateRad( float angle );
00100     
00101     //vector2D product (for 3d vector2Ds - for 2d vector2Ds, something like this is just the "angle" between them):
00102     //vector2D getvector2DProduct(const vector2D& vec) const;
00103     //vector2D& vector2DProduct(const vector2D& vec) const;
00104     
00105     //Angle (deg) between two vector2Ds (using atan2, so between -180 and 180)
00106     float angleDeg( const vector2D& vec ) const;
00107     float angleRad( const vector2D& vec ) const;
00108     float angleDegHoriz( ) const; // particular case when the second vector is just (1,0)
00109     
00110     //Dot Product:
00111     float dot( const vector2D& vec ) const;
00112     
00113     // =================================================================
00114     
00115     // Actual variables:
00116     T x, y; // or make a class "point"
00117     
00118 };
00119 
00120 /////////////////
00121 // Implementation
00122 /////////////////
00123 
00124 template <class T>
00125 inline vector2D<T>::vector2D( T _x, T _y ) {
00126     x = _x; y = _y;
00127 }
00128 
00129 template <class T>
00130 inline void vector2D<T>::set( T _x, T _y ) {
00131     x = _x; y = _y;
00132 }
00133 
00134 template <class T>
00135 inline void vector2D<T>::set( const vector2D<T>& vec ) {
00136     x=vec.x; y=vec.y;
00137 }
00138 
00139 template <class T>
00140 inline bool vector2D<T>::operator==( const vector2D<T>& vec ) {
00141     return (x == vec.x) && (y == vec.y);
00142 }
00143 
00144 template <class T>
00145 inline bool vector2D<T>::operator!=( const vector2D<T>& vec ) {
00146     return (x != vec.x) || (y != vec.y);
00147 }
00148 
00149 template <class T>
00150 inline bool vector2D<T>::match( const vector2D<T>& vec, float tolerance ) {
00151     return (abs(x - vec.x) < tolerance)&& (abs(y - vec.y) < tolerance);
00152 }
00153 
00154 
00155 /*
00156 inline vector2D & operator=( const vector2D& vec ){ // returning a reference to the vector2D object for allowing operator chaining
00157     x = vec.x;
00158     y = vec.y;
00159     return *this;
00160 }
00161  */
00162 
00163 template <class T>
00164 inline void vector2D<T>::operator=( const vector2D<T>& vec ){
00165     x = vec.x;  y = vec.y;
00166 }
00167 
00168 template <class T>
00169 inline vector2D<T> vector2D<T>::operator+( const vector2D<T>& vec ) const {
00170     return vector2D<T>( x+vec.x, y+vec.y);
00171 }
00172 
00173 template <class T>
00174 inline vector2D<T>& vector2D<T>::operator+=( const vector2D<T>& vec ) {
00175     x += vec.x;
00176     y += vec.y;
00177     return *this;
00178 }
00179 
00180 template <class T>
00181 inline vector2D<T> vector2D<T>::operator-( const vector2D<T>& vec ) const {  
00182     return vector2D<T>(x-vec.x, y-vec.y);
00183 }
00184 
00185 template <class T>
00186 inline vector2D<T>& vector2D<T>::operator-=( const vector2D<T>& vec ) {
00187     x -= vec.x;
00188     y -= vec.y;
00189     return *this;
00190 }
00191 
00192 template <class T>
00193 inline vector2D<T> vector2D<T>::operator*( const vector2D<T>& vec ) const {
00194     return vector2D<T>(x*vec.x, y*vec.y);
00195 }
00196 
00197 template <class T>
00198 inline vector2D<T>& vector2D<T>::operator*=( const vector2D<T>& vec ) {
00199     x*=vec.x;
00200     y*=vec.y;
00201     return *this;
00202 }
00203 
00204 template <class T>
00205 inline vector2D<T> vector2D<T>::operator/( const vector2D<T>& vec ) const {
00206     return vector2D<T>( vec.x!=0 ? x/vec.x : x , vec.y!=0 ? y/vec.y : y);
00207 }
00208 
00209 template <class T>
00210 inline vector2D<T>& vector2D<T>::operator/=( const vector2D<T>& vec ) {
00211     vec.x!=0 ? x/=vec.x : x;
00212     vec.y!=0 ? y/=vec.y : y;
00213     return *this;
00214 }
00215 
00216 //operator overloading for float:
00217 /*
00218 inline vector2D<T> & operator=( const float& val ){
00219     x = val;
00220     y = val;
00221     return *this;
00222 }
00223  */
00224 
00225 template <class T>
00226 inline void vector2D<T>::operator=( const T f){
00227     x = f;  y = f;
00228 }
00229 
00230 template <class T>
00231 inline vector2D<T> vector2D<T>::operator+( const T f ) const {
00232     return vector2D<T>( x+f, y+f);
00233 }
00234 
00235 template <class T>
00236 inline vector2D<T>& vector2D<T>::operator+=( const T f ) {
00237     x += f; y += f;
00238     return *this;
00239 }
00240 
00241 template <class T>
00242 inline vector2D<T> vector2D<T>::operator-( const T f ) const {
00243     return vector2D<T>( x-f, y-f);
00244 }
00245 
00246 template <class T>
00247 inline vector2D<T>& vector2D<T>::operator-=( const T f ) {
00248     x -= f; y -= f;
00249     return *this;
00250 }
00251 
00252 template <class T>
00253 inline vector2D<T> vector2D<T>::operator-() const {
00254     return vector2D<T>(-x, -y);
00255 }
00256 
00257 template <class T>
00258 inline vector2D<T> vector2D<T>::operator*( const T f ) const {
00259     return vector2D<T>(x*f, y*f);
00260 }
00261 
00262 template <class T>
00263 inline vector2D<T>& vector2D<T>::operator*=( const T f ) {
00264     x*=f; y*=f;
00265     return *this;
00266 }
00267 
00268 template <class T>
00269 inline vector2D<T> vector2D<T>::operator/( const T f ) const {
00270     //cout << "here" << endl;
00271     if(f == 0) return vector2D<T>(x, y);
00272     return vector2D<T>(x/f, y/f);
00273 }
00274 
00275 template <class T>
00276 inline vector2D<T>& vector2D<T>::operator/=( const T f ) {
00277     if(f == 0) return *this;
00278     x/=f; y/=f;
00279     return *this;
00280 }
00281 
00282 template <class T>
00283 inline vector2D<T> vector2D<T>::getScaled( const float length ) const {
00284     float l = (float)sqrt(x*x + y*y);
00285     if( l > 0 )
00286         return vector2D<T>( (x/l)*length, (y/l)*length );
00287     else
00288         return vector2D<T>();
00289 }
00290 
00291 template <class T>
00292 inline vector2D<T>& vector2D<T>::scale( const float length ) {
00293     float l = (float)sqrt(x*x + y*y);
00294     if (l > 0) {
00295         x = (x/l)*length;
00296         y = (y/l)*length;
00297     }
00298     return *this;
00299 }
00300 
00301 // Rotation
00302 //
00303 //
00304 
00305 template <class T>
00306 inline vector2D<T> vector2D<T>::getRotatedDeg( float angle ) const {
00307     float a = (float)(angle*DEG_TO_RAD);
00308     return vector2D<T>( x*cos(a) - y*sin(a),
00309                     x*sin(a) + y*cos(a) );
00310 }
00311 
00312 template <class T> 
00313 inline vector2D<T> vector2D<T>::getRotatedRad( float angle ) const {
00314     float a = angle;
00315     return vector2D<T>( x*cos(a) - y*sin(a),
00316                     x*sin(a) + y*cos(a) );
00317 }
00318 
00319 template <class T>
00320 inline vector2D<T>& vector2D<T>::rotateDeg( float angle ) {
00321     float a = (float)(angle * DEG_TO_RAD);
00322     float xrot = x*cos(a) - y*sin(a);
00323     y = x*sin(a) + y*cos(a);
00324     x = xrot;
00325     return *this;
00326 }
00327 
00328 template <class T>
00329 inline vector2D<T>& vector2D<T>::rotateRad( float angle ) {
00330     float a = angle;
00331     float xrot = x*cos(a) - y*sin(a);
00332     y = x*sin(a) + y*cos(a);
00333     x = xrot;
00334     return *this;
00335 }
00336 
00337 template <class T>
00338 inline float vector2D<T>::distance( const vector2D<T>& pnt) const {
00339     float vx = x-pnt.x;
00340     float vy = y-pnt.y;
00341     return (float)sqrt(vx*vx + vy*vy);
00342 }
00343 
00344 template <class T>
00345 inline float vector2D<T>::squareDistance( const vector2D<T>& pnt ) const {
00346     float vx = x-pnt.x;
00347     float vy = y-pnt.y;
00348     return vx*vx + vy*vy;
00349 }
00350 
00351 // Normalization:
00352 template <class T>
00353 inline vector2D<T> vector2D<T>::getNormalized() const {
00354     float length = (float)sqrt(x*x + y*y);
00355     if( length > 0 ) {
00356         return vector2D<T>( x/length, y/length );
00357     } else {
00358         return vector2D<T>();
00359     }
00360 }
00361 
00362 template <class T>
00363 inline vector2D<T>& vector2D<T>::normalize() {
00364     float length = (float)sqrt(x*x + y*y);
00365     if( length > 0 ) {
00366         x /= length;
00367         y /= length;
00368     }
00369     return *this;
00370 }
00371 
00372 template <class T>
00373 inline vector2D<T> vector2D<T>::getPerpendicularNormed(int orientation) const {
00374     float length = (float)sqrt( x*x + y*y );
00375     if( length > 0 )
00376         return vector2D<T>( -orientation*(y/length), orientation*x/length );
00377     else
00378         return vector2D<T>(0.0, 0.0); // something very small (will be used to compute a force)
00379 }
00380 
00381 template <class T>
00382 inline vector2D<T>& vector2D<T>::perpendicular(int orientation) {
00383     float length = (float)sqrt( x*x + y*y );
00384     if( length > 0 ) {
00385         float _x = x;
00386         x = -(y/length)*orientation;
00387         y = _x/length*orientation;
00388     }
00389     return *this;
00390 }
00391 
00392 // Length (norm of vector2D<T>):
00393 template <class T>
00394 inline float vector2D<T>::length() const {
00395     return (float)sqrt( x*x + y*y );
00396 }
00397 
00398 template <class T>
00399 inline float vector2D<T>::squareLength() const {
00400     return (float)(x*x + y*y);
00401 }
00402 
00403 // Angle between two vector2Ds:
00404 template <class T>
00405 inline float vector2D<T>::angleDeg( const vector2D<T>& vec ) const {
00406     return (float)(atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y )*RAD_TO_DEG); 
00407 }
00408     
00409 template <class T>
00410 inline float vector2D<T>::angleRad( const vector2D<T>& vec ) const {
00411     return atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y );
00412 }
00413 
00414 template <class T>
00415 inline float vector2D<T>::angleDegHoriz( ) const {
00416     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)
00417 }
00418 
00419 //Dot Product:
00420 template <class T>
00421 inline float vector2D<T>::dot( const vector2D<T>& vec ) const {
00422     return x*vec.x + y*vec.y;
00423 }
00424 
00425 
00426 //handy typedefs:
00427 typedef vector2D<short> vector2Dd;
00428 typedef vector2D<float> vector2Df;
00429 
00430 
00431 #endif