Debian Codegraph @

.deprecated/simd/math_simd_details.h

raw
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38
39 #ifndef FCL_MATH_SIMD_DETAILS_H
40 #define FCL_MATH_SIMD_DETAILS_H
41
42 #include "fcl/common/data_types.h"
43
44 #include <xmmintrin.h>
45 #if defined (__SSE3__)
46 #include <pmmintrin.h>
47 #endif
48 #if defined (__SSE4__)
49 #include <smmintrin.h>
50 #endif
51
52
53 namespace fcl
54 {
55
56 /** @brief FCL internals. Ignore this :) unless you are God */
57 namespace detail
58 {
59
60 const __m128 xmms_0 = {0.f, 0.f, 0.f, 0.f};
61 const __m128d xmmd_0 = {0, 0};
62
63 static inline __m128 vec_sel(__m128 a, __m128 b, __m128 mask)
64 {
65 return _mm_or_ps(_mm_and_ps(mask, b), _mm_andnot_ps(mask, a));
66 }
67 static inline __m128 vec_sel(__m128 a, __m128 b, const unsigned int* mask)
68 {
69 return vec_sel(a, b, _mm_load_ps((float*)mask));
70 }
71
72 static inline __m128 vec_sel(__m128 a, __m128 b, unsigned int mask)
73 {
74 return vec_sel(a, b, _mm_set1_ps(*(float*)&mask));
75 }
76
77 #define vec_splat(a, e) _mm_shuffle_ps((a), (a), _MM_SHUFFLE((e), (e), (e), (e)))
78 #define vec_splatd(a, e) _mm_shuffle_pd((a), (a), _MM_SHUFFLE2((e), (e)))
79
80 #define _mm_ror_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE(((e)+3)%4, ((e)+2)%4, ((e)+1)%4, (e)%4)) : (x))
81
82 #define _mm_rol_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE((7-(e))%4, (6-(e))%4, (5-(e))%4, (4-(e))%4)) : (x))
83
84 static inline __m128 newtonraphson_rsqrt4(const __m128 v)
85 {
86 static const union { float i[4]; __m128 m; } _half4 __attribute__ ((aligned(16))) = {{.5f, .5f, .5f, .5f}};
87 static const union { float i[4]; __m128 m; } _three __attribute__ ((aligned(16))) = {{3.f, 3.f, 3.f, 3.f}};
88 __m128 approx = _mm_rsqrt_ps(v);
89 __m128 muls = _mm_mul_ps(_mm_mul_ps(v, approx), approx);
90 return _mm_mul_ps(_mm_mul_ps(_half4.m, approx), _mm_sub_ps(_three.m, muls));
91 }
92
93 struct sse_meta_f4
94 {
95 typedef float meta_type;
96
97 union {float vs[4]; __m128 v; };
98 sse_meta_f4() : v(_mm_set1_ps(0)) {}
99 sse_meta_f4(float x) : v(_mm_set1_ps(x)) {}
100 sse_meta_f4(float* px) : v(_mm_load_ps(px)) {}
101 sse_meta_f4(__m128 x) : v(x) {}
102 sse_meta_f4(float x, float y, float z, float w = 1) : v(_mm_setr_ps(x, y, z, w)) {}
103 inline void setValue(float x, float y, float z, float w = 1) { v = _mm_setr_ps(x, y, z, w); }
104 inline void setValue(float x) { v = _mm_set1_ps(x); }
105 inline void setValue(__m128 x) { v = x; }
106 inline void negate() { v = _mm_sub_ps(xmms_0, v); }
107
108 inline sse_meta_f4& ubound(const sse_meta_f4& u)
109 {
110 v = _mm_min_ps(v, u.v);
111 return *this;
112 }
113
114 inline sse_meta_f4& lbound(const sse_meta_f4& l)
115 {
116 v = _mm_max_ps(v, l.v);
117 return *this;
118 }
119
120 inline void* operator new [] (size_t n) { return _mm_malloc(n, 16); }
121 inline void operator delete [] (void* x) { if(x) _mm_free(x); }
122 inline float operator [] (size_t i) const { return vs[i]; }
123 inline float& operator [] (size_t i) { return vs[i]; }
124
125 inline sse_meta_f4 operator + (const sse_meta_f4& other) const { return sse_meta_f4(_mm_add_ps(v, other.v)); }
126 inline sse_meta_f4 operator - (const sse_meta_f4& other) const { return sse_meta_f4(_mm_sub_ps(v, other.v)); }
127 inline sse_meta_f4 operator * (const sse_meta_f4& other) const { return sse_meta_f4(_mm_mul_ps(v, other.v)); }
128 inline sse_meta_f4 operator / (const sse_meta_f4& other) const { return sse_meta_f4(_mm_div_ps(v, other.v)); }
129 inline sse_meta_f4& operator += (const sse_meta_f4& other) { v = _mm_add_ps(v, other.v); return *this; }
130 inline sse_meta_f4& operator -= (const sse_meta_f4& other) { v = _mm_sub_ps(v, other.v); return *this; }
131 inline sse_meta_f4& operator *= (const sse_meta_f4& other) { v = _mm_mul_ps(v, other.v); return *this; }
132 inline sse_meta_f4& operator /= (const sse_meta_f4& other) { v = _mm_div_ps(v, other.v); return *this; }
133 inline sse_meta_f4 operator + (float t) const { return sse_meta_f4(_mm_add_ps(v, _mm_set1_ps(t))); }
134 inline sse_meta_f4 operator - (float t) const { return sse_meta_f4(_mm_sub_ps(v, _mm_set1_ps(t))); }
135 inline sse_meta_f4 operator * (float t) const { return sse_meta_f4(_mm_mul_ps(v, _mm_set1_ps(t))); }
136 inline sse_meta_f4 operator / (float t) const { return sse_meta_f4(_mm_div_ps(v, _mm_set1_ps(t))); }
137 inline sse_meta_f4& operator += (float t) { v = _mm_add_ps(v, _mm_set1_ps(t)); return *this; }
138 inline sse_meta_f4& operator -= (float t) { v = _mm_sub_ps(v, _mm_set1_ps(t)); return *this; }
139 inline sse_meta_f4& operator *= (float t) { v = _mm_mul_ps(v, _mm_set1_ps(t)); return *this; }
140 inline sse_meta_f4& operator /= (float t) { v = _mm_div_ps(v, _mm_set1_ps(t)); return *this; }
141 inline sse_meta_f4 operator - () const
142 {
143 static const union { int i[4]; __m128 m; } negativemask __attribute__ ((aligned(16))) = {{0x80000000, 0x80000000, 0x80000000, 0x80000000}};
144 return sse_meta_f4(_mm_xor_ps(negativemask.m, v));
145 }
146 } __attribute__ ((aligned (16)));
147
148 struct sse_meta_d4
149 {
150 typedef double meta_type;
151
152 union {double vs[4]; __m128d v[2]; };
153 sse_meta_d4()
154 {
155 setValue(0.0);
156 }
157
158 sse_meta_d4(double x)
159 {
160 setValue(x);
161 }
162
163 sse_meta_d4(double* px)
164 {
165 v[0] = _mm_load_pd(px);
166 v[1] = _mm_set_pd(0, *(px + 2));
167 }
168
169 sse_meta_d4(__m128d x, __m128d y)
170 {
171 v[0] = x;
172 v[1] = y;
173 }
174
175 sse_meta_d4(double x, double y, double z, double w = 0)
176 {
177 setValue(x, y, z, w);
178 }
179
180 inline void setValue(double x, double y, double z, double w = 0)
181 {
182 v[0] = _mm_setr_pd(x, y);
183 v[1] = _mm_setr_pd(z, w);
184 }
185
186 inline void setValue(double x)
187 {
188 v[0] = _mm_set1_pd(x);
189 v[1] = v[0];
190 }
191
192 inline void setValue(__m128d x, __m128d y)
193 {
194 v[0] = x;
195 v[1] = y;
196 }
197
198 inline void negate()
199 {
200 v[0] = _mm_sub_pd(xmmd_0, v[0]);
201 v[1] = _mm_sub_pd(xmmd_0, v[1]);
202 }
203
204 inline sse_meta_d4& ubound(const sse_meta_d4& u)
205 {
206 v[0] = _mm_min_pd(v[0], u.v[0]);
207 v[1] = _mm_min_pd(v[1], u.v[1]);
208 return *this;
209 }
210
211 inline sse_meta_d4& lbound(const sse_meta_d4& l)
212 {
213 v[0] = _mm_max_pd(v[0], l.v[0]);
214 v[1] = _mm_max_pd(v[1], l.v[1]);
215 return *this;
216 }
217
218 inline void* operator new [] (size_t n)
219 {
220 return _mm_malloc(n, 16);
221 }
222
223 inline void operator delete [] (void* x)
224 {
225 if(x) _mm_free(x);
226 }
227
228 inline double operator [] (size_t i) const { return vs[i]; }
229 inline double& operator [] (size_t i) { return vs[i]; }
230
231 inline sse_meta_d4 operator + (const sse_meta_d4& other) const { return sse_meta_d4(_mm_add_pd(v[0], other.v[0]), _mm_add_pd(v[1], other.v[1])); }
232 inline sse_meta_d4 operator - (const sse_meta_d4& other) const { return sse_meta_d4(_mm_sub_pd(v[0], other.v[0]), _mm_sub_pd(v[1], other.v[1])); }
233 inline sse_meta_d4 operator * (const sse_meta_d4& other) const { return sse_meta_d4(_mm_mul_pd(v[0], other.v[0]), _mm_mul_pd(v[1], other.v[1])); }
234 inline sse_meta_d4 operator / (const sse_meta_d4& other) const { return sse_meta_d4(_mm_div_pd(v[0], other.v[0]), _mm_div_pd(v[1], other.v[1])); }
235 inline sse_meta_d4& operator += (const sse_meta_d4& other) { v[0] = _mm_add_pd(v[0], other.v[0]); v[1] = _mm_add_pd(v[1], other.v[1]); return *this; }
236 inline sse_meta_d4& operator -= (const sse_meta_d4& other) { v[0] = _mm_sub_pd(v[0], other.v[0]); v[1] = _mm_sub_pd(v[1], other.v[1]); return *this; }
237 inline sse_meta_d4& operator *= (const sse_meta_d4& other) { v[0] = _mm_mul_pd(v[0], other.v[0]); v[1] = _mm_mul_pd(v[1], other.v[1]); return *this; }
238 inline sse_meta_d4& operator /= (const sse_meta_d4& other) { v[0] = _mm_div_pd(v[0], other.v[0]); v[1] = _mm_div_pd(v[1], other.v[1]); return *this; }
239 inline sse_meta_d4 operator + (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_add_pd(v[0], d), _mm_add_pd(v[1], d)); }
240 inline sse_meta_d4 operator - (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_sub_pd(v[0], d), _mm_sub_pd(v[1], d)); }
241 inline sse_meta_d4 operator * (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_mul_pd(v[0], d), _mm_mul_pd(v[1], d)); }
242 inline sse_meta_d4 operator / (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_div_pd(v[0], d), _mm_div_pd(v[1], d)); }
243 inline sse_meta_d4& operator += (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_add_pd(v[0], d); v[1] = _mm_add_pd(v[1], d); return *this; }
244 inline sse_meta_d4& operator -= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_sub_pd(v[0], d); v[1] = _mm_sub_pd(v[1], d); return *this; }
245 inline sse_meta_d4& operator *= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_mul_pd(v[0], d); v[1] = _mm_mul_pd(v[1], d); return *this; }
246 inline sse_meta_d4& operator /= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_div_pd(v[0], d); v[1] = _mm_div_pd(v[1], d); return *this; }
247 inline sse_meta_d4 operator - () const
248 {
249 static const union { FCL_INT64 i[2]; __m128d m; } negativemask __attribute__ ((aligned(16))) = {{0x8000000000000000, 0x8000000000000000}};
250 return sse_meta_d4(_mm_xor_pd(v[0], negativemask.m), _mm_xor_pd(v[1], negativemask.m));
251 }
252 } __attribute__ ((aligned (16)));
253
254
255
256 static inline __m128 cross_prod(__m128 x, __m128 y)
257 {
258 // set to a[1][2][0][3] , b[2][0][1][3]
259 // multiply
260 static const int s1 = _MM_SHUFFLE(3, 0, 2, 1);
261 static const int s2 = _MM_SHUFFLE(3, 1, 0, 2);
262 __m128 xa = _mm_mul_ps(_mm_shuffle_ps(x, x, s1), _mm_shuffle_ps(y, y, s2));
263
264 // set to a[2][0][1][3] , b[1][2][0][3]
265 // multiply
266 __m128 xb = _mm_mul_ps(_mm_shuffle_ps(x, x, s2), _mm_shuffle_ps(y, y, s1));
267
268 // subtract
269 return _mm_sub_ps(xa, xb);
270 }
271
272 static inline sse_meta_f4 cross_prod(const sse_meta_f4& x, const sse_meta_f4& y)
273 {
274 return sse_meta_f4(cross_prod(x.v, y.v));
275 }
276
277 static inline void cross_prod(__m128d x0, __m128d x1, __m128d y0, __m128d y1, __m128d* z0, __m128d* z1)
278 {
279 static const int s0 = _MM_SHUFFLE2(0, 0);
280 static const int s1 = _MM_SHUFFLE2(0, 1);
281 static const int s2 = _MM_SHUFFLE2(1, 0);
282 static const int s3 = _MM_SHUFFLE2(1, 1);
283 __m128d xa1 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s1), _mm_shuffle_pd(y1, y0, s0));
284 __m128d ya1 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s2), _mm_shuffle_pd(y0, y1, s3));
285
286 __m128d xa2 = _mm_mul_pd(_mm_shuffle_pd(x1, x0, s0), _mm_shuffle_pd(y0, y1, s1));
287 __m128d ya2 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s3), _mm_shuffle_pd(y0, y1, s2));
288
289 *z0 = _mm_sub_pd(xa1, xa2);
290 *z1 = _mm_sub_pd(ya1, ya2);
291 }
292
293 static inline sse_meta_d4 cross_prod(const sse_meta_d4& x, const sse_meta_d4& y)
294 {
295 __m128d z0, z1;
296 cross_prod(x.v[0], x.v[1], y.v[0], y.v[1], &z0, &z1);
297 return sse_meta_d4(z0, z1);
298 }
299
300
301 static inline __m128 dot_prod3(__m128 x, __m128 y)
302 {
303 register __m128 m = _mm_mul_ps(x, y);
304 return _mm_add_ps(_mm_shuffle_ps(m, m, _MM_SHUFFLE(0, 0, 0, 0)),
305 _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2)));
306 }
307
308 static inline float dot_prod3(const sse_meta_f4& x, const sse_meta_f4& y)
309 {
310 return _mm_cvtss_f32(dot_prod3(x.v, y.v));
311 }
312
313
314 static inline __m128d dot_prod3(__m128d x0, __m128d x1, __m128d y0, __m128d y1)
315 {
316 register __m128d m1 = _mm_mul_pd(x0, y0);
317 register __m128d m2 = _mm_mul_pd(x1, y1);
318 return _mm_add_pd(_mm_add_pd(vec_splatd(m1, 0), vec_splatd(m1, 1)), vec_splatd(m2, 0));
319 }
320
321 static inline double dot_prod3(const sse_meta_d4& x, const sse_meta_d4& y)
322 {
323 double d;
324 _mm_storel_pd(&d, dot_prod3(x.v[0], x.v[1], y.v[0], y.v[1]));
325 return d;
326 }
327
328 static inline __m128 dot_prod4(__m128 x, __m128 y)
329 {
330 #if defined (__SSE4__)
331 return _mm_dp_ps(x, y, 0x71);
332 #elif defined (__SSE3__)
333 register __m128 t = _mm_mul_ps(x, y);
334 t = _mm_hadd_ps(t, t);
335 return _mm_hadd_ps(t, t);
336 #else
337 register __m128 s = _mm_mul_ps(x, y);
338 register __m128 r = _mm_add_ss(s, _mm_movehl_ps(s, s));
339 return _mm_add_ss(r, _mm_shuffle_ps(r, r, 1));
340 #endif
341 }
342
343
344 static inline float dot_prod4(const sse_meta_f4& x, const sse_meta_f4& y)
345 {
346 return _mm_cvtss_f32(dot_prod4(x.v, y.v));
347 }
348
349 static inline __m128d dot_prod4(__m128d x0, __m128d x1, __m128d y0, __m128d y1)
350 {
351 #if defined (__SSE4__)
352 register __m128d t1 = _mm_dp_pd(x0, y0, 0x31);
353 register __m128d t2 = _mm_dp_pd(x1, y1, 0x11);
354 return _mm_add_pd(t1, t2);
355 #elif defined (__SSE3__)
356 register __m128d t1 = _mm_mul_pd(x0, y0);
357 register __m128d t2 = _mm_mul_pd(x1, y1);
358 t1 = _mm_hadd_pd(t1, t1);
359 t2 = _mm_hadd_pd(t2, t2);
360 return _mm_add_pd(t1, t2);
361 #else
362 register __m128d t1 = _mm_mul_pd(x0, y0);
363 register __m128d t2 = _mm_mul_pd(x1, y1);
364 t1 = _mm_add_pd(t1, t2);
365 return _mm_add_pd(t1, _mm_shuffle_pd(t1, t1, 1));
366 #endif
367 }
368
369 static inline double dot_prod4(const sse_meta_d4& x, const sse_meta_d4& y)
370 {
371 double d;
372 _mm_storel_pd(&d, dot_prod4(x.v[0], x.v[1], y.v[0], y.v[1]));
373 return d;
374 }
375
376 static inline sse_meta_f4 min(const sse_meta_f4& x, const sse_meta_f4& y)
377 {
378 return sse_meta_f4(_mm_min_ps(x.v, y.v));
379 }
380
381 static inline sse_meta_d4 min(const sse_meta_d4& x, const sse_meta_d4& y)
382 {
383 return sse_meta_d4(_mm_min_pd(x.v[0], y.v[0]), _mm_min_pd(x.v[1], y.v[1]));
384 }
385
386 static inline sse_meta_f4 max(const sse_meta_f4& x, const sse_meta_f4& y)
387 {
388 return sse_meta_f4(_mm_max_ps(x.v, y.v));
389 }
390
391 static inline sse_meta_d4 max(const sse_meta_d4& x, const sse_meta_d4& y)
392 {
393 return sse_meta_d4(_mm_max_pd(x.v[0], y.v[0]), _mm_max_pd(x.v[1], y.v[1]));
394 }
395
396 static inline sse_meta_f4 abs(const sse_meta_f4& x)
397 {
398 static const union { int i[4]; __m128 m; } abs4mask __attribute__ ((aligned (16))) = {{0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff}};
399 return sse_meta_f4(_mm_and_ps(x.v, abs4mask.m));
400 }
401
402 static inline sse_meta_d4 abs(const sse_meta_d4& x)
403 {
404 static const union { FCL_INT64 i[2]; __m128d m; } abs2mask __attribute__ ((aligned (16))) = {{0x7fffffffffffffff, 0x7fffffffffffffff}};
405 return sse_meta_d4(_mm_and_pd(x.v[0], abs2mask.m), _mm_and_pd(x.v[1], abs2mask.m));
406 }
407
408 static inline bool equal(const sse_meta_f4& x, const sse_meta_f4& y, float epsilon)
409 {
410 register __m128 d = _mm_sub_ps(x.v, y.v);
411 register __m128 e = _mm_set1_ps(epsilon);
412 return ((_mm_movemask_ps(_mm_cmplt_ps(d, e)) & 0x7) == 0x7) && ((_mm_movemask_ps(_mm_cmpgt_ps(d, _mm_sub_ps(xmms_0, e))) & 0x7) == 0x7);
413 }
414
415 static inline bool equal(const sse_meta_d4& x, const sse_meta_d4& y, double epsilon)
416 {
417 register __m128d d = _mm_sub_pd(x.v[0], y.v[0]);
418 register __m128d e = _mm_set1_pd(epsilon);
419
420 if(_mm_movemask_pd(_mm_cmplt_pd(d, e)) != 0x3) return false;
421 if(_mm_movemask_pd(_mm_cmpgt_pd(d, _mm_sub_pd(xmmd_0, e))) != 0x3) return false;
422
423 d = _mm_sub_pd(x.v[1], y.v[1]);
424 if((_mm_movemask_pd(_mm_cmplt_pd(d, e)) & 0x1) != 0x1) return false;
425 if((_mm_movemask_pd(_mm_cmpgt_pd(d, _mm_sub_pd(xmmd_0, e))) & 0x1) != 0x1) return false;
426 return true;
427 }
428
429 static inline sse_meta_f4 normalize3(const sse_meta_f4& x)
430 {
431 register __m128 m = _mm_mul_ps(x.v, x.v);
432 __m128 r = _mm_add_ps(vec_splat(m, 0), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2)));
433 return sse_meta_f4(_mm_mul_ps(x.v, newtonraphson_rsqrt4(r)));
434 }
435
436 static inline sse_meta_f4 normalize3_approx(const sse_meta_f4& x)
437 {
438 register __m128 m = _mm_mul_ps(x.v, x.v);
439 __m128 r = _mm_add_ps(vec_splat(m, 0), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2)));
440 return sse_meta_f4(_mm_mul_ps(x.v, _mm_rsqrt_ps(r)));
441 }
442
443
444 static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128* r0, __m128* r1, __m128* r2)
445 {
446 static const union { unsigned int i[4]; __m128 m; } selectmask __attribute__ ((aligned(16))) = {{0, 0xffffffff, 0, 0}};
447 register __m128 t0, t1;
448 t0 = _mm_unpacklo_ps(c0, c2);
449 t1 = _mm_unpackhi_ps(c0, c2);
450 *r0 = _mm_unpacklo_ps(t0, c1);
451 *r1 = _mm_shuffle_ps(t0, t0, _MM_SHUFFLE(0, 3, 2, 2));
452 *r1 = vec_sel(*r1, c1, selectmask.i);
453 *r2 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(0, 1, 1, 0));
454 *r2 = vec_sel(*r2, vec_splat(c1, 2), selectmask.i);
455 }
456
457
458 static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128* i0, __m128* i1, __m128* i2)
459 {
460 __m128 t0, t1, t2, d, invd;
461 t2 = cross_prod(c0, c1);
462 t0 = cross_prod(c1, c2);
463 t1 = cross_prod(c2, c0);
464 d = dot_prod3(t2, c2);
465 d = vec_splat(d, 0);
466 invd = _mm_rcp_ps(d); // approximate inverse
467 transpose(t0, t1, t2, i0, i1, i2);
468 *i0 = _mm_mul_ps(*i0, invd);
469 *i1 = _mm_mul_ps(*i1, invd);
470 *i2 = _mm_mul_ps(*i2, invd);
471 }
472
473
474 struct sse_meta_f12
475 {
476 typedef float meta_type;
477 typedef sse_meta_f4 vector_type;
478 sse_meta_f4 c[3];
479
480 sse_meta_f12() { setZero(); }
481
482 sse_meta_f12(float xx, float xy, float xz,
483 float yx, float yy, float yz,
484 float zx, float zy, float zz)
485 { setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); }
486
487 sse_meta_f12(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z)
488 { setColumn(x, y, z); }
489
490 sse_meta_f12(__m128 x, __m128 y, __m128 z)
491 { setColumn(x, y, z); }
492
493 inline void setValue(float xx, float xy, float xz,
494 float yx, float yy, float yz,
495 float zx, float zy, float zz)
496 {
497 c[0].setValue(xx, yx, zx, 0);
498 c[1].setValue(xy, yy, zy, 0);
499 c[2].setValue(xz, yz, zz, 0);
500 }
501
502 inline void setIdentity()
503 {
504 c[0].setValue(1, 0, 0, 0);
505 c[1].setValue(0, 1, 0, 0);
506 c[2].setValue(0, 0, 1, 0);
507 }
508
509 inline void setZero()
510 {
511 c[0].setValue(0);
512 c[1].setValue(0);
513 c[2].setValue(0);
514 }
515
516 inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z)
517 {
518 c[0] = x; c[1] = y; c[2] = z;
519 }
520
521 inline void setColumn(__m128 x, __m128 y, __m128 z)
522 {
523 c[0].setValue(x); c[1].setValue(y); c[2].setValue(z);
524 }
525
526 inline const sse_meta_f4& getColumn(size_t i) const
527 {
528 return c[i];
529 }
530
531 inline sse_meta_f4& getColumn(size_t i)
532 {
533 return c[i];
534 }
535
536 inline sse_meta_f4 getRow(size_t i) const
537 {
538 return sse_meta_f4(c[0][i], c[1][i], c[2][i], 0);
539 }
540
541 inline float operator () (size_t i, size_t j) const
542 {
543 return c[j][i];
544 }
545
546 inline float& operator () (size_t i, size_t j)
547 {
548 return c[j][i];
549 }
550
551 inline sse_meta_f4 operator * (const sse_meta_f4& v) const
552 {
553 return sse_meta_f4(_mm_add_ps(_mm_add_ps(_mm_mul_ps(c[0].v, vec_splat(v.v, 0)), _mm_mul_ps(c[1].v, vec_splat(v.v, 1))), _mm_mul_ps(c[2].v, vec_splat(v.v, 2))));
554 }
555
556 inline sse_meta_f12 operator * (const sse_meta_f12& mat) const
557 {
558 return sse_meta_f12((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]);
559 }
560
561 inline sse_meta_f12 operator + (const sse_meta_f12& mat) const
562 {
563 return sse_meta_f12(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2]);
564 }
565
566 inline sse_meta_f12 operator - (const sse_meta_f12& mat) const
567 {
568 return sse_meta_f12(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2]);
569 }
570
571 inline sse_meta_f12 operator + (float t_) const
572 {
573 sse_meta_f4 t(t_);
574 return sse_meta_f12(c[0] + t, c[1] + t, c[2] + t);
575 }
576
577 inline sse_meta_f12 operator - (float t_) const
578 {
579 sse_meta_f4 t(t_);
580 return sse_meta_f12(c[0] - t, c[1] - t, c[2] - t);
581 }
582
583 inline sse_meta_f12 operator * (float t_) const
584 {
585 sse_meta_f4 t(t_);
586 return sse_meta_f12(c[0] * t, c[1] * t, c[2] * t);
587 }
588
589 inline sse_meta_f12 operator / (float t_) const
590 {
591 sse_meta_f4 t(t_);
592 return sse_meta_f12(c[0] / t, c[1] / t, c[2] / t);
593 }
594
595 inline sse_meta_f12& operator *= (const sse_meta_f12& mat)
596 {
597 setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]);
598 return *this;
599 }
600
601 inline sse_meta_f12& operator += (const sse_meta_f12& mat)
602 {
603 c[0] += mat.c[0];
604 c[1] += mat.c[1];
605 c[2] += mat.c[2];
606 return *this;
607 }
608
609 inline sse_meta_f12& operator -= (const sse_meta_f12& mat)
610 {
611 c[0] -= mat.c[0];
612 c[1] -= mat.c[1];
613 c[2] -= mat.c[2];
614 return *this;
615 }
616
617 inline sse_meta_f12& operator += (float t_)
618 {
619 sse_meta_f4 t(t_);
620 c[0] += t;
621 c[1] += t;
622 c[2] += t;
623 return *this;
624 }
625
626 inline sse_meta_f12& operator -= (float t_)
627 {
628 sse_meta_f4 t(t_);
629 c[0] -= t;
630 c[1] -= t;
631 c[2] -= t;
632 return *this;
633 }
634
635 inline sse_meta_f12& operator *= (float t_)
636 {
637 sse_meta_f4 t(t_);
638 c[0] *= t;
639 c[1] *= t;
640 c[2] *= t;
641 return *this;
642 }
643
644 inline sse_meta_f12& operator /= (float t_)
645 {
646 sse_meta_f4 t(t_);
647 c[0] /= t;
648 c[1] /= t;
649 c[2] /= t;
650 return *this;
651 }
652
653 inline sse_meta_f12& inverse()
654 {
655 __m128 inv0, inv1, inv2;
656 detail::inverse(c[0].v, c[1].v, c[2].v, &inv0, &inv1, &inv2);
657 setColumn(inv0, inv1, inv2);
658 return *this;
659 }
660
661 inline sse_meta_f12& transpose()
662 {
663 __m128 r0, r1, r2;
664 detail::transpose(c[0].v, c[1].v, c[2].v, &r0, &r1, &r2);
665 setColumn(r0, r1, r2);
666 return *this;
667 }
668
669 inline sse_meta_f12& abs()
670 {
671 c[0] = detail::abs(c[0]);
672 c[1] = detail::abs(c[1]);
673 c[2] = detail::abs(c[2]);
674 return *this;
675 }
676
677 inline float determinant() const
678 {
679 return _mm_cvtss_f32(dot_prod3(c[2].v, cross_prod(c[0].v, c[1].v)));
680 }
681
682 inline sse_meta_f12 transposeTimes(const sse_meta_f12& other) const
683 {
684 return sse_meta_f12(dot_prod3(c[0], other.c[0]), dot_prod3(c[0], other.c[1]), dot_prod3(c[0], other.c[2]),
685 dot_prod3(c[1], other.c[0]), dot_prod3(c[1], other.c[1]), dot_prod3(c[1], other.c[2]),
686 dot_prod3(c[2], other.c[0]), dot_prod3(c[2], other.c[1]), dot_prod3(c[2], other.c[2]));
687 }
688
689 inline sse_meta_f12 timesTranspose(const sse_meta_f12& m) const
690 {
691 sse_meta_f12 tmp(m);
692 return (*this) * tmp.transpose();
693 }
694
695 inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const
696 {
697 return sse_meta_f4(dot_prod3(c[0], v), dot_prod3(c[1], v), dot_prod3(c[2], v));
698 }
699
700 inline float transposeDot(size_t i, const sse_meta_f4& v) const
701 {
702 return dot_prod3(c[i], v);
703 }
704
705 inline float dot(size_t i, const sse_meta_f4& v) const
706 {
707 return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i];
708 }
709
710 };
711
712 static inline sse_meta_f12 abs(const sse_meta_f12& mat)
713 {
714 return sse_meta_f12(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)));
715 }
716
717 static inline sse_meta_f12 transpose(const sse_meta_f12& mat)
718 {
719 __m128 r0, r1, r2;
720 transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, &r0, &r1, &r2);
721 return sse_meta_f12(r0, r1, r2);
722 }
723
724
725 static inline sse_meta_f12 inverse(const sse_meta_f12& mat)
726 {
727 __m128 inv0, inv1, inv2;
728 inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, &inv0, &inv1, &inv2);
729 return sse_meta_f12(inv0, inv1, inv2);
730 }
731
732
733 static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128 c3,
734 __m128* r0, __m128* r1, __m128* r2, __m128* r3)
735 {
736 __m128 tmp0 = _mm_unpacklo_ps(c0, c2);
737 __m128 tmp1 = _mm_unpacklo_ps(c1, c3);
738 __m128 tmp2 = _mm_unpackhi_ps(c0, c2);
739 __m128 tmp3 = _mm_unpackhi_ps(c1, c3);
740 *r0 = _mm_unpacklo_ps(tmp0, tmp1);
741 *r1 = _mm_unpackhi_ps(tmp0, tmp1);
742 *r2 = _mm_unpacklo_ps(tmp2, tmp3);
743 *r3 = _mm_unpackhi_ps(tmp2, tmp3);
744 }
745
746
747 static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128 c3,
748 __m128* res0, __m128* res1, __m128* res2, __m128* res3)
749 {
750 __m128 Va, Vb, Vc;
751 __m128 r1, r2, r3, tt, tt2;
752 __m128 sum, Det, RDet;
753 __m128 trns0, trns1, trns2, trns3;
754
755 // Calculating the minterms for the first line.
756
757 tt = c3; tt2 = _mm_ror_ps(c2,1);
758 Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'\B7V4
759 Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'\B7V4"
760 Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'\B7V4^
761
762 r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"\B7V4^ - V3^\B7V4"
763 r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^\B7V4' - V3'\B7V4^
764 r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'\B7V4" - V3"\B7V4'
765
766 tt = c1;
767 Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
768 Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
769 Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
770
771 // Calculating the determinant.
772 Det = _mm_mul_ps(sum,c0);
773 Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
774
775 static const union { int i[4]; __m128 m; } Sign_PNPN __attribute__ ((aligned(16))) = {{0x00000000, 0x80000000, 0x00000000, 0x80000000}};
776 static const union { int i[4]; __m128 m; } Sign_NPNP __attribute__ ((aligned(16))) = {{0x80000000, 0x00000000, 0x80000000, 0x00000000}};
777 static const union { float i[4]; __m128 m; } ZERONE __attribute__ ((aligned(16))) = {{1.0f, 0.0f, 0.0f, 1.0f}};
778
779 __m128 mtL1 = _mm_xor_ps(sum,Sign_PNPN.m);
780
781 // Calculating the minterms of the second line (using previous results).
782 tt = _mm_ror_ps(c0,1); sum = _mm_mul_ps(tt,r1);
783 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
784 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
785 __m128 mtL2 = _mm_xor_ps(sum,Sign_NPNP.m);
786
787 // Testing the determinant.
788 Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
789
790 // Calculating the minterms of the third line.
791 tt = _mm_ror_ps(c0,1);
792 Va = _mm_mul_ps(tt,Vb); // V1'\B7V2"
793 Vb = _mm_mul_ps(tt,Vc); // V1'\B7V2^
794 Vc = _mm_mul_ps(tt,c1); // V1'\B7V2
795
796 r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V1"\B7V2^ - V1^\B7V2"
797 r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V1^\B7V2' - V1'\B7V2^
798 r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V1'\B7V2" - V1"\B7V2'
799
800 tt = _mm_ror_ps(c3,1); sum = _mm_mul_ps(tt,r1);
801 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
802 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
803 __m128 mtL3 = _mm_xor_ps(sum,Sign_PNPN.m);
804
805 // Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs).
806 RDet = _mm_div_ss(ZERONE.m, Det); // TODO: just 1.0f?
807 RDet = _mm_shuffle_ps(RDet,RDet,0x00);
808
809 // Devide the first 12 minterms with the determinant.
810 mtL1 = _mm_mul_ps(mtL1, RDet);
811 mtL2 = _mm_mul_ps(mtL2, RDet);
812 mtL3 = _mm_mul_ps(mtL3, RDet);
813
814 // Calculate the minterms of the forth line and devide by the determinant.
815 tt = _mm_ror_ps(c2,1); sum = _mm_mul_ps(tt,r1);
816 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
817 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
818 __m128 mtL4 = _mm_xor_ps(sum,Sign_NPNP.m);
819 mtL4 = _mm_mul_ps(mtL4, RDet);
820
821 // Now we just have to transpose the minterms matrix.
822 trns0 = _mm_unpacklo_ps(mtL1,mtL2);
823 trns1 = _mm_unpacklo_ps(mtL3,mtL4);
824 trns2 = _mm_unpackhi_ps(mtL1,mtL2);
825 trns3 = _mm_unpackhi_ps(mtL3,mtL4);
826 *res0 = _mm_movelh_ps(trns0,trns1);
827 *res1 = _mm_movehl_ps(trns1,trns0);
828 *res2 = _mm_movelh_ps(trns2,trns3);
829 *res3 = _mm_movehl_ps(trns3,trns2);
830 }
831
832
833 struct sse_meta_f16
834 {
835 typedef float meta_type;
836 typedef sse_meta_f4 vector_type;
837 sse_meta_f4 c[4];
838
839 sse_meta_f16() { setZero(); }
840
841 sse_meta_f16(float xx, float xy, float xz, float xw,
842 float yx, float yy, float yz, float yw,
843 float zx, float zy, float zz, float zw,
844 float wx, float wy, float wz, float ww)
845 { setValue(xx, xy, xz, xw, yz, yy, yz, yw, zx, zy, zz, zw, wx, wy, wz, ww); }
846
847 sse_meta_f16(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z, const sse_meta_f4& w)
848 { setColumn(x, y, z, w); }
849
850 sse_meta_f16(__m128 x, __m128 y, __m128 z, __m128 w)
851 { setColumn(x, y, z, w); }
852
853 inline void setValue(float xx, float xy, float xz, float xw,
854 float yx, float yy, float yz, float yw,
855 float zx, float zy, float zz, float zw,
856 float wx, float wy, float wz, float ww)
857 {
858 c[0].setValue(xx, yx, zx, wx);
859 c[1].setValue(xy, yy, zy, wy);
860 c[2].setValue(xz, yz, zz, wz);
861 c[3].setValue(xw, yw, zw, ww);
862 }
863
864 inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z, const sse_meta_f4& w)
865 {
866 c[0] = x; c[1] = y; c[2] = z; c[3] = w;
867 }
868
869 inline void setColumn(__m128 x, __m128 y, __m128 z, __m128 w)
870 {
871 c[0].setValue(x); c[1].setValue(y); c[2].setValue(z); c[3].setValue(w);
872 }
873
874 inline void setIdentity()
875 {
876 c[0].setValue(1, 0, 0, 0);
877 c[1].setValue(0, 1, 0, 0);
878 c[2].setValue(0, 0, 1, 0);
879 c[3].setValue(0, 0, 0, 1);
880 }
881
882 inline void setZero()
883 {
884 c[0].setValue(0);
885 c[1].setValue(0);
886 c[2].setValue(0);
887 c[3].setValue(0);
888 }
889
890 inline const sse_meta_f4& getColumn(size_t i) const
891 {
892 return c[i];
893 }
894
895 inline sse_meta_f4& getColumn(size_t i)
896 {
897 return c[i];
898 }
899
900 inline sse_meta_f4 getRow(size_t i) const
901 {
902 return sse_meta_f4(c[0][i], c[1][i], c[2][i], c[3][i]);
903 }
904
905 inline float operator () (size_t i, size_t j) const
906 {
907 return c[j][i];
908 }
909
910 inline float& operator () (size_t i, size_t j)
911 {
912 return c[j][i];
913 }
914
915 inline sse_meta_f4 operator * (const sse_meta_f4& v) const
916 {
917 return sse_meta_f4(_mm_add_ps(_mm_add_ps(_mm_mul_ps(c[0].v, vec_splat(v.v, 0)), _mm_mul_ps(c[1].v, vec_splat(v.v, 1))),
918 _mm_add_ps(_mm_mul_ps(c[2].v, vec_splat(v.v, 2)), _mm_mul_ps(c[3].v, vec_splat(v.v, 3)))
919 ));
920 }
921
922 inline sse_meta_f16 operator * (const sse_meta_f16& mat) const
923 {
924 return sse_meta_f16((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]);
925 }
926
927
928 inline sse_meta_f16 operator + (const sse_meta_f16& mat) const
929 {
930 return sse_meta_f16(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2], c[3] + mat.c[3]);
931 }
932
933 inline sse_meta_f16 operator - (const sse_meta_f16& mat) const
934 {
935 return sse_meta_f16(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2], c[3] - mat.c[3]);
936 }
937
938 inline sse_meta_f16 operator + (float t_) const
939 {
940 sse_meta_f4 t(t_);
941 return sse_meta_f16(c[0] + t, c[1] + t, c[2] + t, c[3] + t);
942 }
943
944 inline sse_meta_f16 operator - (float t_) const
945 {
946 sse_meta_f4 t(t_);
947 return sse_meta_f16(c[0] - t, c[1] - t, c[2] - t, c[3] - t);
948 }
949
950 inline sse_meta_f16 operator * (float t_) const
951 {
952 sse_meta_f4 t(t_);
953 return sse_meta_f16(c[0] * t, c[1] * t, c[2] * t, c[3] * t);
954 }
955
956 inline sse_meta_f16 operator / (float t_) const
957 {
958 sse_meta_f4 t(t_);
959 return sse_meta_f16(c[0] / t, c[1] / t, c[2] / t, c[3] / t);
960 }
961
962 inline sse_meta_f16& operator *= (const sse_meta_f16& mat)
963 {
964 setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]);
965 return *this;
966 }
967
968 inline sse_meta_f16& operator += (const sse_meta_f16& mat)
969 {
970 c[0] += mat.c[0];
971 c[1] += mat.c[1];
972 c[2] += mat.c[2];
973 c[3] += mat.c[3];
974 return *this;
975 }
976
977 inline sse_meta_f16& operator -= (const sse_meta_f16& mat)
978 {
979 c[0] -= mat.c[0];
980 c[1] -= mat.c[1];
981 c[2] -= mat.c[2];
982 c[3] -= mat.c[3];
983 return *this;
984 }
985
986 inline sse_meta_f16& operator += (float t_)
987 {
988 sse_meta_f4 t(t_);
989 c[0] += t;
990 c[1] += t;
991 c[2] += t;
992 c[3] += t;
993 return *this;
994 }
995
996 inline sse_meta_f16& operator -= (float t_)
997 {
998 sse_meta_f4 t(t_);
999 c[0] -= t;
1000 c[1] -= t;
1001 c[2] -= t;
1002 c[3] -= t;
1003 return *this;
1004 }
1005
1006 inline sse_meta_f16& operator *= (float t_)
1007 {
1008 sse_meta_f4 t(t_);
1009 c[0] *= t;
1010 c[1] *= t;
1011 c[2] *= t;
1012 c[3] *= t;
1013 return *this;
1014 }
1015
1016 inline sse_meta_f16& operator /= (float t_)
1017 {
1018 sse_meta_f4 t(t_);
1019 c[0] /= t;
1020 c[1] /= t;
1021 c[2] /= t;
1022 c[3] /= t;
1023 return *this;
1024 }
1025
1026 inline sse_meta_f16& abs()
1027 {
1028 c[0] = detail::abs(c[0]);
1029 c[1] = detail::abs(c[1]);
1030 c[2] = detail::abs(c[2]);
1031 c[3] = detail::abs(c[3]);
1032 return *this;
1033 }
1034
1035 inline sse_meta_f16& inverse()
1036 {
1037 __m128 r0, r1, r2, r3;
1038 detail::inverse(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3);
1039 setColumn(r0, r1, r2, r3);
1040 return *this;
1041 }
1042
1043 inline sse_meta_f16& transpose()
1044 {
1045 __m128 r0, r1, r2, r3;
1046 detail::transpose(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3);
1047 setColumn(r0, r1, r2, r3);
1048 return *this;
1049 }
1050
1051 inline float determinant() const
1052 {
1053 __m128 Va, Vb, Vc;
1054 __m128 r1, r2, r3, tt, tt2;
1055 __m128 sum, Det;
1056
1057 __m128 _L1 = c[0].v;
1058 __m128 _L2 = c[1].v;
1059 __m128 _L3 = c[2].v;
1060 __m128 _L4 = c[3].v;
1061 // Calculating the minterms for the first line.
1062
1063 // _mm_ror_ps is just a macro using _mm_shuffle_ps().
1064 tt = _L4; tt2 = _mm_ror_ps(_L3,1);
1065 Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'·V4
1066 Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'·V4"
1067 Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'·V4^
1068
1069 r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"·V4^ - V3^·V4"
1070 r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^·V4' - V3'·V4^
1071 r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'·V4" - V3"·V4'
1072
1073 tt = _L2;
1074 Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
1075 Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
1076 Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
1077
1078 // Calculating the determinant.
1079 Det = _mm_mul_ps(sum,_L1);
1080 Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
1081
1082 // Calculating the minterms of the second line (using previous results).
1083 tt = _mm_ror_ps(_L1,1); sum = _mm_mul_ps(tt,r1);
1084 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
1085 tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
1086
1087 // Testing the determinant.
1088 Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
1089 return _mm_cvtss_f32(Det);
1090 }
1091
1092 inline sse_meta_f16 transposeTimes(const sse_meta_f16& other) const
1093 {
1094 return sse_meta_f16(dot_prod4(c[0], other.c[0]), dot_prod4(c[0], other.c[1]), dot_prod4(c[0], other.c[2]), dot_prod4(c[0], other.c[3]),
1095 dot_prod4(c[1], other.c[0]), dot_prod4(c[1], other.c[1]), dot_prod4(c[1], other.c[2]), dot_prod4(c[1], other.c[3]),
1096 dot_prod4(c[2], other.c[0]), dot_prod4(c[2], other.c[1]), dot_prod4(c[2], other.c[2]), dot_prod4(c[2], other.c[3]),
1097 dot_prod4(c[3], other.c[0]), dot_prod4(c[3], other.c[1]), dot_prod4(c[3], other.c[2]), dot_prod4(c[3], other.c[3]));
1098 }
1099
1100 inline sse_meta_f16 timesTranspose(const sse_meta_f16& m) const
1101 {
1102 sse_meta_f16 tmp(m);
1103 return (*this) * tmp.transpose();
1104 }
1105
1106 inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const
1107 {
1108 return sse_meta_f4(dot_prod4(c[0], v), dot_prod4(c[1], v), dot_prod4(c[2], v), dot_prod4(c[3], v));
1109 }
1110
1111 inline float transposeDot(size_t i, const sse_meta_f4& v) const
1112 {
1113 return dot_prod4(c[i], v);
1114 }
1115
1116 inline float dot(size_t i, const sse_meta_f4& v) const
1117 {
1118 return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i] + v[3] * c[3][i];
1119 }
1120
1121 };
1122
1123 static inline sse_meta_f16 abs(const sse_meta_f16& mat)
1124 {
1125 return sse_meta_f16(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)), abs(mat.getColumn(3)));
1126 }
1127
1128 static inline sse_meta_f16 transpose(const sse_meta_f16& mat)
1129 {
1130 __m128 r0, r1, r2, r3;
1131 transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3);
1132 return sse_meta_f16(r0, r1, r2, r3);
1133 }
1134
1135
1136 static inline sse_meta_f16 inverse(const sse_meta_f16& mat)
1137 {
1138 __m128 r0, r1, r2, r3;
1139 inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3);
1140 return sse_meta_f16(r0, r1, r2, r3);
1141 }
1142
1143
1144
1145
1146 } // detail
1147 } // fcl
1148
1149
1150 #endif