Alvaro Cassinelli / Mbed 2 deprecated laserUI

Laser Sensing Display for UI interfaces in the real world

Dependencies:   mbed

Fork of skinGames_forktest by Alvaro Cassinelli

myVectorClass.h@18:d72935b13858, 2012-04-28 (annotated)

Committer:
mbedalvaro
Date:
Sat Apr 28 12:35:21 2012 +0000
Revision:
18:d72935b13858
Parent:
12:0de9cd2bced5
Child:
30:d8af03f01cd4
this works pretty nicely. I will change some parameters on the elastic loop, as well as compute the KINETIC ENERGY

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 0:345b3bc7a0ea 1 // class vector2D (for the time being, only 2d):
mbedalvaro 0:345b3bc7a0ea 2
mbedalvaro 0:345b3bc7a0ea 3 #include "mbed.h"
mbedalvaro 0:345b3bc7a0ea 4
mbedalvaro 0:345b3bc7a0ea 5 #ifndef vector2D_H
mbedalvaro 0:345b3bc7a0ea 6 #define vector2D_H
mbedalvaro 0:345b3bc7a0ea 7
mbedalvaro 0:345b3bc7a0ea 8 #ifndef DEG_TO_RAD
mbedalvaro 0:345b3bc7a0ea 9 #define DEG_TO_RAD (PI/180.0)
mbedalvaro 0:345b3bc7a0ea 10 #endif
mbedalvaro 0:345b3bc7a0ea 11
mbedalvaro 0:345b3bc7a0ea 12 #ifndef RAD_TO_DEG
mbedalvaro 0:345b3bc7a0ea 13 #define RAD_TO_DEG (180.0/PI)
mbedalvaro 0:345b3bc7a0ea 14 #endif
mbedalvaro 0:345b3bc7a0ea 15
mbedalvaro 0:345b3bc7a0ea 16 #ifndef CW
mbedalvaro 0:345b3bc7a0ea 17 #define CW 1.0
mbedalvaro 0:345b3bc7a0ea 18 #endif
mbedalvaro 0:345b3bc7a0ea 19
mbedalvaro 0:345b3bc7a0ea 20 #ifndef CCW
mbedalvaro 0:345b3bc7a0ea 21 #define CCW -1.0
mbedalvaro 0:345b3bc7a0ea 22 #endif
mbedalvaro 0:345b3bc7a0ea 23
mbedalvaro 0:345b3bc7a0ea 24 #ifndef PI
mbedalvaro 0:345b3bc7a0ea 25 #define PI 3.14159265889
mbedalvaro 0:345b3bc7a0ea 26 #endif
mbedalvaro 0:345b3bc7a0ea 27
mbedalvaro 12:0de9cd2bced5 28 template <class T>
mbedalvaro 0:345b3bc7a0ea 29 class vector2D {
mbedalvaro 0:345b3bc7a0ea 30
mbedalvaro 0:345b3bc7a0ea 31 public:
mbedalvaro 0:345b3bc7a0ea 32
mbedalvaro 0:345b3bc7a0ea 33 // Overloaded constructor with parameters:
mbedalvaro 12:0de9cd2bced5 34 vector2D( T _x=0.0f, T _y=0.0f );
mbedalvaro 0:345b3bc7a0ea 35
mbedalvaro 0:345b3bc7a0ea 36 // Explicit setting:
mbedalvaro 12:0de9cd2bced5 37 void set( T _x, T _y );
mbedalvaro 0:345b3bc7a0ea 38 void set( const vector2D& vec );
mbedalvaro 0:345b3bc7a0ea 39
mbedalvaro 0:345b3bc7a0ea 40 // Comparison:
mbedalvaro 0:345b3bc7a0ea 41 bool operator==( const vector2D& vec );
mbedalvaro 0:345b3bc7a0ea 42 bool operator!=( const vector2D& vec );
mbedalvaro 0:345b3bc7a0ea 43 bool match( const vector2D& vec, float tollerance=0.0001 );
mbedalvaro 0:345b3bc7a0ea 44
mbedalvaro 0:345b3bc7a0ea 45 // Overloaded operators:
mbedalvaro 0:345b3bc7a0ea 46 //
mbedalvaro 0:345b3bc7a0ea 47 void operator=( const vector2D& vec ); // I cannot declare this if we want also operator chaining?
mbedalvaro 0:345b3bc7a0ea 48 //vector2D & operator=( const vector2D& vec ); // this is to enable operator chaining (vec1=vec2=vec3).
mbedalvaro 0:345b3bc7a0ea 49 vector2D operator+( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 50 vector2D& operator+=( const vector2D& vec ); // why it has an output? for doing vec1=vec2+=vec3? YES!!! (operator chaining).
mbedalvaro 0:345b3bc7a0ea 51 vector2D operator-( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 52 vector2D& operator-=( const vector2D& vec );
mbedalvaro 0:345b3bc7a0ea 53 vector2D operator*( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 54 vector2D& operator*=( const vector2D& vec );
mbedalvaro 0:345b3bc7a0ea 55 vector2D operator/( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 56 vector2D& operator/=( const vector2D& vec );
mbedalvaro 0:345b3bc7a0ea 57
mbedalvaro 0:345b3bc7a0ea 58 //operator overloading for float:
mbedalvaro 12:0de9cd2bced5 59 void operator=( const T f); // I cannot declare this if we want also operator chaining?
mbedalvaro 0:345b3bc7a0ea 60 //vector2D & operator=( const float& val ); // to allow operator chaining
mbedalvaro 12:0de9cd2bced5 61 vector2D operator+( const T f ) const;
mbedalvaro 12:0de9cd2bced5 62 vector2D& operator+=( const T f );
mbedalvaro 12:0de9cd2bced5 63 vector2D operator-( const T f ) const;
mbedalvaro 12:0de9cd2bced5 64 vector2D& operator-=( const T f );
mbedalvaro 0:345b3bc7a0ea 65 vector2D operator-() const;
mbedalvaro 12:0de9cd2bced5 66
mbedalvaro 12:0de9cd2bced5 67
mbedalvaro 12:0de9cd2bced5 68 // multiplication by a scalar:
mbedalvaro 12:0de9cd2bced5 69 vector2D operator*( const T f ) const;
mbedalvaro 12:0de9cd2bced5 70 vector2D& operator*=( const T f );
mbedalvaro 12:0de9cd2bced5 71 vector2D operator/( const T f ) const;
mbedalvaro 12:0de9cd2bced5 72 vector2D& operator/=( const T f );
mbedalvaro 0:345b3bc7a0ea 73
mbedalvaro 0:345b3bc7a0ea 74 // Distance (between end points of two vector2Ds):
mbedalvaro 0:345b3bc7a0ea 75 float distance( const vector2D& pnt) const;
mbedalvaro 0:345b3bc7a0ea 76 float squareDistance( const vector2D& pnt ) const;
mbedalvaro 0:345b3bc7a0ea 77
mbedalvaro 0:345b3bc7a0ea 78 // Length of vector2D (norm):
mbedalvaro 0:345b3bc7a0ea 79 float length() const;
mbedalvaro 0:345b3bc7a0ea 80 float squareLength() const; // faster, no sqrt
mbedalvaro 0:345b3bc7a0ea 81
mbedalvaro 0:345b3bc7a0ea 82 // Scaling:
mbedalvaro 0:345b3bc7a0ea 83 vector2D getScaled( const float length ) const;
mbedalvaro 0:345b3bc7a0ea 84 vector2D& scale( const float length );
mbedalvaro 0:345b3bc7a0ea 85
mbedalvaro 0:345b3bc7a0ea 86 // Normalization:
mbedalvaro 0:345b3bc7a0ea 87 vector2D getNormalized() const;
mbedalvaro 0:345b3bc7a0ea 88 vector2D& normalize();
mbedalvaro 0:345b3bc7a0ea 89
mbedalvaro 0:345b3bc7a0ea 90 // Perpendicular normalized vector2D.
mbedalvaro 0:345b3bc7a0ea 91 vector2D getPerpendicularNormed(int orientation) const;
mbedalvaro 0:345b3bc7a0ea 92 vector2D& perpendicular(int orientation);
mbedalvaro 0:345b3bc7a0ea 93
mbedalvaro 0:345b3bc7a0ea 94 // Rotation
mbedalvaro 0:345b3bc7a0ea 95 vector2D getRotatedDeg( float angle ) const;
mbedalvaro 0:345b3bc7a0ea 96 vector2D getRotatedRad( float angle ) const;
mbedalvaro 0:345b3bc7a0ea 97 vector2D& rotateDeg( float angle );
mbedalvaro 0:345b3bc7a0ea 98 vector2D& rotateRad( float angle );
mbedalvaro 0:345b3bc7a0ea 99
mbedalvaro 0:345b3bc7a0ea 100 //vector2D product (for 3d vector2Ds - for 2d vector2Ds, something like this is just the "angle" between them):
mbedalvaro 0:345b3bc7a0ea 101 //vector2D getvector2DProduct(const vector2D& vec) const;
mbedalvaro 0:345b3bc7a0ea 102 //vector2D& vector2DProduct(const vector2D& vec) const;
mbedalvaro 0:345b3bc7a0ea 103
mbedalvaro 0:345b3bc7a0ea 104 //Angle (deg) between two vector2Ds (using atan2, so between -180 and 180)
mbedalvaro 0:345b3bc7a0ea 105 float angleDeg( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 106 float angleRad( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 107 float angleDegHoriz( ) const; // particular case when the second vector is just (1,0)
mbedalvaro 0:345b3bc7a0ea 108
mbedalvaro 0:345b3bc7a0ea 109 //Dot Product:
mbedalvaro 0:345b3bc7a0ea 110 float dot( const vector2D& vec ) const;
mbedalvaro 0:345b3bc7a0ea 111
mbedalvaro 0:345b3bc7a0ea 112 // =================================================================
mbedalvaro 0:345b3bc7a0ea 113
mbedalvaro 0:345b3bc7a0ea 114 // Actual variables:
mbedalvaro 12:0de9cd2bced5 115 T x, y; // or make a class "point"
mbedalvaro 0:345b3bc7a0ea 116
mbedalvaro 0:345b3bc7a0ea 117 };
mbedalvaro 0:345b3bc7a0ea 118
mbedalvaro 0:345b3bc7a0ea 119 /////////////////
mbedalvaro 0:345b3bc7a0ea 120 // Implementation
mbedalvaro 0:345b3bc7a0ea 121 /////////////////
mbedalvaro 0:345b3bc7a0ea 122
mbedalvaro 12:0de9cd2bced5 123 template <class T>
mbedalvaro 12:0de9cd2bced5 124 inline vector2D<T>::vector2D( T _x, T _y ) {
mbedalvaro 12:0de9cd2bced5 125 x = _x; y = _y;
mbedalvaro 0:345b3bc7a0ea 126 }
mbedalvaro 0:345b3bc7a0ea 127
mbedalvaro 12:0de9cd2bced5 128 template <class T>
mbedalvaro 12:0de9cd2bced5 129 inline void vector2D<T>::set( T _x, T _y ) {
mbedalvaro 12:0de9cd2bced5 130 x = _x; y = _y;
mbedalvaro 0:345b3bc7a0ea 131 }
mbedalvaro 0:345b3bc7a0ea 132
mbedalvaro 12:0de9cd2bced5 133 template <class T>
mbedalvaro 12:0de9cd2bced5 134 inline void vector2D<T>::set( const vector2D<T>& vec ) {
mbedalvaro 12:0de9cd2bced5 135 x=vec.x; y=vec.y;
mbedalvaro 0:345b3bc7a0ea 136 }
mbedalvaro 0:345b3bc7a0ea 137
mbedalvaro 12:0de9cd2bced5 138 template <class T>
mbedalvaro 12:0de9cd2bced5 139 inline bool vector2D<T>::operator==( const vector2D<T>& vec ) {
mbedalvaro 0:345b3bc7a0ea 140 return (x == vec.x) && (y == vec.y);
mbedalvaro 0:345b3bc7a0ea 141 }
mbedalvaro 0:345b3bc7a0ea 142
mbedalvaro 12:0de9cd2bced5 143 template <class T>
mbedalvaro 12:0de9cd2bced5 144 inline bool vector2D<T>::operator!=( const vector2D<T>& vec ) {
mbedalvaro 0:345b3bc7a0ea 145 return (x != vec.x) || (y != vec.y);
mbedalvaro 0:345b3bc7a0ea 146 }
mbedalvaro 0:345b3bc7a0ea 147
mbedalvaro 12:0de9cd2bced5 148 template <class T>
mbedalvaro 12:0de9cd2bced5 149 inline bool vector2D<T>::match( const vector2D<T>& vec, float tolerance ) {
mbedalvaro 12:0de9cd2bced5 150 return (abs(x - vec.x) < tolerance)&& (abs(y - vec.y) < tolerance);
mbedalvaro 0:345b3bc7a0ea 151 }
mbedalvaro 0:345b3bc7a0ea 152
mbedalvaro 0:345b3bc7a0ea 153
mbedalvaro 0:345b3bc7a0ea 154 /*
mbedalvaro 0:345b3bc7a0ea 155 inline vector2D & operator=( const vector2D& vec ){ // returning a reference to the vector2D object for allowing operator chaining
mbedalvaro 0:345b3bc7a0ea 156 x = vec.x;
mbedalvaro 0:345b3bc7a0ea 157 y = vec.y;
mbedalvaro 0:345b3bc7a0ea 158 return *this;
mbedalvaro 0:345b3bc7a0ea 159 }
mbedalvaro 0:345b3bc7a0ea 160 */
mbedalvaro 0:345b3bc7a0ea 161
mbedalvaro 12:0de9cd2bced5 162 template <class T>
mbedalvaro 12:0de9cd2bced5 163 inline void vector2D<T>::operator=( const vector2D<T>& vec ){
mbedalvaro 12:0de9cd2bced5 164 x = vec.x; y = vec.y;
mbedalvaro 0:345b3bc7a0ea 165 }
mbedalvaro 0:345b3bc7a0ea 166
mbedalvaro 12:0de9cd2bced5 167 template <class T>
mbedalvaro 12:0de9cd2bced5 168 inline vector2D<T> vector2D<T>::operator+( const vector2D<T>& vec ) const {
mbedalvaro 12:0de9cd2bced5 169 return vector2D<T>( x+vec.x, y+vec.y);
mbedalvaro 0:345b3bc7a0ea 170 }
mbedalvaro 0:345b3bc7a0ea 171
mbedalvaro 12:0de9cd2bced5 172 template <class T>
mbedalvaro 12:0de9cd2bced5 173 inline vector2D<T>& vector2D<T>::operator+=( const vector2D<T>& vec ) {
mbedalvaro 0:345b3bc7a0ea 174 x += vec.x;
mbedalvaro 0:345b3bc7a0ea 175 y += vec.y;
mbedalvaro 0:345b3bc7a0ea 176 return *this;
mbedalvaro 0:345b3bc7a0ea 177 }
mbedalvaro 0:345b3bc7a0ea 178
mbedalvaro 12:0de9cd2bced5 179 template <class T>
mbedalvaro 12:0de9cd2bced5 180 inline vector2D<T> vector2D<T>::operator-( const vector2D<T>& vec ) const {
mbedalvaro 12:0de9cd2bced5 181 return vector2D<T>(x-vec.x, y-vec.y);
mbedalvaro 0:345b3bc7a0ea 182 }
mbedalvaro 0:345b3bc7a0ea 183
mbedalvaro 12:0de9cd2bced5 184 template <class T>
mbedalvaro 12:0de9cd2bced5 185 inline vector2D<T>& vector2D<T>::operator-=( const vector2D<T>& vec ) {
mbedalvaro 0:345b3bc7a0ea 186 x -= vec.x;
mbedalvaro 0:345b3bc7a0ea 187 y -= vec.y;
mbedalvaro 0:345b3bc7a0ea 188 return *this;
mbedalvaro 0:345b3bc7a0ea 189 }
mbedalvaro 0:345b3bc7a0ea 190
mbedalvaro 12:0de9cd2bced5 191 template <class T>
mbedalvaro 12:0de9cd2bced5 192 inline vector2D<T> vector2D<T>::operator*( const vector2D<T>& vec ) const {
mbedalvaro 12:0de9cd2bced5 193 return vector2D<T>(x*vec.x, y*vec.y);
mbedalvaro 0:345b3bc7a0ea 194 }
mbedalvaro 0:345b3bc7a0ea 195
mbedalvaro 12:0de9cd2bced5 196 template <class T>
mbedalvaro 12:0de9cd2bced5 197 inline vector2D<T>& vector2D<T>::operator*=( const vector2D<T>& vec ) {
mbedalvaro 0:345b3bc7a0ea 198 x*=vec.x;
mbedalvaro 0:345b3bc7a0ea 199 y*=vec.y;
mbedalvaro 0:345b3bc7a0ea 200 return *this;
mbedalvaro 0:345b3bc7a0ea 201 }
mbedalvaro 0:345b3bc7a0ea 202
mbedalvaro 12:0de9cd2bced5 203 template <class T>
mbedalvaro 12:0de9cd2bced5 204 inline vector2D<T> vector2D<T>::operator/( const vector2D<T>& vec ) const {
mbedalvaro 12:0de9cd2bced5 205 return vector2D<T>( vec.x!=0 ? x/vec.x : x , vec.y!=0 ? y/vec.y : y);
mbedalvaro 0:345b3bc7a0ea 206 }
mbedalvaro 0:345b3bc7a0ea 207
mbedalvaro 12:0de9cd2bced5 208 template <class T>
mbedalvaro 12:0de9cd2bced5 209 inline vector2D<T>& vector2D<T>::operator/=( const vector2D<T>& vec ) {
mbedalvaro 0:345b3bc7a0ea 210 vec.x!=0 ? x/=vec.x : x;
mbedalvaro 0:345b3bc7a0ea 211 vec.y!=0 ? y/=vec.y : y;
mbedalvaro 0:345b3bc7a0ea 212 return *this;
mbedalvaro 0:345b3bc7a0ea 213 }
mbedalvaro 0:345b3bc7a0ea 214
mbedalvaro 0:345b3bc7a0ea 215 //operator overloading for float:
mbedalvaro 0:345b3bc7a0ea 216 /*
mbedalvaro 12:0de9cd2bced5 217 inline vector2D<T> & operator=( const float& val ){
mbedalvaro 0:345b3bc7a0ea 218 x = val;
mbedalvaro 0:345b3bc7a0ea 219 y = val;
mbedalvaro 0:345b3bc7a0ea 220 return *this;
mbedalvaro 0:345b3bc7a0ea 221 }
mbedalvaro 0:345b3bc7a0ea 222 */
mbedalvaro 0:345b3bc7a0ea 223
mbedalvaro 12:0de9cd2bced5 224 template <class T>
mbedalvaro 12:0de9cd2bced5 225 inline void vector2D<T>::operator=( const T f){
mbedalvaro 12:0de9cd2bced5 226 x = f; y = f;
mbedalvaro 0:345b3bc7a0ea 227 }
mbedalvaro 0:345b3bc7a0ea 228
mbedalvaro 12:0de9cd2bced5 229 template <class T>
mbedalvaro 12:0de9cd2bced5 230 inline vector2D<T> vector2D<T>::operator+( const T f ) const {
mbedalvaro 12:0de9cd2bced5 231 return vector2D<T>( x+f, y+f);
mbedalvaro 0:345b3bc7a0ea 232 }
mbedalvaro 0:345b3bc7a0ea 233
mbedalvaro 12:0de9cd2bced5 234 template <class T>
mbedalvaro 12:0de9cd2bced5 235 inline vector2D<T>& vector2D<T>::operator+=( const T f ) {
mbedalvaro 12:0de9cd2bced5 236 x += f; y += f;
mbedalvaro 0:345b3bc7a0ea 237 return *this;
mbedalvaro 0:345b3bc7a0ea 238 }
mbedalvaro 0:345b3bc7a0ea 239
mbedalvaro 12:0de9cd2bced5 240 template <class T>
mbedalvaro 12:0de9cd2bced5 241 inline vector2D<T> vector2D<T>::operator-( const T f ) const {
mbedalvaro 12:0de9cd2bced5 242 return vector2D<T>( x-f, y-f);
mbedalvaro 0:345b3bc7a0ea 243 }
mbedalvaro 0:345b3bc7a0ea 244
mbedalvaro 12:0de9cd2bced5 245 template <class T>
mbedalvaro 12:0de9cd2bced5 246 inline vector2D<T>& vector2D<T>::operator-=( const T f ) {
mbedalvaro 12:0de9cd2bced5 247 x -= f; y -= f;
mbedalvaro 0:345b3bc7a0ea 248 return *this;
mbedalvaro 0:345b3bc7a0ea 249 }
mbedalvaro 0:345b3bc7a0ea 250
mbedalvaro 12:0de9cd2bced5 251 template <class T>
mbedalvaro 12:0de9cd2bced5 252 inline vector2D<T> vector2D<T>::operator-() const {
mbedalvaro 12:0de9cd2bced5 253 return vector2D<T>(-x, -y);
mbedalvaro 0:345b3bc7a0ea 254 }
mbedalvaro 0:345b3bc7a0ea 255
mbedalvaro 12:0de9cd2bced5 256 template <class T>
mbedalvaro 12:0de9cd2bced5 257 inline vector2D<T> vector2D<T>::operator*( const T f ) const {
mbedalvaro 12:0de9cd2bced5 258 return vector2D<T>(x*f, y*f);
mbedalvaro 0:345b3bc7a0ea 259 }
mbedalvaro 0:345b3bc7a0ea 260
mbedalvaro 12:0de9cd2bced5 261 template <class T>
mbedalvaro 12:0de9cd2bced5 262 inline vector2D<T>& vector2D<T>::operator*=( const T f ) {
mbedalvaro 12:0de9cd2bced5 263 x*=f; y*=f;
mbedalvaro 0:345b3bc7a0ea 264 return *this;
mbedalvaro 0:345b3bc7a0ea 265 }
mbedalvaro 0:345b3bc7a0ea 266
mbedalvaro 12:0de9cd2bced5 267 template <class T>
mbedalvaro 12:0de9cd2bced5 268 inline vector2D<T> vector2D<T>::operator/( const T f ) const {
mbedalvaro 0:345b3bc7a0ea 269 //cout << "here" << endl;
mbedalvaro 12:0de9cd2bced5 270 if(f == 0) return vector2D<T>(x, y);
mbedalvaro 12:0de9cd2bced5 271 return vector2D<T>(x/f, y/f);
mbedalvaro 0:345b3bc7a0ea 272 }
mbedalvaro 0:345b3bc7a0ea 273
mbedalvaro 12:0de9cd2bced5 274 template <class T>
mbedalvaro 12:0de9cd2bced5 275 inline vector2D<T>& vector2D<T>::operator/=( const T f ) {
mbedalvaro 0:345b3bc7a0ea 276 if(f == 0) return *this;
mbedalvaro 12:0de9cd2bced5 277 x/=f; y/=f;
mbedalvaro 0:345b3bc7a0ea 278 return *this;
mbedalvaro 0:345b3bc7a0ea 279 }
mbedalvaro 0:345b3bc7a0ea 280
mbedalvaro 12:0de9cd2bced5 281 template <class T>
mbedalvaro 12:0de9cd2bced5 282 inline vector2D<T> vector2D<T>::getScaled( const float length ) const {
mbedalvaro 0:345b3bc7a0ea 283 float l = (float)sqrt(x*x + y*y);
mbedalvaro 0:345b3bc7a0ea 284 if( l > 0 )
mbedalvaro 12:0de9cd2bced5 285 return vector2D<T>( (x/l)*length, (y/l)*length );
mbedalvaro 0:345b3bc7a0ea 286 else
mbedalvaro 12:0de9cd2bced5 287 return vector2D<T>();
mbedalvaro 0:345b3bc7a0ea 288 }
mbedalvaro 0:345b3bc7a0ea 289
mbedalvaro 12:0de9cd2bced5 290 template <class T>
mbedalvaro 12:0de9cd2bced5 291 inline vector2D<T>& vector2D<T>::scale( const float length ) {
mbedalvaro 0:345b3bc7a0ea 292 float l = (float)sqrt(x*x + y*y);
mbedalvaro 0:345b3bc7a0ea 293 if (l > 0) {
mbedalvaro 0:345b3bc7a0ea 294 x = (x/l)*length;
mbedalvaro 0:345b3bc7a0ea 295 y = (y/l)*length;
mbedalvaro 0:345b3bc7a0ea 296 }
mbedalvaro 0:345b3bc7a0ea 297 return *this;
mbedalvaro 0:345b3bc7a0ea 298 }
mbedalvaro 0:345b3bc7a0ea 299
mbedalvaro 0:345b3bc7a0ea 300 // Rotation
mbedalvaro 0:345b3bc7a0ea 301 //
mbedalvaro 0:345b3bc7a0ea 302 //
mbedalvaro 0:345b3bc7a0ea 303
mbedalvaro 12:0de9cd2bced5 304 template <class T>
mbedalvaro 12:0de9cd2bced5 305 inline vector2D<T> vector2D<T>::getRotatedDeg( float angle ) const {
mbedalvaro 0:345b3bc7a0ea 306 float a = (float)(angle*DEG_TO_RAD);
mbedalvaro 12:0de9cd2bced5 307 return vector2D<T>( x*cos(a) - y*sin(a),
mbedalvaro 0:345b3bc7a0ea 308 x*sin(a) + y*cos(a) );
mbedalvaro 0:345b3bc7a0ea 309 }
mbedalvaro 0:345b3bc7a0ea 310
mbedalvaro 12:0de9cd2bced5 311 template <class T>
mbedalvaro 12:0de9cd2bced5 312 inline vector2D<T> vector2D<T>::getRotatedRad( float angle ) const {
mbedalvaro 0:345b3bc7a0ea 313 float a = angle;
mbedalvaro 12:0de9cd2bced5 314 return vector2D<T>( x*cos(a) - y*sin(a),
mbedalvaro 0:345b3bc7a0ea 315 x*sin(a) + y*cos(a) );
mbedalvaro 0:345b3bc7a0ea 316 }
mbedalvaro 0:345b3bc7a0ea 317
mbedalvaro 12:0de9cd2bced5 318 template <class T>
mbedalvaro 12:0de9cd2bced5 319 inline vector2D<T>& vector2D<T>::rotateDeg( float angle ) {
mbedalvaro 0:345b3bc7a0ea 320 float a = (float)(angle * DEG_TO_RAD);
mbedalvaro 0:345b3bc7a0ea 321 float xrot = x*cos(a) - y*sin(a);
mbedalvaro 0:345b3bc7a0ea 322 y = x*sin(a) + y*cos(a);
mbedalvaro 0:345b3bc7a0ea 323 x = xrot;
mbedalvaro 0:345b3bc7a0ea 324 return *this;
mbedalvaro 0:345b3bc7a0ea 325 }
mbedalvaro 0:345b3bc7a0ea 326
mbedalvaro 12:0de9cd2bced5 327 template <class T>
mbedalvaro 12:0de9cd2bced5 328 inline vector2D<T>& vector2D<T>::rotateRad( float angle ) {
mbedalvaro 0:345b3bc7a0ea 329 float a = angle;
mbedalvaro 0:345b3bc7a0ea 330 float xrot = x*cos(a) - y*sin(a);
mbedalvaro 0:345b3bc7a0ea 331 y = x*sin(a) + y*cos(a);
mbedalvaro 0:345b3bc7a0ea 332 x = xrot;
mbedalvaro 0:345b3bc7a0ea 333 return *this;
mbedalvaro 0:345b3bc7a0ea 334 }
mbedalvaro 0:345b3bc7a0ea 335
mbedalvaro 12:0de9cd2bced5 336 template <class T>
mbedalvaro 12:0de9cd2bced5 337 inline float vector2D<T>::distance( const vector2D<T>& pnt) const {
mbedalvaro 0:345b3bc7a0ea 338 float vx = x-pnt.x;
mbedalvaro 0:345b3bc7a0ea 339 float vy = y-pnt.y;
mbedalvaro 0:345b3bc7a0ea 340 return (float)sqrt(vx*vx + vy*vy);
mbedalvaro 0:345b3bc7a0ea 341 }
mbedalvaro 0:345b3bc7a0ea 342
mbedalvaro 12:0de9cd2bced5 343 template <class T>
mbedalvaro 12:0de9cd2bced5 344 inline float vector2D<T>::squareDistance( const vector2D<T>& pnt ) const {
mbedalvaro 0:345b3bc7a0ea 345 float vx = x-pnt.x;
mbedalvaro 0:345b3bc7a0ea 346 float vy = y-pnt.y;
mbedalvaro 0:345b3bc7a0ea 347 return vx*vx + vy*vy;
mbedalvaro 0:345b3bc7a0ea 348 }
mbedalvaro 0:345b3bc7a0ea 349
mbedalvaro 0:345b3bc7a0ea 350 // Normalization:
mbedalvaro 12:0de9cd2bced5 351 template <class T>
mbedalvaro 12:0de9cd2bced5 352 inline vector2D<T> vector2D<T>::getNormalized() const {
mbedalvaro 0:345b3bc7a0ea 353 float length = (float)sqrt(x*x + y*y);
mbedalvaro 0:345b3bc7a0ea 354 if( length > 0 ) {
mbedalvaro 12:0de9cd2bced5 355 return vector2D<T>( x/length, y/length );
mbedalvaro 0:345b3bc7a0ea 356 } else {
mbedalvaro 12:0de9cd2bced5 357 return vector2D<T>();
mbedalvaro 0:345b3bc7a0ea 358 }
mbedalvaro 0:345b3bc7a0ea 359 }
mbedalvaro 0:345b3bc7a0ea 360
mbedalvaro 12:0de9cd2bced5 361 template <class T>
mbedalvaro 12:0de9cd2bced5 362 inline vector2D<T>& vector2D<T>::normalize() {
mbedalvaro 0:345b3bc7a0ea 363 float length = (float)sqrt(x*x + y*y);
mbedalvaro 0:345b3bc7a0ea 364 if( length > 0 ) {
mbedalvaro 0:345b3bc7a0ea 365 x /= length;
mbedalvaro 0:345b3bc7a0ea 366 y /= length;
mbedalvaro 0:345b3bc7a0ea 367 }
mbedalvaro 0:345b3bc7a0ea 368 return *this;
mbedalvaro 0:345b3bc7a0ea 369 }
mbedalvaro 0:345b3bc7a0ea 370
mbedalvaro 12:0de9cd2bced5 371 template <class T>
mbedalvaro 12:0de9cd2bced5 372 inline vector2D<T> vector2D<T>::getPerpendicularNormed(int orientation) const {
mbedalvaro 0:345b3bc7a0ea 373 float length = (float)sqrt( x*x + y*y );
mbedalvaro 0:345b3bc7a0ea 374 if( length > 0 )
mbedalvaro 12:0de9cd2bced5 375 return vector2D<T>( -orientation*(y/length), orientation*x/length );
mbedalvaro 0:345b3bc7a0ea 376 else
mbedalvaro 12:0de9cd2bced5 377 return vector2D<T>(0.0, 0.0); // something very small (will be used to compute a force)
mbedalvaro 0:345b3bc7a0ea 378 }
mbedalvaro 0:345b3bc7a0ea 379
mbedalvaro 12:0de9cd2bced5 380 template <class T>
mbedalvaro 12:0de9cd2bced5 381 inline vector2D<T>& vector2D<T>::perpendicular(int orientation) {
mbedalvaro 0:345b3bc7a0ea 382 float length = (float)sqrt( x*x + y*y );
mbedalvaro 0:345b3bc7a0ea 383 if( length > 0 ) {
mbedalvaro 0:345b3bc7a0ea 384 float _x = x;
mbedalvaro 0:345b3bc7a0ea 385 x = -(y/length)*orientation;
mbedalvaro 0:345b3bc7a0ea 386 y = _x/length*orientation;
mbedalvaro 0:345b3bc7a0ea 387 }
mbedalvaro 0:345b3bc7a0ea 388 return *this;
mbedalvaro 0:345b3bc7a0ea 389 }
mbedalvaro 0:345b3bc7a0ea 390
mbedalvaro 12:0de9cd2bced5 391 // Length (norm of vector2D<T>):
mbedalvaro 12:0de9cd2bced5 392 template <class T>
mbedalvaro 12:0de9cd2bced5 393 inline float vector2D<T>::length() const {
mbedalvaro 0:345b3bc7a0ea 394 return (float)sqrt( x*x + y*y );
mbedalvaro 0:345b3bc7a0ea 395 }
mbedalvaro 0:345b3bc7a0ea 396
mbedalvaro 12:0de9cd2bced5 397 template <class T>
mbedalvaro 12:0de9cd2bced5 398 inline float vector2D<T>::squareLength() const {
mbedalvaro 0:345b3bc7a0ea 399 return (float)(x*x + y*y);
mbedalvaro 0:345b3bc7a0ea 400 }
mbedalvaro 0:345b3bc7a0ea 401
mbedalvaro 0:345b3bc7a0ea 402 // Angle between two vector2Ds:
mbedalvaro 12:0de9cd2bced5 403 template <class T>
mbedalvaro 12:0de9cd2bced5 404 inline float vector2D<T>::angleDeg( const vector2D<T>& vec ) const {
mbedalvaro 18:d72935b13858 405 return (float)(atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y )*RAD_TO_DEG);
mbedalvaro 0:345b3bc7a0ea 406 }
mbedalvaro 0:345b3bc7a0ea 407
mbedalvaro 12:0de9cd2bced5 408 template <class T>
mbedalvaro 12:0de9cd2bced5 409 inline float vector2D<T>::angleRad( const vector2D<T>& vec ) const {
mbedalvaro 0:345b3bc7a0ea 410 return atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y );
mbedalvaro 0:345b3bc7a0ea 411 }
mbedalvaro 0:345b3bc7a0ea 412
mbedalvaro 12:0de9cd2bced5 413 template <class T>
mbedalvaro 12:0de9cd2bced5 414 inline float vector2D<T>::angleDegHoriz( ) const {
mbedalvaro 18:d72935b13858 415 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)
mbedalvaro 0:345b3bc7a0ea 416 }
mbedalvaro 0:345b3bc7a0ea 417
mbedalvaro 12:0de9cd2bced5 418 //handy typedefs:
mbedalvaro 12:0de9cd2bced5 419 typedef vector2D<short> vector2Dd;
mbedalvaro 12:0de9cd2bced5 420 typedef vector2D<float> vector2Df;
mbedalvaro 12:0de9cd2bced5 421
mbedalvaro 0:345b3bc7a0ea 422
mbedalvaro 0:345b3bc7a0ea 423 #endif