Geometry_SSE.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Rohit Garg <rpg.314@gmail.com>
5 // Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
6 //
7 // This Source Code Form is subject to the terms of the Mozilla
8 // Public License v. 2.0. If a copy of the MPL was not distributed
9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10 
11 #ifndef EIGEN_GEOMETRY_SSE_H
12 #define EIGEN_GEOMETRY_SSE_H
13 
14 namespace Eigen {
15 
16 namespace internal {
17 
18 template<class Derived, class OtherDerived>
19 struct quat_product<Architecture::SSE, Derived, OtherDerived, float, Aligned>
20 {
22  {
23  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
25  __m128 a = _a.coeffs().template packet<Aligned>(0);
26  __m128 b = _b.coeffs().template packet<Aligned>(0);
27  __m128 flip1 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,1,2,0,2),
28  vec4f_swizzle1(b,2,0,1,2)),mask);
29  __m128 flip2 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,3,3,3,1),
30  vec4f_swizzle1(b,0,1,2,1)),mask);
31  pstore(&res.x(),
32  _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,vec4f_swizzle1(b,3,3,3,3)),
33  _mm_mul_ps(vec4f_swizzle1(a,2,0,1,0),
34  vec4f_swizzle1(b,1,2,0,0))),
35  _mm_add_ps(flip1,flip2)));
36  return res;
37  }
38 };
39 
40 template<typename VectorLhs,typename VectorRhs>
41 struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true>
42 {
43  static inline typename plain_matrix_type<VectorLhs>::type
44  run(const VectorLhs& lhs, const VectorRhs& rhs)
45  {
46  __m128 a = lhs.template packet<VectorLhs::Flags&AlignedBit ? Aligned : Unaligned>(0);
47  __m128 b = rhs.template packet<VectorRhs::Flags&AlignedBit ? Aligned : Unaligned>(0);
48  __m128 mul1=_mm_mul_ps(vec4f_swizzle1(a,1,2,0,3),vec4f_swizzle1(b,2,0,1,3));
49  __m128 mul2=_mm_mul_ps(vec4f_swizzle1(a,2,0,1,3),vec4f_swizzle1(b,1,2,0,3));
51  pstore(&res.x(),_mm_sub_ps(mul1,mul2));
52  return res;
53  }
54 };
55 
56 
57 
58 
59 template<class Derived, class OtherDerived>
60 struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned>
61 {
63  {
64  const Packet2d mask = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0));
65 
67 
68  const double* a = _a.coeffs().data();
69  Packet2d b_xy = _b.coeffs().template packet<Aligned>(0);
70  Packet2d b_zw = _b.coeffs().template packet<Aligned>(2);
71  Packet2d a_xx = pset1<Packet2d>(a[0]);
72  Packet2d a_yy = pset1<Packet2d>(a[1]);
73  Packet2d a_zz = pset1<Packet2d>(a[2]);
74  Packet2d a_ww = pset1<Packet2d>(a[3]);
75 
76  // two temporaries:
77  Packet2d t1, t2;
78 
79  /*
80  * t1 = ww*xy + yy*zw
81  * t2 = zz*xy - xx*zw
82  * res.xy = t1 +/- swap(t2)
83  */
84  t1 = padd(pmul(a_ww, b_xy), pmul(a_yy, b_zw));
85  t2 = psub(pmul(a_zz, b_xy), pmul(a_xx, b_zw));
86 #ifdef EIGEN_VECTORIZE_SSE3
88  pstore(&res.x(), _mm_addsub_pd(t1, preverse(t2)));
89 #else
90  pstore(&res.x(), padd(t1, pxor(mask,preverse(t2))));
91 #endif
92 
93  /*
94  * t1 = ww*zw - yy*xy
95  * t2 = zz*zw + xx*xy
96  * res.zw = t1 -/+ swap(t2) = swap( swap(t1) +/- t2)
97  */
98  t1 = psub(pmul(a_ww, b_zw), pmul(a_yy, b_xy));
99  t2 = padd(pmul(a_zz, b_zw), pmul(a_xx, b_xy));
100 #ifdef EIGEN_VECTORIZE_SSE3
102  pstore(&res.z(), preverse(_mm_addsub_pd(preverse(t1), t2)));
103 #else
104  pstore(&res.z(), psub(t1, pxor(mask,preverse(t2))));
105 #endif
106 
107  return res;
108 }
109 };
110 
111 } // end namespace internal
112 
113 } // end namespace Eigen
114 
115 #endif // EIGEN_GEOMETRY_SSE_H
static plain_matrix_type< VectorLhs >::type run(const VectorLhs &lhs, const VectorRhs &rhs)
Definition: Geometry_SSE.h:44
#define EIGEN_UNUSED_VARIABLE(var)
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
const internal::traits< Derived >::Coefficients & coeffs() const
void pstore(Scalar *to, const Packet &from)
Packet psub(const Packet &a, const Packet &b)
#define vec4f_swizzle1(v, p, q, r, s)
static Quaternion< double > run(const QuaternionBase< Derived > &_a, const QuaternionBase< OtherDerived > &_b)
Definition: Geometry_SSE.h:62
Base class for quaternion expressions.
Packet pxor(const Packet &a, const Packet &b)
void rhs(const real_t *x, real_t *f)
static Quaternion< float > run(const QuaternionBase< Derived > &_a, const QuaternionBase< OtherDerived > &_b)
Definition: Geometry_SSE.h:21
The quaternion class used to represent 3D orientations and rotations.
Packet pmul(const Packet &a, const Packet &b)
EIGEN_STRONG_INLINE Packet2d pset1< Packet2d >(const double &from)
Packet padd(const Packet &a, const Packet &b)
EIGEN_STRONG_INLINE Packet2cf preverse(const Packet2cf &a)


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:39