backport Rohit's vectorized quaternion product

Eigen/src/Core/arch/SSE/PacketMath.h

@@ -315,4 +315,7 @@ struct ei_palign_impl<Offset,__m128d>
 };
 #endif
 
+#define ei_vec4f_swizzle1(v,p,q,r,s) \
+  (_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), ((s)<<6|(r)<<4|(q)<<2|(p)))))
+
 #endif // EIGEN_PACKET_MATH_SSE_H

Eigen/src/Core/util/Constants.h

@@ -239,4 +239,16 @@ enum {
   HasDirectAccess = DirectAccessBit
 };
 
+const int EiArch_Generic = 0x0;
+const int EiArch_SSE     = 0x1;
+const int EiArch_AltiVec = 0x2;
+
+#if defined EIGEN_VECTORIZE_SSE
+  const int EiArch = EiArch_SSE;
+#elif defined EIGEN_VECTORIZE_ALTIVEC
+  const int EiArch = EiArch_AltiVec;
+#else
+  const int EiArch = EiArch_Generic;
+#endif
+
 #endif // EIGEN_CONSTANTS_H

Eigen/src/Geometry/Quaternion.h

@@ -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
+  return ei_quaternion_product<EiArch>(*this,other);
-  (
-    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()
-  );
 }
 
 /** \sa operator*(Quaternion) */