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

vigra/quaternion.hxx VIGRA

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)