Main Page | Namespace List | Class Hierarchy | Class List | File List | Namespace Members | Class Members | File Members

quatern.h

Go to the documentation of this file.
00001 // -*-c++-*-
00002 #ifndef IG_QUATERN_H
00003 #define IG_QUATERN_H
00004 
00005 // $Id: quatern.h,v 1.35 2005/07/02 15:58:26 hkuiper Exp $
00006 
00007 // CwMtx matrix and vector math library
00008 // Copyright (C) 1999-2001  Harry Kuiper
00009 // Copyright (C) 2000  Will DeVore (template conversion)
00010 // Copyright (C) 2000  Jiri Ecer (constructor from exponential form)
00011 
00012 // This library is free software; you can redistribute it and/or
00013 // modify it under the terms of the GNU Lesser General Public
00014 // License as published by the Free Software Foundation; either
00015 // version 2 of the License, or (at your option) any later version.
00016 
00017 // This library is distributed in the hope that it will be useful,
00018 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00019 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00020 // Lesser General Public License for more details.
00021 
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License along with this library; if not, write to the Free Software
00024 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
00025 // USA
00026 
00027 #ifndef IG_VECTOR_H
00028 #include "vector.h"
00029 #endif
00030 
00031 #ifndef IG_SVECTOR_H
00032 #include "svector.h"
00033 #endif
00034 
00035 namespace CwMtx
00036 {
00037   using std::exp;
00038   using std::log;
00039 
00040   // prefix qtn
00041   template < class T = double >
00042   class CWTQuaternion : public CWTVector<T>
00043   {
00044   public:
00045         
00046     CWTQuaternion(): CWTVector<T>(4U) {};
00047     // assumption: mat is column vector with four elements
00048     CWTQuaternion(const CWTMatrix<T> &mat): CWTVector<T>(mat) {};
00049     // assumption: vec has four elements
00050     CWTQuaternion(const CWTVector<T> &vec): CWTVector<T>(vec) {};
00051     CWTQuaternion(const CWTQuaternion &qtn): CWTVector<T>(qtn) {};
00052     // the space vector will become the quaternion's imaginary part, T
00053     // will become its real part
00054     CWTQuaternion(const CWTSpaceVector<T> &, const T & = CWTZero<T>());
00055     // creates a quaternion, index runs from left to right
00056     CWTQuaternion(const T &, const T &, const T &, const T &);
00057     CWTQuaternion(const T &);
00058     CWTQuaternion(const CWTMatrix<T> &, unsigned, unsigned);
00059     // creates a quaternion mapped into a vector
00060     CWTQuaternion(const CWTVector<T>& vec, unsigned irowStart);
00061 
00062     // constructor from exponential form: q = r*exp(svec*angle), svec
00063     // should be a unity vector, angle is in radians
00064     CWTQuaternion(const T &r, const CWTSpaceVector<T> &svec, const T &angle);
00065 
00066     ~CWTQuaternion() {};
00067 
00068     void dimension() { CWTVector<T>::dimension(4); };
00069     void mapInto(const CWTMatrix<T> &mat,
00070                  unsigned irowStart,
00071                  unsigned icolStart);
00072     void mapInto(const CWTVector<T> &vec, unsigned irowStart);
00073 
00074     CWTQuaternion operator +(const CWTQuaternion &) const;
00075     CWTQuaternion operator -(const CWTQuaternion &) const;
00076     CWTQuaternion operator -() const;
00077     CWTQuaternion operator *(const T &) const;
00078     CWTQuaternion operator *(const CWTQuaternion &) const;
00079     CWTQuaternion operator /(const T &value) const
00080     {
00081       // NOTE: This used to work with g++ <= 3.2.
00082       // return (*this)*static_cast<const T &>(CWTUnity<T>()/value);
00083 
00084       T tTmp = CWTUnity<T>();
00085       tTmp /= value;
00086       return (*this)*tTmp;
00087     }
00088     CWTQuaternion operator /(const CWTQuaternion &) const;
00089 
00090     // not inherited
00091     CWTQuaternion & operator =(const CWTQuaternion &);
00092     CWTQuaternion & operator =(const CWTSpaceVector<T> &);
00093     CWTQuaternion & operator +=(const CWTQuaternion &);
00094     CWTQuaternion & operator -=(const CWTQuaternion &);
00095     CWTQuaternion & operator *=(const T &);
00096     CWTQuaternion & operator *=(const CWTQuaternion &);
00097     CWTQuaternion & operator /=(const T &);
00098     CWTQuaternion & operator /=(const CWTQuaternion &);
00099 
00100     // stores product of qtn*qtn in this
00101     void storeProduct(const CWTQuaternion &, const CWTQuaternion &);
00102     // stores conjugate of argument in this
00103     void storeConjugate(const CWTQuaternion &);
00104     // makes this its own conjugate
00105     void makeConjugate();
00106 
00107     // returns a unit vector with same direction as this
00108     CWTQuaternion unit() const { return (*this)/(this->norm()); }
00109   };
00110 
00111   // Unity quaternion.
00112   template <class T>
00113   class CWTUnity< CWTQuaternion<T> >: public CWTQuaternion<T>
00114   {
00115   public:
00116     CWTUnity()
00117     {
00118       (*this)[0] = (*this)[1] = (*this)[2] = CWTZero<T>();
00119       (*this)[3] = CWTUnity<T>();
00120     }
00121   };
00122 
00123   // Zero quaternion.
00124   template <class T>
00125   class CWTZero< CWTQuaternion<T> >: public CWTQuaternion<T>
00126   {
00127   public:
00128     CWTZero() { fill(CWTZero<T>()); }
00129   };
00130 
00131   //
00132   // Constructors
00133   //
00134   template < class T >
00135   inline CWTQuaternion<T>::CWTQuaternion(const T &elemIm0,
00136                                          const T &elemIm1,
00137                                          const T &elemIm2,
00138                                          const T &elemReal)
00139     :
00140     CWTVector<T>(4U)
00141   {
00142     (*this)[0] = elemIm0;
00143     (*this)[1] = elemIm1;
00144     (*this)[2] = elemIm2;
00145     (*this)[3] = elemReal;
00146   }
00147 
00148   template < class T >
00149   inline CWTQuaternion<T>::CWTQuaternion(const T &elemReal)
00150     :
00151     CWTVector<T>(4U)
00152   {
00153     (*this)[0] = CWTZero<T>();
00154     (*this)[1] = CWTZero<T>();
00155     (*this)[2] = CWTZero<T>();
00156     (*this)[3] = elemReal;
00157   }
00158 
00159   template < class T >
00160   inline CWTQuaternion<T>::CWTQuaternion(const CWTSpaceVector<T> &svecIm,
00161                                          const T &elemReal)
00162     :
00163     CWTVector<T>(4U)
00164   {
00165     (*this)[0] = svecIm[0];
00166     (*this)[1] = svecIm[1];
00167     (*this)[2] = svecIm[2];
00168     (*this)[3] = elemReal;
00169   }
00170 
00171   template < class T >
00172   inline CWTQuaternion<T>::CWTQuaternion(const CWTMatrix<T> &mat,
00173                                          unsigned irowStart,
00174                                          unsigned icolStart)
00175     :
00176     CWTVector<T>(mat, irowStart, icolStart, irowStart + 3)
00177   {
00178   }
00179 
00180   template < class T >
00181   inline CWTQuaternion<T>::CWTQuaternion(const CWTVector<T>& vec,
00182                                          unsigned irowStart)
00183     :
00184     CWTVector<T>(vec, irowStart, irowStart + 3)
00185   {
00186   }
00187 
00188   // NOTE: This only works with template arguments that can be
00189   // converted to floating point types due to the use of sin(3) and
00190   // cos(3).
00191   template < class T >
00192   CWTQuaternion<T>::CWTQuaternion(const T &r,
00193                                   const CWTSpaceVector<T> &svec,
00194                                   const T &angle)
00195     :
00196     CWTVector<T>(4U)
00197   {
00198     T rsina = r*sin(angle);
00199 
00200     (*this)[0] = svec[0]*rsina;
00201     (*this)[1] = svec[1]*rsina;
00202     (*this)[2] = svec[2]*rsina;
00203     (*this)[3] = r*cos(angle);
00204   }
00205 
00206   //
00207   // User Methods
00208   //
00209 
00210   template < class T >
00211   inline void CWTQuaternion<T>::mapInto(const CWTMatrix<T> &mat,
00212                                         unsigned irowStart,
00213                                         unsigned icolStart)
00214   {
00215     CWTVector<T>::mapInto(mat, irowStart, icolStart, irowStart + 3);
00216   }
00217 
00218   template < class T >
00219   inline void CWTQuaternion<T>::mapInto(const CWTVector<T> &vec,
00220                                         unsigned irowStart)
00221   {
00222     CWTVector<T>::mapInto(vec, irowStart, irowStart + 3);
00223   }
00224 
00225   // not inherited
00226   template < class T >
00227   inline CWTQuaternion<T> &
00228   CWTQuaternion<T>::operator =(const CWTQuaternion<T> &qtn)
00229   {
00230     return static_cast<CWTQuaternion &>(CWTMatrix<T>::operator=(qtn));
00231   }
00232 
00233   template < class T >
00234   inline CWTQuaternion<T> &
00235   CWTQuaternion<T>::operator =(const CWTSpaceVector<T> &svec)
00236   {
00237     (*this)[0] = svec[0];
00238     (*this)[1] = svec[1];
00239     (*this)[2] = svec[2];
00240     (*this)[3] = CWTZero<T>();
00241 
00242     return *this;
00243   }
00244 
00245   template < class T >
00246   inline CWTQuaternion<T> &
00247   CWTQuaternion<T>::operator +=(const CWTQuaternion<T> &qtn)
00248   {
00249     return static_cast<CWTQuaternion &>(CWTMatrix<T>::operator+=(qtn));
00250   }
00251 
00252   template < class T >
00253   inline CWTQuaternion<T> &
00254   CWTQuaternion<T>::operator -=(const CWTQuaternion<T> &qtn)
00255   {
00256     return static_cast<CWTQuaternion &>(CWTMatrix<T>::operator-=(qtn));
00257   }
00258 
00259   template < class T >
00260   inline CWTQuaternion<T> &
00261   CWTQuaternion<T>::operator *=(const T &value)
00262   {
00263     return static_cast<CWTQuaternion &>(CWTMatrix<T>::operator*=(value));
00264   }
00265 
00266   template < class T >
00267   inline CWTQuaternion<T> &
00268   CWTQuaternion<T>::operator *=(const CWTQuaternion<T> &qtn)
00269   {
00270     return (*this) = (*this)*qtn;
00271   }
00272 
00273   template < class T >
00274   inline CWTQuaternion<T> & CWTQuaternion<T>::operator /=(const T &value)
00275   {
00276     // NOTE: This used to work with g++ <= 3.2.
00277     // return (*this) *= static_cast<const T &>(CWTUnity<T>()/value);
00278 
00279     T tTmp = CWTUnity<T>();
00280     tTmp /= value;
00281     return (*this) *= tTmp;
00282   }
00283 
00284   template < class T >
00285   inline CWTQuaternion<T> &
00286   CWTQuaternion<T>::operator /=(const CWTQuaternion<T> &qtn)
00287   {
00288     return (*this) = (*this)/qtn;
00289   }
00290 
00291   template < class T >
00292   CWTQuaternion<T>
00293   CWTQuaternion<T>::operator +(const CWTQuaternion<T> &qtn) const
00294   {
00295     return CWTQuaternion(*this) += qtn;
00296   }
00297 
00298   template < class T >
00299   CWTQuaternion<T>
00300   CWTQuaternion<T>::operator -(const CWTQuaternion<T> &qtn) const
00301   {
00302     return CWTQuaternion(*this) -= qtn;
00303   }
00304 
00305   template < class T >
00306   CWTQuaternion<T> CWTQuaternion<T>::operator -() const
00307   {
00308     // NOTE: This used to work with g++ <= 3.2.
00309     // return (*this)*static_cast<const T &>(CWTZero<T>() - CWTUnity<T>());
00310 
00311     T tTmp = CWTZero<T>();
00312     tTmp -= CWTUnity<T>();
00313     return (*this)*tTmp;
00314   }
00315 
00316   template < class T >
00317   CWTQuaternion<T> CWTQuaternion<T>::operator *(const T &value) const
00318   {
00319     return CWTQuaternion(*this) *= value;
00320   }
00321 
00322   template < class T >
00323   void CWTQuaternion<T>::storeProduct(const CWTQuaternion<T> &qtnLeft,
00324                                       const CWTQuaternion<T> &qtnRight)
00325   {
00326     // to reduce the number of calls to the subscript operator
00327     T qtnLeft0 = qtnLeft[0];
00328     T qtnLeft1 = qtnLeft[1];
00329     T qtnLeft2 = qtnLeft[2];
00330     T qtnLeft3 = qtnLeft[3];
00331 
00332     T qtnRight0 = qtnRight[0];
00333     T qtnRight1 = qtnRight[1];
00334     T qtnRight2 = qtnRight[2];
00335     T qtnRight3 = qtnRight[3];
00336 
00337     (*this)[0] =
00338       qtnLeft0*qtnRight3 + qtnLeft1*qtnRight2
00339       - qtnLeft2*qtnRight1 + qtnLeft3*qtnRight0;
00340 
00341     (*this)[1] =
00342       -qtnLeft0*qtnRight2 + qtnLeft1*qtnRight3
00343       + qtnLeft2*qtnRight0 + qtnLeft3*qtnRight1;
00344 
00345     (*this)[2] =
00346       qtnLeft0*qtnRight1 - qtnLeft1*qtnRight0
00347       + qtnLeft2*qtnRight3 + qtnLeft3*qtnRight2;
00348 
00349     (*this)[3] =
00350       -qtnLeft0*qtnRight0 - qtnLeft1*qtnRight1
00351       - qtnLeft2*qtnRight2 + qtnLeft3*qtnRight3;
00352   }
00353 
00354   template < class T >
00355   CWTQuaternion<T>
00356   CWTQuaternion<T>::operator *(const CWTQuaternion<T> &qtn) const
00357   {
00358     CWTQuaternion qtnResult;
00359     qtnResult.storeProduct(*this, qtn);
00360     return qtnResult;
00361   }
00362 
00363   template < class T >
00364   CWTQuaternion<T>
00365   CWTQuaternion<T>::operator /(const CWTQuaternion<T> &qtn) const
00366   {
00367     return (*this)*inv(qtn);
00368   }
00369 
00370   // stores conjugate of argument in this
00371   template < class T >
00372   void CWTQuaternion<T>::storeConjugate(const CWTQuaternion<T> &qtn)
00373   {
00374     // copy argument into this
00375     (*this) = qtn;
00376     makeConjugate();
00377   }
00378 
00379   template < class T >
00380   void CWTQuaternion<T>::makeConjugate()
00381   {
00382     // NOTE: This used to work with g++ <= 3.2.
00383     // T tmp = static_cast<const T &>(CWTZero<T>() - CWTUnity<T>());
00384 
00385     T tTmp = CWTZero<T>();
00386     tTmp -= CWTUnity<T>();
00387 
00388     (*this)[0] *= tTmp;
00389     (*this)[1] *= tTmp;
00390     (*this)[2] *= tTmp;
00391   }
00392 
00393   //
00394   // template functions designed work with the Quaternion class.
00395   //
00396 
00397   template < class T >
00398   inline CWTQuaternion<T> operator *(const T &value,
00399                                      const CWTQuaternion<T> &qtn)
00400   {
00401     return qtn*value;
00402   }
00403 
00404   // return real part of a quaternion
00405   template < class T >
00406   inline T re(const CWTQuaternion<T> &qtn)
00407   {
00408     return qtn[3];
00409   }
00410 
00411   // returns imaginary (vector) part of a quaternion
00412   template < class T >
00413   CWTSpaceVector<T> im(const CWTQuaternion<T> &qtn)
00414   {
00415     return CWTSpaceVector<T>(qtn[0], qtn[1], qtn[2]);
00416   }
00417 
00418   // the conjugate
00419   template < class T >
00420   CWTQuaternion<T> conj(const CWTQuaternion<T> &qtn)
00421   {
00422     // copy input quaternion
00423     CWTQuaternion<T> qtnResult(qtn);
00424     qtnResult.makeConjugate();
00425     return qtnResult;
00426   }
00427 
00428   // the inverse
00429   template < class T >
00430   CWTQuaternion<T> inv(const CWTQuaternion<T> &qtn)
00431   {
00432     T qtn0 = qtn[0];
00433     T qtn1 = qtn[1];
00434     T qtn2 = qtn[2];
00435     T qtn3 = qtn[3];
00436 
00437     // NOTE: This used to work with g++ <= 3.2.
00438     // return
00439     //   conj(qtn)
00440     //     /static_cast<const T &>(qtn0*qtn0 + qtn1*qtn1 + qtn2*qtn2
00441     //                             + qtn3*qtn3);
00442 
00443     T tTmp = qtn0;
00444     tTmp *= qtn0;
00445     tTmp += qtn1*qtn1 + qtn2*qtn2 + qtn3*qtn3;
00446     return conj(qtn)/tTmp;
00447   }
00448 
00449   // the sign of a quaternion is a unit quaternion with the same
00450   // direction
00451   template <class T>
00452   inline CWTQuaternion<T>
00453   sgn(const CWTQuaternion<T> &qtn)
00454   {
00455     return qtn.unit();
00456   }
00457 
00458   // the argument of a quaternion is the angle between it and the
00459   // scalar number 1 (analogous to the argument of a complex number)
00460   template <class T>
00461   inline T
00462   arg(const CWTQuaternion<T> &qtn)
00463   {
00464     return atan2(norm(im(qtn)), re(qtn));
00465   }
00466 
00467   // quaternion exponentiation
00468   template <class T>
00469   CWTQuaternion<T>
00470   exp(const CWTQuaternion<T> &qtn)
00471   {
00472     CWTSpaceVector<T> svec = im(qtn);
00473     T len = norm(svec);
00474 
00475     if (len == CWTZero<T>())
00476       {
00477         return exp(re(qtn))*CWTQuaternion<T>(CWTZero<T>(),
00478                                              CWTZero<T>(),
00479                                              CWTZero<T>(),
00480                                              cos(len));
00481       }
00482     else
00483       {
00484         return exp(re(qtn))*CWTQuaternion<T>(sgn(svec)*sin(len), cos(len));
00485       }
00486   }
00487 
00488   // quaternion logarithm (with base e)
00489   template <class T>
00490   CWTQuaternion<T>
00491   log(const CWTQuaternion<T> &qtn)
00492   {
00493     CWTSpaceVector<T> svec = im(qtn);
00494     T len = norm(svec);
00495 
00496     if (len == CWTZero<T>())
00497       {
00498         return CWTQuaternion<T>(CWTZero<T>(),
00499                                 CWTZero<T>(),
00500                                 CWTZero<T>(),
00501                                 log(norm(qtn)));
00502       }
00503     else
00504       {
00505         return CWTQuaternion<T>(sgn(svec)*arg(qtn), log(norm(qtn)));
00506       }
00507   }
00508 
00509   // quaternion power! raise qtn1 to the power qtn2
00510   template <class T>
00511   inline CWTQuaternion<T>
00512   pow(const CWTQuaternion<T> &qtn1, const CWTQuaternion<T> &qtn2)
00513   {
00514     return exp(qtn2*log(qtn1));
00515   }
00516 }
00517 
00518 #endif // IG_QUATERN_H

Generated on Sun Jul 3 12:18:14 2005 for Matrix and vector library by  doxygen 1.4.2