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 Gael Guennebaud <g.gael@free.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
00011 
00012 namespace Eigen { 
00013 
00014 template<typename Other,
00015          int OtherRows=Other::RowsAtCompileTime,
00016          int OtherCols=Other::ColsAtCompileTime>
00017 struct ei_quaternion_assign_impl;
00018 
00041 template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
00042 {
00043   typedef _Scalar Scalar;
00044 };
00045 
00046 template<typename _Scalar>
00047 class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
00048 {
00049   typedef RotationBase<Quaternion<_Scalar>,3> Base;
00050 
00051 public:
00052   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4)
00053 
00054   using Base::operator*;
00055 
00057   typedef _Scalar Scalar;
00058 
00060   typedef Matrix<Scalar, 4, 1> Coefficients;
00062   typedef Matrix<Scalar,3,1> Vector3;
00064   typedef Matrix<Scalar,3,3> Matrix3;
00066   typedef AngleAxis<Scalar> AngleAxisType;
00067 
00069   inline Scalar x() const { return m_coeffs.coeff(0); }
00071   inline Scalar y() const { return m_coeffs.coeff(1); }
00073   inline Scalar z() const { return m_coeffs.coeff(2); }
00075   inline Scalar w() const { return m_coeffs.coeff(3); }
00076 
00078   inline Scalar& x() { return m_coeffs.coeffRef(0); }
00080   inline Scalar& y() { return m_coeffs.coeffRef(1); }
00082   inline Scalar& z() { return m_coeffs.coeffRef(2); }
00084   inline Scalar& w() { return m_coeffs.coeffRef(3); }
00085 
00087   inline const Block<const Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
00088 
00090   inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
00091 
00093   inline const Coefficients& coeffs() const { return m_coeffs; }
00094 
00096   inline Coefficients& coeffs() { return m_coeffs; }
00097 
00099   inline Quaternion() {}
00100 
00108   inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
00109   { m_coeffs << x, y, z, w; }
00110 
00112   inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
00113 
00115   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00116 
00122   template<typename Derived>
00123   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00124 
00125   Quaternion& operator=(const Quaternion& other);
00126   Quaternion& operator=(const AngleAxisType& aa);
00127   template<typename Derived>
00128   Quaternion& operator=(const MatrixBase<Derived>& m);
00129 
00133   static inline Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
00134 
00137   inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; }
00138 
00142   inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); }
00143 
00147   inline Scalar norm() const { return m_coeffs.norm(); }
00148 
00151   inline void normalize() { m_coeffs.normalize(); }
00154   inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
00155 
00161   inline Scalar eigen2_dot(const Quaternion& other) const { return m_coeffs.eigen2_dot(other.m_coeffs); }
00162 
00163   inline Scalar angularDistance(const Quaternion& other) const;
00164 
00165   Matrix3 toRotationMatrix(void) const;
00166 
00167   template<typename Derived1, typename Derived2>
00168   Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00169 
00170   inline Quaternion operator* (const Quaternion& q) const;
00171   inline Quaternion& operator*= (const Quaternion& q);
00172 
00173   Quaternion inverse(void) const;
00174   Quaternion conjugate(void) const;
00175 
00176   Quaternion slerp(Scalar t, const Quaternion& other) const;
00177 
00178   template<typename Derived>
00179   Vector3 operator* (const MatrixBase<Derived>& vec) const;
00180 
00186   template<typename NewScalarType>
00187   inline typename internal::cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const
00188   { return typename internal::cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
00189 
00191   template<typename OtherScalarType>
00192   inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
00193   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00194 
00199   bool isApprox(const Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00200   { return m_coeffs.isApprox(other.m_coeffs, prec); }
00201 
00202 protected:
00203   Coefficients m_coeffs;
00204 };
00205 
00208 typedef Quaternion<float> Quaternionf;
00211 typedef Quaternion<double> Quaterniond;
00212 
00213 // Generic Quaternion * Quaternion product
00214 template<typename Scalar> inline Quaternion<Scalar>
00215 ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
00216 {
00217   return Quaternion<Scalar>
00218   (
00219     a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00220     a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00221     a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00222     a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00223   );
00224 }
00225 
00227 template <typename Scalar>
00228 inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
00229 {
00230   return ei_quaternion_product(*this,other);
00231 }
00232 
00234 template <typename Scalar>
00235 inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
00236 {
00237   return (*this = *this * other);
00238 }
00239 
00247 template <typename Scalar>
00248 template<typename Derived>
00249 inline typename Quaternion<Scalar>::Vector3
00250 Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
00251 {
00252     // Note that this algorithm comes from the optimization by hand
00253     // of the conversion to a Matrix followed by a Matrix/Vector product.
00254     // It appears to be much faster than the common algorithm found
00255     // in the litterature (30 versus 39 flops). It also requires two
00256     // Vector3 as temporaries.
00257     Vector3 uv;
00258     uv = 2 * this->vec().cross(v);
00259     return v + this->w() * uv + this->vec().cross(uv);
00260 }
00261 
00262 template<typename Scalar>
00263 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
00264 {
00265   m_coeffs = other.m_coeffs;
00266   return *this;
00267 }
00268 
00271 template<typename Scalar>
00272 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
00273 {
00274   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00275   this->w() = ei_cos(ha);
00276   this->vec() = ei_sin(ha) * aa.axis();
00277   return *this;
00278 }
00279 
00285 template<typename Scalar>
00286 template<typename Derived>
00287 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
00288 {
00289   ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
00290   return *this;
00291 }
00292 
00294 template<typename Scalar>
00295 inline typename Quaternion<Scalar>::Matrix3
00296 Quaternion<Scalar>::toRotationMatrix(void) const
00297 {
00298   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00299   // if not inlined then the cost of the return by value is huge ~ +35%,
00300   // however, not inlining this function is an order of magnitude slower, so
00301   // it has to be inlined, and so the return by value is not an issue
00302   Matrix3 res;
00303 
00304   const Scalar tx  = Scalar(2)*this->x();
00305   const Scalar ty  = Scalar(2)*this->y();
00306   const Scalar tz  = Scalar(2)*this->z();
00307   const Scalar twx = tx*this->w();
00308   const Scalar twy = ty*this->w();
00309   const Scalar twz = tz*this->w();
00310   const Scalar txx = tx*this->x();
00311   const Scalar txy = ty*this->x();
00312   const Scalar txz = tz*this->x();
00313   const Scalar tyy = ty*this->y();
00314   const Scalar tyz = tz*this->y();
00315   const Scalar tzz = tz*this->z();
00316 
00317   res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
00318   res.coeffRef(0,1) = txy-twz;
00319   res.coeffRef(0,2) = txz+twy;
00320   res.coeffRef(1,0) = txy+twz;
00321   res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
00322   res.coeffRef(1,2) = tyz-twx;
00323   res.coeffRef(2,0) = txz-twy;
00324   res.coeffRef(2,1) = tyz+twx;
00325   res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
00326 
00327   return res;
00328 }
00329 
00336 template<typename Scalar>
00337 template<typename Derived1, typename Derived2>
00338 inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00339 {
00340   Vector3 v0 = a.normalized();
00341   Vector3 v1 = b.normalized();
00342   Scalar c = v0.eigen2_dot(v1);
00343 
00344   // if dot == 1, vectors are the same
00345   if (ei_isApprox(c,Scalar(1)))
00346   {
00347     // set to identity
00348     this->w() = 1; this->vec().setZero();
00349     return *this;
00350   }
00351   // if dot == -1, vectors are opposites
00352   if (ei_isApprox(c,Scalar(-1)))
00353   {
00354     this->vec() = v0.unitOrthogonal();
00355     this->w() = 0;
00356     return *this;
00357   }
00358 
00359   Vector3 axis = v0.cross(v1);
00360   Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
00361   Scalar invs = Scalar(1)/s;
00362   this->vec() = axis * invs;
00363   this->w() = s * Scalar(0.5);
00364 
00365   return *this;
00366 }
00367 
00374 template <typename Scalar>
00375 inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
00376 {
00377   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00378   Scalar n2 = this->squaredNorm();
00379   if (n2 > 0)
00380     return Quaternion(conjugate().coeffs() / n2);
00381   else
00382   {
00383     // return an invalid result to flag the error
00384     return Quaternion(Coefficients::Zero());
00385   }
00386 }
00387 
00394 template <typename Scalar>
00395 inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
00396 {
00397   return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
00398 }
00399 
00403 template <typename Scalar>
00404 inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
00405 {
00406   double d = ei_abs(this->eigen2_dot(other));
00407   if (d>=1.0)
00408     return 0;
00409   return Scalar(2) * std::acos(d);
00410 }
00411 
00415 template <typename Scalar>
00416 Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
00417 {
00418   static const Scalar one = Scalar(1) - machine_epsilon<Scalar>();
00419   Scalar d = this->eigen2_dot(other);
00420   Scalar absD = ei_abs(d);
00421 
00422   Scalar scale0;
00423   Scalar scale1;
00424 
00425   if (absD>=one)
00426   {
00427     scale0 = Scalar(1) - t;
00428     scale1 = t;
00429   }
00430   else
00431   {
00432     // theta is the angle between the 2 quaternions
00433     Scalar theta = std::acos(absD);
00434     Scalar sinTheta = ei_sin(theta);
00435 
00436     scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
00437     scale1 = ei_sin( ( t * theta) ) / sinTheta;
00438     if (d<0)
00439       scale1 = -scale1;
00440   }
00441 
00442   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00443 }
00444 
00445 // set from a rotation matrix
00446 template<typename Other>
00447 struct ei_quaternion_assign_impl<Other,3,3>
00448 {
00449   typedef typename Other::Scalar Scalar;
00450   static inline void run(Quaternion<Scalar>& q, const Other& mat)
00451   {
00452     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00453     // Ken Shoemake, 1987 SIGGRAPH course notes
00454     Scalar t = mat.trace();
00455     if (t > 0)
00456     {
00457       t = ei_sqrt(t + Scalar(1.0));
00458       q.w() = Scalar(0.5)*t;
00459       t = Scalar(0.5)/t;
00460       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00461       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00462       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00463     }
00464     else
00465     {
00466       int i = 0;
00467       if (mat.coeff(1,1) > mat.coeff(0,0))
00468         i = 1;
00469       if (mat.coeff(2,2) > mat.coeff(i,i))
00470         i = 2;
00471       int j = (i+1)%3;
00472       int k = (j+1)%3;
00473 
00474       t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00475       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00476       t = Scalar(0.5)/t;
00477       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00478       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00479       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00480     }
00481   }
00482 };
00483 
00484 // set from a vector of coefficients assumed to be a quaternion
00485 template<typename Other>
00486 struct ei_quaternion_assign_impl<Other,4,1>
00487 {
00488   typedef typename Other::Scalar Scalar;
00489   static inline void run(Quaternion<Scalar>& q, const Other& vec)
00490   {
00491     q.coeffs() = vec;
00492   }
00493 };
00494 
00495 } // end namespace Eigen


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:51