| 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 | 
  | 
 | 
| 85 | 
  | 
                 | 
| 86 | 
  | 
                return *this; | 
| 87 | 
  | 
            } | 
| 88 | 
+ | 
 | 
| 89 | 
+ | 
            template<typename T> | 
| 90 | 
+ | 
            inline Vector(const T& s){ | 
| 91 | 
+ | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 92 | 
+ | 
                    data_[i] = s; | 
| 93 | 
+ | 
            } | 
| 94 | 
  | 
             | 
| 95 | 
  | 
            /** Constructs and initializes a Vector from an array */             | 
| 96 | 
  | 
            inline Vector( double* v) { | 
| 97 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 98 | 
< | 
                    data_[i] = v[i]; | 
| 97 | 
> | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 98 | 
> | 
                    data_[i] = v[i]; | 
| 99 | 
  | 
            } | 
| 100 | 
  | 
 | 
| 101 | 
  | 
            /**  | 
| 138 | 
  | 
                return data_[i]; | 
| 139 | 
  | 
            } | 
| 140 | 
  | 
 | 
| 141 | 
+ | 
            /** | 
| 142 | 
+ | 
             * Tests if this vetor is equal to other vector | 
| 143 | 
+ | 
             * @return true if equal, otherwise return false | 
| 144 | 
+ | 
             * @param v vector to be compared | 
| 145 | 
+ | 
             */ | 
| 146 | 
+ | 
             inline bool operator ==(const Vector<Real, Dim>& v) { | 
| 147 | 
+ | 
 | 
| 148 | 
+ | 
                for (unsigned int i = 0; i < Dim; i ++) { | 
| 149 | 
+ | 
                    if (!equal(data_[i], v[i])) { | 
| 150 | 
+ | 
                        return false; | 
| 151 | 
+ | 
                    } | 
| 152 | 
+ | 
                } | 
| 153 | 
+ | 
                 | 
| 154 | 
+ | 
                return true; | 
| 155 | 
+ | 
            } | 
| 156 | 
+ | 
 | 
| 157 | 
+ | 
            /** | 
| 158 | 
+ | 
             * Tests if this vetor is not 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 | 
+ | 
                return !(*this == v); | 
| 164 | 
+ | 
            } | 
| 165 | 
+ | 
              | 
| 166 | 
  | 
            /** Negates the value of this vector in place. */            | 
| 167 | 
  | 
            inline void negate() { | 
| 168 | 
< | 
                data_[0] = -data_[0]; | 
| 169 | 
< | 
                data_[1] = -data_[1]; | 
| 121 | 
< | 
                data_[2] = -data_[2]; | 
| 168 | 
> | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 169 | 
> | 
                    data_[i] = -data_[i]; | 
| 170 | 
  | 
            } | 
| 171 | 
  | 
 | 
| 172 | 
  | 
            /** | 
| 184 | 
  | 
            * @param v1 the other vector | 
| 185 | 
  | 
            */ | 
| 186 | 
  | 
            inline void add( const Vector<Real, Dim>& v1 ) { | 
| 187 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 188 | 
< | 
                    data_[i] += v1.data_[i]; | 
| 187 | 
> | 
            for (unsigned int i = 0; i < Dim; i++) | 
| 188 | 
> | 
                data_[i] += v1.data_[i]; | 
| 189 | 
  | 
                } | 
| 190 | 
  | 
 | 
| 191 | 
  | 
            /** | 
| 194 | 
  | 
            * @param v2 the second vector | 
| 195 | 
  | 
            */ | 
| 196 | 
  | 
            inline void add( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 197 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 198 | 
< | 
                    data_[i] = v1.data_[i] + v2.data_[i]; | 
| 197 | 
> | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 198 | 
> | 
                    data_[i] = v1.data_[i] + v2.data_[i]; | 
| 199 | 
  | 
            } | 
| 200 | 
  | 
 | 
| 201 | 
  | 
            /** | 
| 222 | 
  | 
            * @param s the scalar value | 
| 223 | 
  | 
            */ | 
| 224 | 
  | 
            inline void mul( double s ) { | 
| 225 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 225 | 
> | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 226 | 
  | 
                   data_[i] *= s; | 
| 227 | 
  | 
            } | 
| 228 | 
  | 
 | 
| 229 | 
  | 
            /** | 
| 230 | 
  | 
            * Sets the value of this vector to the scalar multiplication of vector v1   | 
| 231 | 
  | 
            * (*this = s * v1). | 
| 232 | 
+ | 
            * @param v1 the vector             | 
| 233 | 
  | 
            * @param s the scalar value | 
| 185 | 
– | 
            * @param v1 the vector | 
| 234 | 
  | 
            */ | 
| 235 | 
< | 
            inline void mul( double s, const Vector<Real, Dim>& v1 ) { | 
| 236 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 235 | 
> | 
            inline void mul( const Vector<Real, Dim>& v1, double s) { | 
| 236 | 
> | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 237 | 
  | 
                    data_[i] = s * v1.data_[i]; | 
| 238 | 
  | 
            } | 
| 239 | 
  | 
 | 
| 242 | 
  | 
            * @param s the scalar value | 
| 243 | 
  | 
            */              | 
| 244 | 
  | 
            inline void div( double s) { | 
| 245 | 
< | 
                for (unsigned int i = 0; i < Dim; i++)             | 
| 245 | 
> | 
                for (unsigned int i = 0; i < Dim; i++)             | 
| 246 | 
  | 
                    data_[i] /= s; | 
| 247 | 
  | 
            } | 
| 248 | 
  | 
 | 
| 252 | 
  | 
            * @param s the scalar value | 
| 253 | 
  | 
            */                          | 
| 254 | 
  | 
            inline void div( const Vector<Real, Dim>& v1, double s ) { | 
| 255 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 255 | 
> | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 256 | 
  | 
                    data_[i] = v1.data_[i] / s; | 
| 257 | 
  | 
            } | 
| 258 | 
  | 
 | 
| 259 | 
  | 
            /** @see #add */ | 
| 260 | 
< | 
            inline Vector<Real, Dim> operator +=( const Vector<Real, Dim>& v1 ) { | 
| 260 | 
> | 
            inline Vector<Real, Dim>& operator +=( const Vector<Real, Dim>& v1 ) { | 
| 261 | 
  | 
                add(v1); | 
| 262 | 
  | 
                return *this; | 
| 263 | 
  | 
            } | 
| 264 | 
  | 
 | 
| 265 | 
  | 
            /** @see #sub */ | 
| 266 | 
< | 
            inline Vector<Real, Dim> operator -=( const Vector<Real, Dim>& v1 ) { | 
| 266 | 
> | 
            inline Vector<Real, Dim>& operator -=( const Vector<Real, Dim>& v1 ) { | 
| 267 | 
  | 
                sub(v1); | 
| 268 | 
  | 
                return *this; | 
| 269 | 
  | 
            } | 
| 270 | 
  | 
 | 
| 271 | 
  | 
            /** @see #mul */ | 
| 272 | 
< | 
            inline Vector<Real, Dim> operator *=( double s) { | 
| 272 | 
> | 
            inline Vector<Real, Dim>& operator *=( double s) { | 
| 273 | 
  | 
                mul(s); | 
| 274 | 
  | 
                return *this; | 
| 275 | 
  | 
            } | 
| 276 | 
  | 
 | 
| 277 | 
  | 
            /** @see #div */ | 
| 278 | 
< | 
            inline Vector<Real, Dim> operator /=( double s ) { | 
| 278 | 
> | 
            inline Vector<Real, Dim>& operator /=( double s ) { | 
| 279 | 
  | 
                div(s); | 
| 280 | 
  | 
                return *this; | 
| 281 | 
  | 
            } | 
| 292 | 
  | 
             * Returns the squared length of this vector. | 
| 293 | 
  | 
             * @return the squared length of this vector | 
| 294 | 
  | 
             */ | 
| 295 | 
< | 
             inline double lengthSquared() { | 
| 295 | 
> | 
             inline double lengthSquare() { | 
| 296 | 
  | 
                return dot(*this, *this); | 
| 297 | 
  | 
            } | 
| 298 | 
  | 
             | 
| 303 | 
  | 
                len = length(); | 
| 304 | 
  | 
                *this /= len; | 
| 305 | 
  | 
            } | 
| 306 | 
+ | 
 | 
| 307 | 
+ | 
            /** | 
| 308 | 
+ | 
             * Tests if this vector is normalized | 
| 309 | 
+ | 
             * @return true if this vector is normalized, otherwise return false | 
| 310 | 
+ | 
             */ | 
| 311 | 
+ | 
            inline bool isNormalized() const | 
| 312 | 
+ | 
            { | 
| 313 | 
+ | 
                return lengthSquare() == 1.0; | 
| 314 | 
+ | 
            }            | 
| 315 | 
  | 
             | 
| 316 | 
  | 
        protected: | 
| 317 | 
< | 
            double data_[3]; | 
| 317 | 
> | 
            double data_[Dim]; | 
| 318 | 
  | 
         | 
| 319 | 
  | 
    }; | 
| 320 | 
  | 
 | 
| 321 | 
  | 
    /** unary minus*/ | 
| 322 | 
< | 
    template<typename Real, int Dim>     | 
| 322 | 
> | 
    template<typename Real, unsigned int Dim>     | 
| 323 | 
  | 
    inline Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1){ | 
| 324 | 
< | 
        Vector tmp(v1); | 
| 325 | 
< | 
        return tmp.negate(); | 
| 324 | 
> | 
        Vector<Real, Dim> tmp(v1); | 
| 325 | 
> | 
        tmp.negate(); | 
| 326 | 
> | 
        return tmp; | 
| 327 | 
  | 
    } | 
| 328 | 
  | 
 | 
| 329 | 
  | 
    /** | 
| 332 | 
  | 
     * @param v1 the first vector | 
| 333 | 
  | 
     * @param v2 the second vector | 
| 334 | 
  | 
     */    | 
| 335 | 
< | 
    template<typename Real, int Dim>     | 
| 335 | 
> | 
    template<typename Real, unsigned int Dim>     | 
| 336 | 
  | 
    inline Vector<Real, Dim> operator +(const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2) { | 
| 337 | 
  | 
        Vector<Real, Dim> result; | 
| 338 | 
  | 
         | 
| 346 | 
  | 
     * @param v1 the first vector | 
| 347 | 
  | 
     * @param v2 the second vector | 
| 348 | 
  | 
     */   | 
| 349 | 
< | 
    template<typename Real, int Dim>     | 
| 349 | 
> | 
    template<typename Real, unsigned int Dim>     | 
| 350 | 
  | 
    Vector<Real, Dim> operator -(const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2) { | 
| 351 | 
  | 
        Vector<Real, Dim> result; | 
| 352 | 
  | 
        result.sub(v1, v2); | 
| 359 | 
  | 
     * @param v1 the source vector | 
| 360 | 
  | 
     * @param s the scalar value | 
| 361 | 
  | 
     */  | 
| 362 | 
< | 
    template<typename Real, int Dim>                  | 
| 362 | 
> | 
    template<typename Real, unsigned int Dim>                  | 
| 363 | 
  | 
    Vector<Real, Dim> operator * ( const Vector<Real, Dim>& v1, double s) {        | 
| 364 | 
  | 
        Vector<Real, Dim> result; | 
| 365 | 
< | 
        result.mul(s, v1); | 
| 365 | 
> | 
        result.mul(v1,s); | 
| 366 | 
  | 
        return result;            | 
| 367 | 
  | 
    } | 
| 368 | 
  | 
     | 
| 372 | 
  | 
     * @param s the scalar value | 
| 373 | 
  | 
     * @param v1 the source vector | 
| 374 | 
  | 
     */   | 
| 375 | 
< | 
    template<typename Real, int Dim> | 
| 375 | 
> | 
    template<typename Real, unsigned int Dim> | 
| 376 | 
  | 
    Vector<Real, Dim> operator * ( double s, const Vector<Real, Dim>& v1 ) { | 
| 377 | 
  | 
        Vector<Real, Dim> result; | 
| 378 | 
< | 
        result.mul(s, v1); | 
| 378 | 
> | 
        result.mul(v1, s); | 
| 379 | 
  | 
        return result;            | 
| 380 | 
  | 
    } | 
| 381 | 
  | 
 | 
| 385 | 
  | 
     * @param v1 the source vector | 
| 386 | 
  | 
     * @param s the scalar value | 
| 387 | 
  | 
     */ | 
| 388 | 
< | 
    template<typename Real, int Dim>     | 
| 388 | 
> | 
    template<typename Real, unsigned int Dim>     | 
| 389 | 
  | 
    Vector<Real, Dim> operator / ( const Vector<Real, Dim>& v1, double s) {        | 
| 390 | 
  | 
        Vector<Real, Dim> result; | 
| 391 | 
  | 
        result.div( v1,s); | 
| 392 | 
  | 
        return result;            | 
| 393 | 
  | 
    } | 
| 394 | 
  | 
     | 
| 337 | 
– | 
    /** | 
| 338 | 
– | 
     * Returns the  value of division of a vector by a scalar.  | 
| 339 | 
– | 
     * @return  the vaule of scalar division of this vector | 
| 340 | 
– | 
     * @param s the scalar value | 
| 341 | 
– | 
     * @param v1 the source vector | 
| 342 | 
– | 
     */ | 
| 343 | 
– | 
    template<typename Real, int Dim>         | 
| 344 | 
– | 
    inline Vector<Real, Dim> operator /( double s, const Vector<Real, Dim>& v1 ) { | 
| 345 | 
– | 
        Vector<Real, Dim> result; | 
| 346 | 
– | 
        result.div( v1,s); | 
| 347 | 
– | 
        return result;            | 
| 348 | 
– | 
    } | 
| 349 | 
– | 
 | 
| 395 | 
  | 
    /** fuzzy comparson */ | 
| 396 | 
< | 
    template<typename Real, int Dim>         | 
| 396 | 
> | 
    template<typename Real, unsigned int Dim>         | 
| 397 | 
  | 
    inline bool epsilonEqual( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 398 | 
  | 
 | 
| 399 | 
  | 
    } | 
| 405 | 
  | 
     * @param v2 second vector | 
| 406 | 
  | 
     * @return the dot product of v1 and v2 | 
| 407 | 
  | 
     */ | 
| 408 | 
< | 
    template<typename Real, int Dim>     | 
| 408 | 
> | 
    template<typename Real, unsigned int Dim>     | 
| 409 | 
  | 
    inline Real dot( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 410 | 
< | 
                Real tmp; | 
| 411 | 
< | 
                tmp = 0; | 
| 410 | 
> | 
        Real tmp; | 
| 411 | 
> | 
        tmp = 0; | 
| 412 | 
  | 
 | 
| 413 | 
< | 
                for (unsigned int i = 0; i < Dim; i++) | 
| 414 | 
< | 
                        tmp += v1[i] + v2[i]; | 
| 415 | 
< | 
                 | 
| 416 | 
< | 
                return tmp; | 
| 413 | 
> | 
        for (unsigned int i = 0; i < Dim; i++) | 
| 414 | 
> | 
            tmp += v1[i] + v2[i]; | 
| 415 | 
> | 
 | 
| 416 | 
> | 
        return tmp; | 
| 417 | 
  | 
    } | 
| 418 | 
  | 
 | 
| 419 | 
  | 
    /** | 
| 422 | 
  | 
     * @param v2 second vector | 
| 423 | 
  | 
     * @return the distance between v1 and v2 | 
| 424 | 
  | 
     */  | 
| 425 | 
< | 
    template<typename Real, int Dim>     | 
| 425 | 
> | 
    template<typename Real, unsigned int Dim>     | 
| 426 | 
  | 
    inline Real distance( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 427 | 
  | 
        Vector<Real, Dim> tempVector = v1 - v2; | 
| 428 | 
  | 
        return tempVector.length(); | 
| 434 | 
  | 
     * @param v2 second vector | 
| 435 | 
  | 
     * @return the squared distance between v1 and v2 | 
| 436 | 
  | 
     */ | 
| 437 | 
< | 
    template<typename Real, int Dim> | 
| 437 | 
> | 
    template<typename Real, unsigned int Dim> | 
| 438 | 
  | 
    inline Real distanceSquare( const Vector<Real, Dim>& v1, const Vector<Real, Dim>& v2 ) { | 
| 439 | 
  | 
        Vector<Real, Dim> tempVector = v1 - v2; | 
| 440 | 
  | 
        return tempVector.lengthSquare(); | 
| 443 | 
  | 
    /** | 
| 444 | 
  | 
     * Write to an output stream | 
| 445 | 
  | 
     */ | 
| 446 | 
< | 
    template<typename Real, int Dim> | 
| 447 | 
< | 
    std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v1 ) { | 
| 448 | 
< | 
         | 
| 446 | 
> | 
    template<typename Real, unsigned int Dim> | 
| 447 | 
> | 
    std::ostream &operator<< ( std::ostream& o, const Vector<Real, Dim>& v) { | 
| 448 | 
> | 
 | 
| 449 | 
> | 
        o << "[" << v[0] << ", " << v[1] << ", " << v[2] << "]" << endl; | 
| 450 | 
  | 
        return o;         | 
| 451 | 
  | 
    } | 
| 452 | 
  | 
     |