00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef EIGEN_GEOMETRY_SSE_H
00027 #define EIGEN_GEOMETRY_SSE_H
00028
00029 namespace Eigen {
00030
00031 namespace internal {
00032
00033 template<class Derived, class OtherDerived>
00034 struct quat_product<Architecture::SSE, Derived, OtherDerived, float, Aligned>
00035 {
00036 static inline Quaternion<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
00037 {
00038 const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
00039 Quaternion<float> res;
00040 __m128 a = _a.coeffs().template packet<Aligned>(0);
00041 __m128 b = _b.coeffs().template packet<Aligned>(0);
00042 __m128 flip1 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,1,2,0,2),
00043 vec4f_swizzle1(b,2,0,1,2)),mask);
00044 __m128 flip2 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,3,3,3,1),
00045 vec4f_swizzle1(b,0,1,2,1)),mask);
00046 pstore(&res.x(),
00047 _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,vec4f_swizzle1(b,3,3,3,3)),
00048 _mm_mul_ps(vec4f_swizzle1(a,2,0,1,0),
00049 vec4f_swizzle1(b,1,2,0,0))),
00050 _mm_add_ps(flip1,flip2)));
00051 return res;
00052 }
00053 };
00054
00055 template<typename VectorLhs,typename VectorRhs>
00056 struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true>
00057 {
00058 static inline typename plain_matrix_type<VectorLhs>::type
00059 run(const VectorLhs& lhs, const VectorRhs& rhs)
00060 {
00061 __m128 a = lhs.template packet<VectorLhs::Flags&AlignedBit ? Aligned : Unaligned>(0);
00062 __m128 b = rhs.template packet<VectorRhs::Flags&AlignedBit ? Aligned : Unaligned>(0);
00063 __m128 mul1=_mm_mul_ps(vec4f_swizzle1(a,1,2,0,3),vec4f_swizzle1(b,2,0,1,3));
00064 __m128 mul2=_mm_mul_ps(vec4f_swizzle1(a,2,0,1,3),vec4f_swizzle1(b,1,2,0,3));
00065 typename plain_matrix_type<VectorLhs>::type res;
00066 pstore(&res.x(),_mm_sub_ps(mul1,mul2));
00067 return res;
00068 }
00069 };
00070
00071
00072
00073
00074 template<class Derived, class OtherDerived>
00075 struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned>
00076 {
00077 static inline Quaternion<double> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b)
00078 {
00079 const Packet2d mask = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0));
00080
00081 Quaternion<double> res;
00082
00083 const double* a = _a.coeffs().data();
00084 Packet2d b_xy = _b.coeffs().template packet<Aligned>(0);
00085 Packet2d b_zw = _b.coeffs().template packet<Aligned>(2);
00086 Packet2d a_xx = pset1<Packet2d>(a[0]);
00087 Packet2d a_yy = pset1<Packet2d>(a[1]);
00088 Packet2d a_zz = pset1<Packet2d>(a[2]);
00089 Packet2d a_ww = pset1<Packet2d>(a[3]);
00090
00091
00092 Packet2d t1, t2;
00093
00094
00095
00096
00097
00098
00099 t1 = padd(pmul(a_ww, b_xy), pmul(a_yy, b_zw));
00100 t2 = psub(pmul(a_zz, b_xy), pmul(a_xx, b_zw));
00101 #ifdef EIGEN_VECTORIZE_SSE3
00102 EIGEN_UNUSED_VARIABLE(mask)
00103 pstore(&res.x(), _mm_addsub_pd(t1, preverse(t2)));
00104 #else
00105 pstore(&res.x(), padd(t1, pxor(mask,preverse(t2))));
00106 #endif
00107
00108
00109
00110
00111
00112
00113 t1 = psub(pmul(a_ww, b_zw), pmul(a_yy, b_xy));
00114 t2 = padd(pmul(a_zz, b_zw), pmul(a_xx, b_xy));
00115 #ifdef EIGEN_VECTORIZE_SSE3
00116 EIGEN_UNUSED_VARIABLE(mask)
00117 pstore(&res.z(), preverse(_mm_addsub_pd(preverse(t1), t2)));
00118 #else
00119 pstore(&res.z(), psub(t1, pxor(mask,preverse(t2))));
00120 #endif
00121
00122 return res;
00123 }
00124 };
00125
00126 }
00127
00128 }
00129
00130 #endif // EIGEN_GEOMETRY_SSE_H