1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr> 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_QUATERNION_H 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define EIGEN_QUATERNION_H 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/*************************************************************************** 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Definition of QuaternionBase<Derived> 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* The implementation is at the end of the file 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath***************************************************************************/ 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Other, 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int OtherRows=Other::RowsAtCompileTime, 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath int OtherCols=Other::ColsAtCompileTime> 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct quaternionbase_assign_impl; 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \geometry_module \ingroup Geometry_Module 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \class QuaternionBase 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Base class for quaternion expressions 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \tparam Derived derived type (CRTP) 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class Quaternion 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass QuaternionBase : public RotationBase<Derived, 3> 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef RotationBase<Derived, 3> Base; 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*; 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::derived; 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::traits<Derived>::Scalar Scalar; 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename NumTraits<Scalar>::Real RealScalar; 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::traits<Derived>::Coefficients Coefficients; 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Flags = Eigen::internal::traits<Derived>::Flags 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // typedef typename Matrix<Scalar,4,1> Coefficients; 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** the type of a 3D vector */ 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,1> Vector3; 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** the equivalent rotation matrix type */ 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,3> Matrix3; 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** the equivalent angle-axis type */ 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AngleAxis<Scalar> AngleAxisType; 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the \c x coefficient */ 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar x() const { return this->derived().coeffs().coeff(0); } 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the \c y coefficient */ 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar y() const { return this->derived().coeffs().coeff(1); } 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the \c z coefficient */ 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar z() const { return this->derived().coeffs().coeff(2); } 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the \c w coefficient */ 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar w() const { return this->derived().coeffs().coeff(3); } 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a reference to the \c x coefficient */ 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a reference to the \c y coefficient */ 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a reference to the \c z coefficient */ 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a reference to the \c w coefficient */ 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a read-only vector expression of the imaginary part (x,y,z) */ 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); } 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a vector expression of the imaginary part (x,y,z) */ 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); } 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); } 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a vector expression of the coefficients (x,y,z,w) */ 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); } 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other); 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other); 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// disabled this copy operator as it is giving very strange compilation errors when compiling 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// useful; however notice that we already have the templated operator= above and e.g. in MatrixBase 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// we didn't have to add, in addition to templated operator=, such a non-templated copy operator. 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Derived& operator=(const QuaternionBase& other) 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// { return operator=<Derived>(other); } 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Derived& operator=(const AngleAxisType& aa); 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m); 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a quaternion representing an identity rotation 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa MatrixBase::Identity() 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); } 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the squared norm of the quaternion's coefficients 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the norm of the quaternion's coefficients 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa QuaternionBase::squaredNorm(), MatrixBase::norm() 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar norm() const { return coeffs().norm(); } 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Normalizes the quaternion \c *this 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa normalized(), MatrixBase::normalize() */ 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline void normalize() { coeffs().normalize(); } 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a normalized copy of \c *this 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa normalize(), MatrixBase::normalized() */ 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); } 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the dot product of \c *this and \a other 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Geometrically speaking, the dot product of two unit quaternions 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * corresponds to the cosine of half the angle between the two rotations. 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa angularDistance() 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); } 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const; 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns an equivalent 3x3 rotation matrix */ 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 toRotationMatrix() const; 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the quaternion which transform \a a into \a b through a rotation */ 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived1, typename Derived2> 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const; 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q); 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the quaternion describing the inverse rotation */ 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternion<Scalar> inverse() const; 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the conjugated quaternion */ 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternion<Scalar> conjugate() const; 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const; 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c true if \c *this is approximately equal to \a other, within the precision 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * determined by \a prec. 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa MatrixBase::isApprox() */ 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class OtherDerived> 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return coeffs().isApprox(other.coeffs(), prec); } 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** return the result vector of \a v through the rotation*/ 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c *this with scalar type casted to \a NewScalarType 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that if \a NewScalarType is equal to the current scalar type of \c *this 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * then this function smartly returns a const reference to \c *this. 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 171c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename NewScalarType> 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const 173c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 174c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived()); 175c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 176c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 177c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifdef EIGEN_QUATERNIONBASE_PLUGIN 178c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath# include EIGEN_QUATERNIONBASE_PLUGIN 179c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif 180c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 181c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 182c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/*************************************************************************** 183c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Definition/implementation of Quaternion<Scalar> 184c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath***************************************************************************/ 185c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 186c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \geometry_module \ingroup Geometry_Module 187c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 188c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \class Quaternion 189c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 190c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief The quaternion class used to represent 3D orientations and rotations 191c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 1927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \tparam _Scalar the scalar type, i.e., the type of the coefficients 1937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. 194c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 195c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of 196c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * orientations and rotations of objects in three dimensions. Compared to other representations 1977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * like Euler angles or 3x3 matrices, quaternions offer the following advantages: 198c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \b compact storage (4 scalars) 199c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \b efficient to compose (28 flops), 200c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \b stable spherical interpolation 201c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 202c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * The following two typedefs are provided for convenience: 203c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \c Quaternionf for \c float 204c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \c Quaterniond for \c double 205c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 2067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized. 2077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * 208c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class AngleAxis, class Transform 209c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 210c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 211c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 212c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar,int _Options> 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct traits<Quaternion<_Scalar,_Options> > 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 215c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Quaternion<_Scalar,_Options> PlainObject; 216c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 217c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<_Scalar,4,1,_Options> Coefficients; 218c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum{ 219c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath IsAligned = internal::traits<Coefficients>::Flags & AlignedBit, 220c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit 221c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 222c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 223c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 224c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 225c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar, int _Options> 226c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> > 227c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 228c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base; 229c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { IsAligned = internal::traits<Quaternion>::IsAligned }; 230c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 231c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 232c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 233c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 234c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) 235c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*=; 236c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 237c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::traits<Quaternion>::Coefficients Coefficients; 238c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename Base::AngleAxisType AngleAxisType; 239c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 240c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Default constructor leaving the quaternion uninitialized. */ 241c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Quaternion() {} 242c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 243c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from 244c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * its four coefficients \a w, \a x, \a y and \a z. 245c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 246c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \warning Note the order of the arguments: the real \a w coefficient first, 247c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * while internally the coefficients are stored in the following order: 248c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * [\c x, \c y, \c z, \c w] 249c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 2507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){} 251c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 252c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initialize a quaternion from the array data */ 253c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Quaternion(const Scalar* data) : m_coeffs(data) {} 254c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 255c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Copy constructor */ 256c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); } 257c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 258c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initializes a quaternion from the angle-axis \a aa */ 259c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } 260c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 261c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initializes a quaternion from either: 262c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * - a rotation matrix expression, 263c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * - a 4D vector expression representing quaternion coefficients. 264c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 265c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 266c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } 267c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 268c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Explicit copy constructor with scalar conversion */ 269c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename OtherScalar, int OtherOptions> 270c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) 271c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { m_coeffs = other.coeffs().template cast<Scalar>(); } 272c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 273c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived1, typename Derived2> 274c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 275c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 276c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Coefficients& coeffs() { return m_coeffs;} 277c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline const Coefficients& coeffs() const { return m_coeffs;} 278c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 279c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) 280c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 281c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathprotected: 282c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Coefficients m_coeffs; 283c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 284c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_PARSED_BY_DOXYGEN 285c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static EIGEN_STRONG_INLINE void _check_template_params() 286c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 287c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, 288c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath INVALID_MATRIX_TEMPLATE_PARAMETERS) 289c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 290c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif 291c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 292c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 293c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 294c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * single precision quaternion type */ 295c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Quaternion<float> Quaternionf; 296c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 297c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * double precision quaternion type */ 298c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Quaternion<double> Quaterniond; 299c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 300c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/*************************************************************************** 301c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Specialization of Map<Quaternion<Scalar>> 302c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath***************************************************************************/ 303c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 304c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 305c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename _Scalar, int _Options> 3067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > 307c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 308c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients; 309c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 310c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 311c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 312c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 313c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename _Scalar, int _Options> 3147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > 315c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 316c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients; 3177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase; 318c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { 319c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Flags = TraitsBase::Flags & ~LvalueBit 320c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 321c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 322c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 323c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 3247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez/** \ingroup Geometry_Module 3257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \brief Quaternion expression mapping a constant memory buffer 326c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 3277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \tparam _Scalar the type of the Quaternion coefficients 3287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \tparam _Options see class Map 329c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 330c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This is a specialization of class Map for Quaternion. This class allows to view 331c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * a 4 scalar memory buffer as an Eigen's Quaternion object. 332c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 333c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class Map, class Quaternion, class QuaternionBase 334c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 335c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar, int _Options> 336c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass Map<const Quaternion<_Scalar>, _Options > 337c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > 338c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 339c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base; 340c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 341c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath public: 342c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 343c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::traits<Map>::Coefficients Coefficients; 344c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) 345c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*=; 346c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 347c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs a Mapped Quaternion object from the pointer \a coeffs 348c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 3497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: 350c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \code *coeffs == {x, y, z, w} \endcode 351c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 352c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ 353c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} 354c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 355c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline const Coefficients& coeffs() const { return m_coeffs;} 356c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 357c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath protected: 358c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Coefficients m_coeffs; 359c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 360c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 3617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez/** \ingroup Geometry_Module 3627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \brief Expression of a quaternion from a memory buffer 363c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 3647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \tparam _Scalar the type of the Quaternion coefficients 3657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \tparam _Options see class Map 366c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 367c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This is a specialization of class Map for Quaternion. This class allows to view 368c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * a 4 scalar memory buffer as an Eigen's Quaternion object. 369c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 370c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class Map, class Quaternion, class QuaternionBase 371c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 372c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar, int _Options> 373c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass Map<Quaternion<_Scalar>, _Options > 374c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> > 375c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 376c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base; 377c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 378c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath public: 379c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 380c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::traits<Map>::Coefficients Coefficients; 381c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) 382c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*=; 383c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 384c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs a Mapped Quaternion object from the pointer \a coeffs 385c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 3867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: 387c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \code *coeffs == {x, y, z, w} \endcode 388c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 389c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ 390c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} 391c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 392c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Coefficients& coeffs() { return m_coeffs; } 393c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline const Coefficients& coeffs() const { return m_coeffs; } 394c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 395c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath protected: 396c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Coefficients m_coeffs; 397c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 398c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 399c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 4007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * Map an unaligned array of single precision scalars as a quaternion */ 401c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Map<Quaternion<float>, 0> QuaternionMapf; 402c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 4037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * Map an unaligned array of double precision scalars as a quaternion */ 404c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Map<Quaternion<double>, 0> QuaternionMapd; 405c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 4067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * Map a 16-byte aligned array of single precision scalars as a quaternion */ 407c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf; 408c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 4097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * Map a 16-byte aligned array of double precision scalars as a quaternion */ 410c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; 411c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 412c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/*************************************************************************** 413c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Implementation of QuaternionBase methods 414c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath***************************************************************************/ 415c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 416c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Generic Quaternion * Quaternion product 417c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This product can be specialized for a given architecture via the Arch template argument. 418c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 419c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product 420c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 421c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){ 422c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Quaternion<Scalar> 423c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ( 424c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), 425c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), 426c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), 427c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x() 428c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ); 429c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 430c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 431c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 432c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 433c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \returns the concatenation of two rotations as a quaternion-quaternion product */ 434c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 435c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class OtherDerived> 436c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar> 437c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const 438c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 439c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value), 440c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 441c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return internal::quat_product<Architecture::Target, Derived, OtherDerived, 442c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typename internal::traits<Derived>::Scalar, 443c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other); 444c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 445c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 446c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \sa operator*(Quaternion) */ 447c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 448c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class OtherDerived> 449c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) 450c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 451c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath derived() = derived() * other.derived(); 452c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 453c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 454c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 455c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Rotation of a vector by a quaternion. 456c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \remarks If the quaternion is used to rotate several points (>1) 457c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * then it is much more efficient to first convert it to a 3x3 Matrix. 458c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Comparison of the operation cost for n transformations: 459c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * - Quaternion2: 30n 460c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * - Via a Matrix3: 24 + 15n 461c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 462c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 463c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 464c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternionBase<Derived>::_transformVector(Vector3 v) const 465c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 466c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Note that this algorithm comes from the optimization by hand 467c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // of the conversion to a Matrix followed by a Matrix/Vector product. 468c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // It appears to be much faster than the common algorithm found 4697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // in the literature (30 versus 39 flops). It also requires two 470c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Vector3 as temporaries. 471c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 uv = this->vec().cross(v); 472c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath uv += uv; 473c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return v + this->w() * uv + this->vec().cross(uv); 474c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 475c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 476c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 477c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) 478c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 479c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath coeffs() = other.coeffs(); 480c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 481c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 482c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 483c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 484c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class OtherDerived> 485c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) 486c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 487c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath coeffs() = other.coeffs(); 488c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 489c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 490c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 491c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this 492c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 493c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 494c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathEIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) 495c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 4967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::cos; 4977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sin; 498c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings 4997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez this->w() = cos(ha); 5007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez this->vec() = sin(ha) * aa.axis(); 501c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 502c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 503c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 504c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Set \c *this from the expression \a xpr: 505c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion 506c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix 507c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * and \a xpr is converted to a quaternion 508c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 509c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 510c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 511c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class MatrixDerived> 512c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) 513c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 514c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value), 515c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 516c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived()); 517c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 518c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 519c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 520c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to 521c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * be normalized, otherwise the result is undefined. 522c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 523c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 524c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline typename QuaternionBase<Derived>::Matrix3 525c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternionBase<Derived>::toRotationMatrix(void) const 526c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 527c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) 528c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // if not inlined then the cost of the return by value is huge ~ +35%, 529c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // however, not inlining this function is an order of magnitude slower, so 530c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // it has to be inlined, and so the return by value is not an issue 531c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 res; 532c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 533c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tx = Scalar(2)*this->x(); 534c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar ty = Scalar(2)*this->y(); 535c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tz = Scalar(2)*this->z(); 536c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar twx = tx*this->w(); 537c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar twy = ty*this->w(); 538c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar twz = tz*this->w(); 539c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar txx = tx*this->x(); 540c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar txy = ty*this->x(); 541c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar txz = tz*this->x(); 542c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tyy = ty*this->y(); 543c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tyz = tz*this->y(); 544c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar tzz = tz*this->z(); 545c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 546c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(0,0) = Scalar(1)-(tyy+tzz); 547c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(0,1) = txy-twz; 548c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(0,2) = txz+twy; 549c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(1,0) = txy+twz; 550c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(1,1) = Scalar(1)-(txx+tzz); 551c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(1,2) = tyz-twx; 552c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(2,0) = txz-twy; 553c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(2,1) = tyz+twx; 554c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(2,2) = Scalar(1)-(txx+tyy); 555c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 556c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return res; 557c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 558c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 559c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Sets \c *this to be a quaternion representing a rotation between 560c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * the two arbitrary vectors \a a and \a b. In other words, the built 561c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * rotation represent a rotation sending the line of direction \a a 562c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * to the line of direction \a b, both lines passing through the origin. 563c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 564c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns a reference to \c *this. 565c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 566c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that the two input vectors do \b not have to be normalized, and 567c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * do not need to have the same norm. 568c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 569c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<class Derived> 570c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived1, typename Derived2> 571c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 572c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 573c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using std::max; 5747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 575c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 v0 = a.normalized(); 576c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 v1 = b.normalized(); 577c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar c = v1.dot(v0); 578c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 579c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // if dot == -1, vectors are nearly opposites 5807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // => accurately compute the rotation axis by computing the 581c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // intersection of the two planes. This is done by solving: 582c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // x^T v0 = 0 583c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // x^T v1 = 0 584c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // under the constraint: 585c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // ||x|| = 1 586c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // which yields a singular value problem 587c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision()) 588c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 5897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez c = (max)(c,Scalar(-1)); 590c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); 591c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); 592c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 axis = svd.matrixV().col(2); 593c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 594c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar w2 = (Scalar(1)+c)*Scalar(0.5); 5957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez this->w() = sqrt(w2); 5967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez this->vec() = axis * sqrt(Scalar(1) - w2); 597c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 598c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 599c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 axis = v0.cross(v1); 6007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar s = sqrt((Scalar(1)+c)*Scalar(2)); 601c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar invs = Scalar(1)/s; 602c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath this->vec() = axis * invs; 603c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath this->w() = s * Scalar(0.5); 604c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 605c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return derived(); 606c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 607c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 608c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 609c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Returns a quaternion representing a rotation between 610c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * the two arbitrary vectors \a a and \a b. In other words, the built 611c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * rotation represent a rotation sending the line of direction \a a 612c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * to the line of direction \a b, both lines passing through the origin. 613c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 614c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns resulting quaternion 615c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 616c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that the two input vectors do \b not have to be normalized, and 617c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * do not need to have the same norm. 618c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 619c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar, int Options> 620c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived1, typename Derived2> 621c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 622c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 623c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternion quat; 624c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath quat.setFromTwoVectors(a, b); 625c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return quat; 626c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 627c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 628c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 629c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \returns the multiplicative inverse of \c *this 630c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that in most cases, i.e., if you simply want the opposite rotation, 631c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * and/or the quaternion is normalized, then it is enough to use the conjugate. 632c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 633c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa QuaternionBase::conjugate() 634c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 635c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 636c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const 637c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 638c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? 639c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar n2 = this->squaredNorm(); 640c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n2 > 0) 641c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Quaternion<Scalar>(conjugate().coeffs() / n2); 642c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 643c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 644c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // return an invalid result to flag the error 645c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Quaternion<Scalar>(Coefficients::Zero()); 646c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 647c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 648c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 649c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse 650c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * if the quaternion is normalized. 651c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * The conjugate of a quaternion represents the opposite rotation. 652c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 653c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa Quaternion2::inverse() 654c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 655c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 656c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline Quaternion<typename internal::traits<Derived>::Scalar> 657c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternionBase<Derived>::conjugate() const 658c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 659c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z()); 660c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 661c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 662c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \returns the angle (in radian) between two rotations 663c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa dot() 664c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 665c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 666c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class OtherDerived> 667c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathinline typename internal::traits<Derived>::Scalar 668c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const 669c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 670c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using std::acos; 6717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 6727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar d = abs(this->dot(other)); 6737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (d>=Scalar(1)) 674c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Scalar(0); 6757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return Scalar(2) * acos(d); 676c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 677c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 6787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 6797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 680c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \returns the spherical linear interpolation between the two quaternions 6817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \c *this and \a other at the parameter \a t in [0;1]. 6827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * 6837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * This represents an interpolation for a constant motion between \c *this and \a other, 6847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * see also http://en.wikipedia.org/wiki/Slerp. 685c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 686c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class Derived> 687c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <class OtherDerived> 688c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathQuaternion<typename internal::traits<Derived>::Scalar> 6897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezQuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const 690c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 691c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using std::acos; 6927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sin; 6937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 694c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); 695c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar d = this->dot(other); 6967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar absD = abs(d); 697c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 698c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar scale0; 699c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar scale1; 700c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 701c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(absD>=one) 702c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 703c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath scale0 = Scalar(1) - t; 704c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath scale1 = t; 705c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 706c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 707c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 708c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // theta is the angle between the 2 quaternions 709c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar theta = acos(absD); 7107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar sinTheta = sin(theta); 711c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 7127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; 7137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez scale1 = sin( ( t * theta) ) / sinTheta; 714c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 715c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(d<0) scale1 = -scale1; 716c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 717c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs()); 718c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 719c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 720c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 721c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 722c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// set from a rotation matrix 723c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Other> 724c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct quaternionbase_assign_impl<Other,3,3> 725c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 726c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename Other::Scalar Scalar; 727c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef DenseIndex Index; 728c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat) 729c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 7307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sqrt; 731c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // This algorithm comes from "Quaternion Calculus and Fast Animation", 732c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Ken Shoemake, 1987 SIGGRAPH course notes 733c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar t = mat.trace(); 734c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (t > Scalar(0)) 735c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 736c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t = sqrt(t + Scalar(1.0)); 737c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.w() = Scalar(0.5)*t; 738c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t = Scalar(0.5)/t; 739c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; 740c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; 741c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; 742c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 743c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 744c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 745c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath DenseIndex i = 0; 746c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (mat.coeff(1,1) > mat.coeff(0,0)) 747c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath i = 1; 748c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (mat.coeff(2,2) > mat.coeff(i,i)) 749c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath i = 2; 750c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath DenseIndex j = (i+1)%3; 751c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath DenseIndex k = (j+1)%3; 752c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 753c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0)); 754c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.coeffs().coeffRef(i) = Scalar(0.5) * t; 755c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t = Scalar(0.5)/t; 756c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; 757c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; 758c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; 759c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 760c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 761c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 762c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 763c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// set from a vector of coefficients assumed to be a quaternion 764c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Other> 765c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct quaternionbase_assign_impl<Other,4,1> 766c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 767c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename Other::Scalar Scalar; 768c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec) 769c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 770c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q.coeffs() = vec; 771c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 772c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 773c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 774c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace internal 775c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 776c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 777c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 778c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_QUATERNION_H 779