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@47:199042980678, 2014-04-17 (annotated)

Committer:
mbedalvaro
Date:
Thu Apr 17 08:04:14 2014 +0000
Revision:
47:199042980678
Parent:
44:2432c218f191 
publishing for sharing with Ken Iwasaki

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 40:3ba2b0ea9f33 1 // classes for 2d and 3d vectors
mbedalvaro 40:3ba2b0ea9f33 2
mbedalvaro 40:3ba2b0ea9f33 3 #ifndef vectors_H
mbedalvaro 40:3ba2b0ea9f33 4 #define vectors_H
mbedalvaro 31:5f039cbddee8 5
mbedalvaro 31:5f039cbddee8 6 #include "mbed.h"
mbedalvaro 31:5f039cbddee8 7
mbedalvaro 31:5f039cbddee8 8 #ifndef DEG_TO_RAD
mbedalvaro 31:5f039cbddee8 9 #define DEG_TO_RAD (PI/180.0)
mbedalvaro 31:5f039cbddee8 10 #endif
mbedalvaro 31:5f039cbddee8 11
mbedalvaro 31:5f039cbddee8 12 #ifndef RAD_TO_DEG
mbedalvaro 31:5f039cbddee8 13 #define RAD_TO_DEG (180.0/PI)
mbedalvaro 31:5f039cbddee8 14 #endif
mbedalvaro 31:5f039cbddee8 15
mbedalvaro 31:5f039cbddee8 16 #ifndef CW
mbedalvaro 31:5f039cbddee8 17 #define CW 1.0
mbedalvaro 31:5f039cbddee8 18 #endif
mbedalvaro 31:5f039cbddee8 19
mbedalvaro 31:5f039cbddee8 20 #ifndef CCW
mbedalvaro 31:5f039cbddee8 21 #define CCW -1.0
mbedalvaro 31:5f039cbddee8 22 #endif
mbedalvaro 31:5f039cbddee8 23
mbedalvaro 31:5f039cbddee8 24 #ifndef PI
mbedalvaro 31:5f039cbddee8 25 #define PI 3.14159265889
mbedalvaro 31:5f039cbddee8 26 #endif
mbedalvaro 31:5f039cbddee8 27
mbedalvaro 40:3ba2b0ea9f33 28 // ================= 2D VECTORS =====================
mbedalvaro 31:5f039cbddee8 29 template <class T>
mbedalvaro 31:5f039cbddee8 30 class vector2D {
mbedalvaro 31:5f039cbddee8 31
mbedalvaro 31:5f039cbddee8 32 public:
mbedalvaro 31:5f039cbddee8 33
mbedalvaro 31:5f039cbddee8 34 // Overloaded constructor with parameters:
mbedalvaro 31:5f039cbddee8 35 vector2D( T _x=0.0f, T _y=0.0f );
mbedalvaro 31:5f039cbddee8 36 // note: we use the default copy constructor
mbedalvaro 31:5f039cbddee8 37
mbedalvaro 31:5f039cbddee8 38 // Explicit setting:
mbedalvaro 31:5f039cbddee8 39 void set( T _x, T _y );
mbedalvaro 31:5f039cbddee8 40 void set( const vector2D& vec );
mbedalvaro 31:5f039cbddee8 41
mbedalvaro 31:5f039cbddee8 42 // Comparison:
mbedalvaro 31:5f039cbddee8 43 bool operator==( const vector2D& vec );
mbedalvaro 31:5f039cbddee8 44 bool operator!=( const vector2D& vec );
mbedalvaro 31:5f039cbddee8 45 bool match( const vector2D& vec, float tollerance=0.0001 );
mbedalvaro 31:5f039cbddee8 46
mbedalvaro 31:5f039cbddee8 47 // Overloaded operators:
mbedalvaro 31:5f039cbddee8 48 //
mbedalvaro 31:5f039cbddee8 49 void operator=( const vector2D& vec ); // I cannot declare this if we want also operator chaining?
mbedalvaro 31:5f039cbddee8 50 //vector2D & operator=( const vector2D& vec ); // this is to enable operator chaining (vec1=vec2=vec3).
mbedalvaro 40:3ba2b0ea9f33 51 vector2D operator+( const vector2D& vec ) const; // note: "const" here means that the member function will not alter any member variable
mbedalvaro 31:5f039cbddee8 52 vector2D& operator+=( const vector2D& vec ); // why it has an output? for doing vec1=vec2+=vec3? YES!!! (operator chaining).
mbedalvaro 31:5f039cbddee8 53 vector2D operator-( const vector2D& vec ) const;
mbedalvaro 31:5f039cbddee8 54 vector2D& operator-=( const vector2D& vec );
mbedalvaro 31:5f039cbddee8 55 vector2D operator*( const vector2D& vec ) const;
mbedalvaro 31:5f039cbddee8 56 vector2D& operator*=( const vector2D& vec );
mbedalvaro 31:5f039cbddee8 57 vector2D operator/( const vector2D& vec ) const;
mbedalvaro 31:5f039cbddee8 58 vector2D& operator/=( const vector2D& vec );
mbedalvaro 31:5f039cbddee8 59
mbedalvaro 31:5f039cbddee8 60 //operator overloading for float:
mbedalvaro 31:5f039cbddee8 61 void operator=( const T f); // I cannot declare this if we want also operator chaining?
mbedalvaro 31:5f039cbddee8 62 //vector2D & operator=( const float& val ); // to allow operator chaining
mbedalvaro 31:5f039cbddee8 63 vector2D operator+( const T f ) const;
mbedalvaro 31:5f039cbddee8 64 vector2D& operator+=( const T f );
mbedalvaro 31:5f039cbddee8 65 vector2D operator-( const T f ) const;
mbedalvaro 31:5f039cbddee8 66 vector2D& operator-=( const T f );
mbedalvaro 31:5f039cbddee8 67 vector2D operator-() const;
mbedalvaro 31:5f039cbddee8 68
mbedalvaro 31:5f039cbddee8 69
mbedalvaro 31:5f039cbddee8 70 // multiplication by a scalar:
mbedalvaro 31:5f039cbddee8 71 vector2D operator*( const T f ) const;
mbedalvaro 31:5f039cbddee8 72 vector2D& operator*=( const T f );
mbedalvaro 31:5f039cbddee8 73 vector2D operator/( const T f ) const;
mbedalvaro 31:5f039cbddee8 74 vector2D& operator/=( const T f );
mbedalvaro 31:5f039cbddee8 75
mbedalvaro 31:5f039cbddee8 76 // Distance (between end points of two vector2Ds):
mbedalvaro 31:5f039cbddee8 77 float distance( const vector2D& pnt) const;
mbedalvaro 31:5f039cbddee8 78 float squareDistance( const vector2D& pnt ) const;
mbedalvaro 31:5f039cbddee8 79
mbedalvaro 31:5f039cbddee8 80 // Length of vector2D (norm):
mbedalvaro 31:5f039cbddee8 81 float length() const;
mbedalvaro 31:5f039cbddee8 82 float squareLength() const; // faster, no sqrt
mbedalvaro 31:5f039cbddee8 83
mbedalvaro 31:5f039cbddee8 84 // Scaling:
mbedalvaro 31:5f039cbddee8 85 vector2D getScaled( const float length ) const;
mbedalvaro 31:5f039cbddee8 86 vector2D& scale( const float length );
mbedalvaro 31:5f039cbddee8 87
mbedalvaro 31:5f039cbddee8 88 // Normalization:
mbedalvaro 31:5f039cbddee8 89 vector2D getNormalized() const;
mbedalvaro 31:5f039cbddee8 90 vector2D& normalize();
mbedalvaro 31:5f039cbddee8 91
mbedalvaro 31:5f039cbddee8 92 // Perpendicular normalized vector2D.
mbedalvaro 31:5f039cbddee8 93 vector2D getPerpendicularNormed(int orientation) const;
mbedalvaro 31:5f039cbddee8 94 vector2D& perpendicular(int orientation);
mbedalvaro 31:5f039cbddee8 95
mbedalvaro 31:5f039cbddee8 96 // Rotation
mbedalvaro 31:5f039cbddee8 97 vector2D getRotatedDeg( float angle ) const;
mbedalvaro 31:5f039cbddee8 98 vector2D getRotatedRad( float angle ) const;
mbedalvaro 31:5f039cbddee8 99 vector2D& rotateDeg( float angle );
mbedalvaro 31:5f039cbddee8 100 vector2D& rotateRad( float angle );
mbedalvaro 31:5f039cbddee8 101
mbedalvaro 31:5f039cbddee8 102 //vector2D product (for 3d vector2Ds - for 2d vector2Ds, something like this is just the "angle" between them):
mbedalvaro 31:5f039cbddee8 103 //vector2D getvector2DProduct(const vector2D& vec) const;
mbedalvaro 31:5f039cbddee8 104 //vector2D& vector2DProduct(const vector2D& vec) const;
mbedalvaro 31:5f039cbddee8 105
mbedalvaro 31:5f039cbddee8 106 //Angle (deg) between two vector2Ds (using atan2, so between -180 and 180)
mbedalvaro 31:5f039cbddee8 107 float angleDeg( const vector2D& vec ) const;
mbedalvaro 31:5f039cbddee8 108 float angleRad( const vector2D& vec ) const;
mbedalvaro 31:5f039cbddee8 109 float angleDegHoriz( ) const; // particular case when the second vector is just (1,0)
mbedalvaro 31:5f039cbddee8 110
mbedalvaro 31:5f039cbddee8 111 //Dot Product:
mbedalvaro 31:5f039cbddee8 112 float dot( const vector2D& vec ) const;
mbedalvaro 31:5f039cbddee8 113
mbedalvaro 31:5f039cbddee8 114 // =================================================================
mbedalvaro 31:5f039cbddee8 115
mbedalvaro 31:5f039cbddee8 116 // Actual variables:
mbedalvaro 31:5f039cbddee8 117 T x, y; // or make a class "point"
mbedalvaro 31:5f039cbddee8 118
mbedalvaro 31:5f039cbddee8 119 };
mbedalvaro 31:5f039cbddee8 120
mbedalvaro 40:3ba2b0ea9f33 121 // 3D VECTORS:
mbedalvaro 40:3ba2b0ea9f33 122 template <class T>
mbedalvaro 40:3ba2b0ea9f33 123 class vector3D {
mbedalvaro 40:3ba2b0ea9f33 124
mbedalvaro 40:3ba2b0ea9f33 125 public:
mbedalvaro 40:3ba2b0ea9f33 126
mbedalvaro 40:3ba2b0ea9f33 127 // Overloaded constructor with parameters:
mbedalvaro 40:3ba2b0ea9f33 128 vector3D( T _x=0.0f, T _y=0.0f , T _z=0.0f );
mbedalvaro 40:3ba2b0ea9f33 129 // note: we use the default copy constructor
mbedalvaro 40:3ba2b0ea9f33 130
mbedalvaro 40:3ba2b0ea9f33 131 // Explicit setting:
mbedalvaro 40:3ba2b0ea9f33 132 void set( T _x, T _y, T _z );
mbedalvaro 40:3ba2b0ea9f33 133 void set( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 134
mbedalvaro 40:3ba2b0ea9f33 135 // Comparison:
mbedalvaro 40:3ba2b0ea9f33 136 bool operator==( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 137 bool operator!=( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 138 bool match( const vector3D& vec, float tollerance=0.0001 );
mbedalvaro 40:3ba2b0ea9f33 139
mbedalvaro 40:3ba2b0ea9f33 140 // Overloaded operators:
mbedalvaro 40:3ba2b0ea9f33 141 //
mbedalvaro 40:3ba2b0ea9f33 142 void operator=( const vector3D& vec ); // I cannot declare this if we want also operator chaining?
mbedalvaro 40:3ba2b0ea9f33 143 //vector3D & operator=( const vector2D& vec ); // this is to enable operator chaining (vec1=vec2=vec3).
mbedalvaro 40:3ba2b0ea9f33 144 vector3D operator+( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 145 vector3D& operator+=( const vector3D& vec ); // why it has an output? for doing vec1=vec2+=vec3? YES!!! (operator chaining).
mbedalvaro 40:3ba2b0ea9f33 146 vector3D operator-( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 147 vector3D& operator-=( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 148 vector3D operator*( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 149 vector3D& operator*=( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 150
mbedalvaro 40:3ba2b0ea9f33 151 // division "dimension by dimension" (like the matlab "./"):
mbedalvaro 40:3ba2b0ea9f33 152 vector3D operator/( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 153 vector3D& operator/=( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 154 // note: division by a vector is properly defined for 2d vectors: norm is divided and argument is substracted (complex division).
mbedalvaro 40:3ba2b0ea9f33 155
mbedalvaro 40:3ba2b0ea9f33 156 //operator overloading for float:
mbedalvaro 40:3ba2b0ea9f33 157 void operator=( const T f); // I cannot declare this if we want also operator chaining?
mbedalvaro 40:3ba2b0ea9f33 158 //vector3D & operator=( const float& val ); // to allow operator chaining
mbedalvaro 40:3ba2b0ea9f33 159 vector3D operator+( const T f ) const;
mbedalvaro 40:3ba2b0ea9f33 160 vector3D& operator+=( const T f );
mbedalvaro 40:3ba2b0ea9f33 161 vector3D operator-( const T f ) const;
mbedalvaro 40:3ba2b0ea9f33 162 vector3D& operator-=( const T f );
mbedalvaro 40:3ba2b0ea9f33 163 vector3D operator-() const;
mbedalvaro 40:3ba2b0ea9f33 164
mbedalvaro 40:3ba2b0ea9f33 165
mbedalvaro 40:3ba2b0ea9f33 166 // multiplication by a scalar:
mbedalvaro 40:3ba2b0ea9f33 167 vector3D operator*( const T f ) const;
mbedalvaro 40:3ba2b0ea9f33 168 vector3D& operator*=( const T f );
mbedalvaro 40:3ba2b0ea9f33 169 vector3D operator/( const T f ) const;
mbedalvaro 40:3ba2b0ea9f33 170 vector3D& operator/=( const T f );
mbedalvaro 40:3ba2b0ea9f33 171
mbedalvaro 40:3ba2b0ea9f33 172 // Distance (between end points of two vector3Ds):
mbedalvaro 40:3ba2b0ea9f33 173 float distance( const vector3D& pnt) const;
mbedalvaro 40:3ba2b0ea9f33 174 float squareDistance( const vector3D& pnt ) const;
mbedalvaro 40:3ba2b0ea9f33 175
mbedalvaro 40:3ba2b0ea9f33 176 // Length of vector3D (norm):
mbedalvaro 40:3ba2b0ea9f33 177 float length() const;
mbedalvaro 40:3ba2b0ea9f33 178 float squareLength() const; // faster, no sqrt
mbedalvaro 40:3ba2b0ea9f33 179
mbedalvaro 40:3ba2b0ea9f33 180 // Scaling:
mbedalvaro 40:3ba2b0ea9f33 181 vector3D getScaled( const float length ) const;
mbedalvaro 40:3ba2b0ea9f33 182 vector3D& scale( const float length );
mbedalvaro 40:3ba2b0ea9f33 183
mbedalvaro 40:3ba2b0ea9f33 184 // Normalization:
mbedalvaro 40:3ba2b0ea9f33 185 vector3D getNormalized() const;
mbedalvaro 40:3ba2b0ea9f33 186 vector3D& normalize();
mbedalvaro 40:3ba2b0ea9f33 187
mbedalvaro 40:3ba2b0ea9f33 188
mbedalvaro 40:3ba2b0ea9f33 189 //Angle (deg) between two vector2Ds (using atan2, so between -180 and 180)
mbedalvaro 40:3ba2b0ea9f33 190 float angleDeg( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 191 float angleRad( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 192
mbedalvaro 40:3ba2b0ea9f33 193 //Dot Product:
mbedalvaro 40:3ba2b0ea9f33 194 float dot( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 195
mbedalvaro 40:3ba2b0ea9f33 196 //Cross product:
mbedalvaro 40:3ba2b0ea9f33 197 vector3D cross( const vector3D& vec ) const;
mbedalvaro 40:3ba2b0ea9f33 198 vector3D& cross( const vector3D& vec );
mbedalvaro 40:3ba2b0ea9f33 199
mbedalvaro 40:3ba2b0ea9f33 200 // =================================================================
mbedalvaro 40:3ba2b0ea9f33 201
mbedalvaro 40:3ba2b0ea9f33 202 // Actual variables:
mbedalvaro 40:3ba2b0ea9f33 203 T x, y, z; // or make a class "point"
mbedalvaro 40:3ba2b0ea9f33 204
mbedalvaro 40:3ba2b0ea9f33 205 };
mbedalvaro 40:3ba2b0ea9f33 206
mbedalvaro 40:3ba2b0ea9f33 207 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
mbedalvaro 31:5f039cbddee8 208 // Implementation
mbedalvaro 40:3ba2b0ea9f33 209 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
mbedalvaro 40:3ba2b0ea9f33 210
mbedalvaro 40:3ba2b0ea9f33 211 // ============================================ 2D vectors ============================================
mbedalvaro 31:5f039cbddee8 212
mbedalvaro 31:5f039cbddee8 213 template <class T>
mbedalvaro 31:5f039cbddee8 214 inline vector2D<T>::vector2D( T _x, T _y ) {
mbedalvaro 31:5f039cbddee8 215 x = _x; y = _y;
mbedalvaro 31:5f039cbddee8 216 }
mbedalvaro 31:5f039cbddee8 217
mbedalvaro 31:5f039cbddee8 218 template <class T>
mbedalvaro 31:5f039cbddee8 219 inline void vector2D<T>::set( T _x, T _y ) {
mbedalvaro 31:5f039cbddee8 220 x = _x; y = _y;
mbedalvaro 31:5f039cbddee8 221 }
mbedalvaro 31:5f039cbddee8 222
mbedalvaro 31:5f039cbddee8 223 template <class T>
mbedalvaro 31:5f039cbddee8 224 inline void vector2D<T>::set( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 225 x=vec.x; y=vec.y;
mbedalvaro 31:5f039cbddee8 226 }
mbedalvaro 31:5f039cbddee8 227
mbedalvaro 31:5f039cbddee8 228 template <class T>
mbedalvaro 31:5f039cbddee8 229 inline bool vector2D<T>::operator==( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 230 return (x == vec.x) && (y == vec.y);
mbedalvaro 31:5f039cbddee8 231 }
mbedalvaro 31:5f039cbddee8 232
mbedalvaro 31:5f039cbddee8 233 template <class T>
mbedalvaro 31:5f039cbddee8 234 inline bool vector2D<T>::operator!=( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 235 return (x != vec.x) || (y != vec.y);
mbedalvaro 31:5f039cbddee8 236 }
mbedalvaro 31:5f039cbddee8 237
mbedalvaro 31:5f039cbddee8 238 template <class T>
mbedalvaro 31:5f039cbddee8 239 inline bool vector2D<T>::match( const vector2D<T>& vec, float tolerance ) {
mbedalvaro 31:5f039cbddee8 240 return (abs(x - vec.x) < tolerance)&& (abs(y - vec.y) < tolerance);
mbedalvaro 31:5f039cbddee8 241 }
mbedalvaro 31:5f039cbddee8 242
mbedalvaro 31:5f039cbddee8 243
mbedalvaro 31:5f039cbddee8 244 /*
mbedalvaro 31:5f039cbddee8 245 inline vector2D & operator=( const vector2D& vec ){ // returning a reference to the vector2D object for allowing operator chaining
mbedalvaro 31:5f039cbddee8 246 x = vec.x;
mbedalvaro 31:5f039cbddee8 247 y = vec.y;
mbedalvaro 31:5f039cbddee8 248 return *this;
mbedalvaro 31:5f039cbddee8 249 }
mbedalvaro 31:5f039cbddee8 250 */
mbedalvaro 31:5f039cbddee8 251
mbedalvaro 31:5f039cbddee8 252 template <class T>
mbedalvaro 31:5f039cbddee8 253 inline void vector2D<T>::operator=( const vector2D<T>& vec ){
mbedalvaro 31:5f039cbddee8 254 x = vec.x; y = vec.y;
mbedalvaro 31:5f039cbddee8 255 }
mbedalvaro 31:5f039cbddee8 256
mbedalvaro 31:5f039cbddee8 257 template <class T>
mbedalvaro 31:5f039cbddee8 258 inline vector2D<T> vector2D<T>::operator+( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 259 return vector2D<T>( x+vec.x, y+vec.y);
mbedalvaro 31:5f039cbddee8 260 }
mbedalvaro 31:5f039cbddee8 261
mbedalvaro 31:5f039cbddee8 262 template <class T>
mbedalvaro 31:5f039cbddee8 263 inline vector2D<T>& vector2D<T>::operator+=( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 264 x += vec.x;
mbedalvaro 31:5f039cbddee8 265 y += vec.y;
mbedalvaro 31:5f039cbddee8 266 return *this;
mbedalvaro 31:5f039cbddee8 267 }
mbedalvaro 31:5f039cbddee8 268
mbedalvaro 31:5f039cbddee8 269 template <class T>
mbedalvaro 31:5f039cbddee8 270 inline vector2D<T> vector2D<T>::operator-( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 271 return vector2D<T>(x-vec.x, y-vec.y);
mbedalvaro 31:5f039cbddee8 272 }
mbedalvaro 31:5f039cbddee8 273
mbedalvaro 31:5f039cbddee8 274 template <class T>
mbedalvaro 31:5f039cbddee8 275 inline vector2D<T>& vector2D<T>::operator-=( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 276 x -= vec.x;
mbedalvaro 31:5f039cbddee8 277 y -= vec.y;
mbedalvaro 31:5f039cbddee8 278 return *this;
mbedalvaro 31:5f039cbddee8 279 }
mbedalvaro 31:5f039cbddee8 280
mbedalvaro 31:5f039cbddee8 281 template <class T>
mbedalvaro 31:5f039cbddee8 282 inline vector2D<T> vector2D<T>::operator*( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 283 return vector2D<T>(x*vec.x, y*vec.y);
mbedalvaro 31:5f039cbddee8 284 }
mbedalvaro 31:5f039cbddee8 285
mbedalvaro 31:5f039cbddee8 286 template <class T>
mbedalvaro 31:5f039cbddee8 287 inline vector2D<T>& vector2D<T>::operator*=( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 288 x*=vec.x;
mbedalvaro 31:5f039cbddee8 289 y*=vec.y;
mbedalvaro 31:5f039cbddee8 290 return *this;
mbedalvaro 31:5f039cbddee8 291 }
mbedalvaro 31:5f039cbddee8 292
mbedalvaro 31:5f039cbddee8 293 template <class T>
mbedalvaro 31:5f039cbddee8 294 inline vector2D<T> vector2D<T>::operator/( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 295 return vector2D<T>( vec.x!=0 ? x/vec.x : x , vec.y!=0 ? y/vec.y : y);
mbedalvaro 31:5f039cbddee8 296 }
mbedalvaro 31:5f039cbddee8 297
mbedalvaro 31:5f039cbddee8 298 template <class T>
mbedalvaro 31:5f039cbddee8 299 inline vector2D<T>& vector2D<T>::operator/=( const vector2D<T>& vec ) {
mbedalvaro 31:5f039cbddee8 300 vec.x!=0 ? x/=vec.x : x;
mbedalvaro 31:5f039cbddee8 301 vec.y!=0 ? y/=vec.y : y;
mbedalvaro 31:5f039cbddee8 302 return *this;
mbedalvaro 31:5f039cbddee8 303 }
mbedalvaro 31:5f039cbddee8 304
mbedalvaro 31:5f039cbddee8 305 //operator overloading for float:
mbedalvaro 31:5f039cbddee8 306 /*
mbedalvaro 31:5f039cbddee8 307 inline vector2D<T> & operator=( const float& val ){
mbedalvaro 31:5f039cbddee8 308 x = val;
mbedalvaro 31:5f039cbddee8 309 y = val;
mbedalvaro 31:5f039cbddee8 310 return *this;
mbedalvaro 31:5f039cbddee8 311 }
mbedalvaro 31:5f039cbddee8 312 */
mbedalvaro 31:5f039cbddee8 313
mbedalvaro 31:5f039cbddee8 314 template <class T>
mbedalvaro 31:5f039cbddee8 315 inline void vector2D<T>::operator=( const T f){
mbedalvaro 31:5f039cbddee8 316 x = f; y = f;
mbedalvaro 31:5f039cbddee8 317 }
mbedalvaro 31:5f039cbddee8 318
mbedalvaro 31:5f039cbddee8 319 template <class T>
mbedalvaro 31:5f039cbddee8 320 inline vector2D<T> vector2D<T>::operator+( const T f ) const {
mbedalvaro 31:5f039cbddee8 321 return vector2D<T>( x+f, y+f);
mbedalvaro 31:5f039cbddee8 322 }
mbedalvaro 31:5f039cbddee8 323
mbedalvaro 31:5f039cbddee8 324 template <class T>
mbedalvaro 31:5f039cbddee8 325 inline vector2D<T>& vector2D<T>::operator+=( const T f ) {
mbedalvaro 31:5f039cbddee8 326 x += f; y += f;
mbedalvaro 31:5f039cbddee8 327 return *this;
mbedalvaro 31:5f039cbddee8 328 }
mbedalvaro 31:5f039cbddee8 329
mbedalvaro 31:5f039cbddee8 330 template <class T>
mbedalvaro 31:5f039cbddee8 331 inline vector2D<T> vector2D<T>::operator-( const T f ) const {
mbedalvaro 31:5f039cbddee8 332 return vector2D<T>( x-f, y-f);
mbedalvaro 31:5f039cbddee8 333 }
mbedalvaro 31:5f039cbddee8 334
mbedalvaro 31:5f039cbddee8 335 template <class T>
mbedalvaro 31:5f039cbddee8 336 inline vector2D<T>& vector2D<T>::operator-=( const T f ) {
mbedalvaro 31:5f039cbddee8 337 x -= f; y -= f;
mbedalvaro 31:5f039cbddee8 338 return *this;
mbedalvaro 31:5f039cbddee8 339 }
mbedalvaro 31:5f039cbddee8 340
mbedalvaro 31:5f039cbddee8 341 template <class T>
mbedalvaro 31:5f039cbddee8 342 inline vector2D<T> vector2D<T>::operator-() const {
mbedalvaro 31:5f039cbddee8 343 return vector2D<T>(-x, -y);
mbedalvaro 31:5f039cbddee8 344 }
mbedalvaro 31:5f039cbddee8 345
mbedalvaro 31:5f039cbddee8 346 template <class T>
mbedalvaro 31:5f039cbddee8 347 inline vector2D<T> vector2D<T>::operator*( const T f ) const {
mbedalvaro 31:5f039cbddee8 348 return vector2D<T>(x*f, y*f);
mbedalvaro 31:5f039cbddee8 349 }
mbedalvaro 31:5f039cbddee8 350
mbedalvaro 31:5f039cbddee8 351 template <class T>
mbedalvaro 31:5f039cbddee8 352 inline vector2D<T>& vector2D<T>::operator*=( const T f ) {
mbedalvaro 31:5f039cbddee8 353 x*=f; y*=f;
mbedalvaro 31:5f039cbddee8 354 return *this;
mbedalvaro 31:5f039cbddee8 355 }
mbedalvaro 31:5f039cbddee8 356
mbedalvaro 31:5f039cbddee8 357 template <class T>
mbedalvaro 31:5f039cbddee8 358 inline vector2D<T> vector2D<T>::operator/( const T f ) const {
mbedalvaro 31:5f039cbddee8 359 //cout << "here" << endl;
mbedalvaro 31:5f039cbddee8 360 if(f == 0) return vector2D<T>(x, y);
mbedalvaro 31:5f039cbddee8 361 return vector2D<T>(x/f, y/f);
mbedalvaro 31:5f039cbddee8 362 }
mbedalvaro 31:5f039cbddee8 363
mbedalvaro 31:5f039cbddee8 364 template <class T>
mbedalvaro 31:5f039cbddee8 365 inline vector2D<T>& vector2D<T>::operator/=( const T f ) {
mbedalvaro 31:5f039cbddee8 366 if(f == 0) return *this;
mbedalvaro 31:5f039cbddee8 367 x/=f; y/=f;
mbedalvaro 31:5f039cbddee8 368 return *this;
mbedalvaro 31:5f039cbddee8 369 }
mbedalvaro 31:5f039cbddee8 370
mbedalvaro 31:5f039cbddee8 371 template <class T>
mbedalvaro 31:5f039cbddee8 372 inline vector2D<T> vector2D<T>::getScaled( const float length ) const {
mbedalvaro 31:5f039cbddee8 373 float l = (float)sqrt(x*x + y*y);
mbedalvaro 31:5f039cbddee8 374 if( l > 0 )
mbedalvaro 31:5f039cbddee8 375 return vector2D<T>( (x/l)*length, (y/l)*length );
mbedalvaro 31:5f039cbddee8 376 else
mbedalvaro 31:5f039cbddee8 377 return vector2D<T>();
mbedalvaro 31:5f039cbddee8 378 }
mbedalvaro 31:5f039cbddee8 379
mbedalvaro 31:5f039cbddee8 380 template <class T>
mbedalvaro 31:5f039cbddee8 381 inline vector2D<T>& vector2D<T>::scale( const float length ) {
mbedalvaro 31:5f039cbddee8 382 float l = (float)sqrt(x*x + y*y);
mbedalvaro 31:5f039cbddee8 383 if (l > 0) {
mbedalvaro 31:5f039cbddee8 384 x = (x/l)*length;
mbedalvaro 31:5f039cbddee8 385 y = (y/l)*length;
mbedalvaro 31:5f039cbddee8 386 }
mbedalvaro 31:5f039cbddee8 387 return *this;
mbedalvaro 31:5f039cbddee8 388 }
mbedalvaro 31:5f039cbddee8 389
mbedalvaro 31:5f039cbddee8 390 // Rotation
mbedalvaro 31:5f039cbddee8 391 //
mbedalvaro 31:5f039cbddee8 392 //
mbedalvaro 31:5f039cbddee8 393
mbedalvaro 31:5f039cbddee8 394 template <class T>
mbedalvaro 31:5f039cbddee8 395 inline vector2D<T> vector2D<T>::getRotatedDeg( float angle ) const {
mbedalvaro 31:5f039cbddee8 396 float a = (float)(angle*DEG_TO_RAD);
mbedalvaro 31:5f039cbddee8 397 return vector2D<T>( x*cos(a) - y*sin(a),
mbedalvaro 31:5f039cbddee8 398 x*sin(a) + y*cos(a) );
mbedalvaro 31:5f039cbddee8 399 }
mbedalvaro 31:5f039cbddee8 400
mbedalvaro 31:5f039cbddee8 401 template <class T>
mbedalvaro 31:5f039cbddee8 402 inline vector2D<T> vector2D<T>::getRotatedRad( float angle ) const {
mbedalvaro 31:5f039cbddee8 403 float a = angle;
mbedalvaro 31:5f039cbddee8 404 return vector2D<T>( x*cos(a) - y*sin(a),
mbedalvaro 31:5f039cbddee8 405 x*sin(a) + y*cos(a) );
mbedalvaro 31:5f039cbddee8 406 }
mbedalvaro 31:5f039cbddee8 407
mbedalvaro 31:5f039cbddee8 408 template <class T>
mbedalvaro 31:5f039cbddee8 409 inline vector2D<T>& vector2D<T>::rotateDeg( float angle ) {
mbedalvaro 31:5f039cbddee8 410 float a = (float)(angle * DEG_TO_RAD);
mbedalvaro 31:5f039cbddee8 411 float xrot = x*cos(a) - y*sin(a);
mbedalvaro 31:5f039cbddee8 412 y = x*sin(a) + y*cos(a);
mbedalvaro 31:5f039cbddee8 413 x = xrot;
mbedalvaro 31:5f039cbddee8 414 return *this;
mbedalvaro 31:5f039cbddee8 415 }
mbedalvaro 31:5f039cbddee8 416
mbedalvaro 31:5f039cbddee8 417 template <class T>
mbedalvaro 31:5f039cbddee8 418 inline vector2D<T>& vector2D<T>::rotateRad( float angle ) {
mbedalvaro 31:5f039cbddee8 419 float a = angle;
mbedalvaro 31:5f039cbddee8 420 float xrot = x*cos(a) - y*sin(a);
mbedalvaro 31:5f039cbddee8 421 y = x*sin(a) + y*cos(a);
mbedalvaro 31:5f039cbddee8 422 x = xrot;
mbedalvaro 31:5f039cbddee8 423 return *this;
mbedalvaro 31:5f039cbddee8 424 }
mbedalvaro 31:5f039cbddee8 425
mbedalvaro 31:5f039cbddee8 426 template <class T>
mbedalvaro 31:5f039cbddee8 427 inline float vector2D<T>::distance( const vector2D<T>& pnt) const {
mbedalvaro 31:5f039cbddee8 428 float vx = x-pnt.x;
mbedalvaro 31:5f039cbddee8 429 float vy = y-pnt.y;
mbedalvaro 31:5f039cbddee8 430 return (float)sqrt(vx*vx + vy*vy);
mbedalvaro 31:5f039cbddee8 431 }
mbedalvaro 31:5f039cbddee8 432
mbedalvaro 31:5f039cbddee8 433 template <class T>
mbedalvaro 31:5f039cbddee8 434 inline float vector2D<T>::squareDistance( const vector2D<T>& pnt ) const {
mbedalvaro 31:5f039cbddee8 435 float vx = x-pnt.x;
mbedalvaro 31:5f039cbddee8 436 float vy = y-pnt.y;
mbedalvaro 31:5f039cbddee8 437 return vx*vx + vy*vy;
mbedalvaro 31:5f039cbddee8 438 }
mbedalvaro 31:5f039cbddee8 439
mbedalvaro 31:5f039cbddee8 440 // Normalization:
mbedalvaro 31:5f039cbddee8 441 template <class T>
mbedalvaro 31:5f039cbddee8 442 inline vector2D<T> vector2D<T>::getNormalized() const {
mbedalvaro 31:5f039cbddee8 443 float length = (float)sqrt(x*x + y*y);
mbedalvaro 31:5f039cbddee8 444 if( length > 0 ) {
mbedalvaro 31:5f039cbddee8 445 return vector2D<T>( x/length, y/length );
mbedalvaro 31:5f039cbddee8 446 } else {
mbedalvaro 31:5f039cbddee8 447 return vector2D<T>();
mbedalvaro 31:5f039cbddee8 448 }
mbedalvaro 31:5f039cbddee8 449 }
mbedalvaro 31:5f039cbddee8 450
mbedalvaro 31:5f039cbddee8 451 template <class T>
mbedalvaro 31:5f039cbddee8 452 inline vector2D<T>& vector2D<T>::normalize() {
mbedalvaro 31:5f039cbddee8 453 float length = (float)sqrt(x*x + y*y);
mbedalvaro 31:5f039cbddee8 454 if( length > 0 ) {
mbedalvaro 31:5f039cbddee8 455 x /= length;
mbedalvaro 31:5f039cbddee8 456 y /= length;
mbedalvaro 31:5f039cbddee8 457 }
mbedalvaro 31:5f039cbddee8 458 return *this;
mbedalvaro 31:5f039cbddee8 459 }
mbedalvaro 31:5f039cbddee8 460
mbedalvaro 31:5f039cbddee8 461 template <class T>
mbedalvaro 31:5f039cbddee8 462 inline vector2D<T> vector2D<T>::getPerpendicularNormed(int orientation) const {
mbedalvaro 31:5f039cbddee8 463 float length = (float)sqrt( x*x + y*y );
mbedalvaro 31:5f039cbddee8 464 if( length > 0 )
mbedalvaro 31:5f039cbddee8 465 return vector2D<T>( -orientation*(y/length), orientation*x/length );
mbedalvaro 31:5f039cbddee8 466 else
mbedalvaro 31:5f039cbddee8 467 return vector2D<T>(0.0, 0.0); // something very small (will be used to compute a force)
mbedalvaro 31:5f039cbddee8 468 }
mbedalvaro 31:5f039cbddee8 469
mbedalvaro 31:5f039cbddee8 470 template <class T>
mbedalvaro 31:5f039cbddee8 471 inline vector2D<T>& vector2D<T>::perpendicular(int orientation) {
mbedalvaro 31:5f039cbddee8 472 float length = (float)sqrt( x*x + y*y );
mbedalvaro 31:5f039cbddee8 473 if( length > 0 ) {
mbedalvaro 31:5f039cbddee8 474 float _x = x;
mbedalvaro 31:5f039cbddee8 475 x = -(y/length)*orientation;
mbedalvaro 31:5f039cbddee8 476 y = _x/length*orientation;
mbedalvaro 31:5f039cbddee8 477 }
mbedalvaro 31:5f039cbddee8 478 return *this;
mbedalvaro 31:5f039cbddee8 479 }
mbedalvaro 31:5f039cbddee8 480
mbedalvaro 31:5f039cbddee8 481 // Length (norm of vector2D<T>):
mbedalvaro 31:5f039cbddee8 482 template <class T>
mbedalvaro 31:5f039cbddee8 483 inline float vector2D<T>::length() const {
mbedalvaro 31:5f039cbddee8 484 return (float)sqrt( x*x + y*y );
mbedalvaro 31:5f039cbddee8 485 }
mbedalvaro 31:5f039cbddee8 486
mbedalvaro 31:5f039cbddee8 487 template <class T>
mbedalvaro 31:5f039cbddee8 488 inline float vector2D<T>::squareLength() const {
mbedalvaro 31:5f039cbddee8 489 return (float)(x*x + y*y);
mbedalvaro 31:5f039cbddee8 490 }
mbedalvaro 31:5f039cbddee8 491
mbedalvaro 31:5f039cbddee8 492 // Angle between two vector2Ds:
mbedalvaro 31:5f039cbddee8 493 template <class T>
mbedalvaro 31:5f039cbddee8 494 inline float vector2D<T>::angleDeg( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 495 return (float)(atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y )*RAD_TO_DEG);
mbedalvaro 31:5f039cbddee8 496 }
mbedalvaro 31:5f039cbddee8 497
mbedalvaro 31:5f039cbddee8 498 template <class T>
mbedalvaro 31:5f039cbddee8 499 inline float vector2D<T>::angleRad( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 500 return atan2( x*vec.y-y*vec.x, x*vec.x + y*vec.y );
mbedalvaro 31:5f039cbddee8 501 }
mbedalvaro 31:5f039cbddee8 502
mbedalvaro 31:5f039cbddee8 503 template <class T>
mbedalvaro 31:5f039cbddee8 504 inline float vector2D<T>::angleDegHoriz( ) const {
mbedalvaro 31:5f039cbddee8 505 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 31:5f039cbddee8 506 }
mbedalvaro 31:5f039cbddee8 507
mbedalvaro 31:5f039cbddee8 508 //Dot Product:
mbedalvaro 31:5f039cbddee8 509 template <class T>
mbedalvaro 31:5f039cbddee8 510 inline float vector2D<T>::dot( const vector2D<T>& vec ) const {
mbedalvaro 31:5f039cbddee8 511 return x*vec.x + y*vec.y;
mbedalvaro 31:5f039cbddee8 512 }
mbedalvaro 31:5f039cbddee8 513
mbedalvaro 40:3ba2b0ea9f33 514 // ============================================ 3D vectors ============================================
mbedalvaro 40:3ba2b0ea9f33 515
mbedalvaro 40:3ba2b0ea9f33 516 template <class T>
mbedalvaro 40:3ba2b0ea9f33 517 inline vector3D<T>::vector3D( T _x, T _y ,T _z ) {
mbedalvaro 40:3ba2b0ea9f33 518 x = _x; y = _y; z = _z;
mbedalvaro 40:3ba2b0ea9f33 519 }
mbedalvaro 40:3ba2b0ea9f33 520
mbedalvaro 40:3ba2b0ea9f33 521 template <class T>
mbedalvaro 40:3ba2b0ea9f33 522 inline void vector3D<T>::set( T _x, T _y ,T _z ) {
mbedalvaro 40:3ba2b0ea9f33 523 x = _x; y = _y; z = _z;
mbedalvaro 40:3ba2b0ea9f33 524 }
mbedalvaro 40:3ba2b0ea9f33 525
mbedalvaro 40:3ba2b0ea9f33 526 template <class T>
mbedalvaro 40:3ba2b0ea9f33 527 inline void vector3D<T>::set( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 528 x=vec.x; y=vec.y; z = vec.z;
mbedalvaro 40:3ba2b0ea9f33 529 }
mbedalvaro 40:3ba2b0ea9f33 530
mbedalvaro 40:3ba2b0ea9f33 531 template <class T>
mbedalvaro 40:3ba2b0ea9f33 532 inline bool vector3D<T>::operator==( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 533 return (x == vec.x) && (y == vec.y)&& (z == vec.z);
mbedalvaro 40:3ba2b0ea9f33 534 }
mbedalvaro 40:3ba2b0ea9f33 535
mbedalvaro 40:3ba2b0ea9f33 536 template <class T>
mbedalvaro 40:3ba2b0ea9f33 537 inline bool vector3D<T>::operator!=( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 538 return (x != vec.x) || (y != vec.y)|| (z != vec.z);
mbedalvaro 40:3ba2b0ea9f33 539 }
mbedalvaro 40:3ba2b0ea9f33 540
mbedalvaro 40:3ba2b0ea9f33 541 template <class T>
mbedalvaro 40:3ba2b0ea9f33 542 inline bool vector3D<T>::match( const vector3D<T>& vec, float tolerance ) {
mbedalvaro 40:3ba2b0ea9f33 543 return (abs(x - vec.x) < tolerance)&& (abs(y - vec.y) < tolerance)&& (abs(z - vec.z) < tolerance);
mbedalvaro 40:3ba2b0ea9f33 544 }
mbedalvaro 40:3ba2b0ea9f33 545
mbedalvaro 40:3ba2b0ea9f33 546
mbedalvaro 40:3ba2b0ea9f33 547 /*
mbedalvaro 40:3ba2b0ea9f33 548 inline vector3D & operator=( const vector3D& vec ){ // returning a reference to the vector3D object for allowing operator chaining
mbedalvaro 40:3ba2b0ea9f33 549 x = vec.x;
mbedalvaro 40:3ba2b0ea9f33 550 y = vec.y;
mbedalvaro 40:3ba2b0ea9f33 551 return *this;
mbedalvaro 40:3ba2b0ea9f33 552 }
mbedalvaro 40:3ba2b0ea9f33 553 */
mbedalvaro 40:3ba2b0ea9f33 554
mbedalvaro 40:3ba2b0ea9f33 555 template <class T>
mbedalvaro 40:3ba2b0ea9f33 556 inline void vector3D<T>::operator=( const vector3D<T>& vec ){
mbedalvaro 40:3ba2b0ea9f33 557 x = vec.x; y = vec.y; z = vec.z;
mbedalvaro 40:3ba2b0ea9f33 558 }
mbedalvaro 40:3ba2b0ea9f33 559
mbedalvaro 40:3ba2b0ea9f33 560 template <class T>
mbedalvaro 40:3ba2b0ea9f33 561 inline vector3D<T> vector3D<T>::operator+( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 562 return vector3D<T>( x+vec.x, y+vec.y, z+vec.z);
mbedalvaro 40:3ba2b0ea9f33 563 }
mbedalvaro 40:3ba2b0ea9f33 564
mbedalvaro 40:3ba2b0ea9f33 565 template <class T>
mbedalvaro 40:3ba2b0ea9f33 566 inline vector3D<T>& vector3D<T>::operator+=( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 567 x += vec.x;
mbedalvaro 40:3ba2b0ea9f33 568 y += vec.y;
mbedalvaro 40:3ba2b0ea9f33 569 z += vec.z;
mbedalvaro 40:3ba2b0ea9f33 570 return *this;
mbedalvaro 40:3ba2b0ea9f33 571 }
mbedalvaro 40:3ba2b0ea9f33 572
mbedalvaro 40:3ba2b0ea9f33 573 template <class T>
mbedalvaro 40:3ba2b0ea9f33 574 inline vector3D<T> vector3D<T>::operator-( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 575 return vector3D<T>(x-vec.x, y-vec.y, z-vec.z);
mbedalvaro 40:3ba2b0ea9f33 576 }
mbedalvaro 40:3ba2b0ea9f33 577
mbedalvaro 40:3ba2b0ea9f33 578 template <class T>
mbedalvaro 40:3ba2b0ea9f33 579 inline vector3D<T>& vector3D<T>::operator-=( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 580 x -= vec.x;
mbedalvaro 40:3ba2b0ea9f33 581 y -= vec.y;
mbedalvaro 40:3ba2b0ea9f33 582 z -= vec.z;
mbedalvaro 40:3ba2b0ea9f33 583 return *this;
mbedalvaro 40:3ba2b0ea9f33 584 }
mbedalvaro 40:3ba2b0ea9f33 585
mbedalvaro 40:3ba2b0ea9f33 586 template <class T>
mbedalvaro 40:3ba2b0ea9f33 587 inline vector3D<T> vector3D<T>::operator*( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 588 return vector3D<T>(x*vec.x, y*vec.y, z*vec.z);
mbedalvaro 40:3ba2b0ea9f33 589 }
mbedalvaro 40:3ba2b0ea9f33 590
mbedalvaro 40:3ba2b0ea9f33 591 template <class T>
mbedalvaro 40:3ba2b0ea9f33 592 inline vector3D<T>& vector3D<T>::operator*=( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 593 x*=vec.x;
mbedalvaro 40:3ba2b0ea9f33 594 y*=vec.y;
mbedalvaro 40:3ba2b0ea9f33 595 z*=vec.z;
mbedalvaro 40:3ba2b0ea9f33 596 return *this;
mbedalvaro 40:3ba2b0ea9f33 597 }
mbedalvaro 40:3ba2b0ea9f33 598
mbedalvaro 40:3ba2b0ea9f33 599 //operator overloading for float:
mbedalvaro 40:3ba2b0ea9f33 600 /*
mbedalvaro 40:3ba2b0ea9f33 601 inline vector3D<T> & operator=( const float& val ){
mbedalvaro 40:3ba2b0ea9f33 602 x = val;
mbedalvaro 40:3ba2b0ea9f33 603 y = val;
mbedalvaro 40:3ba2b0ea9f33 604 return *this;
mbedalvaro 40:3ba2b0ea9f33 605 }
mbedalvaro 40:3ba2b0ea9f33 606 */
mbedalvaro 40:3ba2b0ea9f33 607
mbedalvaro 40:3ba2b0ea9f33 608 template <class T>
mbedalvaro 40:3ba2b0ea9f33 609 inline void vector3D<T>::operator=( const T f){
mbedalvaro 40:3ba2b0ea9f33 610 x = f; y = f; z = f;
mbedalvaro 40:3ba2b0ea9f33 611 }
mbedalvaro 40:3ba2b0ea9f33 612
mbedalvaro 40:3ba2b0ea9f33 613 template <class T>
mbedalvaro 40:3ba2b0ea9f33 614 inline vector3D<T> vector3D<T>::operator+( const T f ) const {
mbedalvaro 40:3ba2b0ea9f33 615 return vector3D<T>( x+f, y+f, z+f);
mbedalvaro 40:3ba2b0ea9f33 616 }
mbedalvaro 40:3ba2b0ea9f33 617
mbedalvaro 40:3ba2b0ea9f33 618 template <class T>
mbedalvaro 40:3ba2b0ea9f33 619 inline vector3D<T>& vector3D<T>::operator+=( const T f ) {
mbedalvaro 40:3ba2b0ea9f33 620 x += f; y += f; z+=f;
mbedalvaro 40:3ba2b0ea9f33 621 return *this;
mbedalvaro 40:3ba2b0ea9f33 622 }
mbedalvaro 40:3ba2b0ea9f33 623
mbedalvaro 40:3ba2b0ea9f33 624 template <class T>
mbedalvaro 40:3ba2b0ea9f33 625 inline vector3D<T> vector3D<T>::operator-( const T f ) const {
mbedalvaro 40:3ba2b0ea9f33 626 return vector3D<T>( x-f, y-f, z-f);
mbedalvaro 40:3ba2b0ea9f33 627 }
mbedalvaro 40:3ba2b0ea9f33 628
mbedalvaro 40:3ba2b0ea9f33 629 template <class T>
mbedalvaro 40:3ba2b0ea9f33 630 inline vector3D<T>& vector3D<T>::operator-=( const T f ) {
mbedalvaro 40:3ba2b0ea9f33 631 x -= f; y -= f; z-=f;
mbedalvaro 40:3ba2b0ea9f33 632 return *this;
mbedalvaro 40:3ba2b0ea9f33 633 }
mbedalvaro 40:3ba2b0ea9f33 634
mbedalvaro 40:3ba2b0ea9f33 635 template <class T>
mbedalvaro 40:3ba2b0ea9f33 636 inline vector3D<T> vector3D<T>::operator-() const {
mbedalvaro 40:3ba2b0ea9f33 637 return vector3D<T>(-x, -y, -z);
mbedalvaro 40:3ba2b0ea9f33 638 }
mbedalvaro 40:3ba2b0ea9f33 639
mbedalvaro 40:3ba2b0ea9f33 640 template <class T>
mbedalvaro 40:3ba2b0ea9f33 641 inline vector3D<T> vector3D<T>::operator*( const T f ) const {
mbedalvaro 40:3ba2b0ea9f33 642 return vector3D<T>(x*f, y*f, z*f);
mbedalvaro 40:3ba2b0ea9f33 643 }
mbedalvaro 40:3ba2b0ea9f33 644
mbedalvaro 40:3ba2b0ea9f33 645 template <class T>
mbedalvaro 40:3ba2b0ea9f33 646 inline vector3D<T>& vector3D<T>::operator*=( const T f ) {
mbedalvaro 40:3ba2b0ea9f33 647 x*=f; y*=f; z*=f;
mbedalvaro 40:3ba2b0ea9f33 648 return *this;
mbedalvaro 40:3ba2b0ea9f33 649 }
mbedalvaro 40:3ba2b0ea9f33 650
mbedalvaro 40:3ba2b0ea9f33 651 template <class T>
mbedalvaro 40:3ba2b0ea9f33 652 inline vector3D<T> vector3D<T>::operator/( const T f ) const {
mbedalvaro 40:3ba2b0ea9f33 653 //cout << "here" << endl;
mbedalvaro 40:3ba2b0ea9f33 654 if(f == 0) return vector3D<T>(x, y);
mbedalvaro 40:3ba2b0ea9f33 655 return vector3D<T>(x/f, y/f, z/f);
mbedalvaro 40:3ba2b0ea9f33 656 }
mbedalvaro 40:3ba2b0ea9f33 657
mbedalvaro 40:3ba2b0ea9f33 658 template <class T>
mbedalvaro 40:3ba2b0ea9f33 659 inline vector3D<T>& vector3D<T>::operator/=( const T f ) {
mbedalvaro 40:3ba2b0ea9f33 660 if(f == 0) return *this;
mbedalvaro 40:3ba2b0ea9f33 661 x/=f; y/=f; z/=f;
mbedalvaro 40:3ba2b0ea9f33 662 return *this;
mbedalvaro 40:3ba2b0ea9f33 663 }
mbedalvaro 40:3ba2b0ea9f33 664
mbedalvaro 40:3ba2b0ea9f33 665 template <class T>
mbedalvaro 40:3ba2b0ea9f33 666 inline vector3D<T> vector3D<T>::getScaled( const float length ) const {
mbedalvaro 40:3ba2b0ea9f33 667 float l = (float)sqrt(x*x + y*y + z*z);
mbedalvaro 40:3ba2b0ea9f33 668 if( l > 0 )
mbedalvaro 40:3ba2b0ea9f33 669 return vector3D<T>( (x/l)*length, (y/l)*length , (z/l)*length);
mbedalvaro 40:3ba2b0ea9f33 670 else
mbedalvaro 40:3ba2b0ea9f33 671 return vector3D<T>();
mbedalvaro 40:3ba2b0ea9f33 672 }
mbedalvaro 40:3ba2b0ea9f33 673
mbedalvaro 40:3ba2b0ea9f33 674 template <class T>
mbedalvaro 40:3ba2b0ea9f33 675 inline vector3D<T>& vector3D<T>::scale( const float length ) {
mbedalvaro 40:3ba2b0ea9f33 676 float l = (float)sqrt(x*x + y*y + z*z);
mbedalvaro 40:3ba2b0ea9f33 677 if (l > 0) {
mbedalvaro 40:3ba2b0ea9f33 678 x = (x/l)*length;
mbedalvaro 40:3ba2b0ea9f33 679 y = (y/l)*length;
mbedalvaro 40:3ba2b0ea9f33 680 z = (z/l)*length;
mbedalvaro 40:3ba2b0ea9f33 681 }
mbedalvaro 40:3ba2b0ea9f33 682 return *this;
mbedalvaro 40:3ba2b0ea9f33 683 }
mbedalvaro 40:3ba2b0ea9f33 684
mbedalvaro 40:3ba2b0ea9f33 685
mbedalvaro 40:3ba2b0ea9f33 686 template <class T>
mbedalvaro 40:3ba2b0ea9f33 687 inline float vector3D<T>::distance( const vector3D<T>& pnt) const {
mbedalvaro 40:3ba2b0ea9f33 688 float vx = x-pnt.x;
mbedalvaro 40:3ba2b0ea9f33 689 float vy = y-pnt.y;
mbedalvaro 40:3ba2b0ea9f33 690 float vz = z-pnt.z;
mbedalvaro 40:3ba2b0ea9f33 691 return (float)sqrt(vx*vx + vy*vy + vz*vz);
mbedalvaro 40:3ba2b0ea9f33 692 }
mbedalvaro 40:3ba2b0ea9f33 693
mbedalvaro 40:3ba2b0ea9f33 694 template <class T>
mbedalvaro 40:3ba2b0ea9f33 695 inline float vector3D<T>::squareDistance( const vector3D<T>& pnt ) const {
mbedalvaro 40:3ba2b0ea9f33 696 float vx = x-pnt.x;
mbedalvaro 40:3ba2b0ea9f33 697 float vy = y-pnt.y;
mbedalvaro 40:3ba2b0ea9f33 698 float vz = z-pnt.z;
mbedalvaro 40:3ba2b0ea9f33 699 return vx*vx + vy*vy+ vz*vz;
mbedalvaro 40:3ba2b0ea9f33 700 }
mbedalvaro 40:3ba2b0ea9f33 701
mbedalvaro 40:3ba2b0ea9f33 702 // Normalization:
mbedalvaro 40:3ba2b0ea9f33 703 template <class T>
mbedalvaro 40:3ba2b0ea9f33 704 inline vector3D<T> vector3D<T>::getNormalized() const {
mbedalvaro 40:3ba2b0ea9f33 705 float length = (float)sqrt(x*x + y*y + z*z);
mbedalvaro 40:3ba2b0ea9f33 706 if( length > 0 ) {
mbedalvaro 40:3ba2b0ea9f33 707 return vector3D<T>( x/length, y/length , z/length);
mbedalvaro 40:3ba2b0ea9f33 708 } else {
mbedalvaro 40:3ba2b0ea9f33 709 return vector3D<T>();
mbedalvaro 40:3ba2b0ea9f33 710 }
mbedalvaro 40:3ba2b0ea9f33 711 }
mbedalvaro 40:3ba2b0ea9f33 712
mbedalvaro 40:3ba2b0ea9f33 713 template <class T>
mbedalvaro 40:3ba2b0ea9f33 714 inline vector3D<T>& vector3D<T>::normalize() {
mbedalvaro 40:3ba2b0ea9f33 715 float length = (float)sqrt(x*x + y*y + z*z);
mbedalvaro 40:3ba2b0ea9f33 716 if( length > 0 ) {
mbedalvaro 40:3ba2b0ea9f33 717 x /= length;
mbedalvaro 40:3ba2b0ea9f33 718 y /= length;
mbedalvaro 40:3ba2b0ea9f33 719 z /= length;
mbedalvaro 40:3ba2b0ea9f33 720 }
mbedalvaro 40:3ba2b0ea9f33 721 return *this;
mbedalvaro 40:3ba2b0ea9f33 722 }
mbedalvaro 40:3ba2b0ea9f33 723
mbedalvaro 40:3ba2b0ea9f33 724 // Length (norm of vector3D<T>):
mbedalvaro 40:3ba2b0ea9f33 725 template <class T>
mbedalvaro 40:3ba2b0ea9f33 726 inline float vector3D<T>::length() const {
mbedalvaro 40:3ba2b0ea9f33 727 return (float)sqrt( x*x + y*y + z*z);
mbedalvaro 40:3ba2b0ea9f33 728 }
mbedalvaro 40:3ba2b0ea9f33 729
mbedalvaro 40:3ba2b0ea9f33 730 template <class T>
mbedalvaro 40:3ba2b0ea9f33 731 inline float vector3D<T>::squareLength() const {
mbedalvaro 40:3ba2b0ea9f33 732 return (float)(x*x + y*y+ z*z);
mbedalvaro 40:3ba2b0ea9f33 733 }
mbedalvaro 40:3ba2b0ea9f33 734
mbedalvaro 40:3ba2b0ea9f33 735 // Angle between two vector3Ds:
mbedalvaro 40:3ba2b0ea9f33 736 template <class T>
mbedalvaro 40:3ba2b0ea9f33 737 inline float vector3D<T>::angleDeg( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 738 //atan2(norm(cross(a,b)), dot(a,b))
mbedalvaro 40:3ba2b0ea9f33 739 return (float)(atan2( (this->cross(vec)).length(),this->dot(vec))*RAD_TO_DEG);
mbedalvaro 40:3ba2b0ea9f33 740 }
mbedalvaro 40:3ba2b0ea9f33 741
mbedalvaro 40:3ba2b0ea9f33 742 template <class T>
mbedalvaro 40:3ba2b0ea9f33 743 inline float vector3D<T>::angleRad( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 744 return atan2((this->cross(vec)).length(),this->dot(vec));
mbedalvaro 40:3ba2b0ea9f33 745 }
mbedalvaro 40:3ba2b0ea9f33 746
mbedalvaro 40:3ba2b0ea9f33 747 //Dot Product:
mbedalvaro 40:3ba2b0ea9f33 748 template <class T>
mbedalvaro 40:3ba2b0ea9f33 749 inline float vector3D<T>::dot( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 750 return x*vec.x + y*vec.y + z*vec.z;
mbedalvaro 40:3ba2b0ea9f33 751 }
mbedalvaro 40:3ba2b0ea9f33 752
mbedalvaro 40:3ba2b0ea9f33 753 // Cross product:
mbedalvaro 40:3ba2b0ea9f33 754 template <class T>
mbedalvaro 40:3ba2b0ea9f33 755 inline vector3D<T> vector3D<T>::cross( const vector3D<T>& vec ) const {
mbedalvaro 40:3ba2b0ea9f33 756 return vector3D<T>( y*vec.z - vec.y*z, -x*vec.z + vec.x * z, x.vec.y - vec.x* y);
mbedalvaro 40:3ba2b0ea9f33 757 }
mbedalvaro 40:3ba2b0ea9f33 758
mbedalvaro 40:3ba2b0ea9f33 759 template <class T>
mbedalvaro 40:3ba2b0ea9f33 760 inline vector3D<T>& vector3D<T>::cross( const vector3D<T>& vec ) {
mbedalvaro 40:3ba2b0ea9f33 761 x = y*vec.z - vec.y*z;
mbedalvaro 40:3ba2b0ea9f33 762 y = -x*vec.z + vec.x * z;
mbedalvaro 40:3ba2b0ea9f33 763 z = x.vec.y - vec.x* y;
mbedalvaro 40:3ba2b0ea9f33 764 return *this;
mbedalvaro 40:3ba2b0ea9f33 765 }
mbedalvaro 40:3ba2b0ea9f33 766
mbedalvaro 40:3ba2b0ea9f33 767 // ======================================================================================
mbedalvaro 31:5f039cbddee8 768
mbedalvaro 31:5f039cbddee8 769 //handy typedefs:
mbedalvaro 31:5f039cbddee8 770 typedef vector2D<short> vector2Dd;
mbedalvaro 31:5f039cbddee8 771 typedef vector2D<float> vector2Df;
mbedalvaro 40:3ba2b0ea9f33 772 typedef vector3D<short> vector3Dd;
mbedalvaro 40:3ba2b0ea9f33 773 typedef vector3D<float> vector3Df;
mbedalvaro 40:3ba2b0ea9f33 774
mbedalvaro 31:5f039cbddee8 775
mbedalvaro 31:5f039cbddee8 776
mbedalvaro 40:3ba2b0ea9f33 777 // ================= other auxiliary structures ============
mbedalvaro 44:2432c218f191 778 struct Box3d {
mbedalvaro 40:3ba2b0ea9f33 779 float minX, maxX;
mbedalvaro 40:3ba2b0ea9f33 780 float minY, maxY;
mbedalvaro 40:3ba2b0ea9f33 781 float minZ, maxZ;
mbedalvaro 40:3ba2b0ea9f33 782 };
mbedalvaro 40:3ba2b0ea9f33 783
mbedalvaro 31:5f039cbddee8 784 #endif
mbedalvaro 40:3ba2b0ea9f33 785