[ VIGRA Homepage | Function Index | Class Index | Namespaces | File List | Main Page ]

vigra/quaternion.hxx |

    00001 /************************************************************************/
    00002 /*                                                                      */
    00003 /*        Copyright 2004-2010 by Hans Meine und Ullrich Koethe          */
    00004 /*                                                                      */
    00005 /*    This file is part of the VIGRA computer vision library.           */
    00006 /*    The VIGRA Website is                                              */
    00007 /*        http://hci.iwr.uni-heidelberg.de/vigra/                       */
    00008 /*    Please direct questions, bug reports, and contributions to        */
    00009 /*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
    00010 /*        vigra@informatik.uni-hamburg.de                               */
    00011 /*                                                                      */
    00012 /*    Permission is hereby granted, free of charge, to any person       */
    00013 /*    obtaining a copy of this software and associated documentation    */
    00014 /*    files (the "Software"), to deal in the Software without           */
    00015 /*    restriction, including without limitation the rights to use,      */
    00016 /*    copy, modify, merge, publish, distribute, sublicense, and/or      */
    00017 /*    sell copies of the Software, and to permit persons to whom the    */
    00018 /*    Software is furnished to do so, subject to the following          */
    00019 /*    conditions:                                                       */
    00020 /*                                                                      */
    00021 /*    The above copyright notice and this permission notice shall be    */
    00022 /*    included in all copies or substantial portions of the             */
    00023 /*    Software.                                                         */
    00024 /*                                                                      */
    00025 /*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
    00026 /*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
    00027 /*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
    00028 /*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
    00029 /*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
    00030 /*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
    00031 /*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
    00032 /*    OTHER DEALINGS IN THE SOFTWARE.                                   */
    00033 /*                                                                      */
    00034 /************************************************************************/
    00035
    00036 #ifndef VIGRA_QUATERNION_HXX
    00037 #define VIGRA_QUATERNION_HXX
    00038
    00039 #include "config.hxx"
    00040 #include "numerictraits.hxx"
    00041 #include "tinyvector.hxx"
    00042 #include "matrix.hxx"
    00043 #include "mathutil.hxx"
    00044 #include <iosfwd>   // ostream
    00045
    00046
    00047 namespace vigra {
    00048
    00049 /** Quaternion class.
    00050
    00051     Quaternions are mainly used as a compact representation for 3D rotations because
    00052     they are much less prone to round-off errors than rotation matrices, especially
    00053     when many rotations are concatenated. In addition, the angle/axis interpretation
    00054     of normalized quaternions is very intuitive. Read the
    00055     <a href="http://en.wikipedia.org/wiki/Quaternion">Wikipedia entry on quaternions</a>
    00056     for more information on the mathematics.
    00057
    00058     See also: \ref QuaternionOperations
    00059 */
    00060 template<class ValueType>
    00061 class Quaternion {
    00062   public:
    00063     typedef TinyVector<ValueType, 3> Vector;
    00064
    00065         /** the quaternion's valuetype
    00066         */
    00067     typedef ValueType value_type;
    00068
    00069         /** reference (return of operator[]).
    00070         */
    00071     typedef ValueType & reference;
    00072
    00073         /** const reference (return of operator[] const).
    00074         */
    00075     typedef ValueType const & const_reference;
    00076
    00077         /** the quaternion's squared norm type
    00078         */
    00079     typedef typename NormTraits<ValueType>::SquaredNormType SquaredNormType;
    00080
    00081         /** the quaternion's norm type
    00082         */
    00083     typedef typename NormTraits<ValueType>::NormType NormType;
    00084
    00085         /** Construct a quaternion with explicit values for the real and imaginary parts.
    00086         */
    00087     Quaternion(ValueType w = 0, ValueType x = 0, ValueType y = 0, ValueType z = 0)
    00088     : w_(w), v_(x, y, z)
    00089     {}
    00090
    00091         /** Construct a quaternion with real value and imaginary vector.
    00092
    00093             Equivalent to <tt>Quaternion(w, v[0], v[1], v[2])</tt>.
    00094         */
    00095     Quaternion(ValueType w, const Vector &v)
    00096     : w_(w), v_(v)
    00097     {}
    00098
    00099         /** Copy constructor.
    00100         */
    00101     Quaternion(const Quaternion &q)
    00102     : w_(q.w_), v_(q.v_)
    00103     {}
    00104
    00105         /** Copy assignment.
    00106         */
    00107     Quaternion & operator=(Quaternion const & other)
    00108     {
    00109         w_ = other.w_;
    00110         v_ = other.v_;
    00111         return *this;
    00112     }
    00113
    00114         /** Assign \a w to the real part and set the imaginary part to zero.
    00115         */
    00116     Quaternion & operator=(ValueType w)
    00117     {
    00118         w_ = w;
    00119         v_.init(0);
    00120         return *this;
    00121     }
    00122
    00123         /**
    00124          * Creates a Quaternion which represents the operation of
    00125          * rotating around the given axis by the given angle.
    00126          *
    00127          * The angle should be in the range -pi..3*pi for sensible
    00128          * results.
    00129          */
    00130     static Quaternion
    00131     createRotation(double angle, const Vector &rotationAxis)
    00132     {
    00133         // the natural range would be -pi..pi, but the reflective
    00134         // behavior around pi is too unexpected:
    00135         if(angle > M_PI)
    00136             angle -= 2.0*M_PI;
    00137         double t = VIGRA_CSTD::sin(angle/2.0);
    00138         double norm = rotationAxis.magnitude();
    00139         return Quaternion(VIGRA_CSTD::sqrt(1.0-t*t), t*rotationAxis/norm);
    00140     }
    00141
    00142         /** Read real part.
    00143         */
    00144     ValueType w() const { return w_; }
    00145         /** Access real part.
    00146         */
    00147     ValueType &w() { return w_; }
    00148         /** Set real part.
    00149         */
    00150     void setW(ValueType w) { w_ = w; }
    00151
    00152         /** Read imaginary part.
    00153         */
    00154     const Vector &v() const { return v_; }
    00155         /** Access imaginary part.
    00156         */
    00157     Vector &v() { return v_; }
    00158         /** Set imaginary part.
    00159         */
    00160     void setV(const Vector & v) { v_ = v; }
    00161         /** Set imaginary part.
    00162         */
    00163     void setV(ValueType x, ValueType y, ValueType z)
    00164     {
    00165         v_[0] = x;
    00166         v_[1] = y;
    00167         v_[2] = z;
    00168     }
    00169
    00170     ValueType x() const { return v_[0]; }
    00171     ValueType y() const { return v_[1]; }
    00172     ValueType z() const { return v_[2]; }
    00173     ValueType &x() { return v_[0]; }
    00174     ValueType &y() { return v_[1]; }
    00175     ValueType &z() { return v_[2]; }
    00176     void setX(ValueType x) { v_[0] = x; }
    00177     void setY(ValueType y) { v_[1] = y; }
    00178     void setZ(ValueType z) { v_[2] = z; }
    00179
    00180         /** Access entry at index (0 <=> w(), 1 <=> v[0] etc.).
    00181         */
    00182     value_type & operator[](int index)
    00183     {
    00184         return (&w_)[index];
    00185     }
    00186
    00187         /** Read entry at index (0 <=> w(), 1 <=> v[0] etc.).
    00188         */
    00189     value_type operator[](int index) const
    00190     {
    00191         return (&w_)[index];
    00192     }
    00193
    00194         /** Magnitude.
    00195         */
    00196     NormType magnitude() const
    00197     {
    00198         return VIGRA_CSTD::sqrt((NormType)squaredMagnitude());
    00199     }
    00200
    00201         /** Squared magnitude.
    00202         */
    00203     SquaredNormType squaredMagnitude() const
    00204     {
    00205         return w_*w_ + v_.squaredMagnitude();
    00206     }
    00207
    00208         /** Add \a w to the real part.
    00209
    00210             If the quaternion represents a rotation, the rotation angle is
    00211             increased by \a w.
    00212         */
    00213     Quaternion &operator+=(value_type const &w)
    00214     {
    00215         w_ += w;
    00216         return *this;
    00217     }
    00218
    00219         /** Add assigment.
    00220         */
    00221     Quaternion &operator+=(Quaternion const &other)
    00222     {
    00223         w_ += other.w_;
    00224         v_ += other.v_;
    00225         return *this;
    00226     }
    00227
    00228         /** Subtract \a w from the real part.
    00229
    00230             If the quaternion represents a rotation, the rotation angle is
    00231             decreased by \a w.
    00232         */
    00233     Quaternion &operator-=(value_type const &w)
    00234     {
    00235         w_ -= w;
    00236         return *this;
    00237     }
    00238
    00239         /** Subtract assigment.
    00240         */
    00241     Quaternion &operator-=(Quaternion const &other)
    00242     {
    00243         w_ -= other.w_;
    00244         v_ -= other.v_;
    00245         return *this;
    00246     }
    00247
    00248         /** Addition.
    00249         */
    00250     Quaternion operator+() const
    00251     {
    00252         return *this;
    00253     }
    00254
    00255         /** Subtraction.
    00256         */
    00257     Quaternion operator-() const
    00258     {
    00259         return Quaternion(-w_, -v_);
    00260     }
    00261
    00262         /** Multiply assignment.
    00263
    00264             If the quaternions represent rotations, the rotations of <tt>this</tt> and
    00265             \a other are concatenated.
    00266         */
    00267     Quaternion &operator*=(Quaternion const &other)
    00268     {
    00269         value_type newW = w_*other.w_ - dot(v_, other.v_);
    00270         v_              = w_ * other.v_ + other.w_ * v_ + cross(v_, other.v_);
    00271         w_              = newW;
    00272         return *this;
    00273     }
    00274
    00275         /** Multiply all entries with the scalar \a scale.
    00276         */
    00277     Quaternion &operator*=(double scale)
    00278     {
    00279         w_ *= scale;
    00280         v_ *= scale;
    00281         return *this;
    00282     }
    00283
    00284         /** Divide assignment.
    00285         */
    00286     Quaternion &operator/=(Quaternion const &other)
    00287     {
    00288         (*this) *= conj(other) / squaredNorm(other);
    00289         return *this;
    00290     }
    00291
    00292         /** Devide all entries by the scalar \a scale.
    00293         */
    00294     Quaternion &operator/=(double scale)
    00295     {
    00296         w_ /= scale;
    00297         v_ /= scale;
    00298         return *this;
    00299     }
    00300
    00301         /** Equal.
    00302         */
    00303     bool operator==(Quaternion const &other) const
    00304     {
    00305       return (w_ == other.w_) && (v_ == other.v_);
    00306     }
    00307
    00308         /** Not equal.
    00309         */
    00310     bool operator!=(Quaternion const &other) const
    00311     {
    00312       return (w_ != other.w_) || (v_ != other.v_);
    00313     }
    00314
    00315         /**
    00316          * Fill the first 3x3 elements of the given matrix with a
    00317          * rotation matrix performing the same 3D rotation as this
    00318          * quaternion.  If matrix is in column-major format, it should
    00319          * be pre-multiplied with the vectors to be rotated, i.e.
    00320          * matrix[0][0-3] will be the rotated X axis.
    00321          */
    00322     template<class MatrixType>
    00323     void fillRotationMatrix(MatrixType &matrix) const
    00324     {
    00325         // scale by 2 / norm
    00326         typename NumericTraits<ValueType>::RealPromote s =
    00327             2 / (typename NumericTraits<ValueType>::RealPromote)squaredMagnitude();
    00328
    00329         Vector
    00330             vs = v_ * s,
    00331             wv = w_ * vs,
    00332             vv = vs * v_;
    00333         value_type
    00334             xy = vs[0] * v_[1],
    00335             xz = vs[0] * v_[2],
    00336             yz = vs[1] * v_[2];
    00337
    00338         matrix[0][0] = 1 - (vv[1] + vv[2]);
    00339         matrix[0][1] =     ( xy   - wv[2]);
    00340         matrix[0][2] =     ( xz   + wv[1]);
    00341
    00342         matrix[1][0] =     ( xy   + wv[2]);
    00343         matrix[1][1] = 1 - (vv[0] + vv[2]);
    00344         matrix[1][2] =     ( yz   - wv[0]);
    00345
    00346         matrix[2][0] =     ( xz   - wv[1]);
    00347         matrix[2][1] =     ( yz   + wv[0]);
    00348         matrix[2][2] = 1 - (vv[0] + vv[1]);
    00349     }
    00350
    00351     void fillRotationMatrix(Matrix<value_type> &matrix) const
    00352     {
    00353         // scale by 2 / norm
    00354         typename NumericTraits<ValueType>::RealPromote s =
    00355             2 / (typename NumericTraits<ValueType>::RealPromote)squaredMagnitude();
    00356
    00357         Vector
    00358             vs = v_ * s,
    00359             wv = w_ * vs,
    00360             vv = vs * v_;
    00361         value_type
    00362             xy = vs[0] * v_[1],
    00363             xz = vs[0] * v_[2],
    00364             yz = vs[1] * v_[2];
    00365
    00366         matrix(0, 0) = 1 - (vv[1] + vv[2]);
    00367         matrix(0, 1) =     ( xy   - wv[2]);
    00368         matrix(0, 2) =     ( xz   + wv[1]);
    00369
    00370         matrix(1, 0) =     ( xy   + wv[2]);
    00371         matrix(1, 1) = 1 - (vv[0] + vv[2]);
    00372         matrix(1, 2) =     ( yz   - wv[0]);
    00373
    00374         matrix(2, 0) =     ( xz   - wv[1]);
    00375         matrix(2, 1) =     ( yz   + wv[0]);
    00376         matrix(2, 2) = 1 - (vv[0] + vv[1]);
    00377     }
    00378
    00379   protected:
    00380     ValueType w_;
    00381     Vector v_;
    00382 };
    00383
    00384 template<class T>
    00385 struct NormTraits<Quaternion<T> >
    00386 {
    00387     typedef Quaternion<T>                                                  Type;
    00388     typedef typename NumericTraits<T>::Promote                             SquaredNormType;
    00389     typedef typename SquareRootTraits<SquaredNormType>::SquareRootResult   NormType;
    00390 };
    00391
    00392
    00393 /** \defgroup QuaternionOperations Quaternion Operations
    00394 */
    00395 //@{
    00396
    00397     /// Create conjugate quaternion.
    00398 template<class ValueType>
    00399 Quaternion<ValueType> conj(Quaternion<ValueType> const & q)
    00400 {
    00401     return Quaternion<ValueType>(q.w(), -q.v());
    00402 }
    00403
    00404     /// Addition.
    00405 template<typename Type>
    00406 inline Quaternion<Type>
    00407 operator+(const Quaternion<Type>& t1,
    00408            const Quaternion<Type>& t2)
    00409 {
    00410   return Quaternion<Type>(t1) += t2;
    00411 }
    00412
    00413     /// Addition of a scalar on the right.
    00414 template<typename Type>
    00415 inline Quaternion<Type>
    00416 operator+(const Quaternion<Type>& t1,
    00417            const Type& t2)
    00418 {
    00419   return Quaternion<Type>(t1) += t2;
    00420 }
    00421
    00422     /// Addition of a scalar on the left.
    00423 template<typename Type>
    00424 inline Quaternion<Type>
    00425 operator+(const Type& t1,
    00426            const Quaternion<Type>& t2)
    00427 {
    00428   return Quaternion<Type>(t1) += t2;
    00429 }
    00430
    00431     /// Subtraction.
    00432 template<typename Type>
    00433 inline Quaternion<Type>
    00434 operator-(const Quaternion<Type>& t1,
    00435            const Quaternion<Type>& t2)
    00436 {
    00437   return Quaternion<Type>(t1) -= t2;
    00438 }
    00439
    00440     /// Subtraction of a scalar on the right.
    00441 template<typename Type>
    00442 inline Quaternion<Type>
    00443 operator-(const Quaternion<Type>& t1,
    00444            const Type& t2)
    00445 {
    00446   return Quaternion<Type>(t1) -= t2;
    00447 }
    00448
    00449     /// Subtraction of a scalar on the left.
    00450 template<typename Type>
    00451 inline Quaternion<Type>
    00452 operator-(const Type& t1,
    00453            const Quaternion<Type>& t2)
    00454 {
    00455   return Quaternion<Type>(t1) -= t2;
    00456 }
    00457
    00458     /// Multiplication.
    00459 template<typename Type>
    00460 inline Quaternion<Type>
    00461 operator*(const Quaternion<Type>& t1,
    00462            const Quaternion<Type>& t2)
    00463 {
    00464   return Quaternion<Type>(t1) *= t2;
    00465 }
    00466
    00467     /// Multiplication with a scalar on the right.
    00468 template<typename Type>
    00469 inline Quaternion<Type>
    00470 operator*(const Quaternion<Type>& t1,
    00471            double t2)
    00472 {
    00473   return Quaternion<Type>(t1) *= t2;
    00474 }
    00475
    00476     /// Multiplication with a scalar on the left.
    00477 template<typename Type>
    00478 inline Quaternion<Type>
    00479 operator*(double t1,
    00480            const Quaternion<Type>& t2)
    00481 {
    00482   return Quaternion<Type>(t1) *= t2;
    00483 }
    00484
    00485     /// Division
    00486 template<typename Type>
    00487 inline Quaternion<Type>
    00488 operator/(const Quaternion<Type>& t1,
    00489            const Quaternion<Type>& t2)
    00490 {
    00491   return Quaternion<Type>(t1) /= t2;
    00492 }
    00493
    00494     /// Division by a scalar.
    00495 template<typename Type>
    00496 inline Quaternion<Type>
    00497 operator/(const Quaternion<Type>& t1,
    00498            double t2)
    00499 {
    00500   return Quaternion<Type>(t1) /= t2;
    00501 }
    00502
    00503     /// Division of a scalar by a Quaternion.
    00504 template<typename Type>
    00505 inline Quaternion<Type>
    00506 operator/(double t1,
    00507            const Quaternion<Type>& t2)
    00508 {
    00509   return Quaternion<Type>(t1) /= t2;
    00510 }
    00511
    00512     /// squared norm
    00513 template<typename Type>
    00514 inline
    00515 typename Quaternion<Type>::SquaredNormType
    00516 squaredNorm(Quaternion<Type> const & q)
    00517 {
    00518     return q.squaredMagnitude();
    00519 }
    00520
    00521     /// norm
    00522 template<typename Type>
    00523 inline
    00524 typename Quaternion<Type>::NormType
    00525 abs(Quaternion<Type> const & q)
    00526 {
    00527     return norm(q);
    00528 }
    00529
    00530 //@}
    00531
    00532 } // namespace vigra
    00533
    00534 namespace std {
    00535
    00536 template<class ValueType>
    00537 inline
    00538 ostream & operator<<(ostream & os, vigra::Quaternion<ValueType> const & q)
    00539 {
    00540     os << q.w() << " " << q.x() << " " << q.y() << " " << q.z();
    00541     return os;
    00542 }
    00543
    00544 template<class ValueType>
    00545 inline
    00546 istream & operator>>(istream & is, vigra::Quaternion<ValueType> & q)
    00547 {
    00548     ValueType w, x, y, z;
    00549     is >> w >> x >> y >> z;
    00550     q.setW(w);
    00551     q.setX(x);
    00552     q.setY(y);
    00553     q.setZ(z);
    00554     return is;
    00555 }
    00556
    00557 } // namespace std
    00558
    00559 #endif // VIGRA_QUATERNION_HXX

© Ullrich Köthe (ullrich.koethe@iwr.uni-heidelberg.de) Heidelberg Collaboratory for Image Processing, University of Heidelberg, Germany | html generated using doxygen and Python vigra 1.9.0 (Tue Nov 6 2012)