Main MRPT website > C++ reference
MRPT logo

Quaternion.h

Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
00006 //
00007 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 #ifndef EIGEN_QUATERNION_H
00027 #define EIGEN_QUATERNION_H
00028 
00029 /***************************************************************************
00030 * Definition of QuaternionBase<Derived>
00031 * The implementation is at the end of the file
00032 ***************************************************************************/
00033 
00034 namespace internal {
00035 template<typename Other,
00036          int OtherRows=Other::RowsAtCompileTime,
00037          int OtherCols=Other::ColsAtCompileTime>
00038 struct quaternionbase_assign_impl;
00039 }
00040 
00041 template<class Derived>
00042 class QuaternionBase : public RotationBase<Derived, 3>
00043 {
00044   typedef RotationBase<Derived, 3> Base;
00045 public:
00046   using Base::operator*;
00047   using Base::derived;
00048 
00049   typedef typename internal::traits<Derived>::Scalar Scalar;
00050   typedef typename NumTraits<Scalar>::Real RealScalar;
00051   typedef typename internal::traits<Derived>::Coefficients Coefficients;
00052 
00053  // typedef typename Matrix<Scalar,4,1> Coefficients;
00054   /** the type of a 3D vector */
00055   typedef Matrix<Scalar,3,1> Vector3;
00056   /** the equivalent rotation matrix type */
00057   typedef Matrix<Scalar,3,3> Matrix3;
00058   /** the equivalent angle-axis type */
00059   typedef AngleAxis<Scalar> AngleAxisType;
00060 
00061 
00062 
00063   /** \returns the \c x coefficient */
00064   inline Scalar x() const { return this->derived().coeffs().coeff(0); }
00065   /** \returns the \c y coefficient */
00066   inline Scalar y() const { return this->derived().coeffs().coeff(1); }
00067   /** \returns the \c z coefficient */
00068   inline Scalar z() const { return this->derived().coeffs().coeff(2); }
00069   /** \returns the \c w coefficient */
00070   inline Scalar w() const { return this->derived().coeffs().coeff(3); }
00071 
00072   /** \returns a reference to the \c x coefficient */
00073   inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
00074   /** \returns a reference to the \c y coefficient */
00075   inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
00076   /** \returns a reference to the \c z coefficient */
00077   inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
00078   /** \returns a reference to the \c w coefficient */
00079   inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
00080 
00081   /** \returns a read-only vector expression of the imaginary part (x,y,z) */
00082   inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
00083 
00084   /** \returns a vector expression of the imaginary part (x,y,z) */
00085   inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
00086 
00087   /** \returns a read-only vector expression of the coefficients (x,y,z,w) */
00088   inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
00089 
00090   /** \returns a vector expression of the coefficients (x,y,z,w) */
00091   inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
00092 
00093   EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
00094   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
00095 
00096 // disabled this copy operator as it is giving very strange compilation errors when compiling
00097 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
00098 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
00099 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
00100 //  Derived& operator=(const QuaternionBase& other)
00101 //  { return operator=<Derived>(other); }
00102 
00103   Derived& operator=(const AngleAxisType& aa);
00104   template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
00105 
00106   /** \returns a quaternion representing an identity rotation
00107     * \sa MatrixBase::Identity()
00108     */
00109   inline static Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
00110 
00111   /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity()
00112     */
00113   inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
00114 
00115   /** \returns the squared norm of the quaternion's coefficients
00116     * \sa QuaternionBase::norm(), MatrixBase::squaredNorm()
00117     */
00118   inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
00119 
00120   /** \returns the norm of the quaternion's coefficients
00121     * \sa QuaternionBase::squaredNorm(), MatrixBase::norm()
00122     */
00123   inline Scalar norm() const { return coeffs().norm(); }
00124 
00125   /** Normalizes the quaternion \c *this
00126     * \sa normalized(), MatrixBase::normalize() */
00127   inline void normalize() { coeffs().normalize(); }
00128   /** \returns a normalized copy of \c *this
00129     * \sa normalize(), MatrixBase::normalized() */
00130   inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
00131 
00132     /** \returns the dot product of \c *this and \a other
00133     * Geometrically speaking, the dot product of two unit quaternions
00134     * corresponds to the cosine of half the angle between the two rotations.
00135     * \sa angularDistance()
00136     */
00137   template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
00138 
00139   template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
00140 
00141   /** \returns an equivalent 3x3 rotation matrix */
00142   Matrix3 toRotationMatrix() const;
00143 
00144   /** \returns the quaternion which transform \a a into \a b through a rotation */
00145   template<typename Derived1, typename Derived2>
00146   Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00147 
00148   template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
00149   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
00150 
00151   /** \returns the quaternion describing the inverse rotation */
00152   Quaternion<Scalar> inverse() const;
00153 
00154   /** \returns the conjugated quaternion */
00155   Quaternion<Scalar> conjugate() const;
00156 
00157   /** \returns an interpolation for a constant motion between \a other and \c *this
00158     * \a t in [0;1]
00159     * see http://en.wikipedia.org/wiki/Slerp
00160     */
00161   template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
00162 
00163   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
00164     * determined by \a prec.
00165     *
00166     * \sa MatrixBase::isApprox() */
00167   template<class OtherDerived>
00168   bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const
00169   { return coeffs().isApprox(other.coeffs(), prec); }
00170 
00171         /** return the result vector of \a v through the rotation*/
00172   EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
00173 
00174   /** \returns \c *this with scalar type casted to \a NewScalarType
00175     *
00176     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
00177     * then this function smartly returns a const reference to \c *this.
00178     */
00179   template<typename NewScalarType>
00180   inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
00181   {
00182     return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(
00183       coeffs().template cast<NewScalarType>());
00184   }
00185   
00186 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
00187 # include EIGEN_QUATERNIONBASE_PLUGIN
00188 #endif
00189 };
00190 
00191 /***************************************************************************
00192 * Definition/implementation of Quaternion<Scalar>
00193 ***************************************************************************/
00194 
00195 /** \geometry_module \ingroup Geometry_Module
00196   *
00197   * \class Quaternion
00198   *
00199   * \brief The quaternion class used to represent 3D orientations and rotations
00200   *
00201   * \param _Scalar the scalar type, i.e., the type of the coefficients
00202   *
00203   * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
00204   * orientations and rotations of objects in three dimensions. Compared to other representations
00205   * like Euler angles or 3x3 matrices, quatertions offer the following advantages:
00206   * \li \b compact storage (4 scalars)
00207   * \li \b efficient to compose (28 flops),
00208   * \li \b stable spherical interpolation
00209   *
00210   * The following two typedefs are provided for convenience:
00211   * \li \c Quaternionf for \c float
00212   * \li \c Quaterniond for \c double
00213   *
00214   * \sa  class AngleAxis, class Transform
00215   */
00216 
00217 namespace internal {
00218 template<typename _Scalar>
00219 struct traits<Quaternion<_Scalar> >
00220 {
00221   typedef Quaternion<_Scalar> PlainObject;
00222   typedef _Scalar Scalar;
00223   typedef Matrix<_Scalar,4,1> Coefficients;
00224   enum{
00225     PacketAccess = Aligned
00226   };
00227 };
00228 }
00229 
00230 template<typename _Scalar>
00231 class Quaternion : public QuaternionBase<Quaternion<_Scalar> >{
00232   typedef QuaternionBase<Quaternion<_Scalar> > Base;
00233 public:
00234   typedef _Scalar Scalar;
00235 
00236   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion<Scalar>)
00237   using Base::operator*=;
00238 
00239   typedef typename internal::traits<Quaternion<Scalar> >::Coefficients Coefficients;
00240   typedef typename Base::AngleAxisType AngleAxisType;
00241 
00242   /** Default constructor leaving the quaternion uninitialized. */
00243   inline Quaternion() {}
00244 
00245   /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
00246     * its four coefficients \a w, \a x, \a y and \a z.
00247     *
00248     * \warning Note the order of the arguments: the real \a w coefficient first,
00249     * while internally the coefficients are stored in the following order:
00250     * [\c x, \c y, \c z, \c w]
00251     */
00252   inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
00253 
00254   /** Constructs and initialize a quaternion from the array data */
00255   inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00256 
00257   /** Copy constructor */
00258   template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
00259 
00260   /** Constructs and initializes a quaternion from the angle-axis \a aa */
00261   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00262 
00263   /** Constructs and initializes a quaternion from either:
00264     *  - a rotation matrix expression,
00265     *  - a 4D vector expression representing quaternion coefficients.
00266     */
00267   template<typename Derived>
00268   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00269 
00270   inline Coefficients& coeffs() { return m_coeffs;}
00271   inline const Coefficients& coeffs() const { return m_coeffs;}
00272 
00273 protected:
00274   Coefficients m_coeffs;
00275 };
00276 
00277 /** \ingroup Geometry_Module
00278   * single precision quaternion type */
00279 typedef Quaternion<float> Quaternionf;
00280 /** \ingroup Geometry_Module
00281   * double precision quaternion type */
00282 typedef Quaternion<double> Quaterniond;
00283 
00284 /***************************************************************************
00285 * Specialization of Map<Quaternion<Scalar>>
00286 ***************************************************************************/
00287 
00288 namespace internal {
00289 template<typename _Scalar, int _PacketAccess>
00290 struct traits<Map<Quaternion<_Scalar>, _PacketAccess> >:
00291 traits<Quaternion<_Scalar> >
00292 {
00293   typedef _Scalar Scalar;
00294   typedef Map<Matrix<_Scalar,4,1>, _PacketAccess> Coefficients;
00295   enum {
00296     PacketAccess = _PacketAccess
00297   };
00298 };
00299 }
00300 
00301 /** \brief Quaternion expression mapping a constant memory buffer
00302   *
00303   * \param _Scalar the type of the Quaternion coefficients
00304   * \param PacketAccess see class Map
00305   *
00306   * This is a specialization of class Map for Quaternion. This class allows to view
00307   * a 4 scalar memory buffer as an Eigen's Quaternion object.
00308   *
00309   * \sa class Map, class Quaternion, class QuaternionBase
00310   */
00311 template<typename _Scalar, int PacketAccess>
00312 class Map<const Quaternion<_Scalar>, PacketAccess >
00313   : public QuaternionBase<Map<const Quaternion<_Scalar>, PacketAccess> >
00314 {
00315     typedef QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> > Base;
00316 
00317   public:
00318     typedef _Scalar Scalar;
00319     typedef typename internal::traits<Map>::Coefficients Coefficients;
00320     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00321     using Base::operator*=;
00322 
00323     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
00324       *
00325       * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
00326       * \code *coeffs == {x, y, z, w} \endcode
00327       *
00328       * If the template parameter PacketAccess is set to Aligned, then the pointer coeffs must be aligned. */
00329     EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00330 
00331     inline const Coefficients& coeffs() const { return m_coeffs;}
00332 
00333   protected:
00334     const Coefficients m_coeffs;
00335 };
00336 
00337 /** \brief Expression of a quaternion from a memory buffer
00338   *
00339   * \param _Scalar the type of the Quaternion coefficients
00340   * \param PacketAccess see class Map
00341   *
00342   * This is a specialization of class Map for Quaternion. This class allows to view
00343   * a 4 scalar memory buffer as an Eigen's  Quaternion object.
00344   *
00345   * \sa class Map, class Quaternion, class QuaternionBase
00346   */
00347 template<typename _Scalar, int PacketAccess>
00348 class Map<Quaternion<_Scalar>, PacketAccess >
00349   : public QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> >
00350 {
00351     typedef QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> > Base;
00352 
00353   public:
00354     typedef _Scalar Scalar;
00355     typedef typename internal::traits<Map>::Coefficients Coefficients;
00356     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00357     using Base::operator*=;
00358 
00359     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
00360       *
00361       * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
00362       * \code *coeffs == {x, y, z, w} \endcode
00363       *
00364       * If the template parameter PacketAccess is set to Aligned, then the pointer coeffs must be aligned. */
00365     EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00366 
00367     inline Coefficients& coeffs() { return m_coeffs; }
00368     inline const Coefficients& coeffs() const { return m_coeffs; }
00369 
00370   protected:
00371     Coefficients m_coeffs;
00372 };
00373 
00374 /** \ingroup Geometry_Module
00375   * Map an unaligned array of single precision scalar as a quaternion */
00376 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
00377 /** \ingroup Geometry_Module
00378   * Map an unaligned array of double precision scalar as a quaternion */
00379 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
00380 /** \ingroup Geometry_Module
00381   * Map a 16-bits aligned array of double precision scalars as a quaternion */
00382 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
00383 /** \ingroup Geometry_Module
00384   * Map a 16-bits aligned array of double precision scalars as a quaternion */
00385 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
00386 
00387 /***************************************************************************
00388 * Implementation of QuaternionBase methods
00389 ***************************************************************************/
00390 
00391 // Generic Quaternion * Quaternion product
00392 // This product can be specialized for a given architecture via the Arch template argument.
00393 namespace internal {
00394 template<int Arch, class Derived1, class Derived2, typename Scalar, int PacketAccess> struct quat_product
00395 {
00396   EIGEN_STRONG_INLINE static Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
00397     return Quaternion<Scalar>
00398     (
00399       a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00400       a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00401       a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00402       a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00403     );
00404   }
00405 };
00406 }
00407 
00408 /** \returns the concatenation of two rotations as a quaternion-quaternion product */
00409 template <class Derived>
00410 template <class OtherDerived>
00411 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00412 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
00413 {
00414   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00415    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00416   return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00417                          typename internal::traits<Derived>::Scalar,
00418                          internal::traits<Derived>::PacketAccess && internal::traits<OtherDerived>::PacketAccess>::run(*this, other);
00419 }
00420 
00421 /** \sa operator*(Quaternion) */
00422 template <class Derived>
00423 template <class OtherDerived>
00424 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
00425 {
00426   derived() = derived() * other.derived();
00427   return derived();
00428 }
00429 
00430 /** Rotation of a vector by a quaternion.
00431   * \remarks If the quaternion is used to rotate several points (>1)
00432   * then it is much more efficient to first convert it to a 3x3 Matrix.
00433   * Comparison of the operation cost for n transformations:
00434   *   - Quaternion2:    30n
00435   *   - Via a Matrix3: 24 + 15n
00436   */
00437 template <class Derived>
00438 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
00439 QuaternionBase<Derived>::_transformVector(Vector3 v) const
00440 {
00441     // Note that this algorithm comes from the optimization by hand
00442     // of the conversion to a Matrix followed by a Matrix/Vector product.
00443     // It appears to be much faster than the common algorithm found
00444     // in the litterature (30 versus 39 flops). It also requires two
00445     // Vector3 as temporaries.
00446     Vector3 uv = this->vec().cross(v);
00447     uv += uv;
00448     return v + this->w() * uv + this->vec().cross(uv);
00449 }
00450 
00451 template<class Derived>
00452 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
00453 {
00454   coeffs() = other.coeffs();
00455   return derived();
00456 }
00457 
00458 template<class Derived>
00459 template<class OtherDerived>
00460 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00461 {
00462   coeffs() = other.coeffs();
00463   return derived();
00464 }
00465 
00466 /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
00467   */
00468 template<class Derived>
00469 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
00470 {
00471   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00472   this->w() = internal::cos(ha);
00473   this->vec() = internal::sin(ha) * aa.axis();
00474   return derived();
00475 }
00476 
00477 /** Set \c *this from the expression \a xpr:
00478   *   - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
00479   *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
00480   *     and \a xpr is converted to a quaternion
00481   */
00482 
00483 template<class Derived>
00484 template<class MatrixDerived>
00485 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
00486 {
00487   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00488    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00489   internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00490   return derived();
00491 }
00492 
00493 /** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to
00494   * be normalized, otherwise the result is undefined.
00495   */
00496 template<class Derived>
00497 inline typename QuaternionBase<Derived>::Matrix3
00498 QuaternionBase<Derived>::toRotationMatrix(void) const
00499 {
00500   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00501   // if not inlined then the cost of the return by value is huge ~ +35%,
00502   // however, not inlining this function is an order of magnitude slower, so
00503   // it has to be inlined, and so the return by value is not an issue
00504   Matrix3 res;
00505 
00506   const Scalar tx  = 2*this->x();
00507   const Scalar ty  = 2*this->y();
00508   const Scalar tz  = 2*this->z();
00509   const Scalar twx = tx*this->w();
00510   const Scalar twy = ty*this->w();
00511   const Scalar twz = tz*this->w();
00512   const Scalar txx = tx*this->x();
00513   const Scalar txy = ty*this->x();
00514   const Scalar txz = tz*this->x();
00515   const Scalar tyy = ty*this->y();
00516   const Scalar tyz = tz*this->y();
00517   const Scalar tzz = tz*this->z();
00518 
00519   res.coeffRef(0,0) = 1-(tyy+tzz);
00520   res.coeffRef(0,1) = txy-twz;
00521   res.coeffRef(0,2) = txz+twy;
00522   res.coeffRef(1,0) = txy+twz;
00523   res.coeffRef(1,1) = 1-(txx+tzz);
00524   res.coeffRef(1,2) = tyz-twx;
00525   res.coeffRef(2,0) = txz-twy;
00526   res.coeffRef(2,1) = tyz+twx;
00527   res.coeffRef(2,2) = 1-(txx+tyy);
00528 
00529   return res;
00530 }
00531 
00532 /** Sets \c *this to be a quaternion representing a rotation between
00533   * the two arbitrary vectors \a a and \a b. In other words, the built
00534   * rotation represent a rotation sending the line of direction \a a
00535   * to the line of direction \a b, both lines passing through the origin.
00536   *
00537   * \returns a reference to \c *this.
00538   *
00539   * Note that the two input vectors do \b not have to be normalized, and
00540   * do not need to have the same norm.
00541   */
00542 template<class Derived>
00543 template<typename Derived1, typename Derived2>
00544 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00545 {
00546   Vector3 v0 = a.normalized();
00547   Vector3 v1 = b.normalized();
00548   Scalar c = v1.dot(v0);
00549 
00550   // if dot == -1, vectors are nearly opposites
00551   // => accuraletly compute the rotation axis by computing the
00552   //    intersection of the two planes. This is done by solving:
00553   //       x^T v0 = 0
00554   //       x^T v1 = 0
00555   //    under the constraint:
00556   //       ||x|| = 1
00557   //    which yields a singular value problem
00558   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00559   {
00560     c = std::max<Scalar>(c,-1);
00561     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00562     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00563     Vector3 axis = svd.matrixV().col(2);
00564 
00565     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00566     this->w() = internal::sqrt(w2);
00567     this->vec() = axis * internal::sqrt(Scalar(1) - w2);
00568     return derived();
00569   }
00570   Vector3 axis = v0.cross(v1);
00571   Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
00572   Scalar invs = Scalar(1)/s;
00573   this->vec() = axis * invs;
00574   this->w() = s * Scalar(0.5);
00575 
00576   return derived();
00577 }
00578 
00579 /** \returns the multiplicative inverse of \c *this
00580   * Note that in most cases, i.e., if you simply want the opposite rotation,
00581   * and/or the quaternion is normalized, then it is enough to use the conjugate.
00582   *
00583   * \sa QuaternionBase::conjugate()
00584   */
00585 template <class Derived>
00586 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
00587 {
00588   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00589   Scalar n2 = this->squaredNorm();
00590   if (n2 > 0)
00591     return Quaternion<Scalar>(conjugate().coeffs() / n2);
00592   else
00593   {
00594     // return an invalid result to flag the error
00595     return Quaternion<Scalar>(Coefficients::Zero());
00596   }
00597 }
00598 
00599 /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
00600   * if the quaternion is normalized.
00601   * The conjugate of a quaternion represents the opposite rotation.
00602   *
00603   * \sa Quaternion2::inverse()
00604   */
00605 template <class Derived>
00606 inline Quaternion<typename internal::traits<Derived>::Scalar>
00607 QuaternionBase<Derived>::conjugate() const
00608 {
00609   return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
00610 }
00611 
00612 /** \returns the angle (in radian) between two rotations
00613   * \sa dot()
00614   */
00615 template <class Derived>
00616 template <class OtherDerived>
00617 inline typename internal::traits<Derived>::Scalar
00618 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
00619 {
00620   double d = internal::abs(this->dot(other));
00621   if (d>=1.0)
00622     return Scalar(0);
00623   return static_cast<Scalar>(2 * std::acos(d));
00624 }
00625 
00626 /** \returns the spherical linear interpolation between the two quaternions
00627   * \c *this and \a other at the parameter \a t
00628   */
00629 template <class Derived>
00630 template <class OtherDerived>
00631 Quaternion<typename internal::traits<Derived>::Scalar>
00632 QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const
00633 {
00634   static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00635   Scalar d = this->dot(other);
00636   Scalar absD = internal::abs(d);
00637 
00638   Scalar scale0;
00639   Scalar scale1;
00640 
00641   if (absD>=one)
00642   {
00643     scale0 = Scalar(1) - t;
00644     scale1 = t;
00645   }
00646   else
00647   {
00648     // theta is the angle between the 2 quaternions
00649     Scalar theta = std::acos(absD);
00650     Scalar sinTheta = internal::sin(theta);
00651 
00652     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00653     scale1 = internal::sin( ( t * theta) ) / sinTheta;
00654     if (d<0)
00655       scale1 = -scale1;
00656   }
00657 
00658   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00659 }
00660 
00661 namespace internal {
00662 
00663 // set from a rotation matrix
00664 template<typename Other>
00665 struct quaternionbase_assign_impl<Other,3,3>
00666 {
00667   typedef typename Other::Scalar Scalar;
00668   typedef DenseIndex Index;
00669   template<class Derived> inline static void run(QuaternionBase<Derived>& q, const Other& mat)
00670   {
00671     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00672     // Ken Shoemake, 1987 SIGGRAPH course notes
00673     Scalar t = mat.trace();
00674     if (t > Scalar(0))
00675     {
00676       t = sqrt(t + Scalar(1.0));
00677       q.w() = Scalar(0.5)*t;
00678       t = Scalar(0.5)/t;
00679       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00680       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00681       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00682     }
00683     else
00684     {
00685       DenseIndex i = 0;
00686       if (mat.coeff(1,1) > mat.coeff(0,0))
00687         i = 1;
00688       if (mat.coeff(2,2) > mat.coeff(i,i))
00689         i = 2;
00690       DenseIndex j = (i+1)%3;
00691       DenseIndex k = (j+1)%3;
00692 
00693       t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00694       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00695       t = Scalar(0.5)/t;
00696       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00697       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00698       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00699     }
00700   }
00701 };
00702 
00703 // set from a vector of coefficients assumed to be a quaternion
00704 template<typename Other>
00705 struct quaternionbase_assign_impl<Other,4,1>
00706 {
00707   typedef typename Other::Scalar Scalar;
00708   template<class Derived> inline static void run(QuaternionBase<Derived>& q, const Other& vec)
00709   {
00710     q.coeffs() = vec;
00711   }
00712 };
00713 
00714 } // end namespace internal
00715 
00716 #endif // EIGEN_QUATERNION_H



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011