Added an extensible mechanism to support any kind of rotation

representation in Transform via the template static class
ToRotationMatrix.
Added a lightweight AngleAxis class (similar to Rotation2D).

Eigen/Geometry

@@ -7,6 +7,7 @@ namespace Eigen {
 
 #include "src/Geometry/Cross.h"
 #include "src/Geometry/Quaternion.h"
+#include "src/Geometry/Rotation.h"
 #include "src/Geometry/Transform.h"
 
 // the Geometry module use cwiseCos and cwiseSin which are defined in the Array module

Eigen/src/Core/util/ForwardDeclarations.h

@@ -61,6 +61,9 @@ template<typename MatrixType, unsigned int Mode> class Extract;
 template<typename Derived, bool HasArrayFlag = int(ei_traits<Derived>::Flags) & ArrayBit> class ArrayBase {};
 template<typename Lhs, typename Rhs> class Cross;
 template<typename Scalar> class Quaternion;
+template<typename Scalar> class Rotation2D;
+template<typename Scalar> class AngleAxis;
+template<typename Scalar,int Dim> class Transform;
 
 
 template<typename Scalar> struct ei_scalar_sum_op;

Eigen/src/Geometry/Quaternion.h

@@ -228,15 +228,15 @@ Quaternion<Scalar>::toRotationMatrix(void) const
   Scalar tyz = tz*this->y();
   Scalar tzz = tz*this->z();
 
-  res(0,0) = 1-(tyy+tzz);
+  res.coeffRef(0,0) = 1-(tyy+tzz);
-  res(0,1) = txy-twz;
+  res.coeffRef(0,1) = txy-twz;
-  res(0,2) = txz+twy;
+  res.coeffRef(0,2) = txz+twy;
-  res(1,0) = txy+twz;
+  res.coeffRef(1,0) = txy+twz;
-  res(1,1) = 1-(txx+tzz);
+  res.coeffRef(1,1) = 1-(txx+tzz);
-  res(1,2) = tyz-twx;
+  res.coeffRef(1,2) = tyz-twx;
-  res(2,0) = txz-twy;
+  res.coeffRef(2,0) = txz-twy;
-  res(2,1) = tyz+twx;
+  res.coeffRef(2,1) = tyz+twx;
-  res(2,2) = 1-(txx+tyy);
+  res.coeffRef(2,2) = 1-(txx+tyy);
 
   return res;
 }
@@ -250,7 +250,7 @@ Quaternion<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Deri
 {
   // FIXME maybe this function could accept 4x4 and 3x4 matrices as well ? (simply update the assert)
   // FIXME this function could also be static and returns a temporary ?
-  ei_assert(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3);
+  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3,you_did_a_programming_error);
   // This algorithm comes from  "Quaternion Calculus and Fast Animation",
   // Ken Shoemake, 1987 SIGGRAPH course notes
   Scalar t = mat.trace();

Eigen/src/Geometry/Rotation.h

@@ -0,0 +1,256 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ROTATION_H
+#define EIGEN_ROTATION_H
+
+// this file aims to contains the various representations of rotation/orientation
+// in 2D and 3D space excepted Matrix and Quaternion.
+
+/** \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 <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
+  *
+  */
+template<typename Scalar, int Dim, typename RotationType>
+struct ToRotationMatrix;
+
+// 2D rotation to matrix
+template<typename Scalar, typename OtherScalarType>
+struct ToRotationMatrix<Scalar, 2, OtherScalarType>
+{
+  inline static Matrix<Scalar,2,2> convert(const OtherScalarType& r)
+  { return Rotation2D<Scalar>(r).toRotationMatrix(); }
+};
+
+// 2D rotation to matrix
+template<typename Scalar, typename OtherScalarType>
+struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
+{
+  inline static Matrix<Scalar,2,2> convert(const Rotation2D<OtherScalarType>& r)
+  { return Rotation2D<Scalar>(r).toRotationMatrix(); }
+};
+
+// quaternion to matrix
+template<typename Scalar, typename OtherScalarType>
+struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
+{
+  inline static Matrix<Scalar,3,3> convert(const Quaternion<OtherScalarType>& q)
+  { return q.toRotationMatrix(); }
+};
+
+// angle axis to matrix
+template<typename Scalar, typename OtherScalarType>
+struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
+{
+  inline static Matrix<Scalar,3,3> convert(const AngleAxis<OtherScalarType>& aa)
+  { return aa.toRotationMatrix(); }
+};
+
+// matrix xpr to matrix xpr
+template<typename Scalar, int Dim, typename OtherDerived>
+struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
+{
+  inline static const MatrixBase<OtherDerived>& convert(const MatrixBase<OtherDerived>& mat)
+  {
+    EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim,
+      you_did_a_programming_error);
+    return mat;
+  }
+};
+
+/** \class Rotation2D
+  *
+  * \brief Represents a rotation/orientation in a 2 dimensional space.
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  *
+  * This class is equivalent to a single scalar representating the rotation angle
+  * in radian with some additional features such as the conversion from/to
+  * rotation matrix. Moreover this class aims to provide a similar interface
+  * to Quaternion in order to facilitate the writting of generic algorithm
+  * dealing with rotations.
+  *
+  * \sa class Quaternion, class Transform
+  */
+template<typename _Scalar>
+class Rotation2D
+{
+public:
+  enum { Dim = 2 };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,2,2> Matrix2;
+
+protected:
+
+  Scalar m_angle;
+
+public:
+
+  inline Rotation2D(Scalar a) : m_angle(a) {}
+  inline operator Scalar& () { return m_angle; }
+  inline operator Scalar () const { return m_angle; }
+
+  template<typename Derived>
+  Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
+  Matrix2 toRotationMatrix(void) const;
+
+  /** \returns the spherical interpolation between \c *this and \a other using
+    * parameter \a t. It is equivalent to a linear interpolation.
+    */
+  inline Rotation2D slerp(Scalar t, const Rotation2D& other) const
+  { return m_angle * (1-t) + t * other; }
+};
+
+/** Set \c *this from a 2x2 rotation matrix \a mat.
+  * In other words, this function extract the rotation angle
+  * from the rotation matrix.
+  */
+template<typename Scalar>
+template<typename Derived>
+Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
+{
+  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,you_did_a_programming_error);
+  m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0));
+  return *this;
+}
+
+/** Constructs and \returns an equivalent 2x2 rotation matrix.
+  */
+template<typename Scalar>
+typename Rotation2D<Scalar>::Matrix2
+Rotation2D<Scalar>::toRotationMatrix(void) const
+{
+  Scalar sinA = ei_sin(m_angle);
+  Scalar cosA = ei_cos(m_angle);
+  return (Matrix2() << cosA, -sinA, sinA, cosA).finished();
+}
+
+/** \class AngleAxis
+  *
+  * \brief Represents a rotation in a 3 dimensional space as a rotation angle around a 3D axis
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients.
+  *
+  * \sa class Quaternion, class Transform
+  */
+template<typename _Scalar>
+class AngleAxis
+{
+public:
+  enum { Dim = 3 };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,3,3> Matrix3;
+  typedef Matrix<Scalar,3,1> Vector3;
+
+protected:
+
+  Vector3 m_axis;
+  Scalar m_angle;
+
+public:
+
+  AngleAxis() {}
+  template<typename Derived>
+  inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
+
+  Scalar angle() const { return m_angle; }
+  Scalar& angle() { return m_angle; }
+
+  const Vector3& axis() const { return m_axis; }
+  Vector3& axis() { return m_axis; }
+
+  template<typename Derived>
+  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
+  Matrix3 toRotationMatrix(void) const;
+};
+
+/** Set \c *this from a 3x3 rotation matrix \a mat.
+  * In other words, this function extract the rotation angle
+  * from the rotation matrix.
+  */
+template<typename Scalar>
+template<typename Derived>
+AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
+{
+  // Since a direct conversion would not be really faster,
+  // let's use the robust Quaternion implementation:
+  Quaternion<Scalar>().fromRotationMatrix(mat).toAngleAxis(m_angle, m_axis);
+
+  return *this;
+}
+
+/** Constructs and \returns an equivalent 2x2 rotation matrix.
+  */
+template<typename Scalar>
+typename AngleAxis<Scalar>::Matrix3
+AngleAxis<Scalar>::toRotationMatrix(void) const
+{
+  Matrix3 res;
+  Vector3 sin_axis  = ei_sin(m_angle) * m_axis;
+  Scalar c = ei_cos(m_angle);
+  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
+
+  Scalar tmp;
+  tmp = cos1_axis.x() * m_axis.y();
+  res.coeffRef(0,1) = tmp - sin_axis.z();
+  res.coeffRef(1,0) = tmp + sin_axis.z();
+
+  tmp = cos1_axis.x() * m_axis.z();
+  res.coeffRef(0,2) = tmp + sin_axis.y();
+  res.coeffRef(2,0) = tmp - sin_axis.y();
+
+  tmp = cos1_axis.y() * m_axis.z();
+  res.coeffRef(1,2) = tmp - sin_axis.x();
+  res.coeffRef(2,1) = tmp + sin_axis.x();
+
+  res.diagonal() = Vector3::constant(c) + cos1_axis.cwiseProduct(m_axis);
+
+  return res;
+}
+
+#endif // EIGEN_ROTATION_H

Eigen/src/Geometry/Transform.h

@@ -25,94 +25,6 @@
 #ifndef EIGEN_TRANSFORM_H
 #define EIGEN_TRANSFORM_H
 
-/** \class Orientation2D
-  *
-  * \brief Represents an orientation/rotation in a 2 dimensional space.
-  *
-  * \param _Scalar the scalar type, i.e., the type of the coefficients
-  *
-  * This class is equivalent to a single scalar representating the rotation angle
-  * in radian with some additional features such as the conversion from/to
-  * rotation matrix. Moreover this class aims to provide a similar interface
-  * to Quaternion in order to facilitate the writting of generic algorithm
-  * dealing with rotations.
-  *
-  * \sa class Quaternion, class Transform
-  */
-template<typename _Scalar>
-class Orientation2D
-{
-public:
-  enum { Dim = 2 };
-  /** the scalar type of the coefficients */
-  typedef _Scalar Scalar;
-  typedef Matrix<Scalar,2,2> Matrix2;
-
-protected:
-
-  Scalar m_angle;
-
-public:
-
-  inline Orientation2D(Scalar a) : m_angle(a) {}
-  inline operator Scalar& () { return m_angle; }
-  inline operator Scalar () const { return m_angle; }
-
-  template<typename Derived>
-  Orientation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
-  Matrix2 toRotationMatrix(void) const;
-
-  Orientation2D slerp(Scalar t, const Orientation2D& other) const;
-};
-
-/** returns the default type used to represent an orientation.
-  */
-template<typename Scalar, int Dim>
-struct ei_get_orientation_type;
-
-template<typename Scalar>
-struct ei_get_orientation_type<Scalar,2>
-{ typedef Orientation2D<Scalar> type; };
-
-template<typename Scalar>
-struct ei_get_orientation_type<Scalar,3>
-{ typedef Quaternion<Scalar> type; };
-
-/** Set \c *this from a 2x2 rotation matrix \a mat.
-  * In other words, this function extract the rotation angle
-  * from the rotation matrix.
-  */
-template<typename Scalar>
-template<typename Derived>
-Orientation2D<Scalar>& Orientation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
-{
-  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,you_did_a_programming_error);
-  m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0));
-  return *this;
-}
-
-/** Constructs and \returns an equivalent 2x2 rotation matrix.
-  */
-template<typename Scalar>
-typename Orientation2D<Scalar>::Matrix2
-Orientation2D<Scalar>::toRotationMatrix(void) const
-{
-  Scalar sinA = ei_sin(m_angle);
-  Scalar cosA = ei_cos(m_angle);
-  return Matrix2(cosA, -sinA, sinA, cosA);
-}
-
-/** \returns the spherical interpolation between \c *this and \a other using
-  * parameter \a t. It is equivalent to a linear interpolation.
-  */
-template<typename Scalar>
-Orientation2D<Scalar>
-Orientation2D<Scalar>::slerp(Scalar t, const Orientation2D& other) const
-{
-  return m_angle * (1-t) + t * other;
-}
-
-
 /** \class Transform
   *
   * \brief Represents an homogeneous transformation in a N dimensional space
@@ -140,7 +52,6 @@ public:
   typedef Block<MatrixType,Dim,Dim> AffineMatrixRef;
   typedef Matrix<Scalar,Dim,1> VectorType;
   typedef Block<MatrixType,Dim,1> VectorRef;
-  typedef typename ei_get_orientation_type<Scalar,Dim>::type Orientation;
 
 protected:
 
@@ -160,7 +71,7 @@ public:
   { m_matrix = other.m_matrix; }
 
   inline Transform& operator=(const Transform& other)
-  { m_matrix = other.m_matrix; }
+  { m_matrix = other.m_matrix; return *this; }
 
   template<typename OtherDerived>
   inline explicit Transform(const MatrixBase<OtherDerived>& other)
@@ -168,7 +79,7 @@ public:
 
   template<typename OtherDerived>
   inline Transform& operator=(const MatrixBase<OtherDerived>& other)
-  { m_matrix = other; }
+  { m_matrix = other; return *this; }
 
   #ifdef EIGEN_QT_SUPPORT
   inline Transform(const QMatrix& other);
@@ -177,9 +88,9 @@ public:
   #endif
 
   /** \returns a read-only expression of the transformation matrix */
-  inline const MatrixType matrix() const { return m_matrix; }
+  inline const MatrixType& matrix() const { return m_matrix; }
   /** \returns a writable expression of the transformation matrix */
-  inline MatrixType matrix() { return m_matrix; }
+  inline MatrixType& matrix() { return m_matrix; }
 
   /** \returns a read-only expression of the affine (linear) part of the transformation */
   inline const AffineMatrixRef affine() const { return m_matrix.template block<Dim,Dim>(0,0); }
@@ -202,7 +113,7 @@ public:
   operator * (const MatrixBase<OtherDerived> &other) const;
 
   /** Contatenates two transformations */
-  Product<MatrixType,MatrixType>
+  const Product<MatrixType,MatrixType>
   operator * (const Transform& other) const
   { return m_matrix * other.matrix(); }
 
@@ -220,6 +131,12 @@ public:
   template<typename OtherDerived>
   Transform& pretranslate(const MatrixBase<OtherDerived> &other);
 
+  template<typename RotationType>
+  Transform& rotate(const RotationType& rotation);
+
+  template<typename RotationType>
+  Transform& prerotate(const RotationType& rotation);
+
   template<typename OtherDerived>
   Transform& shear(Scalar sx, Scalar sy);
 
@@ -229,9 +146,9 @@ public:
   AffineMatrixType extractRotation() const;
   AffineMatrixType extractRotationNoShear() const;
 
-  template<typename PositionDerived, typename ScaleDerived>
+  template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
   Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
-    const Orientation& orientation, const MatrixBase<ScaleDerived> &scale);
+    const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
 
   const Inverse<MatrixType, false> inverse() const
   { return m_matrix.inverse(); }
@@ -258,6 +175,7 @@ Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QMatrix& other)
   m_matrix << other.m11(), other.m21(), other.dx(),
               other.m12(), other.m22(), other.dy(),
               0, 0, 1;
+   return *this;
 }
 
 /** \returns a QMatrix from \c *this assuming the dimension is 2.
@@ -306,11 +224,11 @@ Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other)
 {
   EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime)
     && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error);
-  m_matrix.template block<3,4>(0,0) = (other.asDiagonal() * m_matrix.template block<3,4>(0,0)).lazy();
+  m_matrix.template block<Dim,HDim>(0,0) = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0)).lazy();
   return *this;
 }
 
-/** Applies on the right translation matrix represented by the vector \a other
+/** Applies on the right the translation matrix represented by the vector \a other
   * to \c *this and returns a reference to \c *this.
   * \sa pretranslate()
   */
@@ -325,7 +243,7 @@ Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other)
   return *this;
 }
 
-/** Applies on the left translation matrix represented by the vector \a other
+/** Applies on the left the translation matrix represented by the vector \a other
   * to \c *this and returns a reference to \c *this.
   * \sa translate()
   */
@@ -340,6 +258,49 @@ Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other)
   return *this;
 }
 
+/** Applies on the right the rotation represented by the rotation \a rotation
+  * 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<>.
+  *
+  * Natively supported types includes:
+  *   - any scalar (2D),
+  *   - a Dim x Dim matrix expression,
+  *   - Quaternion (3D),
+  *   - AngleAxis (3D)
+  *
+  * 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)
+  */
+template<typename Scalar, int Dim>
+template<typename RotationType>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::rotate(const RotationType& rotation)
+{
+  affine() *= ToRotationMatrix<Scalar,Dim,RotationType>::convert(rotation);
+  return *this;
+}
+
+/** Applies on the left the rotation represented by the rotation \a rotation
+  * to \c *this and returns a reference to \c *this.
+  *
+  * See rotate(RotationType) for further details.
+  *
+  * \sa rotate(RotationType), rotate(Scalar)
+  */
+template<typename Scalar, int Dim>
+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);
+  return *this;
+}
+
 /** Applies on the right the shear transformation represented
   * by the vector \a other to \c *this and returns a reference to \c *this.
   * \warning 2D only.
@@ -369,7 +330,7 @@ Transform<Scalar,Dim>::preshear(Scalar sx, Scalar sy)
 {
   EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime)
     && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error);
-  m_matrix.template block<3,4>(0,0) = AffineMatrixType(1, sx, sy, 1) * m_matrix.template block<3,4>(0,0);
+  m_matrix.template block<Dim,HDim>(0,0) = AffineMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
   return *this;
 }
 
@@ -399,16 +360,17 @@ Transform<Scalar,Dim>::extractRotationNoShear() const
   * of a 3D object.
   */
 template<typename Scalar, int Dim>
-template<typename PositionDerived, typename ScaleDerived>
+template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
 Transform<Scalar,Dim>&
 Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
-  const Orientation& orientation, const MatrixBase<ScaleDerived> &scale)
+  const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
 {
-  affine() = orientation.toRotationMatrix();
+  affine() = ToRotationMatrix<Scalar,Dim,OrientationType>::convert(orientation);
   translation() = position;
   m_matrix(Dim,Dim) = 1.;
   m_matrix.template block<1,Dim>(Dim,0).setZero();
   affine() *= scale.asDiagonal();
+  return *this;
 }
 
 //----------

test/geometry.cpp

@@ -24,6 +24,7 @@
 
 #include "main.h"
 #include <Eigen/Geometry>
+#include <Eigen/LU>
 
 template<typename Scalar> void geometry(void)
 {
@@ -31,8 +32,10 @@ template<typename Scalar> void geometry(void)
      Cross.h Quaternion.h, Transform.cpp
   */
 
+  typedef Matrix<Scalar,2,2> Matrix2;
   typedef Matrix<Scalar,3,3> Matrix3;
   typedef Matrix<Scalar,4,4> Matrix4;
+  typedef Matrix<Scalar,2,1> Vector2;
   typedef Matrix<Scalar,3,1> Vector3;
   typedef Matrix<Scalar,4,1> Vector4;
   typedef Quaternion<Scalar> Quaternion;
@@ -42,38 +45,38 @@ template<typename Scalar> void geometry(void)
     v1 = Vector3::random(),
     v2 = Vector3::random();
 
-  Scalar a;
+  Scalar a = ei_random<Scalar>(-M_PI, M_PI);
 
   q1.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized());
   q2.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized());
 
   // rotation matrix conversion
-//   VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
+  VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
-//   VERIFY_IS_APPROX(q1 * q2 * v2,
+  VERIFY_IS_APPROX(q1 * q2 * v2,
-//     q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
+    q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
-//   VERIFY_IS_NOT_APPROX(q2 * q1 * v2,
+  VERIFY_IS_NOT_APPROX(q2 * q1 * v2,
-//     q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
+    q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
-//   q2.fromRotationMatrix(q1.toRotationMatrix());
+  q2.fromRotationMatrix(q1.toRotationMatrix());
-//   VERIFY_IS_APPROX(q1*v1,q2*v1);
+  VERIFY_IS_APPROX(q1*v1,q2*v1);
-//
+
-//   // Euler angle conversion
+  // Euler angle conversion
-//   VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1);
+  VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1);
-//   v2 = q2.toEulerAngles();
+  v2 = q2.toEulerAngles();
-//   VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2);
+  VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2);
-//   VERIFY_IS_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2);
+  VERIFY_IS_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2);
-//
+
-//   // angle-axis conversion
+  // angle-axis conversion
-//   q1.toAngleAxis(a, v2);
+  q1.toAngleAxis(a, v2);
-//   VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1);
+  VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1);
-//   VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1);
+  VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1);
-//
+
-//   // from two vector creation
+  // from two vector creation
-//   VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
+  VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
-//   VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
+  VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
-//
+
-//   // inverse and conjugate
+  // inverse and conjugate
-//   VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
+  VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
-//   VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
+  VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
 
   // cross product
   VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
@@ -83,6 +86,18 @@ template<typename Scalar> void geometry(void)
       (v0.cross(v1).cross(v0)).normalized();
   VERIFY(m.isOrtho());
 
+  // AngleAxis
+  VERIFY_IS_APPROX(AngleAxis<Scalar>(a,v1.normalized()).toRotationMatrix(),
+    q2.fromAngleAxis(a,v1.normalized()).toRotationMatrix());
+
+  AngleAxis<Scalar> aa1;
+  m = q1.toRotationMatrix();
+  Vector3 tax; Scalar tan;
+  q2.fromRotationMatrix(m).toAngleAxis(tan,tax);
+  VERIFY_IS_APPROX(aa1.fromRotationMatrix(m).toRotationMatrix(),
+    q2.fromRotationMatrix(m).toRotationMatrix());
+
+
   // Transform
   // TODO complete the tests !
   typedef Transform<Scalar,2> Transform2;
@@ -119,6 +134,19 @@ template<typename Scalar> void geometry(void)
 
   t1.fromPositionOrientationScale(v0, q1, v1);
   VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
+
+  // 2D transformation
+  Transform2 t20, t21;
+  Vector2 v20 = Vector2::random();
+  Vector2 v21 = Vector2::random();
+  t21.setIdentity();
+  t21.affine() = Rotation2D<Scalar>(a).toRotationMatrix();
+  VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(),
+    t21.pretranslate(v20).scale(v21).matrix());
+
+  t21.setIdentity();
+  t21.affine() = Rotation2D<Scalar>(-a).toRotationMatrix();
+  VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) * (t21.prescale(v21.cwiseInverse()).translate(-v20))).isIdentity() );
 }
 
 void test_geometry()