GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-06-03 02:03:59 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add factory/getters for quat coeffs in both orders

Browse Source
This commit is contained in:
Hs293Go 2025-05-24 04:10:50 -04:00
parent d81aa18f4d
commit a7f183cadb
2 changed files with 88 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
Eigen/src/Geometry/Quaternion.h
View File

@ -85,6 +85,29 @@ class QuaternionBase : public RotationBase<Derived, 3> {
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<Derived>::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<Derived>::Coefficients coeffsScalarLast() const {
return derived().coeffsScalarLast();
}
/** \returns a vector expression of the coefficients (x,y,z,w) */
EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
@ -357,12 +380,23 @@ class Quaternion : public QuaternionBase<Quaternion<Scalar_, Options_> > {
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 <typename Derived1, typename Derived2>
EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& 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<const Quaternion<Scalar_>, Options_> : public QuaternionBase<Map<const
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()};
}
protected:
const Coefficients m_coeffs;
};
@ -473,6 +513,12 @@ class Map<Quaternion<Scalar_>, Options_> : public QuaternionBase<Map<Quaternion<
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()};
}
protected:
Coefficients m_coeffs;
};
@ -694,6 +740,35 @@ EIGEN_DEVICE_FUNC Quaternion<Scalar, Options> Quaternion<Scalar, Options>::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 <typename Scalar, int Options>
EIGEN_DEVICE_FUNC Quaternion<Scalar, Options> Quaternion<Scalar, Options>::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 <typename Scalar, int Options>
EIGEN_DEVICE_FUNC Quaternion<Scalar, Options> Quaternion<Scalar, Options>::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

13
test/geo_quaternion.cpp
View File

@ -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;