| 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 |  | /** | 
| 61 |  | * @class Vector Vector.hpp "math/Vector.hpp" | 
| 62 |  | * @brief Fix length vector class | 
| 65 |  | class Vector{ | 
| 66 |  | public: | 
| 67 |  |  | 
| 68 | + | typedef Real ElemType; | 
| 69 | + | typedef Real* ElemPoinerType; | 
| 70 | + |  | 
| 71 |  | /** default constructor */ | 
| 72 |  | inline Vector(){ | 
| 73 |  | for (unsigned int i = 0; i < Dim; i++) | 
| 89 |  |  | 
| 90 |  | return *this; | 
| 91 |  | } | 
| 92 | + |  | 
| 93 | + | template<typename T> | 
| 94 | + | inline Vector(const T& s){ | 
| 95 | + | for (unsigned int i = 0; i < Dim; i++) | 
| 96 | + | data_[i] = s; | 
| 97 | + | } | 
| 98 |  |  | 
| 99 |  | /** Constructs and initializes a Vector from an array */ | 
| 100 | < | inline Vector( double* v) { | 
| 101 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 102 | < | data_[i] = v[i]; | 
| 100 | > | inline Vector( Real* v) { | 
| 101 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 102 | > | data_[i] = v[i]; | 
| 103 |  | } | 
| 104 |  |  | 
| 105 |  | /** | 
| 107 |  | * @return reference of ith element | 
| 108 |  | * @param i index | 
| 109 |  | */ | 
| 110 | < | inline double& operator[](unsigned int  i) { | 
| 110 | > | inline Real& operator[](unsigned int  i) { | 
| 111 |  | assert( i < Dim); | 
| 112 |  | return data_[i]; | 
| 113 |  | } | 
| 117 |  | * @return reference of ith element | 
| 118 |  | * @param i index | 
| 119 |  | */ | 
| 120 | < | inline double& operator()(unsigned int  i) { | 
| 120 | > | inline Real& operator()(unsigned int  i) { | 
| 121 |  | assert( i < Dim); | 
| 122 |  | return data_[i]; | 
| 123 |  | } | 
| 127 |  | * @return reference of ith element | 
| 128 |  | * @param i index | 
| 129 |  | */ | 
| 130 | < | inline  const double& operator[](unsigned int i) const { | 
| 130 | > | inline  const Real& operator[](unsigned int i) const { | 
| 131 |  | assert( i < Dim); | 
| 132 |  | return data_[i]; | 
| 133 |  | } | 
| 137 |  | * @return reference of ith element | 
| 138 |  | * @param i index | 
| 139 |  | */ | 
| 140 | < | inline  const double& operator()(unsigned int i) const { | 
| 140 | > | inline  const Real& operator()(unsigned int i) const { | 
| 141 |  | assert( i < Dim); | 
| 142 |  | return data_[i]; | 
| 143 |  | } | 
| 144 |  |  | 
| 145 | + | /** Copy the internal data to an array*/ | 
| 146 | + | void getArray(Real* array) { | 
| 147 | + | for (unsigned int i = 0; i < Dim; i ++) { | 
| 148 | + | array[i] = data_[i]; | 
| 149 | + | } | 
| 150 | + | } | 
| 151 | + |  | 
| 152 | + | /** Returns the pointer of internal array */ | 
| 153 | + | Real* getArrayPointer() { | 
| 154 | + | return data_; | 
| 155 | + | } | 
| 156 | + |  | 
| 157 | + | /** | 
| 158 | + | * Tests if this vetor is equal to other vector | 
| 159 | + | * @return true if equal, otherwise return false | 
| 160 | + | * @param v vector to be compared | 
| 161 | + | */ | 
| 162 | + | inline bool operator ==(const Vector<Real, Dim>& v) { | 
| 163 | + |  | 
| 164 | + | for (unsigned int i = 0; i < Dim; i ++) { | 
| 165 | + | if (!equal(data_[i], v[i])) { | 
| 166 | + | return false; | 
| 167 | + | } | 
| 168 | + | } | 
| 169 | + |  | 
| 170 | + | return true; | 
| 171 | + | } | 
| 172 | + |  | 
| 173 | + | /** | 
| 174 | + | * Tests if this vetor is not equal to other vector | 
| 175 | + | * @return true if equal, otherwise return false | 
| 176 | + | * @param v vector to be compared | 
| 177 | + | */ | 
| 178 | + | inline bool operator !=(const Vector<Real, Dim>& v) { | 
| 179 | + | return !(*this == v); | 
| 180 | + | } | 
| 181 | + |  | 
| 182 |  | /** Negates the value of this vector in place. */ | 
| 183 |  | inline void negate() { | 
| 184 | < | data_[0] = -data_[0]; | 
| 185 | < | data_[1] = -data_[1]; | 
| 122 | < | data_[2] = -data_[2]; | 
| 184 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 185 | > | data_[i] = -data_[i]; | 
| 186 |  | } | 
| 187 |  |  | 
| 188 |  | /** | 
| 200 |  | * @param v1 the other vector | 
| 201 |  | */ | 
| 202 |  | inline void add( const Vector<Real, Dim>& v1 ) { | 
| 203 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 204 | < | data_[i] += v1.data_[i]; | 
| 205 | < | } | 
| 203 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 204 | > | data_[i] += v1.data_[i]; | 
| 205 | > | } | 
| 206 |  |  | 
| 207 |  | /** | 
| 208 |  | * Sets the value of this vector to the sum of v1 and v2 (*this = v1 + v2). | 
| 210 |  | * @param v2 the second vector | 
| 211 |  | */ | 
| 212 |  | inline void add( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 213 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 214 | < | data_[i] = v1.data_[i] + v2.data_[i]; | 
| 213 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 214 | > | data_[i] = v1.data_[i] + v2.data_[i]; | 
| 215 |  | } | 
| 216 |  |  | 
| 217 |  | /** | 
| 237 |  | * Sets the value of this vector to the scalar multiplication of itself (*this *= s). | 
| 238 |  | * @param s the scalar value | 
| 239 |  | */ | 
| 240 | < | inline void mul( double s ) { | 
| 241 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 240 | > | inline void mul( Real s ) { | 
| 241 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 242 |  | data_[i] *= s; | 
| 243 |  | } | 
| 244 |  |  | 
| 245 |  | /** | 
| 246 |  | * Sets the value of this vector to the scalar multiplication of vector v1 | 
| 247 |  | * (*this = s * v1). | 
| 248 | + | * @param v1 the vector | 
| 249 |  | * @param s the scalar value | 
| 186 | – | * @param v1 the vector | 
| 250 |  | */ | 
| 251 | < | inline void mul( double s, const Vector<Real, Dim>& v1 ) { | 
| 252 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 251 | > | inline void mul( const Vector<Real, Dim>& v1, Real s) { | 
| 252 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 253 |  | data_[i] = s * v1.data_[i]; | 
| 254 |  | } | 
| 255 |  |  | 
| 257 |  | * Sets the value of this vector to the scalar division of itself  (*this /= s ). | 
| 258 |  | * @param s the scalar value | 
| 259 |  | */ | 
| 260 | < | inline void div( double s) { | 
| 261 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 260 | > | inline void div( Real s) { | 
| 261 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 262 |  | data_[i] /= s; | 
| 263 |  | } | 
| 264 |  |  | 
| 267 |  | * @param v1 the source vector | 
| 268 |  | * @param s the scalar value | 
| 269 |  | */ | 
| 270 | < | inline void div( const Vector<Real, Dim>& v1, double s ) { | 
| 271 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 270 | > | inline void div( const Vector<Real, Dim>& v1, Real s ) { | 
| 271 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 272 |  | data_[i] = v1.data_[i] / s; | 
| 273 |  | } | 
| 274 |  |  | 
| 285 |  | } | 
| 286 |  |  | 
| 287 |  | /** @see #mul */ | 
| 288 | < | inline Vector<Real, Dim>& operator *=( double s) { | 
| 288 | > | inline Vector<Real, Dim>& operator *=( Real s) { | 
| 289 |  | mul(s); | 
| 290 |  | return *this; | 
| 291 |  | } | 
| 292 |  |  | 
| 293 |  | /** @see #div */ | 
| 294 | < | inline Vector<Real, Dim>& operator /=( double s ) { | 
| 294 | > | inline Vector<Real, Dim>& operator /=( Real s ) { | 
| 295 |  | div(s); | 
| 296 |  | return *this; | 
| 297 |  | } | 
| 300 |  | * Returns the length of this vector. | 
| 301 |  | * @return the length of this vector | 
| 302 |  | */ | 
| 303 | < | inline double length() { | 
| 304 | < | return sqrt(lengthSquared()); | 
| 303 | > | inline Real length() { | 
| 304 | > | return sqrt(lengthSquare()); | 
| 305 |  | } | 
| 306 |  |  | 
| 307 |  | /** | 
| 308 |  | * Returns the squared length of this vector. | 
| 309 |  | * @return the squared length of this vector | 
| 310 |  | */ | 
| 311 | < | inline double lengthSquared() { | 
| 311 | > | inline Real lengthSquare() { | 
| 312 |  | return dot(*this, *this); | 
| 313 |  | } | 
| 314 |  |  | 
| 315 |  | /** Normalizes this vector in place */ | 
| 316 |  | inline void normalize() { | 
| 317 | < | double len; | 
| 317 | > | Real len; | 
| 318 |  |  | 
| 319 |  | len = length(); | 
| 320 | + |  | 
| 321 | + | //if (len < oopse:epsilon) | 
| 322 | + | //  throw(); | 
| 323 | + |  | 
| 324 |  | *this /= len; | 
| 325 |  | } | 
| 326 | + |  | 
| 327 | + | /** | 
| 328 | + | * Tests if this vector is normalized | 
| 329 | + | * @return true if this vector is normalized, otherwise return false | 
| 330 | + | */ | 
| 331 | + | inline bool isNormalized() { | 
| 332 | + | return equal(lengthSquare(), 1.0); | 
| 333 | + | } | 
| 334 |  |  | 
| 335 |  | protected: | 
| 336 | < | double data_[3]; | 
| 336 | > | Real data_[Dim]; | 
| 337 |  |  | 
| 338 |  | }; | 
| 339 |  |  | 
| 340 |  | /** unary minus*/ | 
| 341 |  | template<typename Real, unsigned int Dim> | 
| 342 |  | inline Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1){ | 
| 343 | < | Vector tmp(v1); | 
| 344 | < | return tmp.negate(); | 
| 343 | > | Vector<Real, Dim> tmp(v1); | 
| 344 | > | tmp.negate(); | 
| 345 | > | return tmp; | 
| 346 |  | } | 
| 347 |  |  | 
| 348 |  | /** | 
| 379 |  | * @param s the scalar value | 
| 380 |  | */ | 
| 381 |  | template<typename Real, unsigned int Dim> | 
| 382 | < | Vector<Real, Dim> operator * ( const Vector<Real, Dim>& v1, double s) { | 
| 382 | > | Vector<Real, Dim> operator * ( const Vector<Real, Dim>& v1, Real s) { | 
| 383 |  | Vector<Real, Dim> result; | 
| 384 | < | result.mul(s, v1); | 
| 384 | > | result.mul(v1,s); | 
| 385 |  | return result; | 
| 386 |  | } | 
| 387 |  |  | 
| 392 |  | * @param v1 the source vector | 
| 393 |  | */ | 
| 394 |  | template<typename Real, unsigned int Dim> | 
| 395 | < | Vector<Real, Dim> operator * ( double s, const Vector<Real, Dim>& v1 ) { | 
| 395 | > | Vector<Real, Dim> operator * ( Real s, const Vector<Real, Dim>& v1 ) { | 
| 396 |  | Vector<Real, Dim> result; | 
| 397 | < | result.mul(s, v1); | 
| 397 | > | result.mul(v1, s); | 
| 398 |  | return result; | 
| 399 |  | } | 
| 400 |  |  | 
| 405 |  | * @param s the scalar value | 
| 406 |  | */ | 
| 407 |  | template<typename Real, unsigned int Dim> | 
| 408 | < | Vector<Real, Dim> operator / ( const Vector<Real, Dim>& v1, double s) { | 
| 408 | > | Vector<Real, Dim> operator / ( const Vector<Real, Dim>& v1, Real s) { | 
| 409 |  | Vector<Real, Dim> result; | 
| 410 |  | result.div( v1,s); | 
| 411 |  | return result; | 
| 412 |  | } | 
| 413 |  |  | 
| 414 |  | /** | 
| 339 | – | * Returns the  value of division of a vector by a scalar. | 
| 340 | – | * @return  the vaule of scalar division of this vector | 
| 341 | – | * @param s the scalar value | 
| 342 | – | * @param v1 the source vector | 
| 343 | – | */ | 
| 344 | – | template<typename Real, unsigned int Dim> | 
| 345 | – | inline Vector<Real, Dim> operator /( double s, const Vector<Real, Dim>& v1 ) { | 
| 346 | – | Vector<Real, Dim> result; | 
| 347 | – | result.div( v1,s); | 
| 348 | – | return result; | 
| 349 | – | } | 
| 350 | – |  | 
| 351 | – | /** fuzzy comparson */ | 
| 352 | – | template<typename Real, unsigned int Dim> | 
| 353 | – | inline bool epsilonEqual( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 354 | – |  | 
| 355 | – | } | 
| 356 | – |  | 
| 357 | – |  | 
| 358 | – | /** | 
| 415 |  | * Returns the dot product of two Vectors | 
| 416 |  | * @param v1 first vector | 
| 417 |  | * @param v2 second vector | 
| 419 |  | */ | 
| 420 |  | template<typename Real, unsigned int Dim> | 
| 421 |  | inline Real dot( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 422 | < | Real tmp; | 
| 423 | < | tmp = 0; | 
| 422 | > | Real tmp; | 
| 423 | > | tmp = 0; | 
| 424 |  |  | 
| 425 | < | for (unsigned int i = 0; i < Dim; i++) | 
| 426 | < | tmp += v1[i] + v2[i]; | 
| 427 | < |  | 
| 428 | < | return tmp; | 
| 425 | > | for (unsigned int i = 0; i < Dim; i++) | 
| 426 | > | tmp += v1[i] * v2[i]; | 
| 427 | > |  | 
| 428 | > | return tmp; | 
| 429 |  | } | 
| 430 |  |  | 
| 431 |  | /** | 
| 456 |  | * Write to an output stream | 
| 457 |  | */ | 
| 458 |  | template<typename Real, unsigned int Dim> | 
| 459 | < | std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v1 ) { | 
| 459 | > | std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v) { | 
| 460 | > |  | 
| 461 | > | o << "[ "; | 
| 462 |  |  | 
| 463 | + | for (unsigned int i = 0 ; i< Dim; i++) { | 
| 464 | + | o << v[i]; | 
| 465 | + |  | 
| 466 | + | if (i  != Dim -1) { | 
| 467 | + | o<< ", "; | 
| 468 | + | } | 
| 469 | + | } | 
| 470 | + |  | 
| 471 | + | o << " ]"; | 
| 472 |  | return o; | 
| 473 |  | } | 
| 474 |  |  |