work on rotations in the Geometry module:

- convertions are done trough constructors and operator=
 - added a EulerAngles class

Eigen/Geometry

@@ -7,6 +7,8 @@ namespace Eigen {
 
 #include "src/Geometry/Cross.h"
 #include "src/Geometry/Quaternion.h"
+#include "src/Geometry/AngleAxis.h"
+#include "src/Geometry/EulerAngles.h"
 #include "src/Geometry/Rotation.h"
 #include "src/Geometry/Transform.h"
 

Eigen/src/Core/util/ForwardDeclarations.h

@@ -56,11 +56,6 @@ template<int Direction, typename UnaryOp, typename MatrixType> class PartialRedu
 template<typename MatrixType, unsigned int Mode> class Part;
 template<typename MatrixType, unsigned int Mode> class Extract;
 template<typename MatrixType> class Array;
-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 Lhs, typename Rhs> struct ei_product_mode;
 template<typename Lhs, typename Rhs, int ProductMode = ei_product_mode<Lhs,Rhs>::value> struct ProductReturnType;
@@ -98,4 +93,12 @@ void ei_cache_friendly_product(
 template<typename ExpressionType, bool CheckExistence = true> class Inverse;
 template<typename MatrixType> class QR;
 
+// Geometry module:
+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> class EulerAngles;
+template<typename Scalar,int Dim> class Transform;
+
 #endif // EIGEN_FORWARDDECLARATIONS_H

Eigen/src/Geometry/AngleAxis.h

@@ -0,0 +1,147 @@
+// 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_ANGLEAXIS_H
+#define EIGEN_ANGLEAXIS_H
+
+/** \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 EulerAngles, 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;
+  typedef Quaternion<Scalar> QuaternionType;
+  typedef EulerAngles<Scalar> EulerAnglesType;
+
+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) {}
+  inline AngleAxis(const QuaternionType& q) { *this = q; }
+  inline AngleAxis(const EulerAnglesType& ea) { *this = ea; }
+  template<typename Derived>
+  inline AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
+
+  Scalar angle() const { return m_angle; }
+  Scalar& angle() { return m_angle; }
+
+  const Vector3& axis() const { return m_axis; }
+  Vector3& axis() { return m_axis; }
+
+  AngleAxis& operator=(const QuaternionType& q);
+  AngleAxis& operator=(const EulerAnglesType& ea);
+  template<typename Derived>
+  AngleAxis& operator=(const MatrixBase<Derived>& m);
+
+  template<typename Derived>
+  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
+  Matrix3 toRotationMatrix(void) const;
+};
+
+/** Set \c *this from a quaternion.
+  * The axis is normalized.
+  */
+template<typename Scalar>
+AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
+{
+  Scalar n2 = q.vec().norm2();
+  if (ei_isMuchSmallerThan(n2,Scalar(1)))
+  {
+    m_angle = 0;
+    m_axis << 1, 0, 0;
+  }
+  else
+  {
+    m_angle = 2*std::acos(q.w());
+    m_axis = q.vec() / ei_sqrt(n2);
+  }
+  return *this;
+}
+
+/** Set \c *this from Euler angles \a ea.
+  */
+template<typename Scalar>
+AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const EulerAnglesType& ea)
+{
+  return *this = QuaternionType(ea);
+}
+
+/** Set \c *this from a 3x3 rotation matrix \a mat.
+  */
+template<typename Scalar>
+template<typename Derived>
+AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
+{
+  // Since a direct conversion would not be really faster,
+  // let's use the robust Quaternion implementation:
+  return *this = QuaternionType(mat);
+}
+
+/** Constructs and \returns an equivalent 3x3 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_ANGLEAXIS_H

Eigen/src/Geometry/EulerAngles.h

@@ -0,0 +1,158 @@
+// 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_EULERANGLES_H
+#define EIGEN_EULERANGLES_H
+
+template<typename Other,
+         int OtherRows=Other::RowsAtCompileTime,
+         int OtherCols=Other::ColsAtCompileTime>
+struct ei_eulerangles_assign_impl;
+
+/** \class EulerAngles
+  *
+  * \brief Represents a rotation in a 3 dimensional space as three Euler angles
+  *
+  * \param _Scalar the scalar type, i.e., the type of the angles.
+  *
+  * \sa class Quaternion, class AngleAxis, class Transform
+  */
+template<typename _Scalar>
+class EulerAngles
+{
+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;
+  typedef Quaternion<Scalar> QuaternionType;
+  typedef AngleAxis<Scalar> AngleAxisType;
+
+protected:
+
+  Vector3 m_angles;
+
+public:
+
+  EulerAngles() {}
+  template<typename Derived>
+  inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
+  inline EulerAngles(const QuaternionType& q) { *this = q; }
+  inline EulerAngles(const AngleAxisType& aa) { *this = aa; }
+  template<typename Derived>
+  inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
+
+  Scalar angle(int i) const { return m_angles.coeff(i); }
+  Scalar& angle(int i) { return m_angles.coeffRef(i); }
+
+  const Vector3& coeffs() const { return m_angles; }
+  Vector3& coeffs() { return m_angles; }
+
+  EulerAngles& operator=(const QuaternionType& q);
+  EulerAngles& operator=(const AngleAxisType& ea);
+  template<typename Derived>
+  EulerAngles& operator=(const MatrixBase<Derived>& m);
+
+  template<typename Derived>
+  EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m);
+  Matrix3 toRotationMatrix(void) const;
+};
+
+/** Set \c *this from a quaternion.
+  * The axis is normalized.
+  */
+template<typename Scalar>
+EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const QuaternionType& q)
+{
+  Scalar y2 = q.y() * q.y();
+  m_angles.coeffRef(0) = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
+  m_angles.coeffRef(1) = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
+  m_angles.coeffRef(2) = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
+  return *this;
+}
+
+/** Set \c *this from Euler angles \a ea.
+  */
+template<typename Scalar>
+EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const AngleAxisType& aa)
+{
+  return *this = QuaternionType(aa);
+}
+
+/** Set \c *this from the expression \a xpr:
+  *   - if \a xpr is a 3x1 vector, then \a xpr is assumed to be a vector of angles
+  *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
+  *     and \a xpr is converted to Euler angles
+  */
+template<typename Scalar>
+template<typename Derived>
+EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const MatrixBase<Derived>& other)
+{
+  ei_eulerangles_assign_impl<Derived>::run(*this,other.derived());
+  return *this;
+}
+
+/** Constructs and \returns an equivalent 3x3 rotation matrix.
+  */
+template<typename Scalar>
+typename EulerAngles<Scalar>::Matrix3
+EulerAngles<Scalar>::toRotationMatrix(void) const
+{
+  Vector3 c = m_angles.cwiseCos();
+  Vector3 s = m_angles.cwiseSin();
+  return Matrix3() <<
+    c.y()*c.z(),                    -c.y()*s.z(),                   s.y(),
+    c.z()*s.x()*s.y()+c.x()*s.z(),  c.x()*c.z()-s.x()*s.y()*s.z(),  -c.y()*s.x(),
+    -c.x()*c.z()*s.y()+s.x()*s.z(), c.z()*s.x()+c.x()*s.y()*s.z(),  c.x()*c.y();
+}
+
+// set from a rotation matrix
+template<typename Other>
+struct ei_eulerangles_assign_impl<Other,3,3>
+{
+  typedef typename Other::Scalar Scalar;
+  inline static void run(EulerAngles<Scalar>& ea, const Other& mat)
+  {
+    // mat =  cy*cz          -cy*sz           sy
+    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+    ea.angle(1) = std::asin(mat.coeff(0,2));
+    ea.angle(0) = std::atan2(-mat.coeff(1,2),mat.coeff(2,2));
+    ea.angle(2) = std::atan2(-mat.coeff(0,1),mat.coeff(0,0));
+  }
+};
+
+// set from a vector of angles
+template<typename Other>
+struct ei_eulerangles_assign_impl<Other,3,1>
+{
+  typedef typename Other::Scalar Scalar;
+  inline static void run(EulerAngles<Scalar>& ea, const Other& vec)
+  {
+    ea.coeffs() = vec;
+  }
+};
+
+#endif // EIGEN_EULERANGLES_H

Eigen/src/Geometry/Quaternion.h

@@ -25,6 +25,11 @@
 #ifndef EIGEN_QUATERNION_H
 #define EIGEN_QUATERNION_H
 
+template<typename Other,
+         int OtherRows=Other::RowsAtCompileTime,
+         int OtherCols=Other::ColsAtCompileTime>
+struct ei_quaternion_assign_impl;
+
 /** \class Quaternion
   *
   * \brief The quaternion class used to represent 3D orientations and rotations
@@ -39,6 +44,7 @@
   *   - efficient to compose (28 flops),
   *   - stable spherical interpolation
   *
+  * \sa  class AngleAxis, class EulerAngles, class Transform
   */
 template<typename _Scalar>
 class Quaternion
@@ -53,6 +59,8 @@ public:
 
   typedef Matrix<Scalar,3,1> Vector3;
   typedef Matrix<Scalar,3,3> Matrix3;
+  typedef AngleAxis<Scalar> AngleAxisType;
+  typedef EulerAngles<Scalar> EulerAnglesType;
 
   inline Scalar x() const { return m_coeffs.coeff(0); }
   inline Scalar y() const { return m_coeffs.coeff(1); }
@@ -71,10 +79,10 @@ public:
   inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
 
   /** \returns a read-only vector expression of the coefficients */
-  inline const Coefficients& _coeffs() const { return m_coeffs; }
+  inline const Coefficients& coeffs() const { return m_coeffs; }
 
   /** \returns a vector expression of the coefficients */
-  inline Coefficients& _coeffs() { return m_coeffs; }
+  inline Coefficients& coeffs() { return m_coeffs; }
 
   // FIXME what is the prefered order: w x,y,z or x,y,z,w ?
   inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0)
@@ -88,14 +96,16 @@ public:
   /** Copy constructor */
   inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
 
-  /** This is a special case of the templated operator=. Its purpose is to
+  explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
-    * prevent a default operator= from hiding the templated operator=.
+  explicit inline Quaternion(const EulerAnglesType& ea) { *this = ea; }
-    */
+  template<typename Derived>
-  inline Quaternion& operator=(const Quaternion& other)
+  explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
-  {
+
-    m_coeffs = other.m_coeffs;
+  Quaternion& operator=(const Quaternion& other);
-    return *this;
+  Quaternion& operator=(const AngleAxisType& aa);
-  }
+  Quaternion& operator=(EulerAnglesType ea);
+  template<typename Derived>
+  Quaternion& operator=(const MatrixBase<Derived>& m);
 
   /** \returns a quaternion representing an identity rotation
     * \sa MatrixBase::identity()
@@ -116,20 +126,10 @@ public:
     */
   inline Scalar norm() const { return m_coeffs.norm(); }
 
-  template<typename Derived>
-  Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m);
   Matrix3 toRotationMatrix(void) const;
 
-  template<typename Derived>
-  Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis);
-  void toAngleAxis(Scalar& angle, Vector3& axis) const;
-
-  Quaternion& fromEulerAngles(Vector3 eulerAngles);
-
-  Vector3 toEulerAngles(void) const;
-
   template<typename Derived1, typename Derived2>
-  Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
+  Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
 
   inline Quaternion operator* (const Quaternion& q) const;
   inline Quaternion& operator*= (const Quaternion& q);
@@ -142,24 +142,6 @@ public:
   template<typename Derived>
   Vector3 operator* (const MatrixBase<Derived>& vec) const;
 
-protected:
-
-  /** Constructor copying the value of the expression \a other */
-  template<typename OtherDerived>
-  inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other)
-  {
-    m_coeffs = other;
-  }
-
-  /** Copies the value of the expression \a other into \c *this.
-    */
-  template<typename OtherDerived>
-  inline Quaternion& operator=(const MatrixBase<OtherDerived>& other)
-  {
-    m_coeffs = other.derived();
-    return *this;
-  }
-
 };
 
 /** \returns the concatenation of two rotations as a quaternion-quaternion product */
@@ -203,16 +185,71 @@ Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
     return v + this->w() * uv + this->vec().cross(uv);
 }
 
+template<typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
+{
+  m_coeffs = other.m_coeffs;
+  return *this;
+}
+
+/** Set \c *this from an angle-axis \a aa
+  * and returns a reference to \c *this
+  */
+template<typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
+{
+  Scalar ha = 0.5*aa.angle();
+  this->w() = ei_cos(ha);
+  this->vec() = ei_sin(ha) * aa.axis();
+  return *this;
+}
+
+/** Set \c *this from the rotation defined by the Euler angles \a ea,
+  * and returns a reference to \c *this
+  */
+template<typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(EulerAnglesType ea)
+{
+  ea.coeffs() *= 0.5;
+
+  Vector3 cosines = ea.coeffs().cwiseCos();
+  Vector3 sines   = ea.coeffs().cwiseSin();
+
+  Scalar cYcZ = cosines.y() * cosines.z();
+  Scalar sYsZ = sines.y() * sines.z();
+  Scalar sYcZ = sines.y() * cosines.z();
+  Scalar cYsZ = cosines.y() * sines.z();
+
+  this->w() = cosines.x() * cYcZ + sines.x()   * sYsZ;
+  this->x() = sines.x()   * cYcZ - cosines.x() * sYsZ;
+  this->y() = cosines.x() * sYcZ + sines.x()   * cYsZ;
+  this->z() = cosines.x() * cYsZ - sines.x()   * sYcZ;
+
+  return *this;
+}
+
+/** Set \c *this from the expression \a xpr:
+  *   - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
+  *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
+  *     and \a xpr is converted to a quaternion
+  */
+template<typename Scalar>
+template<typename Derived>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
+{
+  ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
+  return *this;
+}
+
 /** Convert the quaternion to a 3x3 rotation matrix */
 template<typename Scalar>
 inline typename Quaternion<Scalar>::Matrix3
 Quaternion<Scalar>::toRotationMatrix(void) const
 {
-  // FIXME another option would be to declare toRotationMatrix like that:
+  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
-  // OtherDerived& toRotationMatrix(MatrixBase<OtherDerived>& m)
+  // if not inlined then the cost of the return by value is huge ~ +35%,
-  // it would fill m and returns a ref to m.
+  // however, not inlining this function is an order of magnitude slower, so
-  // the advantages is that way we can accept 4x4 and 3x4 matrices filling the rest of the
+  // it has to be inlined, and so the return by value is not an issue
-  // matrix with I... ??
   Matrix3 res;
 
   Scalar tx  = 2*this->x();
@@ -241,132 +278,13 @@ Quaternion<Scalar>::toRotationMatrix(void) const
   return res;
 }
 
-/** updates \c *this from the rotation matrix \a m and returns a reference to \c *this
-  * \warning the size of the input matrix expression \a m must be 3x3 at compile time
-  */
-template<typename Scalar>
-template<typename Derived>
-Quaternion<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
-{
-  // 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 ?
-  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();
-  if (t > 0)
-  {
-    t = ei_sqrt(t + 1.0);
-    this->w() = 0.5*t;
-    t = 0.5/t;
-    this->x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
-    this->y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
-    this->z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
-  }
-  else
-  {
-    int i = 0;
-    if (mat.coeff(1,1) > mat.coeff(0,0))
-      i = 1;
-    if (mat.coeff(2,2) > mat.coeff(i,i))
-      i = 2;
-    int j = (i+1)%3;
-    int k = (j+1)%3;
-
-    t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0);
-    m_coeffs.coeffRef(i) = 0.5 * t;
-    t = 0.5/t;
-    this->w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
-    m_coeffs.coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
-    m_coeffs.coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
-  }
-
-  return *this;
-}
-
-/** updates \c *this from the rotation defined by axis \a axis and angle \a angle
-  * and returns a reference to \c *this
-  * \warning the size of the input vector expression \a axis must be 3 at compile time
-  */
-template<typename Scalar>
-template<typename Derived>
-inline Quaternion<Scalar>& Quaternion<Scalar>
-::fromAngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis)
-{
-  ei_assert(Derived::SizeAtCompileTime==3);
-  Scalar ha = 0.5*angle;
-  this->w() = ei_cos(ha);
-  this->vec() = ei_sin(ha) * axis;
-  return *this;
-}
-
-/** Computes and returns the angle and axis of the rotation represented by the quaternion.
-  * The values are returned in the arguments \a angle and \a axis respectively.
-  * The returned axis is normalized.
-  */
-template <typename Scalar>
-void Quaternion<Scalar>::toAngleAxis(Scalar& angle, Vector3& axis) const
-{
-  // FIXME should we split this function to an "angle" and an "axis" functions ?
-  // the drawbacks is that this approach would require to compute twice the norm of (x,y,z)...
-  // or we returns a Vector4, or a small AngleAxis object... ???
-  Scalar n2 = this->vec().norm2();
-  if (ei_isMuchSmallerThan(n2,Scalar(1)))
-  {
-    angle = 0;
-    axis << 1, 0, 0;
-  }
-  else
-  {
-    angle = 2*std::acos(this->w());
-    axis = this->vec() / ei_sqrt(n2);
-  }
-}
-
-/** updates \c *this from the rotation defined by the Euler angles \a eulerAngles,
-  * and returns a reference to \c *this
-  */
-template <typename Scalar>
-Quaternion<Scalar>& Quaternion<Scalar>::fromEulerAngles(Vector3 eulerAngles)
-{
-  // FIXME should the arguments be 3 scalars or a single Vector3 ?
-  eulerAngles *= 0.5;
-
-  Vector3 cosines = eulerAngles.cwiseCos();
-  Vector3 sines   = eulerAngles.cwiseSin();
-
-  Scalar cYcZ = cosines.y() * cosines.z();
-  Scalar sYsZ = sines.y() * sines.z();
-  Scalar sYcZ = sines.y() * cosines.z();
-  Scalar cYsZ = cosines.y() * sines.z();
-
-  this->w() = cosines.x() * cYcZ + sines.x()   * sYsZ;
-  this->x() = sines.x()   * cYcZ - cosines.x() * sYsZ;
-  this->y() = cosines.x() * sYcZ + sines.x()   * cYsZ;
-  this->z() = cosines.x() * cYsZ - sines.x()   * sYcZ;
-
-  return *this;
-}
-
-/** Computes and returns the Euler angles corresponding to the quaternion \c *this.
-  */
-template <typename Scalar>
-typename Quaternion<Scalar>::Vector3 Quaternion<Scalar>::toEulerAngles(void) const
-{
-  Scalar y2 = this->y() * this->y();
-  return Vector3(
-    std::atan2(2*(this->w()*this->x() + this->y()*this->z()), (1 - 2*(this->x()*this->x() + y2))),
-    std::asin( 2*(this->w()*this->y() - this->z()*this->x())),
-    std::atan2(2*(this->w()*this->z() + this->x()*this->y()), (1 - 2*(y2 + this->z()*this->z()))));
-}
-
 /** Makes a quaternion representing the rotation between two vectors \a a and \a b.
   * \returns a reference to the actual quaternion
   * Note that the two input vectors have \b not to be normalized.
   */
 template<typename Scalar>
 template<typename Derived1, typename Derived2>
-inline Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
+inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
 {
   Vector3 v0 = a.normalized();
   Vector3 v1 = b.normalized();
@@ -399,11 +317,11 @@ inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
   // FIXME should this funtion be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
   Scalar n2 = this->norm2();
   if (n2 > 0)
-    return conjugate()._coeffs() / n2;
+    return Quaternion(conjugate().coeffs() / n2);
   else
   {
     // return an invalid result to flag the error
-    return Coefficients::zero();
+    return Quaternion(Coefficients::zero());
   }
 }
 
@@ -448,4 +366,54 @@ Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other)
   return scale0 * (*this) + scale1 * other;
 }
 
+// set from a rotation matrix
+template<typename Other>
+struct ei_quaternion_assign_impl<Other,3,3>
+{
+  typedef typename Other::Scalar Scalar;
+  inline static void run(Quaternion<Scalar>& q, const Other& mat)
+  {
+    // This algorithm comes from  "Quaternion Calculus and Fast Animation",
+    // Ken Shoemake, 1987 SIGGRAPH course notes
+    Scalar t = mat.trace();
+    if (t > 0)
+    {
+      t = ei_sqrt(t + 1.0);
+      q.w() = 0.5*t;
+      t = 0.5/t;
+      q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
+      q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
+      q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
+    }
+    else
+    {
+      int i = 0;
+      if (mat.coeff(1,1) > mat.coeff(0,0))
+        i = 1;
+      if (mat.coeff(2,2) > mat.coeff(i,i))
+        i = 2;
+      int j = (i+1)%3;
+      int k = (j+1)%3;
+
+      t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0);
+      q.coeffs().coeffRef(i) = 0.5 * t;
+      t = 0.5/t;
+      q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
+      q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
+      q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
+    }
+  }
+};
+
+// set from a vector of coefficients assumed to be a quaternion
+template<typename Other>
+struct ei_quaternion_assign_impl<Other,4,1>
+{
+  typedef typename Other::Scalar Scalar;
+  inline static void run(Quaternion<Scalar>& q, const Other& vec)
+  {
+    q.coeffs() = vec;
+  }
+};
+
 #endif // EIGEN_QUATERNION_H

Eigen/src/Geometry/Rotation.h

@@ -65,7 +65,7 @@ struct ToRotationMatrix<Scalar, 2, OtherScalarType>
   { return Rotation2D<Scalar>(r).toRotationMatrix(); }
 };
 
-// 2D rotation to matrix
+// 2D rotation to rotation matrix
 template<typename Scalar, typename OtherScalarType>
 struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
 {
@@ -73,7 +73,7 @@ struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
   { return Rotation2D<Scalar>(r).toRotationMatrix(); }
 };
 
-// quaternion to matrix
+// quaternion to rotation matrix
 template<typename Scalar, typename OtherScalarType>
 struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
 {
@@ -81,7 +81,7 @@ struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
   { return q.toRotationMatrix(); }
 };
 
-// angle axis to matrix
+// angle axis to rotation matrix
 template<typename Scalar, typename OtherScalarType>
 struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
 {
@@ -89,6 +89,14 @@ struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
   { return aa.toRotationMatrix(); }
 };
 
+// euler angles to rotation matrix
+template<typename Scalar, typename OtherScalarType>
+struct ToRotationMatrix<Scalar, 3, EulerAngles<OtherScalarType> >
+{
+  inline static Matrix<Scalar,3,3> convert(const EulerAngles<OtherScalarType>& ea)
+  { return ea.toRotationMatrix(); }
+};
+
 // matrix xpr to matrix xpr
 template<typename Scalar, int Dim, typename OtherDerived>
 struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
@@ -169,88 +177,4 @@ Rotation2D<Scalar>::toRotationMatrix(void) const
   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

@@ -384,20 +384,20 @@ Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDer
 template<typename Other, int Dim, int HDim>
 struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim>
 {
-  typedef Transform<typename Other::Scalar,Dim> Transform;
+  typedef Transform<typename Other::Scalar,Dim> TransformType;
-  typedef typename Transform::MatrixType MatrixType;
+  typedef typename TransformType::MatrixType MatrixType;
   typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
-  static ResultType run(const Transform& tr, const Other& other)
+  static ResultType run(const TransformType& tr, const Other& other)
   { return tr.matrix() * other; }
 };
 
 template<typename Other, int Dim, int HDim>
 struct ei_transform_product_impl<Other,Dim,HDim, HDim,1>
 {
-  typedef Transform<typename Other::Scalar,Dim> Transform;
+  typedef Transform<typename Other::Scalar,Dim> TransformType;
-  typedef typename Transform::MatrixType MatrixType;
+  typedef typename TransformType::MatrixType MatrixType;
   typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
-  static ResultType run(const Transform& tr, const Other& other)
+  static ResultType run(const TransformType& tr, const Other& other)
   { return tr.matrix() * other; }
 };
 
@@ -405,17 +405,17 @@ template<typename Other, int Dim, int HDim>
 struct ei_transform_product_impl<Other,Dim,HDim, Dim,1>
 {
   typedef typename Other::Scalar Scalar;
-  typedef Transform<Scalar,Dim> Transform;
+  typedef Transform<Scalar,Dim> TransformType;
-  typedef typename Transform::AffineMatrixRef MatrixType;
+  typedef typename TransformType::AffineMatrixRef MatrixType;
   typedef const CwiseUnaryOp<
       ei_scalar_multiple_op<Scalar>,
       NestByValue<CwiseBinaryOp<
         ei_scalar_sum_op<Scalar>,
         NestByValue<typename ProductReturnType<NestByValue<MatrixType>,Other>::Type >,
-        NestByValue<typename Transform::VectorRef> > >
+        NestByValue<typename TransformType::VectorRef> > >
       > ResultType;
   // FIXME shall we offer an optimized version when the last row is known to be 0,0...,0,1 ?
-  static ResultType run(const Transform& tr, const Other& other)
+  static ResultType run(const TransformType& tr, const Other& other)
   { return ((tr.affine().nestByValue() * other).nestByValue() + tr.translation().nestByValue()).nestByValue()
           * (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); }
 };

test/geometry.cpp

@@ -39,6 +39,8 @@ template<typename Scalar> void geometry(void)
   typedef Matrix<Scalar,3,1> Vector3;
   typedef Matrix<Scalar,4,1> Vector4;
   typedef Quaternion<Scalar> Quaternion;
+  typedef AngleAxis<Scalar> AngleAxis;
+  typedef EulerAngles<Scalar> EulerAngles;
 
   Quaternion q1, q2;
   Vector3 v0 = Vector3::random(),
@@ -47,8 +49,8 @@ template<typename Scalar> void geometry(void)
 
   Scalar a = ei_random<Scalar>(-M_PI, M_PI);
 
-  q1.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized());
+  q1 = AngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized());
-  q2.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized());
+  q2 = AngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized());
 
   // rotation matrix conversion
   VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
@@ -56,23 +58,23 @@ template<typename Scalar> void geometry(void)
     q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
   VERIFY_IS_NOT_APPROX(q2 * q1 * v2,
     q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
-  q2.fromRotationMatrix(q1.toRotationMatrix());
+  q2 = q1.toRotationMatrix();
   VERIFY_IS_APPROX(q1*v1,q2*v1);
 
   // Euler angle conversion
-  VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1);
+  VERIFY_IS_APPROX(Quaternion(EulerAngles(q1)) * v1, q1 * v1);
-  v2 = q2.toEulerAngles();
+  EulerAngles ea = q2;
-  VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2);
+  VERIFY_IS_APPROX(EulerAngles(Quaternion(ea)).coeffs(), ea.coeffs());
-  VERIFY_IS_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2);
+  VERIFY_IS_NOT_APPROX(EulerAngles(Quaternion(EulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))))).coeffs(), v2);
 
   // angle-axis conversion
-  q1.toAngleAxis(a, v2);
+  AngleAxis aa = q1;
-  VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1);
+  VERIFY_IS_APPROX(q1 * v1, Quaternion(aa) * v1);
-  VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1);
+  VERIFY_IS_NOT_APPROX(q1 * v1, Quaternion(AngleAxis(aa.angle()*2,aa.axis())) * v1);
 
   // from two vector creation
-  VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
+  VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
-  VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
+  VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
 
   // inverse and conjugate
   VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
@@ -87,15 +89,14 @@ template<typename Scalar> void geometry(void)
   VERIFY(m.isOrtho());
 
   // AngleAxis
-  VERIFY_IS_APPROX(AngleAxis<Scalar>(a,v1.normalized()).toRotationMatrix(),
+  VERIFY_IS_APPROX(AngleAxis(a,v1.normalized()).toRotationMatrix(),
-    q2.fromAngleAxis(a,v1.normalized()).toRotationMatrix());
+    Quaternion(AngleAxis(a,v1.normalized())).toRotationMatrix());
 
-  AngleAxis<Scalar> aa1;
+  AngleAxis aa1;
   m = q1.toRotationMatrix();
-  Vector3 tax; Scalar tan;
+  aa1 = m;
-  q2.fromRotationMatrix(m).toAngleAxis(tan,tax);
+  VERIFY_IS_APPROX(AngleAxis(m).toRotationMatrix(),
-  VERIFY_IS_APPROX(aa1.fromRotationMatrix(m).toRotationMatrix(),
+    Quaternion(m).toRotationMatrix());
-    q2.fromRotationMatrix(m).toRotationMatrix());
 
 
   // Transform
@@ -106,7 +107,7 @@ template<typename Scalar> void geometry(void)
   a = 0;
   while (ei_abs(a)<0.1)
     a = ei_random<Scalar>(-0.4*M_PI, 0.4*M_PI);
-  q1.fromAngleAxis(a, v0.normalized());
+  q1 = AngleAxis(a, v0.normalized());
   Transform3 t0, t1, t2;
   t0.setIdentity();
   t0.affine() = q1.toRotationMatrix();