| 35 |  |  | 
| 36 |  | #include <cassert> | 
| 37 |  | #include <cmath> | 
| 38 | + | #include <iostream> | 
| 39 |  |  | 
| 40 |  | namespace oopse { | 
| 41 |  |  | 
| 42 | + | const double epsilon = 0.000001; | 
| 43 | + |  | 
| 44 | + | template<typename T> | 
| 45 | + | inline bool equal(T e1, T e2) { | 
| 46 | + | return e1 == e2; | 
| 47 | + | } | 
| 48 | + |  | 
| 49 | + | template<> | 
| 50 | + | inline bool equal(float e1, float e2) { | 
| 51 | + | return fabs(e1 - e2) < epsilon; | 
| 52 | + | } | 
| 53 | + |  | 
| 54 | + | template<> | 
| 55 | + | inline bool equal(double e1, double e2) { | 
| 56 | + | return fabs(e1 - e2) < epsilon; | 
| 57 | + | } | 
| 58 | + |  | 
| 59 |  | /** | 
| 60 |  | * @class Vector Vector.hpp "math/Vector.hpp" | 
| 61 |  | * @brief Fix length vector class | 
| 62 |  | */ | 
| 63 | < | template<typename Real, int Dim> | 
| 63 | > | template<typename Real, unsigned int Dim> | 
| 64 |  | class Vector{ | 
| 65 |  | public: | 
| 66 |  |  | 
| 132 |  | return data_[i]; | 
| 133 |  | } | 
| 134 |  |  | 
| 135 | + | /** | 
| 136 | + | * Tests if this vetor is equal to other vector | 
| 137 | + | * @return true if equal, otherwise return false | 
| 138 | + | * @param v vector to be compared | 
| 139 | + | */ | 
| 140 | + | inline bool operator ==(const Vector<Real, Dim>& v) { | 
| 141 | + |  | 
| 142 | + | for (unsigned int i = 0; i < Dim; i ++) { | 
| 143 | + | if (!equal(data_[i], v[i])) { | 
| 144 | + | return false; | 
| 145 | + | } | 
| 146 | + | } | 
| 147 | + |  | 
| 148 | + | return true; | 
| 149 | + | } | 
| 150 | + |  | 
| 151 | + | /** | 
| 152 | + | * Tests if this vetor is not equal to other vector | 
| 153 | + | * @return true if equal, otherwise return false | 
| 154 | + | * @param v vector to be compared | 
| 155 | + | */ | 
| 156 | + | inline bool operator !=(const Vector<Real, Dim>& v) { | 
| 157 | + | return !(*this == v); | 
| 158 | + | } | 
| 159 | + |  | 
| 160 |  | /** Negates the value of this vector in place. */ | 
| 161 |  | inline void negate() { | 
| 162 |  | data_[0] = -data_[0]; | 
| 252 |  | } | 
| 253 |  |  | 
| 254 |  | /** @see #add */ | 
| 255 | < | inline Vector<Real, Dim> operator +=( const Vector<Real, Dim>& v1 ) { | 
| 255 | > | inline Vector<Real, Dim>& operator +=( const Vector<Real, Dim>& v1 ) { | 
| 256 |  | add(v1); | 
| 257 |  | return *this; | 
| 258 |  | } | 
| 259 |  |  | 
| 260 |  | /** @see #sub */ | 
| 261 | < | inline Vector<Real, Dim> operator -=( const Vector<Real, Dim>& v1 ) { | 
| 261 | > | inline Vector<Real, Dim>& operator -=( const Vector<Real, Dim>& v1 ) { | 
| 262 |  | sub(v1); | 
| 263 |  | return *this; | 
| 264 |  | } | 
| 265 |  |  | 
| 266 |  | /** @see #mul */ | 
| 267 | < | inline Vector<Real, Dim> operator *=( double s) { | 
| 267 | > | inline Vector<Real, Dim>& operator *=( double s) { | 
| 268 |  | mul(s); | 
| 269 |  | return *this; | 
| 270 |  | } | 
| 271 |  |  | 
| 272 |  | /** @see #div */ | 
| 273 | < | inline Vector<Real, Dim> operator /=( double s ) { | 
| 273 | > | inline Vector<Real, Dim>& operator /=( double s ) { | 
| 274 |  | div(s); | 
| 275 |  | return *this; | 
| 276 |  | } | 
| 305 |  | }; | 
| 306 |  |  | 
| 307 |  | /** unary minus*/ | 
| 308 | < | template<typename Real, int Dim> | 
| 308 | > | template<typename Real, unsigned int Dim> | 
| 309 |  | inline Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1){ | 
| 310 |  | Vector tmp(v1); | 
| 311 |  | return tmp.negate(); | 
| 317 |  | * @param v1 the first vector | 
| 318 |  | * @param v2 the second vector | 
| 319 |  | */ | 
| 320 | < | template<typename Real, int Dim> | 
| 320 | > | template<typename Real, unsigned int Dim> | 
| 321 |  | inline Vector<Real, Dim> operator +(const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2) { | 
| 322 |  | Vector<Real, Dim> result; | 
| 323 |  |  | 
| 331 |  | * @param v1 the first vector | 
| 332 |  | * @param v2 the second vector | 
| 333 |  | */ | 
| 334 | < | template<typename Real, int Dim> | 
| 334 | > | template<typename Real, unsigned int Dim> | 
| 335 |  | Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2) { | 
| 336 |  | Vector<Real, Dim> result; | 
| 337 |  | result.sub(v1, v2); | 
| 344 |  | * @param v1 the source vector | 
| 345 |  | * @param s the scalar value | 
| 346 |  | */ | 
| 347 | < | template<typename Real, int Dim> | 
| 347 | > | template<typename Real, unsigned int Dim> | 
| 348 |  | Vector<Real, Dim> operator * ( const Vector<Real, Dim>& v1, double s) { | 
| 349 |  | Vector<Real, Dim> result; | 
| 350 |  | result.mul(s, v1); | 
| 357 |  | * @param s the scalar value | 
| 358 |  | * @param v1 the source vector | 
| 359 |  | */ | 
| 360 | < | template<typename Real, int Dim> | 
| 360 | > | template<typename Real, unsigned int Dim> | 
| 361 |  | Vector<Real, Dim> operator * ( double s, const Vector<Real, Dim>& v1 ) { | 
| 362 |  | Vector<Real, Dim> result; | 
| 363 |  | result.mul(s, v1); | 
| 370 |  | * @param v1 the source vector | 
| 371 |  | * @param s the scalar value | 
| 372 |  | */ | 
| 373 | < | template<typename Real, int Dim> | 
| 373 | > | template<typename Real, unsigned int Dim> | 
| 374 |  | Vector<Real, Dim> operator / ( const Vector<Real, Dim>& v1, double s) { | 
| 375 |  | Vector<Real, Dim> result; | 
| 376 |  | result.div( v1,s); | 
| 383 |  | * @param s the scalar value | 
| 384 |  | * @param v1 the source vector | 
| 385 |  | */ | 
| 386 | < | template<typename Real, int Dim> | 
| 386 | > | template<typename Real, unsigned int Dim> | 
| 387 |  | inline Vector<Real, Dim> operator /( double s, const Vector<Real, Dim>& v1 ) { | 
| 388 |  | Vector<Real, Dim> result; | 
| 389 |  | result.div( v1,s); | 
| 391 |  | } | 
| 392 |  |  | 
| 393 |  | /** fuzzy comparson */ | 
| 394 | < | template<typename Real, int Dim> | 
| 394 | > | template<typename Real, unsigned int Dim> | 
| 395 |  | inline bool epsilonEqual( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 396 |  |  | 
| 397 |  | } | 
| 403 |  | * @param v2 second vector | 
| 404 |  | * @return the dot product of v1 and v2 | 
| 405 |  | */ | 
| 406 | < | template<typename Real, int Dim> | 
| 406 | > | template<typename Real, unsigned int Dim> | 
| 407 |  | inline Real dot( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 408 |  | Real tmp; | 
| 409 |  | tmp = 0; | 
| 420 |  | * @param v2 second vector | 
| 421 |  | * @return the distance between v1 and v2 | 
| 422 |  | */ | 
| 423 | < | template<typename Real, int Dim> | 
| 423 | > | template<typename Real, unsigned int Dim> | 
| 424 |  | inline Real distance( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 425 |  | Vector<Real, Dim> tempVector = v1 - v2; | 
| 426 |  | return tempVector.length(); | 
| 432 |  | * @param v2 second vector | 
| 433 |  | * @return the squared distance between v1 and v2 | 
| 434 |  | */ | 
| 435 | < | template<typename Real, int Dim> | 
| 435 | > | template<typename Real, unsigned int Dim> | 
| 436 |  | inline Real distanceSquare( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 437 |  | Vector<Real, Dim> tempVector = v1 - v2; | 
| 438 |  | return tempVector.lengthSquare(); | 
| 441 |  | /** | 
| 442 |  | * Write to an output stream | 
| 443 |  | */ | 
| 444 | < | template<typename Real, int Dim> | 
| 444 | > | template<typename Real, unsigned int Dim> | 
| 445 |  | std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v1 ) { | 
| 446 |  |  | 
| 447 |  | return o; |