add optimized cross3 function (code from Rohit Garg)

1 files changed, 15 insertions, 0 deletions

diff --git a/Eigen/src/Geometry/arch/Geometry_SSE.h b/Eigen/src/Geometry/arch/Geometry_SSE.h
index 9be5ecd6f..db8c7cc1f 100644
--- a/Eigen/src/Geometry/arch/Geometry_SSE.h
+++ b/Eigen/src/Geometry/arch/Geometry_SSE.h
@@ -45,4 +45,19 @@ ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quate
   return res;
 }
 
+template<typename VectorLhs,typename VectorRhs>
+struct ei_cross3_impl<EiArch_SSE,VectorLhs,VectorRhs,float,true> {
+  inline static typename ei_plain_matrix_type<VectorLhs>::type
+  run(const VectorLhs& lhs, const VectorRhs& rhs)
+  {
+    __m128 a = lhs.coeffs().packet<VectorLhs::Flags&AlignedBit ? Aligned : Unaligned>(0);
+    __m128 b = rhs.coeffs().packet<VectorRhs::Flags&AlignedBit ? Aligned : Unaligned>(0);
+    __m128 mul1=_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,3),ei_vec4f_swizzle1(b,2,0,1,3));
+    __m128 mul2=_mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,3),ei_vec4f_swizzle1(b,1,2,0,3));
+    typename ei_plain_matrix_type<VectorLhs>::type res;
+    ei_pstore(&res.x(),_mm_sub_ps(mul1,mul2));
+    return res;
+  }
+};
+
 #endif // EIGEN_GEOMETRY_SSE_H