* added a RotationBase class following the CRT pattern

This allow code factorization and generic template specialization
  of functions
* added any_rotation * {Translation,Scaling,Transform} products methods
* rewrite of the actually broken ToRoationMatrix helper class to
  a global ei_toRotationMatrix function.

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"

Eigen/src/Geometry/AngleAxis.h

@@ -46,9 +46,18 @@
   *
   * \sa class Quaternion, class Transform, MatrixBase::UnitX()
   */
-template<typename _Scalar>
+
-class AngleAxis
+template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
 {
+  typedef _Scalar Scalar;
+};
+
+template<typename _Scalar>
+class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
+{
+  typedef RotationBase<AngleAxis<_Scalar>,3> Base;
+  using Base::operator*;
+
 public:
   enum { Dim = 3 };
   /** the scalar type of the coefficients */

Eigen/src/Geometry/Quaternion.h

@@ -51,9 +51,17 @@ struct ei_quaternion_assign_impl;
   *
   * \sa  class AngleAxis, class Transform
   */
-template<typename _Scalar>
+
-class Quaternion
+template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
 {
+  typedef _Scalar Scalar;
+};
+
+template<typename _Scalar>
+class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
+{
+  typedef RotationBase<Quaternion<_Scalar>,3> Base;
+  using Base::operator*;
   typedef Matrix<_Scalar, 4, 1> Coefficients;
   Coefficients m_coeffs;
 

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 Common base class for compact rotation representations
-  *
-  * \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 <MatrixExpression> convert(const RotationType& r);
-  * \endcode
-  * where \c <MatrixExpression> 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
   *
+  * \param Derived is the derived type, i.e., a rotation type
+  * \param _Dim the dimension of the space
   */
-template<typename Scalar, int Dim, typename RotationType>
+template<typename Derived, int _Dim>
-struct ToRotationMatrix;
+class RotationBase
-
-// 2D rotation to matrix
-template<typename Scalar, typename OtherScalarType>
-struct ToRotationMatrix<Scalar, 2, OtherScalarType>
 {
-  inline static Matrix<Scalar,2,2> convert(const OtherScalarType& r)
+  public:
-  { return Rotation2D<Scalar>(r).toRotationMatrix(); }
+    enum { Dim = _Dim };
-};
+    /** the scalar type of the coefficients */
+    typedef typename ei_traits<Derived>::Scalar Scalar;
+    
+    /** corresponding linear transformation matrix type */
+    typedef Matrix<Scalar,Dim,Dim> RotationMatrixType;
 
-// 2D rotation to rotation matrix
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
-template<typename Scalar, typename OtherScalarType>
+    inline Derived& derived() { return *static_cast<Derived*>(this); }
-struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
-{
-  inline static Matrix<Scalar,2,2> convert(const Rotation2D<OtherScalarType>& r)
-  { return Rotation2D<Scalar>(r).toRotationMatrix(); }
-};
 
-// quaternion to rotation matrix
+    /** \returns an equivalent rotation matrix */
-template<typename Scalar, typename OtherScalarType>
+    inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); }
-struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
-{
-  inline static Matrix<Scalar,3,3> convert(const Quaternion<OtherScalarType>& q)
-  { return q.toRotationMatrix(); }
-};
 
-// angle axis to rotation matrix
+    /** \returns the concatenation of the rotation \c *this with a translation \a t */
-template<typename Scalar, typename OtherScalarType>
+    inline Transform<Scalar,Dim> operator*(const Translation<Scalar,Dim>& t) const
-struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
+    { return toRotationMatrix() * t; }
-{
-  inline static Matrix<Scalar,3,3> convert(const AngleAxis<OtherScalarType>& aa)
-  { return aa.toRotationMatrix(); }
-};
 
-// matrix xpr to matrix xpr
+    /** \returns the concatenation of the rotation \c *this with a scaling \a s */
-template<typename Scalar, int Dim, typename OtherDerived>
+    inline RotationMatrixType operator*(const Scaling<Scalar,Dim>& s) const
-struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
+    { return toRotationMatrix() * s; }
-{
+
-  inline static const MatrixBase<OtherDerived>& convert(const MatrixBase<OtherDerived>& mat)
+    /** \returns the concatenation of the rotation \c *this with an affine transformation \a t */
-  {
+    inline Transform<Scalar,Dim> operator*(const Transform<Scalar,Dim>& t) const
-    EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim,
+    { return toRotationMatrix() * t; }
-      you_did_a_programming_error);
+    
-    return mat;
-  }
 };
 
 /** \geometry_module \ingroup GeometryModule
@@ -119,9 +82,17 @@ struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
   *
   * \sa class Quaternion, class Transform
   */
-template<typename _Scalar>
+template<typename _Scalar> struct ei_traits<Rotation2D<_Scalar> >
-class Rotation2D
 {
+  typedef _Scalar Scalar;
+};
+
+template<typename _Scalar>
+class Rotation2D : public RotationBase<Rotation2D<_Scalar>,2>
+{
+  typedef RotationBase<Rotation2D<_Scalar>,2> Base;
+  using Base::operator*;
+
 public:
   enum { Dim = 2 };
   /** the scalar type of the coefficients */
@@ -206,4 +177,43 @@ Rotation2D<Scalar>::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<typename Scalar, int Dim>
+inline static Matrix<Scalar,2,2> ei_toRotationMatrix(const Scalar& s)
+{
+  EIGEN_STATIC_ASSERT(Dim==2,you_did_a_programming_error);
+  return Rotation2D<Scalar>(s).toRotationMatrix();
+}
+
+template<typename Scalar, int Dim, typename OtherDerived>
+inline static Matrix<Scalar,Dim,Dim> ei_toRotationMatrix(const RotationBase<OtherDerived,Dim>& r)
+{
+  return r.toRotationMatrix();
+}
+
+template<typename Scalar, int Dim, typename OtherDerived>
+inline static const MatrixBase<OtherDerived>& ei_toRotationMatrix(const MatrixBase<OtherDerived>& mat)
+{
+  EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim,
+    you_did_a_programming_error);
+  return mat;
+}
+
 #endif // EIGEN_ROTATION_H

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<typename Derived>
+  inline LinearMatrixType operator*(const RotationBase<Derived,Dim>& r) const
+  { return *this * r.toRotationMatrix(); }
+
   /** Applies scaling to vector */
   inline VectorType operator* (const VectorType& other) const
   { return coeffs().asDiagonal() * other; }

Eigen/src/Geometry/Transform.h

@@ -218,6 +218,13 @@ public:
     return res;
   }
 
+//   template<typename Derived>
+//   inline Transform& operator=(const Rotation<Derived,Dim>& t);
+  template<typename Derived>
+  inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); }
+  template<typename Derived>
+  inline Transform operator*(const RotationBase<Derived,Dim>& r) const;
+
   LinearMatrixType extractRotation(TransformTraits traits = GenericAffine) const;
 
   template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
@@ -349,7 +356,7 @@ Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &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<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &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<typename Scalar, int Dim>
 template<typename RotationType>
 Transform<Scalar,Dim>&
 Transform<Scalar,Dim>::rotate(const RotationType& rotation)
 {
-  linear() *= ToRotationMatrix<Scalar,Dim,RotationType>::convert(rotation);
+  linear() *= ei_toRotationMatrix<Scalar,Dim>(rotation);
   return *this;
 }
 
@@ -383,7 +390,7 @@ template<typename RotationType>
 Transform<Scalar,Dim>&
 Transform<Scalar,Dim>::prerotate(const RotationType& rotation)
 {
-  m_matrix.template block<Dim,HDim>(0,0) = ToRotationMatrix<Scalar,Dim,RotationType>::convert(rotation)
+  m_matrix.template block<Dim,HDim>(0,0) = ei_toRotationMatrix<Scalar,Dim>(rotation)
                                          * m_matrix.template block<Dim,HDim>(0,0);
   return *this;
 }
@@ -454,6 +461,15 @@ inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const ScalingType&
   return res;
 }
 
+template<typename Scalar, int Dim>
+template<typename Derived>
+inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const RotationBase<Derived,Dim>& r) const
+{
+  Transform res = *this;
+  res.rotate(r.derived());
+  return res;
+}
+
 /***************************
 *** Specialial functions ***
 ***************************/
@@ -511,7 +527,7 @@ Transform<Scalar,Dim>&
 Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
   const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
 {
-  linear() = ToRotationMatrix<Scalar,Dim,OrientationType>::convert(orientation);
+  linear() = ei_toRotationMatrix<Scalar,Dim>(orientation);
   linear() *= scale.asDiagonal();
   translation() = position;
   m_matrix(Dim,Dim) = 1.;

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<typename Derived>
+  inline TransformType operator*(const RotationBase<Derived,Dim>& 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)

test/geometry.cpp

@@ -173,6 +173,14 @@ template<typename Scalar> void geometry(void)
   VERIFY( (t20.fromPositionOrientationScale(v20,a,v21)
         * (t21.prescale(v21.cwise().inverse()).translate(-v20))).isIdentity(test_precision<Scalar>()) );
 
+
+  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<typename Scalar> 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);