| Re: [eigen] Mapping array of scalars into quaternions |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
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.
> In your patch, just let the Quaternion class untouched.
You just have to replace the Quat class by the class Quaternion that you
will modify and replace the occurence of Quat by Quaternion.
I have some remarks.
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.
--
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