devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

backport Rohit's vectorized quaternion product

Browse Source
This commit is contained in:
Benoit Jacob
2009-09-22 13:39:30 -04:00
parent b66516e746
commit 8c2ace33c9
3 changed files with 50 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -224,17 +224,45 @@ typedef Quaternion<float> Quaternionf;
* double precision quaternion type */
typedef Quaternion<double> Quaterniond;
// Generic Quaternion * Quaternion product
template<int Arch,typename Scalar> inline Quaternion<Scalar>
ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
{
return Quaternion<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()
);
}
#ifdef EIGEN_VECTORIZE_SSE
template<> inline Quaternion<float>
ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quaternion<float>& _b)
{
const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
Quaternion<float> res;
__m128 a = _a.coeffs().packet<Aligned>(0);
__m128 b = _b.coeffs().packet<Aligned>(0);
__m128 flip1 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,2),
ei_vec4f_swizzle1(b,2,0,1,2)),mask);
__m128 flip2 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,3,3,3,1),
ei_vec4f_swizzle1(b,0,1,2,1)),mask);
ei_pstore(&res.x(),
_mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,ei_vec4f_swizzle1(b,3,3,3,3)),
_mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,0),
ei_vec4f_swizzle1(b,1,2,0,0))),
_mm_add_ps(flip1,flip2)));
return res;
}
#endif
/** \returns the concatenation of two rotations as a quaternion-quaternion product */
template <typename Scalar>
inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
{
return Quaternion
(
this->w() * other.w() - this->x() * other.x() - this->y() * other.y() - this->z() * other.z(),
this->w() * other.x() + this->x() * other.w() + this->y() * other.z() - this->z() * other.y(),
this->w() * other.y() + this->y() * other.w() + this->z() * other.x() - this->x() * other.z(),
this->w() * other.z() + this->z() * other.w() + this->x() * other.y() - this->y() * other.x()
);
return ei_quaternion_product<EiArch>(*this,other);
}
/** \sa operator*(Quaternion) */