Re: [eigen] Mapping array of scalars into quaternions

[ Thread Index | Date Index | More lists.tuxfamily.org/eigen Archives ]


The new version of the patch.

I have a question, why the quaternion is stored in the following order x,y,z,w. It seems more natural to have w,x,y,z ? Other library like boost::quaternion or ogre3D used the w,x,y,z order. It will be easier to map quaternion from these libraries if we use the same order and more coherent to the natural writing of quaternion.

--
Mathieu
# HG changeset patch
# User Mathieu Gautier <mathieu.gautier@xxxxxx>
# Date 1258126911 -3600
# Node ID 7456b5d088e85417c68eefc320d208c491b0cbca
# Parent  5536e0c23ba27b64fc4215fe78b3fb7a21169a4a
* add Map<Quaternion> test based on Map from test/map.cpp
* replace implicit constructor AngleAxis(QuaternionBase&) by an explicit one, it seems ambiguous for the compiler
* remove explicit constructor with conversion type quaternion(Quaternion&): conflict between constructor.
* modify EIGEN_INHERIT_ASSIGNEMENT_OPERATORS to suit Quaternion class

diff -r 5536e0c23ba2 -r 7456b5d088e8 Eigen/src/Core/util/Macros.h
--- a/Eigen/src/Core/util/Macros.h	Fri Nov 13 13:05:57 2009 +0000
+++ b/Eigen/src/Core/util/Macros.h	Fri Nov 13 16:41:51 2009 +0100
@@ -168,7 +168,7 @@
 #endif
 
 // EIGEN_FORCE_INLINE means "inline as much as possible"
-#if (defined _MSC_VER)
+#if (defined _MSC_VER) || (defined __intel_compiler)
 #define EIGEN_STRONG_INLINE __forceinline
 #else
 #define EIGEN_STRONG_INLINE inline
@@ -261,25 +261,25 @@
   #define EIGEN_REF_TO_TEMPORARY const &
 #endif
 
-#ifdef _MSC_VER
+#if defined(_MSC_VER) && (!defined(__INTEL_COMPILER))
+#define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
+using Base::operator =;
+#else
+#define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
+using Base::operator =; \
+EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) \
+{ \
+  Base::operator=(other); \
+  return *this; \
+}
+#endif
+
 #define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
-using Base::operator =; \
-using Base::operator +=; \
-using Base::operator -=; \
-using Base::operator *=; \
-using Base::operator /=;
-#else
-#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
-using Base::operator =; \
 using Base::operator +=; \
 using Base::operator -=; \
 using Base::operator *=; \
 using Base::operator /=; \
-EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) \
-{ \
-  return Base::operator=(other); \
-}
-#endif
+EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
 
 #define _EIGEN_GENERIC_PUBLIC_INTERFACE(Derived, BaseClass) \
 typedef BaseClass Base; \
diff -r 5536e0c23ba2 -r 7456b5d088e8 Eigen/src/Geometry/AngleAxis.h
--- a/Eigen/src/Geometry/AngleAxis.h	Fri Nov 13 13:05:57 2009 +0000
+++ b/Eigen/src/Geometry/AngleAxis.h	Fri Nov 13 16:41:51 2009 +0100
@@ -89,7 +89,7 @@
   template<typename Derived>
   inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
   /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
-  inline AngleAxis(const QuaternionType& q) { *this = q; }
+  template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
   /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
   template<typename Derived>
   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
@@ -116,7 +116,8 @@
   AngleAxis inverse() const
   { return AngleAxis(-m_angle, m_axis); }
 
-  AngleAxis& operator=(const QuaternionType& q);
+  template<class QuatDerived>
+  AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
   template<typename Derived>
   AngleAxis& operator=(const MatrixBase<Derived>& m);
 
@@ -160,7 +161,8 @@
   * The axis is normalized.
   */
 template<typename Scalar>
-AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
+template<typename QuatDerived>
+AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
 {
   Scalar n2 = q.vec().squaredNorm();
   if (n2 < precision<Scalar>()*precision<Scalar>())
diff -r 5536e0c23ba2 -r 7456b5d088e8 Eigen/src/Geometry/Quaternion.h
--- a/Eigen/src/Geometry/Quaternion.h	Fri Nov 13 13:05:57 2009 +0000
+++ b/Eigen/src/Geometry/Quaternion.h	Fri Nov 13 16:41:51 2009 +0100
@@ -88,7 +88,8 @@
   /** \returns a vector expression of the coefficients (x,y,z,w) */
   inline typename ei_traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
 
-  template<class OtherDerived> Derived& operator=(const QuaternionBase<OtherDerived>& other);
+  EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
+  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
 
 // disabled this copy operator as it is giving very strange compilation errors when compiling
 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
@@ -133,19 +134,28 @@
     */
   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;
+  template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
 
+	/** \returns an equivalent 3x3 rotation matrix */
   Matrix3 toRotationMatrix() const;
 
+	/** \returns the quaternion which transform \a a into \a b through a rotation */
   template<typename Derived1, typename Derived2>
   Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
 
-  template<class OtherDerived> inline Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
-  template<class OtherDerived> inline Derived& operator*= (const QuaternionBase<OtherDerived>& q);
+  template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
+  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
 
+	/** \returns the quaternion describing the inverse rotation */
   Quaternion<Scalar> inverse() const;
+
+	/** \returns the conjugated quaternion */
   Quaternion<Scalar> conjugate() const;
 
+	/** \returns an interpolation for a constant motion between \a other and \c *this 
+		* \a t in [0;1]
+		* see http://en.wikipedia.org/wiki/Slerp		
+	*/
   template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
 
   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
@@ -156,7 +166,8 @@
   bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = precision<Scalar>()) const
   { return coeffs().isApprox(other.coeffs(), prec); }
 
-  Vector3 _transformVector(Vector3 v) const;
+	/** return the result vector of \a v through the rotation*/
+  EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
 
   /** \returns \c *this with scalar type casted to \a NewScalarType
     *
@@ -210,10 +221,11 @@
 template<typename _Scalar>
 class Quaternion : public QuaternionBase<Quaternion<_Scalar> >{
   typedef QuaternionBase<Quaternion<_Scalar> > Base;
-public:
-  using Base::operator=;
+public: 
+  typedef _Scalar Scalar;
 
-  typedef _Scalar Scalar;
+  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion<Scalar>)
+  using Base::operator*=;
 
   typedef typename ei_traits<Quaternion<Scalar> >::Coefficients Coefficients;
   typedef typename Base::AngleAxisType AngleAxisType;
@@ -228,15 +240,13 @@
     * while internally the coefficients are stored in the following order:
     * [\c x, \c y, \c z, \c w]
     */
-  inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
-  { coeffs() << x, y, z, w; }
+  inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
 
-  /** Constructs and initialize a quaternion from the array data
-    * This constructor is also used to map an array */
+  /** Constructs and initialize a quaternion from the array data */
   inline Quaternion(const Scalar* data) : m_coeffs(data) {}
 
   /** Copy constructor */
-//  template<class Derived> inline Quaternion(const QuaternionBase<Derived>& other) { m_coeffs = other.coeffs(); } [XXX] redundant with 703
+  template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
 
   /** Constructs and initializes a quaternion from the angle-axis \a aa */
   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
@@ -248,11 +258,6 @@
   template<typename Derived>
   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
 
-  /** Copy constructor with scalar type conversion */
-  template<typename Derived>
-  inline explicit Quaternion(const QuaternionBase<Derived>& other)
-  { m_coeffs = other.coeffs().template cast<Scalar>(); }
-
   inline Coefficients& coeffs() { return m_coeffs;}
   inline const Coefficients& coeffs() const { return m_coeffs;}
 
@@ -289,7 +294,7 @@
 ei_traits<Quaternion<_Scalar> >
 {
   typedef _Scalar Scalar;
-  typedef Map<Matrix<_Scalar,4,1> > Coefficients;
+  typedef Map<Matrix<_Scalar,4,1>, _PacketAccess> Coefficients;
   enum {
     PacketAccess = _PacketAccess
   };
@@ -297,13 +302,22 @@
 
 template<typename _Scalar, int PacketAccess>
 class Map<Quaternion<_Scalar>, PacketAccess >
-  : public QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> >,
-    ei_no_assignment_operator
+  : public QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> >
 {
+	public:
+    typedef _Scalar Scalar;
+		typedef Map<Quaternion<Scalar>, PacketAccess > MapQuat;
+		
+	private:
+ 		Map<Quaternion<Scalar>, PacketAccess >();
+ 		Map<Quaternion<Scalar>, PacketAccess >(const Map<Quaternion<Scalar>, PacketAccess>&);
+
+    typedef QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> > Base;
   public:
+  	EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(MapQuat)
+    using Base::operator*=;
 
-    typedef _Scalar Scalar;
-    typedef typename ei_traits<Map>::Coefficients Coefficients;
+    typedef typename ei_traits<Map<Quaternion<Scalar>, PacketAccess> >::Coefficients Coefficients;
 
     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
       *
@@ -311,7 +325,7 @@
       * \code *coeffs == {x, y, z, w} \endcode
       *
       * If the template paramter PacketAccess is set to Aligned, then the pointer coeffs must be aligned. */
-    inline Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
+    EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
 
     inline Coefficients& coeffs() { return m_coeffs;}
     inline const Coefficients& coeffs() const { return m_coeffs;}
@@ -320,10 +334,18 @@
     Coefficients m_coeffs;
 };
 
-typedef Map<Quaternion<double> >          QuaternionMapd;
-typedef Map<Quaternion<float> >           QuaternionMapf;
+/** \ingroup Geometry_Module
+  * Map an unaligned array of single precision scalar as a quaternion */
+typedef Map<Quaternion<float>, 0>         QuaternionMapf;
+/** \ingroup Geometry_Module
+  * Map an unaligned array of double precision scalar as a quaternion */
+typedef Map<Quaternion<double>, 0>        QuaternionMapd;
+/** \ingroup Geometry_Module
+  * Map a 16-bits aligned array of double precision scalars as a quaternion */
+typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
+/** \ingroup Geometry_Module
+  * Map a 16-bits aligned array of double precision scalars as a quaternion */
 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
-typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
 
 /***************************************************************************
 * Implementation of QuaternionBase methods
@@ -333,7 +355,7 @@
 // This product can be specialized for a given architecture via the Arch template argument.
 template<int Arch, class Derived1, class Derived2, typename Scalar, int PacketAccess> struct ei_quat_product
 {
-  inline static Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
+  EIGEN_STRONG_INLINE static Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
     return Quaternion<Scalar>
     (
       a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
@@ -347,7 +369,7 @@
 /** \returns the concatenation of two rotations as a quaternion-quaternion product */
 template <class Derived>
 template <class OtherDerived>
-inline Quaternion<typename ei_traits<Derived>::Scalar>
+EIGEN_STRONG_INLINE Quaternion<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),
@@ -360,7 +382,7 @@
 /** \sa operator*(Quaternion) */
 template <class Derived>
 template <class OtherDerived>
-inline Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
+EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
 {
   return (derived() = derived() * other.derived());
 }
@@ -373,7 +395,7 @@
   *   - Via a Matrix3: 24 + 15n
   */
 template <class Derived>
-inline typename QuaternionBase<Derived>::Vector3
+EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
 QuaternionBase<Derived>::_transformVector(Vector3 v) const
 {
     // Note that this algorithm comes from the optimization by hand
@@ -386,8 +408,15 @@
 }
 
 template<class Derived>
+EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
+{
+  coeffs() = other.coeffs();
+  return derived();
+}
+
+template<class Derived>
 template<class OtherDerived>
-inline Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
+EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
 {
   coeffs() = other.coeffs();
   return derived();
@@ -396,7 +425,7 @@
 /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
   */
 template<class Derived>
-inline Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
+EIGEN_STRONG_INLINE 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);
diff -r 5536e0c23ba2 -r 7456b5d088e8 Eigen/src/Geometry/RotationBase.h
--- a/Eigen/src/Geometry/RotationBase.h	Fri Nov 13 13:05:57 2009 +0000
+++ b/Eigen/src/Geometry/RotationBase.h	Fri Nov 13 16:41:51 2009 +0100
@@ -73,7 +73,7 @@
       *  - a vector of size Dim
       */
     template<typename OtherDerived>
-    inline typename ei_rotation_base_generic_product_selector<Derived,OtherDerived,OtherDerived::IsVectorAtCompileTime>::ReturnType
+    EIGEN_STRONG_INLINE typename ei_rotation_base_generic_product_selector<Derived,OtherDerived,OtherDerived::IsVectorAtCompileTime>::ReturnType
     operator*(const AnyMatrixBase<OtherDerived>& e) const
     { return ei_rotation_base_generic_product_selector<Derived,OtherDerived>::run(derived(), e.derived()); }
 
@@ -107,7 +107,7 @@
 {
   enum { Dim = RotationDerived::Dim };
   typedef Matrix<typename RotationDerived::Scalar,Dim,1> ReturnType;
-  inline static ReturnType run(const RotationDerived& r, const OtherVectorType& v)
+  EIGEN_STRONG_INLINE static ReturnType run(const RotationDerived& r, const OtherVectorType& v)
   {
     return r._transformVector(v);
   }
diff -r 5536e0c23ba2 -r 7456b5d088e8 test/geo_quaternion.cpp
--- a/test/geo_quaternion.cpp	Fri Nov 13 13:05:57 2009 +0000
+++ b/test/geo_quaternion.cpp	Fri Nov 13 16:41:51 2009 +0100
@@ -2,6 +2,7 @@
 // for linear algebra.
 //
 // Copyright (C) 2008-2009 Gael Guennebaud <g.gael@xxxxxxx>
+// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@xxxxxx>
 //
 // Eigen is free software; you can redistribute it and/or
 // modify it under the terms of the GNU Lesser General Public
@@ -84,7 +85,7 @@
 
 
   // angle-axis conversion
-  AngleAxisx aa = q1;
+  AngleAxisx aa = AngleAxisx(q1);
   VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
   VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
 
@@ -110,10 +111,35 @@
   VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);
 }
 
+template<typename Scalar> void mapQuaternion(void){
+  typedef Map<Quaternion<Scalar>, Aligned> MQuaternionA;
+  typedef Map<Quaternion<Scalar> > MQuaternionUA;
+  typedef Quaternion<Scalar> Quaternionx;
+
+	EIGEN_ALIGN16 Scalar array1[4];
+	EIGEN_ALIGN16 Scalar array2[4];
+	EIGEN_ALIGN16 Scalar array3[4+1];
+	Scalar* array3unaligned = array3+1;
+
+  MQuaternionA(array1).coeffs().setRandom();
+  (MQuaternionA(array2)) = MQuaternionA(array1);
+  (MQuaternionUA(array3unaligned)) = MQuaternionA(array1);
+
+  Quaternionx q1 = MQuaternionA(array1);
+  Quaternionx q2 = MQuaternionA(array2);
+  Quaternionx q3 = MQuaternionUA(array3unaligned);
+
+  VERIFY_IS_APPROX(q1.coeffs(), q2.coeffs());
+  VERIFY_IS_APPROX(q1.coeffs(), q3.coeffs());
+  VERIFY_RAISES_ASSERT((MQuaternionA(array3unaligned)));
+}
+
 void test_geo_quaternion()
 {
   for(int i = 0; i < g_repeat; i++) {
     CALL_SUBTEST_1( quaternion<float>() );
     CALL_SUBTEST_2( quaternion<double>() );
+    CALL_SUBTEST( mapQuaternion<float>() );
+    CALL_SUBTEST( mapQuaternion<double>() );
   }
 }
diff -r 5536e0c23ba2 -r 7456b5d088e8 test/geo_transformations.cpp
--- a/test/geo_transformations.cpp	Fri Nov 13 13:05:57 2009 +0000
+++ b/test/geo_transformations.cpp	Fri Nov 13 16:41:51 2009 +0100
@@ -81,7 +81,7 @@
     * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1)));
 
   // angle-axis conversion
-  AngleAxisx aa = q1;
+  AngleAxisx aa = AngleAxisx(q1);
   VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
   VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
 


Mail converted by MHonArc 2.6.19+ http://listengine.tuxfamily.org/