diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 147e6e3ae..f2d2d051f 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -85,6 +85,29 @@ class QuaternionBase : public RotationBase { return derived().coeffs(); } + /** \returns a vector containing the coefficients, rearranged into the order [\c w, \c x, \c y, \c z]. + * + * This is the order expected by the \code Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& + * z) \endcode constructor, but not the order of the internal vector representation. Therefore, it returns a newly + * constructed vector. + * + * \sa QuaternionBase::coeffsScalarLast() + * */ + EIGEN_DEVICE_FUNC inline typename internal::traits::Coefficients coeffsScalarFirst() const { + return derived().coeffsScalarFirst(); + } + + /** \returns a vector containing the coefficients in their original order [\c x, \c y, \c z, \c w]. + * + * This is equivalent to \code coeffs() \endcode, but returns a newly constructed vector for uniformity with \code + * coeffsScalarFirst() \endcode. + * + * \sa QuaternionBase::coeffsScalarFirst() + * */ + EIGEN_DEVICE_FUNC inline typename internal::traits::Coefficients coeffsScalarLast() const { + return derived().coeffsScalarLast(); + } + /** \returns a vector expression of the coefficients (x,y,z,w) */ EIGEN_DEVICE_FUNC inline typename internal::traits::Coefficients& coeffs() { return derived().coeffs(); } @@ -357,12 +380,23 @@ class Quaternion : public QuaternionBase > { EIGEN_DEVICE_FUNC static Quaternion UnitRandom(); + EIGEN_DEVICE_FUNC static Quaternion FromCoeffsScalarLast(const Scalar& x, const Scalar& y, const Scalar& z, + const Scalar& w); + + EIGEN_DEVICE_FUNC static Quaternion FromCoeffsScalarFirst(const Scalar& w, const Scalar& x, const Scalar& y, + const Scalar& z); + template EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase& a, const MatrixBase& b); EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } + EIGEN_DEVICE_FUNC inline Coefficients coeffsScalarLast() const { return m_coeffs; } + + EIGEN_DEVICE_FUNC inline Coefficients coeffsScalarFirst() const { + return {m_coeffs.w(), m_coeffs.x(), m_coeffs.y(), m_coeffs.z()}; + } EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment)) #ifdef EIGEN_QUATERNION_PLUGIN @@ -437,6 +471,12 @@ class Map, Options_> : public QuaternionBase, Options_> : public QuaternionBase Quaternion::UnitR return Quaternion(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)); } +/** Constructs a quaternion from its coefficients in the order [\c x, \c y, \c z, \c w], i.e. vector part [\c x, \c y, + * \c z] first, scalar part \a w LAST. + * + * This factory accepts the parameters in the same order as the underlying coefficient vector. Consider using this + * factory function to make the parameter ordering explicit. + */ +template +EIGEN_DEVICE_FUNC Quaternion Quaternion::FromCoeffsScalarLast(const Scalar& x, + const Scalar& y, + const Scalar& z, + const Scalar& w) { + return Quaternion(w, x, y, z); +} + +/** Constructs a quaternion from its coefficients in the order [\c w, \c x, \c y, \c z], i.e. scalar part \a w FIRST, + * vector part [\c x, \c y, \c z] last. + * + * This factory accepts the parameters in the same order as the constructor \code Quaternion(const Scalar& w, const + * Scalar& x, const Scalar& y, const Scalar& z) \endcode. Consider using this factory function to make the parameter + * ordering explicit. + */ +template +EIGEN_DEVICE_FUNC Quaternion Quaternion::FromCoeffsScalarFirst(const Scalar& w, + const Scalar& x, + const Scalar& y, + const Scalar& z) { + return Quaternion(w, x, y, z); +} + /** Returns 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 diff --git a/test/geo_quaternion.cpp b/test/geo_quaternion.cpp index b7e4dc5ab..159a937b1 100644 --- a/test/geo_quaternion.cpp +++ b/test/geo_quaternion.cpp @@ -78,6 +78,19 @@ void quaternion(void) { VERIFY(ss.str() == "0i + 0j + 0k + 1"); #endif + // Consistent handling of scalar first/last conventions regardless of Eigen's own coefficient layout + const Scalar w(a); + const Vector3 xyz(v0); + q1 = Quaternionx::FromCoeffsScalarFirst(w, xyz.x(), xyz.y(), xyz.z()); + q2 = Quaternionx::FromCoeffsScalarLast(xyz.x(), xyz.y(), xyz.z(), w); + VERIFY_IS_EQUAL(q1, q2); + + VERIFY_IS_EQUAL(q1.coeffsScalarFirst()[0], w); + VERIFY_IS_EQUAL(q1.coeffsScalarFirst()(seqN(1, 3)), xyz); + + VERIFY_IS_EQUAL(q1.coeffsScalarLast()[3], w); + VERIFY_IS_EQUAL(q1.coeffsScalarLast()(seqN(0, 3)), xyz); + // concatenation q1 *= q2;