diff --git a/Eigen/Geometry b/Eigen/Geometry index ab3a9b9d6..3bb4c8801 100644 --- a/Eigen/Geometry +++ b/Eigen/Geometry @@ -28,9 +28,9 @@ namespace Eigen { #include "src/Array/PartialRedux.h" #include "src/Geometry/OrthoMethods.h" +#include "src/Geometry/Rotation.h" #include "src/Geometry/Quaternion.h" #include "src/Geometry/AngleAxis.h" -#include "src/Geometry/Rotation.h" #include "src/Geometry/Transform.h" #include "src/Geometry/Translation.h" #include "src/Geometry/Scaling.h" diff --git a/Eigen/src/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h index 502cbd0ec..0d43b277e 100644 --- a/Eigen/src/Geometry/AngleAxis.h +++ b/Eigen/src/Geometry/AngleAxis.h @@ -46,9 +46,18 @@ * * \sa class Quaternion, class Transform, MatrixBase::UnitX() */ -template -class AngleAxis + +template struct ei_traits > { + typedef _Scalar Scalar; +}; + +template +class AngleAxis : public RotationBase,3> +{ + typedef RotationBase,3> Base; + using Base::operator*; + public: enum { Dim = 3 }; /** the scalar type of the coefficients */ diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 483d4c0d7..fc1537711 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -51,9 +51,17 @@ struct ei_quaternion_assign_impl; * * \sa class AngleAxis, class Transform */ -template -class Quaternion + +template struct ei_traits > { + typedef _Scalar Scalar; +}; + +template +class Quaternion : public RotationBase,3> +{ + typedef RotationBase,3> Base; + using Base::operator*; typedef Matrix<_Scalar, 4, 1> Coefficients; Coefficients m_coeffs; diff --git a/Eigen/src/Geometry/Rotation.h b/Eigen/src/Geometry/Rotation.h index 9ce6e71ca..696fcdd4e 100644 --- a/Eigen/src/Geometry/Rotation.h +++ b/Eigen/src/Geometry/Rotation.h @@ -28,79 +28,42 @@ // this file aims to contains the various representations of rotation/orientation // in 2D and 3D space excepted Matrix and Quaternion. -/** \internal +/** \class RotationBase * - * \class ToRotationMatrix - * - * \brief Template static struct to convert any rotation representation to a matrix form - * - * \param Scalar the numeric type of the matrix coefficients - * \param Dim the dimension of the current space - * \param RotationType the input type of the rotation - * - * This class defines a single static member with the following prototype: - * \code - * static convert(const RotationType& r); - * \endcode - * where \c must be a fixed-size matrix expression of size Dim x Dim and - * coefficient type Scalar. - * - * Default specializations are provided for: - * - any scalar type (2D), - * - any matrix expression, - * - Quaternion, - * - AngleAxis. - * - * Currently ToRotationMatrix is only used by Transform. - * - * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis + * \brief Common base class for compact rotation representations * + * \param Derived is the derived type, i.e., a rotation type + * \param _Dim the dimension of the space */ -template -struct ToRotationMatrix; - -// 2D rotation to matrix -template -struct ToRotationMatrix +template +class RotationBase { - inline static Matrix convert(const OtherScalarType& r) - { return Rotation2D(r).toRotationMatrix(); } -}; + public: + enum { Dim = _Dim }; + /** the scalar type of the coefficients */ + typedef typename ei_traits::Scalar Scalar; + + /** corresponding linear transformation matrix type */ + typedef Matrix RotationMatrixType; -// 2D rotation to rotation matrix -template -struct ToRotationMatrix > -{ - inline static Matrix convert(const Rotation2D& r) - { return Rotation2D(r).toRotationMatrix(); } -}; + inline const Derived& derived() const { return *static_cast(this); } + inline Derived& derived() { return *static_cast(this); } -// quaternion to rotation matrix -template -struct ToRotationMatrix > -{ - inline static Matrix convert(const Quaternion& q) - { return q.toRotationMatrix(); } -}; + /** \returns an equivalent rotation matrix */ + inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } -// angle axis to rotation matrix -template -struct ToRotationMatrix > -{ - inline static Matrix convert(const AngleAxis& aa) - { return aa.toRotationMatrix(); } -}; + /** \returns the concatenation of the rotation \c *this with a translation \a t */ + inline Transform operator*(const Translation& t) const + { return toRotationMatrix() * t; } -// matrix xpr to matrix xpr -template -struct ToRotationMatrix > -{ - inline static const MatrixBase& convert(const MatrixBase& mat) - { - EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim, - you_did_a_programming_error); - return mat; - } + /** \returns the concatenation of the rotation \c *this with a scaling \a s */ + inline RotationMatrixType operator*(const Scaling& s) const + { return toRotationMatrix() * s; } + + /** \returns the concatenation of the rotation \c *this with an affine transformation \a t */ + inline Transform operator*(const Transform& t) const + { return toRotationMatrix() * t; } + }; /** \geometry_module \ingroup GeometryModule @@ -119,9 +82,17 @@ struct ToRotationMatrix > * * \sa class Quaternion, class Transform */ -template -class Rotation2D +template struct ei_traits > { + typedef _Scalar Scalar; +}; + +template +class Rotation2D : public RotationBase,2> +{ + typedef RotationBase,2> Base; + using Base::operator*; + public: enum { Dim = 2 }; /** the scalar type of the coefficients */ @@ -206,4 +177,43 @@ Rotation2D::toRotationMatrix(void) const return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); } +/** \internal + * + * Helper function to return an arbitrary rotation object to a rotation matrix. + * + * \param Scalar the numeric type of the matrix coefficients + * \param Dim the dimension of the current space + * + * It returns a Dim x Dim fixed size matrix. + * + * Default specializations are provided for: + * - any scalar type (2D), + * - any matrix expression, + * - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D) + * + * Currently ei_toRotationMatrix is only used by Transform. + * + * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis + */ +template +inline static Matrix ei_toRotationMatrix(const Scalar& s) +{ + EIGEN_STATIC_ASSERT(Dim==2,you_did_a_programming_error); + return Rotation2D(s).toRotationMatrix(); +} + +template +inline static Matrix ei_toRotationMatrix(const RotationBase& r) +{ + return r.toRotationMatrix(); +} + +template +inline static const MatrixBase& ei_toRotationMatrix(const MatrixBase& mat) +{ + EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim, + you_did_a_programming_error); + return mat; +} + #endif // EIGEN_ROTATION_H diff --git a/Eigen/src/Geometry/Scaling.h b/Eigen/src/Geometry/Scaling.h index 73938ac8b..4712b7d94 100644 --- a/Eigen/src/Geometry/Scaling.h +++ b/Eigen/src/Geometry/Scaling.h @@ -105,6 +105,10 @@ public: friend inline LinearMatrixType operator* (const LinearMatrixType& other, const Scaling& s) { return other * s.coeffs().asDiagonal(); } + template + inline LinearMatrixType operator*(const RotationBase& r) const + { return *this * r.toRotationMatrix(); } + /** Applies scaling to vector */ inline VectorType operator* (const VectorType& other) const { return coeffs().asDiagonal() * other; } diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index 1a60fe0ab..da457fc38 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -218,6 +218,13 @@ public: return res; } +// template +// inline Transform& operator=(const Rotation& t); + template + inline Transform& operator*=(const RotationBase& r) { return rotate(r.toRotationMatrix()); } + template + inline Transform operator*(const RotationBase& r) const; + LinearMatrixType extractRotation(TransformTraits traits = GenericAffine) const; template @@ -349,7 +356,7 @@ Transform::pretranslate(const MatrixBase &other) * to \c *this and returns a reference to \c *this. * * The template parameter \a RotationType is the type of the rotation which - * must be registered by ToRotationMatrix<>. + * must be known by ei_toRotationMatrix<>. * * Natively supported types includes: * - any scalar (2D), @@ -360,14 +367,14 @@ Transform::pretranslate(const MatrixBase &other) * This mechanism is easily extendable to support user types such as Euler angles, * or a pair of Quaternion for 4D rotations. * - * \sa rotate(Scalar), class Quaternion, class AngleAxis, class ToRotationMatrix, prerotate(RotationType) + * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType) */ template template Transform& Transform::rotate(const RotationType& rotation) { - linear() *= ToRotationMatrix::convert(rotation); + linear() *= ei_toRotationMatrix(rotation); return *this; } @@ -383,7 +390,7 @@ template Transform& Transform::prerotate(const RotationType& rotation) { - m_matrix.template block(0,0) = ToRotationMatrix::convert(rotation) + m_matrix.template block(0,0) = ei_toRotationMatrix(rotation) * m_matrix.template block(0,0); return *this; } @@ -454,6 +461,15 @@ inline Transform Transform::operator*(const ScalingType& return res; } +template +template +inline Transform Transform::operator*(const RotationBase& r) const +{ + Transform res = *this; + res.rotate(r.derived()); + return res; +} + /*************************** *** Specialial functions *** ***************************/ @@ -511,7 +527,7 @@ Transform& Transform::fromPositionOrientationScale(const MatrixBase &position, const OrientationType& orientation, const MatrixBase &scale) { - linear() = ToRotationMatrix::convert(orientation); + linear() = ei_toRotationMatrix(orientation); linear() *= scale.asDiagonal(); translation() = position; m_matrix(Dim,Dim) = 1.; diff --git a/Eigen/src/Geometry/Translation.h b/Eigen/src/Geometry/Translation.h index b65aca9eb..f35670546 100644 --- a/Eigen/src/Geometry/Translation.h +++ b/Eigen/src/Geometry/Translation.h @@ -93,6 +93,10 @@ public: /** Concatenates a translation and a linear transformation */ inline TransformType operator* (const LinearMatrixType& linear) const; + template + inline TransformType operator*(const RotationBase& r) const + { return *this * r.toRotationMatrix(); } + /** Concatenates a linear transformation and a translation */ // its a nightmare to define a templated friend function outside its declaration friend inline TransformType operator* (const LinearMatrixType& linear, const Translation& t) diff --git a/test/geometry.cpp b/test/geometry.cpp index 807b951c7..2bad57f62 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp @@ -173,6 +173,14 @@ template void geometry(void) VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) * (t21.prescale(v21.cwise().inverse()).translate(-v20))).isIdentity(test_precision()) ); + + t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix()); + t1.setIdentity(); t1.scale(v0).rotate(q1); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + + t0.setIdentity(); t0.scale(v0).rotate(AngleAxis(q1)); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + // Transform - new API // 3D t0.setIdentity(); @@ -211,6 +219,36 @@ template void geometry(void) t1 = Translation3(v0) * t1; VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + // transform * quaternion + t0.rotate(q1); + t1 = t1 * q1; + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + + // translation * quaternion + t0.translate(v1).rotate(q1); + t1 = t1 * (Translation3(v1) * q1); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + + // scaling * quaternion + t0.scale(v1).rotate(q1); + t1 = t1 * (Scaling3(v1) * q1); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + + // quaternion * transform + t0.prerotate(q1); + t1 = q1 * t1; + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + + // quaternion * translation + t0.rotate(q1).translate(v1); + t1 = t1 * (q1 * Translation3(v1)); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + + // quaternion * scaling + t0.rotate(q1).scale(v1); + t1 = t1 * (q1 * Scaling3(v1)); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + // translation * vector t0.setIdentity(); t0.translate(v0);