| 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 |  |  | 
| 88 |  |  | 
| 89 |  | /** Constructs and initializes a Vector from an array */ | 
| 90 |  | inline Vector( double* v) { | 
| 91 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 92 | < | data_[i] = v[i]; | 
| 91 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 92 | > | data_[i] = v[i]; | 
| 93 |  | } | 
| 94 |  |  | 
| 95 |  | /** | 
| 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]; | 
| 179 |  | * @param v1 the other vector | 
| 180 |  | */ | 
| 181 |  | inline void add( const Vector<Real, Dim>& v1 ) { | 
| 182 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 183 | < | data_[i] += v1.data_[i]; | 
| 182 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 183 | > | data_[i] += v1.data_[i]; | 
| 184 |  | } | 
| 185 |  |  | 
| 186 |  | /** | 
| 189 |  | * @param v2 the second vector | 
| 190 |  | */ | 
| 191 |  | inline void add( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 192 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 193 | < | data_[i] = v1.data_[i] + v2.data_[i]; | 
| 192 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 193 | > | data_[i] = v1.data_[i] + v2.data_[i]; | 
| 194 |  | } | 
| 195 |  |  | 
| 196 |  | /** | 
| 217 |  | * @param s the scalar value | 
| 218 |  | */ | 
| 219 |  | inline void mul( double s ) { | 
| 220 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 220 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 221 |  | data_[i] *= s; | 
| 222 |  | } | 
| 223 |  |  | 
| 228 |  | * @param v1 the vector | 
| 229 |  | */ | 
| 230 |  | inline void mul( double s, const Vector<Real, Dim>& v1 ) { | 
| 231 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 231 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 232 |  | data_[i] = s * v1.data_[i]; | 
| 233 |  | } | 
| 234 |  |  | 
| 237 |  | * @param s the scalar value | 
| 238 |  | */ | 
| 239 |  | inline void div( double s) { | 
| 240 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 240 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 241 |  | data_[i] /= s; | 
| 242 |  | } | 
| 243 |  |  | 
| 247 |  | * @param s the scalar value | 
| 248 |  | */ | 
| 249 |  | inline void div( const Vector<Real, Dim>& v1, double s ) { | 
| 250 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 250 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 251 |  | data_[i] = v1.data_[i] / s; | 
| 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 |  | } | 
| 287 |  | * Returns the squared length of this vector. | 
| 288 |  | * @return the squared length of this vector | 
| 289 |  | */ | 
| 290 | < | inline double lengthSquared() { | 
| 290 | > | inline double lengthSquare() { | 
| 291 |  | return dot(*this, *this); | 
| 292 |  | } | 
| 293 |  |  | 
| 298 |  | len = length(); | 
| 299 |  | *this /= len; | 
| 300 |  | } | 
| 301 | + |  | 
| 302 | + | /** | 
| 303 | + | * Tests if this vector is normalized | 
| 304 | + | * @return true if this vector is normalized, otherwise return false | 
| 305 | + | */ | 
| 306 | + | inline bool isNormalized() const | 
| 307 | + | { | 
| 308 | + | return lengthSquare() == 1.0; | 
| 309 | + | } | 
| 310 |  |  | 
| 311 |  | protected: | 
| 312 |  | double data_[3]; | 
| 314 |  | }; | 
| 315 |  |  | 
| 316 |  | /** unary minus*/ | 
| 317 | < | template<typename Real, int Dim> | 
| 317 | > | template<typename Real, unsigned int Dim> | 
| 318 |  | inline Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1){ | 
| 319 |  | Vector tmp(v1); | 
| 320 |  | return tmp.negate(); | 
| 326 |  | * @param v1 the first vector | 
| 327 |  | * @param v2 the second vector | 
| 328 |  | */ | 
| 329 | < | template<typename Real, int Dim> | 
| 329 | > | template<typename Real, unsigned int Dim> | 
| 330 |  | inline Vector<Real, Dim> operator +(const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2) { | 
| 331 |  | Vector<Real, Dim> result; | 
| 332 |  |  | 
| 340 |  | * @param v1 the first vector | 
| 341 |  | * @param v2 the second vector | 
| 342 |  | */ | 
| 343 | < | template<typename Real, int Dim> | 
| 343 | > | template<typename Real, unsigned int Dim> | 
| 344 |  | Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2) { | 
| 345 |  | Vector<Real, Dim> result; | 
| 346 |  | result.sub(v1, v2); | 
| 353 |  | * @param v1 the source vector | 
| 354 |  | * @param s the scalar value | 
| 355 |  | */ | 
| 356 | < | template<typename Real, int Dim> | 
| 356 | > | template<typename Real, unsigned int Dim> | 
| 357 |  | Vector<Real, Dim> operator * ( const Vector<Real, Dim>& v1, double s) { | 
| 358 |  | Vector<Real, Dim> result; | 
| 359 |  | result.mul(s, v1); | 
| 366 |  | * @param s the scalar value | 
| 367 |  | * @param v1 the source vector | 
| 368 |  | */ | 
| 369 | < | template<typename Real, int Dim> | 
| 369 | > | template<typename Real, unsigned int Dim> | 
| 370 |  | Vector<Real, Dim> operator * ( double s, const Vector<Real, Dim>& v1 ) { | 
| 371 |  | Vector<Real, Dim> result; | 
| 372 |  | result.mul(s, v1); | 
| 379 |  | * @param v1 the source vector | 
| 380 |  | * @param s the scalar value | 
| 381 |  | */ | 
| 382 | < | template<typename Real, int Dim> | 
| 382 | > | template<typename Real, unsigned int Dim> | 
| 383 |  | Vector<Real, Dim> operator / ( const Vector<Real, Dim>& v1, double s) { | 
| 384 |  | Vector<Real, Dim> result; | 
| 385 |  | result.div( v1,s); | 
| 392 |  | * @param s the scalar value | 
| 393 |  | * @param v1 the source vector | 
| 394 |  | */ | 
| 395 | < | template<typename Real, int Dim> | 
| 395 | > | template<typename Real, unsigned int Dim> | 
| 396 |  | inline Vector<Real, Dim> operator /( double s, const Vector<Real, Dim>& v1 ) { | 
| 397 |  | Vector<Real, Dim> result; | 
| 398 |  | result.div( v1,s); | 
| 400 |  | } | 
| 401 |  |  | 
| 402 |  | /** fuzzy comparson */ | 
| 403 | < | template<typename Real, int Dim> | 
| 403 | > | template<typename Real, unsigned int Dim> | 
| 404 |  | inline bool epsilonEqual( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 405 |  |  | 
| 406 |  | } | 
| 412 |  | * @param v2 second vector | 
| 413 |  | * @return the dot product of v1 and v2 | 
| 414 |  | */ | 
| 415 | < | template<typename Real, int Dim> | 
| 415 | > | template<typename Real, unsigned int Dim> | 
| 416 |  | inline Real dot( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 417 | < | Real tmp; | 
| 418 | < | tmp = 0; | 
| 417 | > | Real tmp; | 
| 418 | > | tmp = 0; | 
| 419 |  |  | 
| 420 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 421 | < | tmp += v1[i] + v2[i]; | 
| 422 | < |  | 
| 423 | < | return tmp; | 
| 420 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 421 | > | tmp += v1[i] + v2[i]; | 
| 422 | > |  | 
| 423 | > | return tmp; | 
| 424 |  | } | 
| 425 |  |  | 
| 426 |  | /** | 
| 429 |  | * @param v2 second vector | 
| 430 |  | * @return the distance between v1 and v2 | 
| 431 |  | */ | 
| 432 | < | template<typename Real, int Dim> | 
| 432 | > | template<typename Real, unsigned int Dim> | 
| 433 |  | inline Real distance( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 434 |  | Vector<Real, Dim> tempVector = v1 - v2; | 
| 435 |  | return tempVector.length(); | 
| 441 |  | * @param v2 second vector | 
| 442 |  | * @return the squared distance between v1 and v2 | 
| 443 |  | */ | 
| 444 | < | template<typename Real, int Dim> | 
| 444 | > | template<typename Real, unsigned int Dim> | 
| 445 |  | inline Real distanceSquare( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 446 |  | Vector<Real, Dim> tempVector = v1 - v2; | 
| 447 |  | return tempVector.lengthSquare(); | 
| 450 |  | /** | 
| 451 |  | * Write to an output stream | 
| 452 |  | */ | 
| 453 | < | template<typename Real, int Dim> | 
| 454 | < | std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v1 ) { | 
| 455 | < |  | 
| 453 | > | template<typename Real, unsigned int Dim> | 
| 454 | > | std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v) { | 
| 455 | > |  | 
| 456 | > | o << "[" << v[0] << ", " << v[1] << ", " << v[2] << "]" << endl; | 
| 457 |  | return o; | 
| 458 |  | } | 
| 459 |  |  |