27 #ifndef EIGEN_INVERSE_SSE_H 28 #define EIGEN_INVERSE_SSE_H 34 template<
typename MatrixType,
typename ResultType>
35 struct compute_inverse_size4<Architecture::SSE, float, MatrixType, ResultType>
38 MatrixAlignment = bool(MatrixType::Flags&
AlignedBit),
39 ResultAlignment = bool(ResultType::Flags&
AlignedBit),
43 static void run(
const MatrixType& matrix, ResultType& result)
45 EIGEN_ALIGN16
const unsigned int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 };
48 __m128 _L1 = matrix.template packet<MatrixAlignment>( 0);
49 __m128 _L2 = matrix.template packet<MatrixAlignment>( 4);
50 __m128 _L3 = matrix.template packet<MatrixAlignment>( 8);
51 __m128 _L4 = matrix.template packet<MatrixAlignment>(12);
60 if(!StorageOrdersMatch)
62 A = _mm_unpacklo_ps(_L1, _L2);
63 B = _mm_unpacklo_ps(_L3, _L4);
64 C = _mm_unpackhi_ps(_L1, _L2);
65 D = _mm_unpackhi_ps(_L3, _L4);
69 A = _mm_movelh_ps(_L1, _L2);
70 B = _mm_movehl_ps(_L2, _L1);
71 C = _mm_movelh_ps(_L3, _L4);
72 D = _mm_movehl_ps(_L4, _L3);
75 __m128 iA, iB, iC, iD,
77 __m128 dA, dB, dC, dD;
78 __m128 det, d, d1, d2;
82 AB = _mm_mul_ps(_mm_shuffle_ps(A,A,0x0F), B);
83 AB = _mm_sub_ps(AB,_mm_mul_ps(_mm_shuffle_ps(A,A,0xA5), _mm_shuffle_ps(B,B,0x4E)));
85 DC = _mm_mul_ps(_mm_shuffle_ps(D,D,0x0F), C);
86 DC = _mm_sub_ps(DC,_mm_mul_ps(_mm_shuffle_ps(D,D,0xA5), _mm_shuffle_ps(C,C,0x4E)));
89 dA = _mm_mul_ps(_mm_shuffle_ps(A, A, 0x5F),A);
90 dA = _mm_sub_ss(dA, _mm_movehl_ps(dA,dA));
92 dB = _mm_mul_ps(_mm_shuffle_ps(B, B, 0x5F),B);
93 dB = _mm_sub_ss(dB, _mm_movehl_ps(dB,dB));
96 dC = _mm_mul_ps(_mm_shuffle_ps(C, C, 0x5F),C);
97 dC = _mm_sub_ss(dC, _mm_movehl_ps(dC,dC));
99 dD = _mm_mul_ps(_mm_shuffle_ps(D, D, 0x5F),D);
100 dD = _mm_sub_ss(dD, _mm_movehl_ps(dD,dD));
103 d = _mm_mul_ps(_mm_shuffle_ps(DC,DC,0xD8),AB);
106 iD = _mm_mul_ps(_mm_shuffle_ps(C,C,0xA0), _mm_movelh_ps(AB,AB));
107 iD = _mm_add_ps(iD,_mm_mul_ps(_mm_shuffle_ps(C,C,0xF5), _mm_movehl_ps(AB,AB)));
109 iA = _mm_mul_ps(_mm_shuffle_ps(B,B,0xA0), _mm_movelh_ps(DC,DC));
110 iA = _mm_add_ps(iA,_mm_mul_ps(_mm_shuffle_ps(B,B,0xF5), _mm_movehl_ps(DC,DC)));
113 d = _mm_add_ps(d, _mm_movehl_ps(d, d));
114 d = _mm_add_ss(d, _mm_shuffle_ps(d, d, 1));
115 d1 = _mm_mul_ss(dA,dD);
116 d2 = _mm_mul_ss(dB,dC);
119 iD = _mm_sub_ps(_mm_mul_ps(D,_mm_shuffle_ps(dA,dA,0)), iD);
122 iA = _mm_sub_ps(_mm_mul_ps(A,_mm_shuffle_ps(dD,dD,0)), iA);
125 det = _mm_sub_ss(_mm_add_ss(d1,d2),d);
126 rd = _mm_div_ss(_mm_set_ss(1.0f), det);
133 iB = _mm_mul_ps(D, _mm_shuffle_ps(AB,AB,0x33));
134 iB = _mm_sub_ps(iB, _mm_mul_ps(_mm_shuffle_ps(D,D,0xB1), _mm_shuffle_ps(AB,AB,0x66)));
136 iC = _mm_mul_ps(A, _mm_shuffle_ps(DC,DC,0x33));
137 iC = _mm_sub_ps(iC, _mm_mul_ps(_mm_shuffle_ps(A,A,0xB1), _mm_shuffle_ps(DC,DC,0x66)));
139 rd = _mm_shuffle_ps(rd,rd,0);
140 rd = _mm_xor_ps(rd, _mm_load_ps((
float*)_Sign_PNNP));
143 iB = _mm_sub_ps(_mm_mul_ps(C,_mm_shuffle_ps(dB,dB,0)), iB);
146 iC = _mm_sub_ps(_mm_mul_ps(B,_mm_shuffle_ps(dC,dC,0)), iC);
149 iA = _mm_mul_ps(rd,iA);
150 iB = _mm_mul_ps(rd,iB);
151 iC = _mm_mul_ps(rd,iC);
152 iD = _mm_mul_ps(rd,iD);
154 DenseIndex res_stride = result.outerStride();
155 float* res = result.data();
156 pstoret<float, Packet4f, ResultAlignment>(res+0, _mm_shuffle_ps(iA,iB,0x77));
157 pstoret<float, Packet4f, ResultAlignment>(res+res_stride, _mm_shuffle_ps(iA,iB,0x22));
158 pstoret<float, Packet4f, ResultAlignment>(res+2*res_stride, _mm_shuffle_ps(iC,iD,0x77));
159 pstoret<float, Packet4f, ResultAlignment>(res+3*res_stride, _mm_shuffle_ps(iC,iD,0x22));
164 template<
typename MatrixType,
typename ResultType>
165 struct compute_inverse_size4<Architecture::SSE, double, MatrixType, ResultType>
168 MatrixAlignment = bool(MatrixType::Flags&
AlignedBit),
169 ResultAlignment = bool(ResultType::Flags&
AlignedBit),
172 static void run(
const MatrixType& matrix, ResultType& result)
174 const __m128d _Sign_NP = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0));
175 const __m128d _Sign_PN = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0));
184 __m128d A1, A2, B1, B2, C1, C2, D1, D2;
186 if(StorageOrdersMatch)
188 A1 = matrix.template packet<MatrixAlignment>( 0); B1 = matrix.template packet<MatrixAlignment>( 2);
189 A2 = matrix.template packet<MatrixAlignment>( 4); B2 = matrix.template packet<MatrixAlignment>( 6);
190 C1 = matrix.template packet<MatrixAlignment>( 8); D1 = matrix.template packet<MatrixAlignment>(10);
191 C2 = matrix.template packet<MatrixAlignment>(12); D2 = matrix.template packet<MatrixAlignment>(14);
196 A1 = matrix.template packet<MatrixAlignment>( 0); C1 = matrix.template packet<MatrixAlignment>( 2);
197 A2 = matrix.template packet<MatrixAlignment>( 4); C2 = matrix.template packet<MatrixAlignment>( 6);
199 A1 = _mm_unpacklo_pd(A1,A2);
200 A2 = _mm_unpackhi_pd(tmp,A2);
202 C1 = _mm_unpacklo_pd(C1,C2);
203 C2 = _mm_unpackhi_pd(tmp,C2);
205 B1 = matrix.template packet<MatrixAlignment>( 8); D1 = matrix.template packet<MatrixAlignment>(10);
206 B2 = matrix.template packet<MatrixAlignment>(12); D2 = matrix.template packet<MatrixAlignment>(14);
208 B1 = _mm_unpacklo_pd(B1,B2);
209 B2 = _mm_unpackhi_pd(tmp,B2);
211 D1 = _mm_unpacklo_pd(D1,D2);
212 D2 = _mm_unpackhi_pd(tmp,D2);
215 __m128d iA1, iA2, iB1, iB2, iC1, iC2, iD1, iD2,
217 __m128d dA, dB, dC, dD;
218 __m128d det, d1, d2, rd;
221 dA = _mm_shuffle_pd(A2, A2, 1);
222 dA = _mm_mul_pd(A1, dA);
223 dA = _mm_sub_sd(dA, _mm_shuffle_pd(dA,dA,3));
225 dB = _mm_shuffle_pd(B2, B2, 1);
226 dB = _mm_mul_pd(B1, dB);
227 dB = _mm_sub_sd(dB, _mm_shuffle_pd(dB,dB,3));
230 AB1 = _mm_mul_pd(B1, _mm_shuffle_pd(A2,A2,3));
231 AB2 = _mm_mul_pd(B2, _mm_shuffle_pd(A1,A1,0));
232 AB1 = _mm_sub_pd(AB1, _mm_mul_pd(B2, _mm_shuffle_pd(A1,A1,3)));
233 AB2 = _mm_sub_pd(AB2, _mm_mul_pd(B1, _mm_shuffle_pd(A2,A2,0)));
236 dC = _mm_shuffle_pd(C2, C2, 1);
237 dC = _mm_mul_pd(C1, dC);
238 dC = _mm_sub_sd(dC, _mm_shuffle_pd(dC,dC,3));
240 dD = _mm_shuffle_pd(D2, D2, 1);
241 dD = _mm_mul_pd(D1, dD);
242 dD = _mm_sub_sd(dD, _mm_shuffle_pd(dD,dD,3));
245 DC1 = _mm_mul_pd(C1, _mm_shuffle_pd(D2,D2,3));
246 DC2 = _mm_mul_pd(C2, _mm_shuffle_pd(D1,D1,0));
247 DC1 = _mm_sub_pd(DC1, _mm_mul_pd(C2, _mm_shuffle_pd(D1,D1,3)));
248 DC2 = _mm_sub_pd(DC2, _mm_mul_pd(C1, _mm_shuffle_pd(D2,D2,0)));
251 d1 = _mm_mul_pd(AB1, _mm_shuffle_pd(DC1, DC2, 0));
252 d2 = _mm_mul_pd(AB2, _mm_shuffle_pd(DC1, DC2, 3));
253 rd = _mm_add_pd(d1, d2);
254 rd = _mm_add_sd(rd, _mm_shuffle_pd(rd, rd,3));
257 iD1 = _mm_mul_pd(AB1, _mm_shuffle_pd(C1,C1,0));
258 iD2 = _mm_mul_pd(AB1, _mm_shuffle_pd(C2,C2,0));
259 iD1 = _mm_add_pd(iD1, _mm_mul_pd(AB2, _mm_shuffle_pd(C1,C1,3)));
260 iD2 = _mm_add_pd(iD2, _mm_mul_pd(AB2, _mm_shuffle_pd(C2,C2,3)));
263 iA1 = _mm_mul_pd(DC1, _mm_shuffle_pd(B1,B1,0));
264 iA2 = _mm_mul_pd(DC1, _mm_shuffle_pd(B2,B2,0));
265 iA1 = _mm_add_pd(iA1, _mm_mul_pd(DC2, _mm_shuffle_pd(B1,B1,3)));
266 iA2 = _mm_add_pd(iA2, _mm_mul_pd(DC2, _mm_shuffle_pd(B2,B2,3)));
269 dA = _mm_shuffle_pd(dA,dA,0);
270 iD1 = _mm_sub_pd(_mm_mul_pd(D1, dA), iD1);
271 iD2 = _mm_sub_pd(_mm_mul_pd(D2, dA), iD2);
274 dD = _mm_shuffle_pd(dD,dD,0);
275 iA1 = _mm_sub_pd(_mm_mul_pd(A1, dD), iA1);
276 iA2 = _mm_sub_pd(_mm_mul_pd(A2, dD), iA2);
278 d1 = _mm_mul_sd(dA, dD);
279 d2 = _mm_mul_sd(dB, dC);
282 iB1 = _mm_mul_pd(D1, _mm_shuffle_pd(AB2,AB1,1));
283 iB2 = _mm_mul_pd(D2, _mm_shuffle_pd(AB2,AB1,1));
284 iB1 = _mm_sub_pd(iB1, _mm_mul_pd(_mm_shuffle_pd(D1,D1,1), _mm_shuffle_pd(AB2,AB1,2)));
285 iB2 = _mm_sub_pd(iB2, _mm_mul_pd(_mm_shuffle_pd(D2,D2,1), _mm_shuffle_pd(AB2,AB1,2)));
288 det = _mm_add_sd(d1, d2);
289 det = _mm_sub_sd(det, rd);
292 iC1 = _mm_mul_pd(A1, _mm_shuffle_pd(DC2,DC1,1));
293 iC2 = _mm_mul_pd(A2, _mm_shuffle_pd(DC2,DC1,1));
294 iC1 = _mm_sub_pd(iC1, _mm_mul_pd(_mm_shuffle_pd(A1,A1,1), _mm_shuffle_pd(DC2,DC1,2)));
295 iC2 = _mm_sub_pd(iC2, _mm_mul_pd(_mm_shuffle_pd(A2,A2,1), _mm_shuffle_pd(DC2,DC1,2)));
297 rd = _mm_div_sd(_mm_set_sd(1.0), det);
301 rd = _mm_shuffle_pd(rd,rd,0);
304 dB = _mm_shuffle_pd(dB,dB,0);
305 iB1 = _mm_sub_pd(_mm_mul_pd(C1, dB), iB1);
306 iB2 = _mm_sub_pd(_mm_mul_pd(C2, dB), iB2);
308 d1 = _mm_xor_pd(rd, _Sign_PN);
309 d2 = _mm_xor_pd(rd, _Sign_NP);
312 dC = _mm_shuffle_pd(dC,dC,0);
313 iC1 = _mm_sub_pd(_mm_mul_pd(B1, dC), iC1);
314 iC2 = _mm_sub_pd(_mm_mul_pd(B2, dC), iC2);
316 DenseIndex res_stride = result.outerStride();
317 double* res = result.data();
318 pstoret<double, Packet2d, ResultAlignment>(res+0, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1));
319 pstoret<double, Packet2d, ResultAlignment>(res+res_stride, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2));
320 pstoret<double, Packet2d, ResultAlignment>(res+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1));
321 pstoret<double, Packet2d, ResultAlignment>(res+res_stride+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2));
322 pstoret<double, Packet2d, ResultAlignment>(res+2*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1));
323 pstoret<double, Packet2d, ResultAlignment>(res+3*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2));
324 pstoret<double, Packet2d, ResultAlignment>(res+2*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1));
325 pstoret<double, Packet2d, ResultAlignment>(res+3*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2));
333 #endif // EIGEN_INVERSE_SSE_H
Definition: Eigen_Colamd.h:50
const unsigned int RowMajorBit
Definition: Constants.h:53
const unsigned int AlignedBit
Definition: Constants.h:147