From 670651e2e0932c5edfe2a2da4b9f3c42af3b7dec Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Mon, 9 Nov 2009 10:48:18 +0100 Subject: [PATCH] Quaternion: fix compilation, cleaning --- Eigen/src/Geometry/Quaternion.h | 392 +++++++++++++++++--------------- 1 file changed, 213 insertions(+), 179 deletions(-) diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 67b040165..12c07d76c 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -26,6 +26,149 @@ #ifndef EIGEN_QUATERNION_H #define EIGEN_QUATERNION_H +/*************************************************************************** +* Definition of QuaternionBase +* The implementation is at the end of the file +***************************************************************************/ + +template +struct ei_quaternionbase_assign_impl; + +template +class QuaternionBase : public RotationBase +{ + typedef RotationBase Base; +public: + using Base::operator*; + using Base::derived; + + typedef typename ei_traits::Scalar Scalar; + typedef typename NumTraits::Real RealScalar; + typedef typename ei_traits::Coefficients Coefficients; + + // typedef typename Matrix Coefficients; + /** the type of a 3D vector */ + typedef Matrix Vector3; + /** the equivalent rotation matrix type */ + typedef Matrix Matrix3; + /** the equivalent angle-axis type */ + typedef AngleAxis AngleAxisType; + + + + /** \returns the \c x coefficient */ + inline Scalar x() const { return this->derived().coeffs().coeff(0); } + /** \returns the \c y coefficient */ + inline Scalar y() const { return this->derived().coeffs().coeff(1); } + /** \returns the \c z coefficient */ + inline Scalar z() const { return this->derived().coeffs().coeff(2); } + /** \returns the \c w coefficient */ + inline Scalar w() const { return this->derived().coeffs().coeff(3); } + + /** \returns a reference to the \c x coefficient */ + inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } + /** \returns a reference to the \c y coefficient */ + inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } + /** \returns a reference to the \c z coefficient */ + inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } + /** \returns a reference to the \c w coefficient */ + inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } + + /** \returns a read-only vector expression of the imaginary part (x,y,z) */ + inline const VectorBlock vec() const { return coeffs().template start<3>(); } + + /** \returns a vector expression of the imaginary part (x,y,z) */ + inline VectorBlock vec() { return coeffs().template start<3>(); } + + /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ + inline const typename ei_traits::Coefficients& coeffs() const { return derived().coeffs(); } + + /** \returns a vector expression of the coefficients (x,y,z,w) */ + inline typename ei_traits::Coefficients& coeffs() { return derived().coeffs(); } + + template Derived& operator=(const QuaternionBase& other); + Derived& operator=(const QuaternionBase& other) + { return operator=(other); } + Derived& operator=(const AngleAxisType& aa); + template Derived& operator=(const MatrixBase& m); + + /** \returns a quaternion representing an identity rotation + * \sa MatrixBase::Identity() + */ + inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); } + + /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() + */ + inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } + + /** \returns the squared norm of the quaternion's coefficients + * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() + */ + inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } + + /** \returns the norm of the quaternion's coefficients + * \sa QuaternionBase::squaredNorm(), MatrixBase::norm() + */ + inline Scalar norm() const { return coeffs().norm(); } + + /** Normalizes the quaternion \c *this + * \sa normalized(), MatrixBase::normalize() */ + inline void normalize() { coeffs().normalize(); } + /** \returns a normalized copy of \c *this + * \sa normalize(), MatrixBase::normalized() */ + inline Quaternion normalized() const { return Quaternion(coeffs().normalized()); } + + /** \returns the dot product of \c *this and \a other + * Geometrically speaking, the dot product of two unit quaternions + * corresponds to the cosine of half the angle between the two rotations. + * \sa angularDistance() + */ + template inline Scalar dot(const QuaternionBase& other) const { return coeffs().dot(other.coeffs()); } + + template inline Scalar angularDistance(const QuaternionBase& other) const; + + Matrix3 toRotationMatrix() const; + + template + Derived& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); + + template inline Quaternion operator* (const QuaternionBase& q) const; + template inline Derived& operator*= (const QuaternionBase& q); + + Quaternion inverse() const; + Quaternion conjugate() const; + + template Quaternion slerp(Scalar t, const QuaternionBase& other) const; + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + template + bool isApprox(const QuaternionBase& other, RealScalar prec = precision()) const + { return coeffs().isApprox(other.coeffs(), prec); } + + Vector3 _transformVector(Vector3 v) const; + + /** \returns \c *this with scalar type casted to \a NewScalarType + * + * Note that if \a NewScalarType is equal to the current scalar type of \c *this + * then this function smartly returns a const reference to \c *this. + */ + template + inline typename ei_cast_return_type >::type cast() const + { + return typename ei_cast_return_type >::type( + coeffs().template cast()); + } +}; + +/*************************************************************************** +* Definition/implementation of Quaternion +***************************************************************************/ + /** \geometry_module \ingroup Geometry_Module * * \class Quaternion @@ -48,152 +191,13 @@ * \sa class AngleAxis, class Transform */ -template -struct ei_quaternionbase_assign_impl; - -template class Quaternion; // [XXX] => remove when Quaternion becomes Quaternion - -template -struct ei_traits > -{ - typedef typename ei_traits::Scalar Scalar; - enum { - PacketAccess = ei_traits::PacketAccess - }; -}; - -template -class QuaternionBase : public RotationBase -{ - typedef RotationBase Base; -public: - using Base::operator*; - - typedef typename ei_traits >::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - - // typedef typename Matrix Coefficients; - /** the type of a 3D vector */ - typedef Matrix Vector3; - /** the equivalent rotation matrix type */ - typedef Matrix Matrix3; - /** the equivalent angle-axis type */ - typedef AngleAxis AngleAxisType; - - /** \returns the \c x coefficient */ - inline Scalar x() const { return this->derived().coeffs().coeff(0); } - /** \returns the \c y coefficient */ - inline Scalar y() const { return this->derived().coeffs().coeff(1); } - /** \returns the \c z coefficient */ - inline Scalar z() const { return this->derived().coeffs().coeff(2); } - /** \returns the \c w coefficient */ - inline Scalar w() const { return this->derived().coeffs().coeff(3); } - - /** \returns a reference to the \c x coefficient */ - inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } - /** \returns a reference to the \c y coefficient */ - inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } - /** \returns a reference to the \c z coefficient */ - inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } - /** \returns a reference to the \c w coefficient */ - inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } - - /** \returns a read-only vector expression of the imaginary part (x,y,z) */ - inline const VectorBlock::Coefficients,3> vec() const { return this->derived().coeffs().template start<3>(); } - - /** \returns a vector expression of the imaginary part (x,y,z) */ - inline VectorBlock::Coefficients,3> vec() { return this->derived().coeffs().template start<3>(); } - - /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ - inline const typename ei_traits::Coefficients& coeffs() const { return this->derived().coeffs(); } - - /** \returns a vector expression of the coefficients (x,y,z,w) */ - inline typename ei_traits::Coefficients& coeffs() { return this->derived().coeffs(); } - - template QuaternionBase& operator=(const QuaternionBase& other); - QuaternionBase& operator=(const AngleAxisType& aa); - template - QuaternionBase& operator=(const MatrixBase& m); - - /** \returns a quaternion representing an identity rotation - * \sa MatrixBase::Identity() - */ - inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); } - - /** \sa Quaternion2::Identity(), MatrixBase::setIdentity() - */ - inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } - - /** \returns the squared norm of the quaternion's coefficients - * \sa Quaternion2::norm(), MatrixBase::squaredNorm() - */ - inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } - - /** \returns the norm of the quaternion's coefficients - * \sa Quaternion2::squaredNorm(), MatrixBase::norm() - */ - inline Scalar norm() const { return coeffs().norm(); } - - /** Normalizes the quaternion \c *this - * \sa normalized(), MatrixBase::normalize() */ - inline void normalize() { coeffs().normalize(); } - /** \returns a normalized version of \c *this - * \sa normalize(), MatrixBase::normalized() */ - inline Quaternion normalized() const { return Quaternion(coeffs().normalized()); } - - /** \returns the dot product of \c *this and \a other - * Geometrically speaking, the dot product of two unit quaternions - * corresponds to the cosine of half the angle between the two rotations. - * \sa angularDistance() - */ - template inline Scalar dot(const QuaternionBase& other) const { return coeffs().dot(other.coeffs()); } - - template inline Scalar angularDistance(const QuaternionBase& other) const; - - Matrix3 toRotationMatrix(void) const; - - template - QuaternionBase& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); - - template inline Quaternion operator* (const QuaternionBase& q) const; - template inline QuaternionBase& operator*= (const QuaternionBase& q); - - Quaternion inverse(void) const; - Quaternion conjugate(void) const; - - template Quaternion slerp(Scalar t, const QuaternionBase& other) const; - - /** \returns \c true if \c *this is approximately equal to \a other, within the precision - * determined by \a prec. - * - * \sa MatrixBase::isApprox() */ - bool isApprox(const QuaternionBase& other, RealScalar prec = precision()) const - { return coeffs().isApprox(other.coeffs(), prec); } - - Vector3 _transformVector(Vector3 v) const; - - /** \returns \c *this with scalar type casted to \a NewScalarType - * - * Note that if \a NewScalarType is equal to the current scalar type of \c *this - * then this function smartly returns a const reference to \c *this. - */ - template - inline typename ei_cast_return_type >::type cast() const - { - return typename ei_cast_return_type >::type( - coeffs().template cast()); - } -}; - template struct ei_traits > { typedef _Scalar Scalar; typedef Matrix<_Scalar,4,1> Coefficients; enum{ - PacketAccess = Aligned + PacketAccess = Aligned }; }; @@ -250,16 +254,29 @@ protected: Coefficients m_coeffs; }; -/* ########### Map */ +/** \ingroup Geometry_Module + * single precision quaternion type */ +typedef Quaternion Quaternionf; +/** \ingroup Geometry_Module + * double precision quaternion type */ +typedef Quaternion Quaterniond; + +/*************************************************************************** +* Specialization of Map> +***************************************************************************/ /** \class Map * \nonstableyet * - * \brief Expression of a quaternion + * \brief Expression of a quaternion from a memory buffer * - * \param Scalar the type of the vector of diagonal coefficients + * \param _Scalar the type of the Quaternion coefficients + * \param PacketAccess see class Map * - * \sa class Quaternion, class QuaternionBase + * This is a specialization of class Map for Quaternion. This class allows to view + * a 4 scalar memory buffer as an Eigen's Quaternion object. + * + * \sa class Map, class Quaternion, class QuaternionBase */ template struct ei_traits, _PacketAccess> >: @@ -273,15 +290,23 @@ ei_traits > }; template -class Map, PacketAccess > : public QuaternionBase, PacketAccess> >, ei_no_assignment_operator { +class Map, PacketAccess > + : public QuaternionBase, PacketAccess> >, + ei_no_assignment_operator +{ public: - + typedef _Scalar Scalar; + typedef typename ei_traits::Coefficients Coefficients; - typedef typename ei_traits, PacketAccess> >::Coefficients Coefficients; + /** Constructs a Mapped Quaternion object from the pointer \a coeffs + * + * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: + * \code *coeffs == {x, y, z, w} \endcode + * + * If the template paramter PacketAccess is set to Aligned, then the pointer coeffs must be aligned. */ + inline Map(const Scalar* coeffs) : m_coeffs(coeffs) {} - inline Map, PacketAccess >(const Scalar* coeffs) : m_coeffs(coeffs) {} - inline Coefficients& coeffs() { return m_coeffs;} inline const Coefficients& coeffs() const { return m_coeffs;} @@ -289,15 +314,20 @@ class Map, PacketAccess > : public QuaternionBase > QuaternionMapd; -typedef Map > QuaternionMapf; -typedef Map, Aligned> QuaternionMapAlignedd; -typedef Map, Aligned> QuaternionMapAlignedf; +typedef Map > QuaternionMapd; +typedef Map > QuaternionMapf; +typedef Map, Aligned> QuaternionMapAlignedd; +typedef Map, Aligned> QuaternionMapAlignedf; + +/*************************************************************************** +* Implementation of QuaternionBase methods +***************************************************************************/ // Generic Quaternion * Quaternion product -template struct ei_quat_product +// This product can be specialized for a given architecture via the Arch template argument. +template struct ei_quat_product { - inline static Quaternion run(const QuaternionBase& a, const QuaternionBase& b){ + inline static Quaternion run(const QuaternionBase& a, const QuaternionBase& b){ return Quaternion ( a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), @@ -311,21 +341,22 @@ template template -inline Quaternion >::Scalar> QuaternionBase::operator* (const QuaternionBase& other) const +inline Quaternion::Scalar> +QuaternionBase::operator* (const QuaternionBase& other) const { EIGEN_STATIC_ASSERT((ei_is_same_type::ret), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - return ei_quat_product::Scalar, - ei_traits::PacketAccess && ei_traits::PacketAccess>::run(*this, other); + return ei_quat_product::Scalar, + ei_traits::PacketAccess && ei_traits::PacketAccess>::run(*this, other); } /** \sa operator*(Quaternion) */ template template -inline QuaternionBase& QuaternionBase::operator*= (const QuaternionBase& other) +inline Derived& QuaternionBase::operator*= (const QuaternionBase& other) { - return (*this = *this * other); + return (derived() = derived() * other.derived()); } /** Rotation of a vector by a quaternion. @@ -350,21 +381,21 @@ QuaternionBase::_transformVector(Vector3 v) const template template -inline QuaternionBase& QuaternionBase::operator=(const QuaternionBase& other) +inline Derived& QuaternionBase::operator=(const QuaternionBase& other) { coeffs() = other.coeffs(); - return *this; + return derived(); } /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this */ template -inline QuaternionBase& QuaternionBase::operator=(const AngleAxisType& aa) +inline Derived& QuaternionBase::operator=(const AngleAxisType& aa) { Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings this->w() = ei_cos(ha); this->vec() = ei_sin(ha) * aa.axis(); - return *this; + return derived(); } /** Set \c *this from the expression \a xpr: @@ -375,12 +406,12 @@ inline QuaternionBase& QuaternionBase::operator=(const AngleAx template template -inline QuaternionBase& QuaternionBase::operator=(const MatrixBase& xpr) +inline Derived& QuaternionBase::operator=(const MatrixBase& xpr) { EIGEN_STATIC_ASSERT((ei_is_same_type::ret), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) ei_quaternionbase_assign_impl::run(*this, xpr.derived()); - return *this; + return derived(); } /** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to @@ -434,7 +465,7 @@ QuaternionBase::toRotationMatrix(void) const */ template template -inline QuaternionBase& QuaternionBase::setFromTwoVectors(const MatrixBase& a, const MatrixBase& b) +inline Derived& QuaternionBase::setFromTwoVectors(const MatrixBase& a, const MatrixBase& b) { Vector3 v0 = a.normalized(); Vector3 v1 = b.normalized(); @@ -458,7 +489,7 @@ inline QuaternionBase& QuaternionBase::setFromTwoVectors(const Scalar w2 = (Scalar(1)+c)*Scalar(0.5); this->w() = ei_sqrt(w2); this->vec() = axis * ei_sqrt(Scalar(1) - w2); - return *this; + return derived(); } Vector3 axis = v0.cross(v1); Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2)); @@ -466,17 +497,17 @@ inline QuaternionBase& QuaternionBase::setFromTwoVectors(const this->vec() = axis * invs; this->w() = s * Scalar(0.5); - return *this; + return derived(); } /** \returns the multiplicative inverse of \c *this * Note that in most cases, i.e., if you simply want the opposite rotation, * and/or the quaternion is normalized, then it is enough to use the conjugate. * - * \sa Quaternion2::conjugate() + * \sa QuaternionBase::conjugate() */ template -inline Quaternion >::Scalar> QuaternionBase::inverse() const +inline Quaternion::Scalar> QuaternionBase::inverse() const { // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? Scalar n2 = this->squaredNorm(); @@ -485,7 +516,7 @@ inline Quaternion >::Scalar> Quaterni else { // return an invalid result to flag the error - return Quaternion(ei_traits::Coefficients::Zero()); + return Quaternion(Coefficients::Zero()); } } @@ -496,7 +527,8 @@ inline Quaternion >::Scalar> Quaterni * \sa Quaternion2::inverse() */ template -inline Quaternion >::Scalar> QuaternionBase::conjugate() const +inline Quaternion::Scalar> +QuaternionBase::conjugate() const { return Quaternion(this->w(),-this->x(),-this->y(),-this->z()); } @@ -506,11 +538,12 @@ inline Quaternion >::Scalar> Quaterni */ template template -inline typename ei_traits >::Scalar QuaternionBase::angularDistance(const QuaternionBase& other) const +inline typename ei_traits::Scalar +QuaternionBase::angularDistance(const QuaternionBase& other) const { double d = ei_abs(this->dot(other)); if (d>=1.0) - return 0; + return Scalar(0); return Scalar(2) * std::acos(d); } @@ -519,13 +552,14 @@ inline typename ei_traits >::Scalar QuaternionBase template -Quaternion >::Scalar> QuaternionBase::slerp(Scalar t, const QuaternionBase& other) const +Quaternion::Scalar> +QuaternionBase::slerp(Scalar t, const QuaternionBase& other) const { static const Scalar one = Scalar(1) - precision(); Scalar d = this->dot(other); Scalar absD = ei_abs(d); if (absD>=one) - return Quaternion(*this); + return Quaternion(derived()); // theta is the angle between the 2 quaternions Scalar theta = std::acos(absD); @@ -549,7 +583,7 @@ struct ei_quaternionbase_assign_impl // This algorithm comes from "Quaternion Calculus and Fast Animation", // Ken Shoemake, 1987 SIGGRAPH course notes Scalar t = mat.trace(); - if (t > 0) + if (t > Scalar(0)) { t = ei_sqrt(t + Scalar(1.0)); q.w() = Scalar(0.5)*t;