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. Eigen itself is part of the KDE project.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
00026 
00027 template<typename Other,
00028          int OtherRows=Other::RowsAtCompileTime,
00029          int OtherCols=Other::ColsAtCompileTime>
00030 struct ei_quaternion_assign_impl;
00031 
00054 template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
00055 {
00056   typedef _Scalar Scalar;
00057 };
00058 
00059 template<typename _Scalar>
00060 class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
00061 {
00062   typedef RotationBase<Quaternion<_Scalar>,3> Base;
00063 
00064 public:
00065   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4)
00066 
00067   using Base::operator*;
00068 
00070   typedef _Scalar Scalar;
00071 
00073   typedef Matrix<Scalar, 4, 1> Coefficients;
00075   typedef Matrix<Scalar,3,1> Vector3;
00077   typedef Matrix<Scalar,3,3> Matrix3;
00079   typedef AngleAxis<Scalar> AngleAxisType;
00080 
00082   inline Scalar x() const { return m_coeffs.coeff(0); }
00084   inline Scalar y() const { return m_coeffs.coeff(1); }
00086   inline Scalar z() const { return m_coeffs.coeff(2); }
00088   inline Scalar w() const { return m_coeffs.coeff(3); }
00089 
00091   inline Scalar& x() { return m_coeffs.coeffRef(0); }
00093   inline Scalar& y() { return m_coeffs.coeffRef(1); }
00095   inline Scalar& z() { return m_coeffs.coeffRef(2); }
00097   inline Scalar& w() { return m_coeffs.coeffRef(3); }
00098 
00100   inline const Block<const Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
00101 
00103   inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
00104 
00106   inline const Coefficients& coeffs() const { return m_coeffs; }
00107 
00109   inline Coefficients& coeffs() { return m_coeffs; }
00110 
00112   inline Quaternion() {}
00113 
00121   inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
00122   { m_coeffs << x, y, z, w; }
00123 
00125   inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
00126 
00128   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00129 
00135   template<typename Derived>
00136   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00137 
00138   Quaternion& operator=(const Quaternion& other);
00139   Quaternion& operator=(const AngleAxisType& aa);
00140   template<typename Derived>
00141   Quaternion& operator=(const MatrixBase<Derived>& m);
00142 
00146   inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
00147 
00150   inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; }
00151 
00155   inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); }
00156 
00160   inline Scalar norm() const { return m_coeffs.norm(); }
00161 
00164   inline void normalize() { m_coeffs.normalize(); }
00167   inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
00168 
00174   inline Scalar eigen2_dot(const Quaternion& other) const { return m_coeffs.eigen2_dot(other.m_coeffs); }
00175 
00176   inline Scalar angularDistance(const Quaternion& other) const;
00177 
00178   Matrix3 toRotationMatrix(void) const;
00179 
00180   template<typename Derived1, typename Derived2>
00181   Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00182 
00183   inline Quaternion operator* (const Quaternion& q) const;
00184   inline Quaternion& operator*= (const Quaternion& q);
00185 
00186   Quaternion inverse(void) const;
00187   Quaternion conjugate(void) const;
00188 
00189   Quaternion slerp(Scalar t, const Quaternion& other) const;
00190 
00191   template<typename Derived>
00192   Vector3 operator* (const MatrixBase<Derived>& vec) const;
00193 
00199   template<typename NewScalarType>
00200   inline typename internal::cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const
00201   { return typename internal::cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
00202 
00204   template<typename OtherScalarType>
00205   inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
00206   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00207 
00212   bool isApprox(const Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00213   { return m_coeffs.isApprox(other.m_coeffs, prec); }
00214 
00215 protected:
00216   Coefficients m_coeffs;
00217 };
00218 
00221 typedef Quaternion<float> Quaternionf;
00224 typedef Quaternion<double> Quaterniond;
00225 
00226 // Generic Quaternion * Quaternion product
00227 template<typename Scalar> inline Quaternion<Scalar>
00228 ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
00229 {
00230   return Quaternion<Scalar>
00231   (
00232     a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00233     a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00234     a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00235     a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00236   );
00237 }
00238 
00240 template <typename Scalar>
00241 inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
00242 {
00243   return ei_quaternion_product(*this,other);
00244 }
00245 
00247 template <typename Scalar>
00248 inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
00249 {
00250   return (*this = *this * other);
00251 }
00252 
00260 template <typename Scalar>
00261 template<typename Derived>
00262 inline typename Quaternion<Scalar>::Vector3
00263 Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
00264 {
00265     // Note that this algorithm comes from the optimization by hand
00266     // of the conversion to a Matrix followed by a Matrix/Vector product.
00267     // It appears to be much faster than the common algorithm found
00268     // in the litterature (30 versus 39 flops). It also requires two
00269     // Vector3 as temporaries.
00270     Vector3 uv;
00271     uv = 2 * this->vec().cross(v);
00272     return v + this->w() * uv + this->vec().cross(uv);
00273 }
00274 
00275 template<typename Scalar>
00276 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
00277 {
00278   m_coeffs = other.m_coeffs;
00279   return *this;
00280 }
00281 
00284 template<typename Scalar>
00285 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
00286 {
00287   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00288   this->w() = ei_cos(ha);
00289   this->vec() = ei_sin(ha) * aa.axis();
00290   return *this;
00291 }
00292 
00298 template<typename Scalar>
00299 template<typename Derived>
00300 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
00301 {
00302   ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
00303   return *this;
00304 }
00305 
00307 template<typename Scalar>
00308 inline typename Quaternion<Scalar>::Matrix3
00309 Quaternion<Scalar>::toRotationMatrix(void) const
00310 {
00311   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00312   // if not inlined then the cost of the return by value is huge ~ +35%,
00313   // however, not inlining this function is an order of magnitude slower, so
00314   // it has to be inlined, and so the return by value is not an issue
00315   Matrix3 res;
00316 
00317   const Scalar tx  = 2*this->x();
00318   const Scalar ty  = 2*this->y();
00319   const Scalar tz  = 2*this->z();
00320   const Scalar twx = tx*this->w();
00321   const Scalar twy = ty*this->w();
00322   const Scalar twz = tz*this->w();
00323   const Scalar txx = tx*this->x();
00324   const Scalar txy = ty*this->x();
00325   const Scalar txz = tz*this->x();
00326   const Scalar tyy = ty*this->y();
00327   const Scalar tyz = tz*this->y();
00328   const Scalar tzz = tz*this->z();
00329 
00330   res.coeffRef(0,0) = 1-(tyy+tzz);
00331   res.coeffRef(0,1) = txy-twz;
00332   res.coeffRef(0,2) = txz+twy;
00333   res.coeffRef(1,0) = txy+twz;
00334   res.coeffRef(1,1) = 1-(txx+tzz);
00335   res.coeffRef(1,2) = tyz-twx;
00336   res.coeffRef(2,0) = txz-twy;
00337   res.coeffRef(2,1) = tyz+twx;
00338   res.coeffRef(2,2) = 1-(txx+tyy);
00339 
00340   return res;
00341 }
00342 
00349 template<typename Scalar>
00350 template<typename Derived1, typename Derived2>
00351 inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00352 {
00353   Vector3 v0 = a.normalized();
00354   Vector3 v1 = b.normalized();
00355   Scalar c = v0.eigen2_dot(v1);
00356 
00357   // if dot == 1, vectors are the same
00358   if (ei_isApprox(c,Scalar(1)))
00359   {
00360     // set to identity
00361     this->w() = 1; this->vec().setZero();
00362     return *this;
00363   }
00364   // if dot == -1, vectors are opposites
00365   if (ei_isApprox(c,Scalar(-1)))
00366   {
00367     this->vec() = v0.unitOrthogonal();
00368     this->w() = 0;
00369     return *this;
00370   }
00371 
00372   Vector3 axis = v0.cross(v1);
00373   Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
00374   Scalar invs = Scalar(1)/s;
00375   this->vec() = axis * invs;
00376   this->w() = s * Scalar(0.5);
00377 
00378   return *this;
00379 }
00380 
00387 template <typename Scalar>
00388 inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
00389 {
00390   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00391   Scalar n2 = this->squaredNorm();
00392   if (n2 > 0)
00393     return Quaternion(conjugate().coeffs() / n2);
00394   else
00395   {
00396     // return an invalid result to flag the error
00397     return Quaternion(Coefficients::Zero());
00398   }
00399 }
00400 
00407 template <typename Scalar>
00408 inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
00409 {
00410   return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
00411 }
00412 
00416 template <typename Scalar>
00417 inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
00418 {
00419   double d = ei_abs(this->eigen2_dot(other));
00420   if (d>=1.0)
00421     return 0;
00422   return Scalar(2) * std::acos(d);
00423 }
00424 
00428 template <typename Scalar>
00429 Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
00430 {
00431   static const Scalar one = Scalar(1) - machine_epsilon<Scalar>();
00432   Scalar d = this->eigen2_dot(other);
00433   Scalar absD = ei_abs(d);
00434 
00435   Scalar scale0;
00436   Scalar scale1;
00437 
00438   if (absD>=one)
00439   {
00440     scale0 = Scalar(1) - t;
00441     scale1 = t;
00442   }
00443   else
00444   {
00445     // theta is the angle between the 2 quaternions
00446     Scalar theta = std::acos(absD);
00447     Scalar sinTheta = ei_sin(theta);
00448 
00449     scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
00450     scale1 = ei_sin( ( t * theta) ) / sinTheta;
00451     if (d<0)
00452       scale1 = -scale1;
00453   }
00454 
00455   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00456 }
00457 
00458 // set from a rotation matrix
00459 template<typename Other>
00460 struct ei_quaternion_assign_impl<Other,3,3>
00461 {
00462   typedef typename Other::Scalar Scalar;
00463   inline static void run(Quaternion<Scalar>& q, const Other& mat)
00464   {
00465     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00466     // Ken Shoemake, 1987 SIGGRAPH course notes
00467     Scalar t = mat.trace();
00468     if (t > 0)
00469     {
00470       t = ei_sqrt(t + Scalar(1.0));
00471       q.w() = Scalar(0.5)*t;
00472       t = Scalar(0.5)/t;
00473       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00474       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00475       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00476     }
00477     else
00478     {
00479       int i = 0;
00480       if (mat.coeff(1,1) > mat.coeff(0,0))
00481         i = 1;
00482       if (mat.coeff(2,2) > mat.coeff(i,i))
00483         i = 2;
00484       int j = (i+1)%3;
00485       int k = (j+1)%3;
00486 
00487       t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00488       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00489       t = Scalar(0.5)/t;
00490       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00491       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00492       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00493     }
00494   }
00495 };
00496 
00497 // set from a vector of coefficients assumed to be a quaternion
00498 template<typename Other>
00499 struct ei_quaternion_assign_impl<Other,4,1>
00500 {
00501   typedef typename Other::Scalar Scalar;
00502   inline static void run(Quaternion<Scalar>& q, const Other& vec)
00503   {
00504     q.coeffs() = vec;
00505   }
00506 };


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:33:16