| [eigen] Mapping array of scalars into quaternions |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
Hi,
I want to implement a class which hold both a quaternion and a vector3
to discribe a position of a frame in respect to an other frame
(Something similar to Frame in KDL). For some historic reason, the
memory layout must be a continuous array of scalar (Scalar[7]), the 3
first scalars are the vector3 and the other are the quaternion. Besides
I have to interface this structure with other libraries which return an
array of 7 scalars. I also have to map array of 4 scalars into the
Quaternion class.
I though of something like that :
template<typename _Scalars>
struct Frame {
_Scalars[7] data;
Map<Matrix<3,1,_Scalars>> v;
Map<Quaternion<_Scalars>> q;
};
and setting the Map accordingly. However, there's no such concept of Map
for the Quaternion class or RotationBase.
So, I modified the Quaternion class to use either a Matrix<Scalar,4,1>
to store the quaternion coefficients or a Map<Matrix<Scalar,4,1>> to map
an array of scalars. I added a argument to the template prototype of the
quaternion class which I call StorageType which is either the Matrix or
Map<Matrix>.
Since I have almost no experience with Eigen, I don't know if it's the
better choice and if it could cause alignement issues. I have attached
my modifications as a patch. This implementation may be incomplete.
So, to summarize, the Frame class would be:
template<typename _Scalars>
struct Frame {
_Scalars[7] data;
Map<Matrix<3,1,_Scalars>> v;
QuaternionMap<_Scalars> q;
};
where v is a mapping of data and q a mapping of data+3.
--
Mathieu Gautier
# HG changeset patch
# User Mathieu Gautier <mathieu.gautier@xxxxxx>
# Date 1256125668 -7200
# Node ID bae1fcae32256eb29a7e1af8286816df8b98e284
# Parent 754dfef12c186fb259e088dc5a6a58e06445d3bd
* Quaternion could now map an array of scalars
diff -r 754dfef12c18 -r bae1fcae3225 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 Wed Oct 21 13:47:48 2009 +0200
@@ -129,7 +129,7 @@
// Geometry module:
template<typename Derived, int _Dim> class RotationBase;
template<typename Lhs, typename Rhs> class Cross;
-template<typename Scalar> class Quaternion;
+template<typename Scalar, class StorageType> class Quaternion;
template<typename Scalar> class Rotation2D;
template<typename Scalar> class AngleAxis;
template<typename Scalar,int Dim,int Mode=Affine> class Transform;
diff -r 754dfef12c18 -r bae1fcae3225 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 Wed Oct 21 13:47:48 2009 +0200
@@ -52,15 +52,15 @@
* \sa class AngleAxis, class Transform
*/
-template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
+template<typename _Scalar, class StorageType> struct ei_traits<Quaternion<_Scalar, StorageType> >
{
typedef _Scalar Scalar;
};
-template<typename _Scalar>
-class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
+template<typename _Scalar, class StorageType = Matrix<_Scalar, 4, 1>>
+class Quaternion : public RotationBase<Quaternion<_Scalar, StorageType>,3>
{
- typedef RotationBase<Quaternion<_Scalar>,3> Base;
+ typedef RotationBase<Quaternion<_Scalar, StorageType>,3> Base;
@@ -72,8 +72,11 @@
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
+ typedef Quaternion<Scalar, Map<Matrix<Scalar, 4, 1>>> QuaternionMap;
+
/** the type of the Coefficients 4-vector */
- typedef Matrix<Scalar, 4, 1> Coefficients;
+// typedef Matrix<Scalar, 4, 1> Coefficients;
+ typedef StorageType Coefficients;
/** the type of a 3D vector */
typedef Matrix<Scalar,3,1> Vector3;
/** the equivalent rotation matrix type */
@@ -124,8 +127,12 @@
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 */
+ inline Quaternion(const Scalar* data) : m_coeffs(data) {}
+
/** Copy constructor */
- inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
+ template<class OtherStorageType> inline Quaternion(const Quaternion<Scalar, OtherStorageType>& other) { m_coeffs = other.coeffs(); }
/** Constructs and initializes a quaternion from the angle-axis \a aa */
explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
@@ -146,7 +153,7 @@
/** \returns a quaternion representing an identity rotation
* \sa MatrixBase::Identity()
*/
- inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
+ inline static Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
/** \sa Quaternion::Identity(), MatrixBase::setIdentity()
*/
@@ -167,29 +174,29 @@
inline void normalize() { m_coeffs.normalize(); }
/** \returns a normalized version of \c *this
* \sa normalize(), MatrixBase::normalized() */
- inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
+ inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(m_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()
*/
- inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
+ template<class OtherStorageType> inline Scalar dot(const Quaternion<Scalar, OtherStorageType>& other) const { return m_coeffs.dot(other.coeffs()); }
- inline Scalar angularDistance(const Quaternion& other) const;
+ template<class OtherStorageType> inline Scalar angularDistance(const Quaternion<Scalar, OtherStorageType>& other) const;
Matrix3 toRotationMatrix(void) const;
template<typename Derived1, typename Derived2>
Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
- inline Quaternion operator* (const Quaternion& q) const;
- inline Quaternion& operator*= (const Quaternion& q);
+ template<class OtherStorageType> inline Quaternion<Scalar> operator* (const Quaternion<Scalar, OtherStorageType>& q) const;
+ template<class OtherStorageType> inline Quaternion& operator*= (const Quaternion<Scalar, OtherStorageType>& q);
- Quaternion inverse(void) const;
- Quaternion conjugate(void) const;
+ Quaternion<Scalar> inverse(void) const;
+ Quaternion<Scalar> conjugate(void) const;
- Quaternion slerp(Scalar t, const Quaternion& other) const;
+ template<class OtherStorageType> Quaternion<Scalar> slerp(Scalar t, const Quaternion<Scalar, OtherStorageType>& other) const;
/** \returns \c *this with scalar type casted to \a NewScalarType
*
@@ -201,8 +208,8 @@
{ return typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
/** Copy constructor with scalar type conversion */
- template<typename OtherScalarType>
- inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
+ template<typename OtherScalarType, class OtherStorageType>
+ inline explicit Quaternion(const Quaternion<OtherScalarType, OtherStorageType>& other)
{ m_coeffs = other.coeffs().template cast<Scalar>(); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
@@ -225,9 +232,16 @@
* double precision quaternion type */
typedef Quaternion<double> Quaterniond;
+/** \ingroup Geometry_Module
+ * double precision quaternion type */
+typedef Quaternion<float>::QuaternionMap QuaternionMapf;
+/** \ingroup Geometry_Module
+ * double precision quaternion type */
+typedef Quaternion<double>::QuaternionMap QuaternionMapd;
+
// Generic Quaternion * Quaternion product
-template<int Arch,typename Scalar> inline Quaternion<Scalar>
-ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
+template<int Arch,typename Scalar, class StorageType, class OtherStorageType> inline Quaternion<Scalar>
+ei_quaternion_product(const Quaternion<Scalar, StorageType>& a, const Quaternion<Scalar, OtherStorageType>& b)
{
return Quaternion<Scalar>
(
@@ -239,15 +253,17 @@
}
/** \returns the concatenation of two rotations as a quaternion-quaternion product */
-template <typename Scalar>
-inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
+template <typename Scalar, class StorageType>
+template <class OtherStorageType>
+inline Quaternion<Scalar> Quaternion<Scalar, StorageType>::operator* (const Quaternion<Scalar, OtherStorageType>& other) const
{
- return ei_quaternion_product<EiArch>(*this,other);
+ return ei_quaternion_product<EiArch, Scalar, StorageType, OtherStorageType>(*this, other);
}
/** \sa operator*(Quaternion) */
-template <typename Scalar>
-inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
+template <typename Scalar, class StorageType>
+template <class OtherStorageType>
+inline Quaternion<Scalar, StorageType>& Quaternion<Scalar, StorageType>::operator*= (const Quaternion<Scalar, OtherStorageType>& other)
{
return (*this = *this * other);
}
@@ -259,9 +275,9 @@
* - Quaternion: 30n
* - Via a Matrix3: 24 + 15n
*/
-template <typename Scalar>
-inline typename Quaternion<Scalar>::Vector3
-Quaternion<Scalar>::_transformVector(Vector3 v) const
+template <typename Scalar, class StorageType>
+inline typename Quaternion<Scalar, StorageType>::Vector3
+Quaternion<Scalar, StorageType>::_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.
@@ -272,8 +288,8 @@
return v + this->w() * uv + this->vec().cross(uv);
}
-template<typename Scalar>
-inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
+template<typename Scalar, class StorageType>
+inline Quaternion<Scalar, StorageType>& Quaternion<Scalar, StorageType>::operator=(const Quaternion& other)
{
m_coeffs = other.m_coeffs;
return *this;
@@ -281,8 +297,8 @@
/** 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)
+template<typename Scalar, class StorageType>
+inline Quaternion<Scalar, StorageType>& Quaternion<Scalar, StorageType>::operator=(const AngleAxisType& aa)
{
Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
this->w() = ei_cos(ha);
@@ -295,9 +311,9 @@
* - 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 Scalar, class StorageType>
template<typename Derived>
-inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
+inline Quaternion<Scalar, StorageType>& Quaternion<Scalar, StorageType>::operator=(const MatrixBase<Derived>& xpr)
{
ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
return *this;
@@ -306,9 +322,9 @@
/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to
* be normalized, otherwise the result is undefined.
*/
-template<typename Scalar>
-inline typename Quaternion<Scalar>::Matrix3
-Quaternion<Scalar>::toRotationMatrix(void) const
+template<typename Scalar, class StorageType>
+inline typename Quaternion<Scalar, StorageType>::Matrix3
+Quaternion<Scalar, StorageType>::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%,
@@ -352,9 +368,9 @@
* Note that the two input vectors do \b not have to be normalized, and
* do not need to have the same norm.
*/
-template<typename Scalar>
+template<typename Scalar, class StorageType>
template<typename Derived1, typename Derived2>
-inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
+inline Quaternion<Scalar, StorageType>& Quaternion<Scalar, StorageType>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
{
Vector3 v0 = a.normalized();
Vector3 v1 = b.normalized();
@@ -395,17 +411,17 @@
*
* \sa Quaternion::conjugate()
*/
-template <typename Scalar>
-inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
+template <typename Scalar, class StorageType>
+inline Quaternion<Scalar> Quaternion<Scalar, StorageType>::inverse() const
{
// FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
Scalar n2 = this->squaredNorm();
if (n2 > 0)
- return Quaternion(conjugate().coeffs() / n2);
+ return Quaternion<Scalar>(conjugate().coeffs() / n2);
else
{
// return an invalid result to flag the error
- return Quaternion(Coefficients::Zero());
+ return Quaternion<Scalar>(Coefficients::Zero());
}
}
@@ -415,17 +431,18 @@
*
* \sa Quaternion::inverse()
*/
-template <typename Scalar>
-inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
+template <typename Scalar, class StorageType>
+inline Quaternion<Scalar> Quaternion<Scalar, StorageType>::conjugate() const
{
- return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
+ return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
}
/** \returns the angle (in radian) between two rotations
* \sa dot()
*/
-template <typename Scalar>
-inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
+template <typename Scalar, class StorageType>
+template <class OtherStorageType>
+inline Scalar Quaternion<Scalar, StorageType>::angularDistance(const Quaternion<Scalar, OtherStorageType>& other) const
{
double d = ei_abs(this->dot(other));
if (d>=1.0)
@@ -436,14 +453,15 @@
/** \returns the spherical linear interpolation between the two quaternions
* \c *this and \a other at the parameter \a t
*/
-template <typename Scalar>
-Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
+template <typename Scalar, class StorageType>
+template <class OtherStorageType>
+Quaternion<Scalar> Quaternion<Scalar, StorageType>::slerp(Scalar t, const Quaternion<Scalar, OtherStorageType>& other) const
{
static const Scalar one = Scalar(1) - precision<Scalar>();
Scalar d = this->dot(other);
Scalar absD = ei_abs(d);
if (absD>=one)
- return *this;
+ return Quaternion<Scalar>(*this);
// theta is the angle between the 2 quaternions
Scalar theta = std::acos(absD);
@@ -454,7 +472,7 @@
if (d<0)
scale1 = -scale1;
- return Quaternion(scale0 * m_coeffs + scale1 * other.m_coeffs);
+ return Quaternion<Scalar>(scale0 * m_coeffs + scale1 * other.coeffs());
}
// set from a rotation matrix