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

Add SSE vectorization of Quaternion::conjugate. Significant speed-up when combined with products like q1*q2.conjugate()

Browse Source
This commit is contained in:
Gael Guennebaud 2015-03-02 20:09:33 +01:00
parent ef09ce4552
commit 3109f0e74e
2 changed files with 51 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -441,7 +441,7 @@ QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) c
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
return internal::quat_product<Architecture::Target, Derived, OtherDerived,
typename internal::traits<Derived>::Scalar,
internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
(internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned)?Aligned:Unaligned>::run(*this, other);
}
/** \sa operator*(Quaternion) */
@ -646,6 +646,16 @@ inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Der
}
}
// Generic conjugate of a Quaternion
namespace internal {
template<int Arch, class Derived, typename Scalar, int _Options> struct quat_conj
{
static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived>& q){
return Quaternion<Scalar>(q.w(),-q.x(),-q.y(),-q.z());
}
};
}
/** \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.
@ -656,7 +666,10 @@ template <class Derived>
inline Quaternion<typename internal::traits<Derived>::Scalar>
QuaternionBase<Derived>::conjugate() const
{
return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
return internal::quat_conj<Architecture::Target, Derived,
typename internal::traits<Derived>::Scalar,
internal::traits<Derived>::IsAligned?Aligned:Unaligned>::run(*this);
}
/** \returns the angle (in radian) between two rotations

46
Eigen/src/Geometry/arch/Geometry_SSE.h
View File

@ -20,23 +20,35 @@ struct quat_product<Architecture::SSE, Derived, OtherDerived, float, Aligned>
{
static inline Quaternion<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
{
const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
Quaternion<float> res;
const __m128 mask = _mm_setr_ps(0.f,0.f,0.f,-0.f);
__m128 a = _a.coeffs().template packet<Aligned>(0);
__m128 b = _b.coeffs().template packet<Aligned>(0);
__m128 flip1 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,1,2,0,2),
vec4f_swizzle1(b,2,0,1,2)),mask);
__m128 flip2 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,3,3,3,1),
vec4f_swizzle1(b,0,1,2,1)),mask);
__m128 s1 = _mm_mul_ps(vec4f_swizzle1(a,1,2,0,2),vec4f_swizzle1(b,2,0,1,2));
__m128 s2 = _mm_mul_ps(vec4f_swizzle1(a,3,3,3,1),vec4f_swizzle1(b,0,1,2,1));
pstore(&res.x(),
_mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,vec4f_swizzle1(b,3,3,3,3)),
_mm_mul_ps(vec4f_swizzle1(a,2,0,1,0),
vec4f_swizzle1(b,1,2,0,0))),
_mm_add_ps(flip1,flip2)));
_mm_xor_ps(mask,_mm_add_ps(s1,s2))));
return res;
}
};
template<class Derived, int Alignment>
struct quat_conj<Architecture::SSE, Derived, float, Alignment>
{
static inline Quaternion<float> run(const QuaternionBase<Derived>& q)
{
Quaternion<float> res;
const __m128 mask = _mm_setr_ps(-0.f,-0.f,-0.f,0.f);
pstore(&res.x(), _mm_xor_ps(mask, q.coeffs().template packet<Alignment>(0)));
return res;
}
};
template<typename VectorLhs,typename VectorRhs>
struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true>
{
@ -56,8 +68,8 @@ struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true>
template<class Derived, class OtherDerived>
struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned>
template<class Derived, class OtherDerived, int Alignment>
struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Alignment>
{
static inline Quaternion<double> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
{
@ -66,8 +78,8 @@ struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned>
Quaternion<double> res;
const double* a = _a.coeffs().data();
Packet2d b_xy = _b.coeffs().template packet<Aligned>(0);
Packet2d b_zw = _b.coeffs().template packet<Aligned>(2);
Packet2d b_xy = _b.coeffs().template packet<Alignment>(0);
Packet2d b_zw = _b.coeffs().template packet<Alignment>(2);
Packet2d a_xx = pset1<Packet2d>(a[0]);
Packet2d a_yy = pset1<Packet2d>(a[1]);
Packet2d a_zz = pset1<Packet2d>(a[2]);
@ -108,6 +120,20 @@ struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned>
}
};
template<class Derived, int Alignment>
struct quat_conj<Architecture::SSE, Derived, double, Alignment>
{
static inline Quaternion<double> run(const QuaternionBase<Derived>& q)
{
Quaternion<double> res;
const __m128d mask0 = _mm_setr_pd(-0.,-0.);
const __m128d mask2 = _mm_setr_pd(-0.,0.);
pstore(&res.x(), _mm_xor_pd(mask0, q.coeffs().template packet<Alignment>(0)));
pstore(&res.z(), _mm_xor_pd(mask2, q.coeffs().template packet<Alignment>(2)));
return res;
}
};
} // end namespace internal
} // end namespace Eigen