Re: [eigen] Mapping array of scalars into quaternions

[Benoit Jacob <jacob.benoit.1@xxxxxxxxx>]

ok, sorry it got me so long. i've been on a honeymoon with my new
computer, getting the new hardware working with linux...

First of all I want to say that your patch is good work already and
your questions are good too. It just needs a bit more work before it
can go in (most importantly, it currently doesnt compile). comments
below.

2009/10/23 Mathieu Gautier <mathieu.gautier@xxxxxx>:
>> That would be great -- i could take care of doing Quaternion then,
>> that would be a piece of cake once you've done the rest.
>
> It's done. I put the classes at the end of the Quaternion.h file.
>
> I have implemented :
>
> * QuaternionBase class which hold much of the former Quaternion class method
> * QuaternionWrapper which map a array of scalars
> * Quat to test the operation between a QuaternionWrapper and a Quaternion
> and some operation (such as Identity) return a Quaternion.

ok so here are my comments.

The first thing that i did after applying your patch was:

    make test_geo_quaternion

which is just a basic syntax check as your new classes aren't touched
by the existing test. I got many errors, starting with:

eigen2/Eigen/src/Geometry/Quaternion.h:529: error: ‘Scalar’ was not
declared in this scope

Indeed, 'Scalar' isn't known by the QuaternionBase class. Yet this
class uses 'Scalar', for example in the prototype of the x() method.
So i'm puzzled, how could the code work for you?

The solution to this problem might interest you, since below you write
that you aren't sure what ei_traits is for.

The naive solution, to define Scalar in QuaternionBase, would be to
add this typedef in QuaternionBase:

typedef typename Derived::Scalar Scalar;

however, this can't work, because Derived can't be instantiated before
QuaternionBase<Derived> is. The problem is that these 2 classes want
to use each other! The solution that we found to this problem was to
add an external struct ei_traits<Derived> that's used by both Derived
and Quaternion<Derived>, but doesn't use them. This struct would
provide the Scalar typedef. Chech for example what we do in
Eigen/src/Core/Matrix.h: we define ei_traits<Matrix> containing the
Scalar typedef. Then check in MatrixBase.h, we use ei_traits<Derived>
to get the Scalar typedef.

Since you created a ei_traits<Derived> struct already, you just have
to add the Scalar typedef there:

  typedef _Scalar Scalar;   // at line 635

then in QuaternionBase you do:

  typedef typename ei_traits<Derived>::Scalar Scalar;

Notice that in ei_traits<QuaternionWrapper>, line 714 you do:

  typedef typename Coefficients::Scalar Scalar;

that can be simplified as

  typedef _Scalar Scalar;


The second error that I got was that the derived() methods that you
call in QuaternionBase, aren't defined. It should be just a matter of
static_cast'ing the 'this' pointer, like in MatrixBase.h.

Then i decided to let you fix your patch before continuing ;)

Here is another nitpick. In QuaternionWrapper you define Coefficients
and CoeffType. CoeffType:
 * is unused
 * has a name too similar to Coefficients
 * is a bit dangerous because it has an alignment requirement that
isn't guaranteed at all.
So please remove it :)

Finally let me mention what the supreme refinement would be. As you
can see from the other thread on this list, Gael just improved Map so
it is now possible to specify that the pointer is 16-byte aligned.
This allows vectorization. You could add a templated parameter to
QuaternionWrapper to allow specifying that, and define typedefs
QuaternionMapAlignedf, etc.

Now if you do this, one should let QuaternionBase know that the
pointer is aligned. in the ei_traits, you'd add an enum that says
whether the pointer is aligned. When it's aligned,
QuaternionBase::operator* could use the vectorized path!

> I used asserts to check the scalars type on some operation (assignation and
> *), but I don't know if it's the best choice or if I can achieve with a
> smarter use of templates.
>
> I am not sure about my uses of traits and typedef, I try to stay close to
> DiagonalMatrix, but I'm not comfortable with that.
>
> If the modification are included, I can extend the units tests to fit these
> changes and try to complete the class documentation.

That would be awesome. Start from test/geo_quaternion.cpp, to cover
QuaternionWrapper, or perhaps copy it (hg cp ...) into a new test
geo_quaternionwrapper.cpp. Then you add it in test/CMakeLists.txt.

Thanks for your effort.
Benoit

>
> --
> Mathieu
>
>
> # HG changeset patch
> # User Mathieu Gautier <mathieu.gautier@xxxxxx>
> # Date 1256310829 -7200
> # Node ID a2c5adefbf0204bddd1ea6922dca2b1c6d0782ed
> # Parent  754dfef12c186fb259e088dc5a6a58e06445d3bd
> * QuaternionBase and QuaternionWrapper (Quat is needed in order to build)
>
> diff -r 754dfef12c18 -r a2c5adefbf02
> Eigen/src/Core/util/ForwardDeclarations.h
> --- a/Eigen/src/Core/util/ForwardDeclarations.h Tue Oct 20 23:25:49 2009
> -0400
> +++ b/Eigen/src/Core/util/ForwardDeclarations.h Fri Oct 23 17:13:49 2009
> +0200
> @@ -130,6 +130,7 @@
>  template<typename Derived, int _Dim> class RotationBase;
>  template<typename Lhs, typename Rhs> class Cross;
>  template<typename Scalar> class Quaternion;
> +template<typename Scalar> class QuaternionWrapper;
>  template<typename Scalar> class Rotation2D;
>  template<typename Scalar> class AngleAxis;
>  template<typename Scalar,int Dim,int Mode=Affine> class Transform;
> diff -r 754dfef12c18 -r a2c5adefbf02 Eigen/src/Geometry/Quaternion.h
> --- a/Eigen/src/Geometry/Quaternion.h   Tue Oct 20 23:25:49 2009 -0400
> +++ b/Eigen/src/Geometry/Quaternion.h   Fri Oct 23 17:13:49 2009 +0200
> @@ -507,4 +507,529 @@
>   }
>  };
>
> +/*###################################################################
> +      QuaternionBase and QuaternionWrapper and Quat
> +  ###################################################################*/
> +
> +template<typename Other,
> +         int OtherRows=Other::RowsAtCompileTime,
> +         int OtherCols=Other::ColsAtCompileTime>
> +struct ei_quaternionbase_assign_impl;
> +
> +template<typename Scalar> class Quat; // [XXX] remove
> +
> +template<class Derived>
> +class QuaternionBase : public RotationBase<Derived, 3> {
> +  typedef RotationBase<Derived, 3> Base;
> +public:
> +  using Base::operator*;
> +
> + // typedef typename Matrix<Scalar,4,1> Coefficients;
> +  /** the type of a 3D vector */
> +  typedef Matrix<Scalar,3,1> Vector3;
> +  /** the equivalent rotation matrix type */
> +  typedef Matrix<Scalar,3,3> Matrix3;
> +  /** the equivalent angle-axis type */
> +  typedef AngleAxis<Scalar> AngleAxisType;
> +
> +  /** \returns the \c x coefficient */
> +  inline Scalar x() const { return derived().coeffs().coeff(0); }
> +  /** \returns the \c y coefficient */
> +  inline Scalar y() const { return derived().coeffs().coeff(1); }
> +  /** \returns the \c z coefficient */
> +  inline Scalar z() const { return derived().coeffs().coeff(2); }
> +  /** \returns the \c w coefficient */
> +  inline Scalar w() const { return derived().coeffs().coeff(3); }
> +
> +  /** \returns a reference to the \c x coefficient */
> +  inline Scalar& x() { return derived().coeffs().coeffRef(0); }
> +  /** \returns a reference to the \c y coefficient */
> +  inline Scalar& y() { return derived().coeffs().coeffRef(1); }
> +  /** \returns a reference to the \c z coefficient */
> +  inline Scalar& z() { return derived().coeffs().coeffRef(2); }
> +  /** \returns a reference to the \c w coefficient */
> +  inline Scalar& w() { return derived().coeffs().coeffRef(3); }
> +
> +  /** \returns a read-only vector expression of the imaginary part (x,y,z)
> */
> +  inline const Block<typename ei_traits<Derived>::Coefficients,3,1> vec()
> const { return derived().coeffs().template start<3>(); }
> +
> +  /** \returns a vector expression of the imaginary part (x,y,z) */
> +  inline Block<typename ei_traits<Derived>::Coefficients,3,1> vec() {
> return derived().coeffs().template start<3>(); }
> +
> +  /** \returns a read-only vector expression of the coefficients (x,y,z,w)
> */
> +  inline const typename ei_traits<Derived>::Coefficients& coeffs() const {
> return derived().coeffs(); }
> +
> +  /** \returns a vector expression of the coefficients (x,y,z,w) */
> +  inline typename ei_traits<Derived>::Coefficients& coeffs() { return
> derived().coeffs(); }
> +
> +  template<class OtherDerived> QuaternionBase& operator=(const
> QuaternionBase<OtherDerived>& other);
> +  QuaternionBase& operator=(const AngleAxisType& aa);
> +  template<class OtherDerived>
> +  QuaternionBase& operator=(const MatrixBase<OtherDerived>& m);
> +
> +  /** \returns a quaternion representing an identity rotation
> +    * \sa MatrixBase::Identity()
> +    */
> +  inline static Quat<Scalar> Identity() { return Quat<Scalar>(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 Quat<Scalar> normalized() const { return
> Quat<Scalar>(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<class OtherDerived> inline Scalar dot(const
> QuaternionBase<OtherDerived>& other) const { return
> coeffs().dot(other.coeffs()); }
> +
> +  template<class OtherDerived> inline Scalar angularDistance(const
> QuaternionBase<OtherDerived>& other) const;
> +
> +  Matrix3 toRotationMatrix(void) const;
> +
> +  template<typename Derived1, typename Derived2>
> +  QuaternionBase& setFromTwoVectors(const MatrixBase<Derived1>& a, const
> MatrixBase<Derived2>& b);
> +
> +  template<class OtherDerived> inline Quat<Scalar> operator* (const
> QuaternionBase<OtherDerived>& q) const;
> +  template<class OtherDerived> inline QuaternionBase& operator*= (const
> QuaternionBase<OtherDerived>& q);
> +
> +  Quat<Scalar> inverse(void) const;
> +  Quat<Scalar> conjugate(void) const;
> +
> +  template<class OtherDerived> Quat<Scalar> slerp(Scalar t, const
> QuaternionBase<OtherDerived>& 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, typename
> NumTraits<Scalar>::Real prec = precision<Scalar>()) const
> +  { return coeffs().isApprox(other.coeffs(), prec); }
> +
> +  Vector3 _transformVector(Vector3 v) const;
> +
> +};
> +
> +/* ########### Quat -> Quaternion */
> +
> +template<typename _Scalar>
> +struct ei_traits<Quat<_Scalar> >
> + : ei_traits<Matrix<_Scalar,4,1> >
> +{
> +  typedef typename Matrix<_Scalar,4,1> Coefficients;
> +};
> +
> +template<typename _Scalar>
> +class Quat : public QuaternionBase<Quat<_Scalar> >{
> +  typedef QuaternionBase<Quat<Scalar> > Base;
> +public:
> +
> +  typename typedef ei_traits<Quat<_Scalar> >::Coefficients Coefficients;
> +
> +  using Base::operator=;
> +
> +  /** Default constructor leaving the quaternion uninitialized. */
> +  inline Quat() {}
> +
> +  /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
> +    * its four coefficients \a w, \a x, \a y and \a z.
> +    *
> +    * \warning Note the order of the arguments: the real \a w coefficient
> first,
> +    * while internally the coefficients are stored in the following order:
> +    * [\c x, \c y, \c z, \c w]
> +    */
> +  inline Quat(Scalar w, Scalar x, Scalar y, Scalar z)
> +  { coeffs() << x, y, z, w; }
> +
> +  /** Constructs and initialize a quaternion from the array data
> +    * This constructor is also used to map an array */
> +  inline Quat(const Scalar* data) : m_coeffs(data) {}
> +
> +  /** Copy constructor */
> +  inline Quat(const QuaternionBase& other) { m_coeffs = other.coeffs(); }
> +
> +  /** Constructs and initializes a quaternion from the angle-axis \a aa */
> +  explicit inline Quat(const AngleAxisType& aa) { *this = aa; }
> +
> +  /** Constructs and initializes a quaternion from either:
> +    *  - a rotation matrix expression,
> +    *  - a 4D vector expression representing quaternion coefficients.
> +    */
> +  template<typename Derived>
> +  explicit inline Quat(const MatrixBase<Derived>& other) { *this = other; }
> +
> +  /** \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<typename NewScalarType>
> +  inline typename ei_cast_return_type<QuaternionBase,
> QuaternionBase<NewScalarType> >::type cast() const
> +  { return typename ei_cast_return_type<QuaternionBase,
> QuaternionBase<NewScalarType> >::type(*this); }
> +
> +  /** Copy constructor with scalar type conversion */
> +  template<typename OtherScalarType>
> +  inline explicit Quat(const QuaternionBase<OtherScalarType>& other)
> +  { m_coeffs = other.coeffs().template cast<Scalar>(); }
> +
> +  inline Coefficients& coeffs() { return m_coeffs;}
> +  inline const Coefficients& coeffs() const { return m_coeffs;}
> +
> +protected:
> +  Coefficients m_coeffs;
> +};
> +
> +/* ########### QuaternionWrapper */
> +
> +/** \class QuaternionWrapper
> +  * \nonstableyet
> +  *
> +  * \brief Expression of a quaternion
> +  *
> +  * \param Scalar the type of the vector of diagonal coefficients
> +  *
> +  * \sa class Quaternion, class QuaternionBase
> +  */
> +template<typename _Scalar>
> +struct ei_traits<QuaternionWrapper<_Scalar> >
> +{
> +  typedef Map<Matrix<_Scalar, 4, 1> > Coefficients;
> +  typedef Matrix<_Scalar, 4, 1> CoeffType;
> +  typedef typename Coefficients::Scalar Scalar;
> + /* enum {
> +    RowsAtCompileTime = Coefficients::SizeAtCompileTime,
> +    ColsAtCompileTime = Coefficients::SizeAtCompileTime,
> +    MaxRowsAtCompileTime = Coefficients::SizeAtCompileTime,
> +    MaxColsAtCompileTime = Coefficients::SizeAtCompileTime,
> +    Flags = 0
> +  }; */ //[XXX] ??
> +};
> +
> +template<typename _Scalar>
> +class QuaternionWrapper : public QuaternionBase<QuaternionWrapper<_Scalar>
>>, ei_no_assignment_operator {
> +  public:
> +    typename typedef ei_traits<QuaternionWrapper<_Scalar> >::Coefficients
> Coefficients;
> +    typename typedef ei_traits<QuaternionWrapper<_Scalar> >::CoeffType
> CoeffType;
> +
> +    inline QuaternionWrapper(const Scalar* coeffs) : m_coeffs(coeffs) {}
> +
> +    inline Coefficients& coeffs() { return m_coeffs;}
> +    inline const Coefficients& coeffs() const { return m_coeffs;}
> +
> +  protected:
> +    typename Coefficients m_coeffs;
> +};
> +
> +typedef QuaternionWrapper<double> QuaternionMapd;
> +typedef QuaternionWrapper<float> QuaternionMapf;
> +
> +// Generic Quaternion * Quaternion product
> +template<class Derived, class OtherDerived> inline Quat<typename
> ei_traits<Derived>::Scalar>
> +ei_quat_product(const QuaternionBase<Derived>& a, const
> QuaternionBase<OtherDerived>& b)
> +{
> +  return Quat<ei_traits<Derived>::Scalar>
> +  (
> +    a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
> +    a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
> +    a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
> +    a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
> +  );
> +}
> +
> +/** \returns the concatenation of two rotations as a quaternion-quaternion
> product */
> +template <class Derived>
> +template <class OtherDerived>
> +inline Quat<typename ei_traits<Derived>::Scalar>
> QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>&
> other) const
> +{
> +  EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename
> OtherDerived::Scalar>::ret),
> +
> YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
> +  return ei_quat_product(*this, other);
> +}
> +
> +/** \sa operator*(Quaternion) */
> +template <class Derived>
> +template <class OtherDerived>
> +inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator*= (const
> QuaternionBase<OtherDerived>& other)
> +{
> +  return (*this = *this * other);
> +}
> +
> +/** Rotation of a vector by a quaternion.
> +  * \remarks If the quaternion is used to rotate several points (>1)
> +  * then it is much more efficient to first convert it to a 3x3 Matrix..
> +  * Comparison of the operation cost for n transformations:
> +  *   - Quaternion2:    30n
> +  *   - Via a Matrix3: 24 + 15n
> +  */
> +template <class Derived>
> +inline typename QuaternionBase<Derived>::Vector3
> +QuaternionBase<Derived>::_transformVector(Vector3 v) const
> +{
> +    // Note that this algorithm comes from the optimization by hand
> +    // of the conversion to a Matrix followed by a Matrix/Vector product.
> +    // It appears to be much faster than the common algorithm found
> +    // in the litterature (30 versus 39 flops). It also requires two
> +    // Vector3 as temporaries.
> +    Vector3 uv = Scalar(2) * this->vec().cross(v);
> +    return v + this->w() * uv + this->vec().cross(uv);
> +}
> +
> +template<class Derived>
> +template<class OtherDerived>
> +inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const
> QuaternionBase<OtherDerived>& other)
> +{
> +  m_coeffs = other.coeffs();
> +  return *this;
> +}
> +
> +/** Set \c *this from an angle-axis \a aa and returns a reference to \c
> *this
> +  */
> +template<class Derived>
> +inline QuaternionBase<Derived>& QuaternionBase<Derived>::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;
> +}
> +
> +/** 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<class Derived>
> +template<class MatrixDerived>
> +inline QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const
> MatrixBase<MatrixDerived>& xpr)
> +{
> +  EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename
> MatrixDerived::Scalar>::ret),
> +
> YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
> +  ei_quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
> +  return *this;
> +}
> +
> +/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is
> required to
> +  * be normalized, otherwise the result is undefined.
> +  */
> +template<class Derived>
> +inline typename QuaternionBase<Derived>::Matrix3
> +QuaternionBase<Derived>::toRotationMatrix(void) const
> +{
> +  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not
> gcc 4.3 !!)
> +  // if not inlined then the cost of the return by value is huge ~ +35%,
> +  // however, not inlining this function is an order of magnitude slower,
> so
> +  // it has to be inlined, and so the return by value is not an issue
> +  Matrix3 res;
> +
> +  const Scalar tx  = 2*this->x();
> +  const Scalar ty  = 2*this->y();
> +  const Scalar tz  = 2*this->z();
> +  const Scalar twx = tx*this->w();
> +  const Scalar twy = ty*this->w();
> +  const Scalar twz = tz*this->w();
> +  const Scalar txx = tx*this->x();
> +  const Scalar txy = ty*this->x();
> +  const Scalar txz = tz*this->x();
> +  const Scalar tyy = ty*this->y();
> +  const Scalar tyz = tz*this->y();
> +  const Scalar tzz = tz*this->z();
> +
> +  res.coeffRef(0,0) = 1-(tyy+tzz);
> +  res.coeffRef(0,1) = txy-twz;
> +  res.coeffRef(0,2) = txz+twy;
> +  res.coeffRef(1,0) = txy+twz;
> +  res.coeffRef(1,1) = 1-(txx+tzz);
> +  res.coeffRef(1,2) = tyz-twx;
> +  res.coeffRef(2,0) = txz-twy;
> +  res.coeffRef(2,1) = tyz+twx;
> +  res.coeffRef(2,2) = 1-(txx+tyy);
> +
> +  return res;
> +}
> +
> +/** Sets \c *this to be a quaternion representing a rotation between
> +  * the two arbitrary vectors \a a and \a b. In other words, the built
> +  * rotation represent a rotation sending the line of direction \a a
> +  * to the line of direction \a b, both lines passing through the origin.
> +  *
> +  * \returns a reference to \c *this.
> +  *
> +  * Note that the two input vectors do \b not have to be normalized, and
> +  * do not need to have the same norm.
> +  */
> +template<class Derived>
> +template<typename Derived1, typename Derived2>
> +inline QuaternionBase<Derived>&
> QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a,
> const MatrixBase<Derived2>& b)
> +{
> +  Vector3 v0 = a.normalized();
> +  Vector3 v1 = b.normalized();
> +  Scalar c = v1.dot(v0);
> +
> +  // if dot == -1, vectors are nearly opposites
> +  // => accuraletly compute the rotation axis by computing the
> +  //    intersection of the two planes. This is done by solving:
> +  //       x^T v0 = 0
> +  //       x^T v1 = 0
> +  //    under the constraint:
> +  //       ||x|| = 1
> +  //    which yields a singular value problem
> +  if (c < Scalar(-1)+precision<Scalar>())
> +  {
> +    c = std::max<Scalar>(c,-1);
> +    Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
> +    SVD<Matrix<Scalar,2,3> > svd(m);
> +    Vector3 axis = svd.matrixV().col(2);
> +
> +    Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
> +    this->w() = ei_sqrt(w2);
> +    this->vec() = axis * ei_sqrt(Scalar(1) - w2);
> +    return *this;
> +  }
> +  Vector3 axis = v0.cross(v1);
> +  Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
> +  Scalar invs = Scalar(1)/s;
> +  this->vec() = axis * invs;
> +  this->w() = s * Scalar(0.5);
> +
> +  return *this;
> +}
> +
> +/** \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()
> +  */
> +template <class Derived>
> +inline Quat<typename ei_traits<Derived>::Scalar>
> QuaternionBase<Derived>::inverse() const
> +{
> +  // FIXME should this function be called multiplicativeInverse and
> conjugate() be called inverse() or opposite()  ??
> +  Scalar n2 = this->squaredNorm();
> +  if (n2 > 0)
> +    return Quat<Scalar>(conjugate().coeffs() / n2);
> +  else
> +  {
> +    // return an invalid result to flag the error
> +    return Quat<Scalar>(Coefficients::Zero());
> +  }
> +}
> +
> +/** \returns the conjugate of the \c *this which is equal to the
> multiplicative inverse
> +  * if the quaternion is normalized.
> +  * The conjugate of a quaternion represents the opposite rotation.
> +  *
> +  * \sa Quaternion2::inverse()
> +  */
> +template <class Derived>
> +inline Quat<typename ei_traits<Derived>::Scalar>
> QuaternionBase<Derived>::conjugate() const
> +{
> +  return Quat<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
> +}
> +
> +/** \returns the angle (in radian) between two rotations
> +  * \sa dot()
> +  */
> +template <class Derived>
> +template <class OtherDerived>
> +inline typename ei_traits<Derived>::Scalar
> QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>&
> other) const
> +{
> +  double d = ei_abs(this->dot(other));
> +  if (d>=1.0)
> +    return 0;
> +  return Scalar(2) * std::acos(d);
> +}
> +
> +/** \returns the spherical linear interpolation between the two quaternions
> +  * \c *this and \a other at the parameter \a t
> +  */
> +template <class Derived>
> +template <class OtherDerived>
> +Quat<typename ei_traits<Derived>::Scalar>
> QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>&
> other) const
> +{
> +  static const Scalar one = Scalar(1) - precision<Scalar>();
> +  Scalar d = this->dot(other);
> +  Scalar absD = ei_abs(d);
> +  if (absD>=one)
> +    return Quat<Scalar>(*this);
> +
> +  // theta is the angle between the 2 quaternions
> +  Scalar theta = std::acos(absD);
> +  Scalar sinTheta = ei_sin(theta);
> +
> +  Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
> +  Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta;
> +  if (d<0)
> +    scale1 = -scale1;
> +
> +  return Quat<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
> +}
> +
> +// set from a rotation matrix
> +template<typename Other>
> +struct ei_quaternionbase_assign_impl<Other,3,3>
> +{
> +  typedef typename Other::Scalar Scalar;
> +  template<class Derived> inline static void run(QuaternionBase<Derived>&
> 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 + Scalar(1.0));
> +      q.w() = Scalar(0.5)*t;
> +      t = Scalar(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) +
> Scalar(1.0));
> +      q.coeffs().coeffRef(i) = Scalar(0.5) * t;
> +      t = Scalar(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_quaternionbase_assign_impl<Other,4,1>
> +{
> +  typedef typename Other::Scalar Scalar;
> +  template<class Derived> inline static void run(QuaternionBase<Derived>&
> q, const Other& vec)
> +  {
> +    q.coeffs() = vec;
> +  }
> +};
> +
> +
>  #endif // EIGEN_QUATERNION_H
>
>