Bullet Collision Detection & Physics Library
btVector3.cpp
Go to the documentation of this file.
1/*
2 Copyright (c) 2011 Apple Inc.
3 http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14
15 This source version has been altered.
16 */
17
18#if defined (_WIN32) || defined (__i386__)
19#define BT_USE_SSE_IN_API
20#endif
21
22
23#include "btVector3.h"
24
25
26
27#if defined BT_USE_SIMD_VECTOR3
28
29#if DEBUG
30#include <string.h>//for memset
31#endif
32
33
34#ifdef __APPLE__
35#include <stdint.h>
36typedef float float4 __attribute__ ((vector_size(16)));
37#else
38#define float4 __m128
39#endif
40//typedef uint32_t uint4 __attribute__ ((vector_size(16)));
41
42
43#if defined BT_USE_SSE || defined _WIN32
44
45#define LOG2_ARRAY_SIZE 6
46#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE)
47
48#include <emmintrin.h>
49
50long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult );
51long _maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult )
52{
53 const float4 *vertices = (const float4*) vv;
54 static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
55 float4 dotMax = btAssign128( -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY );
56 float4 vvec = _mm_loadu_ps( vec );
57 float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa ));
58 float4 vLo = _mm_movelh_ps( vvec, vvec );
59
60 long maxIndex = -1L;
61
62 size_t segment = 0;
63 float4 stack_array[ STACK_ARRAY_COUNT ];
64
65#if DEBUG
66 //memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
67#endif
68
69 size_t index;
70 float4 max;
71 // Faster loop without cleanup code for full tiles
72 for ( segment = 0; segment + STACK_ARRAY_COUNT*4 <= count; segment += STACK_ARRAY_COUNT*4 )
73 {
74 max = dotMax;
75
76 for( index = 0; index < STACK_ARRAY_COUNT; index+= 4 )
77 { // do four dot products at a time. Carefully avoid touching the w element.
78 float4 v0 = vertices[0];
79 float4 v1 = vertices[1];
80 float4 v2 = vertices[2];
81 float4 v3 = vertices[3]; vertices += 4;
82
83 float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
84 float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
85 float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
86 float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
87
88 lo0 = lo0*vLo;
89 lo1 = lo1*vLo;
90 float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
91 float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
92 float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
93 z = z*vHi;
94 x = x+y;
95 x = x+z;
96 stack_array[index] = x;
97 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
98
99 v0 = vertices[0];
100 v1 = vertices[1];
101 v2 = vertices[2];
102 v3 = vertices[3]; vertices += 4;
103
104 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
105 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
106 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
107 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
108
109 lo0 = lo0*vLo;
110 lo1 = lo1*vLo;
111 z = _mm_shuffle_ps(hi0, hi1, 0x88);
112 x = _mm_shuffle_ps(lo0, lo1, 0x88);
113 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
114 z = z*vHi;
115 x = x+y;
116 x = x+z;
117 stack_array[index+1] = x;
118 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
119
120 v0 = vertices[0];
121 v1 = vertices[1];
122 v2 = vertices[2];
123 v3 = vertices[3]; vertices += 4;
124
125 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
126 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
127 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
128 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
129
130 lo0 = lo0*vLo;
131 lo1 = lo1*vLo;
132 z = _mm_shuffle_ps(hi0, hi1, 0x88);
133 x = _mm_shuffle_ps(lo0, lo1, 0x88);
134 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
135 z = z*vHi;
136 x = x+y;
137 x = x+z;
138 stack_array[index+2] = x;
139 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
140
141 v0 = vertices[0];
142 v1 = vertices[1];
143 v2 = vertices[2];
144 v3 = vertices[3]; vertices += 4;
145
146 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
147 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
148 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
149 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
150
151 lo0 = lo0*vLo;
152 lo1 = lo1*vLo;
153 z = _mm_shuffle_ps(hi0, hi1, 0x88);
154 x = _mm_shuffle_ps(lo0, lo1, 0x88);
155 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
156 z = z*vHi;
157 x = x+y;
158 x = x+z;
159 stack_array[index+3] = x;
160 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
161
162 // It is too costly to keep the index of the max here. We will look for it again later. We save a lot of work this way.
163 }
164
165 // If we found a new max
166 if( 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(max, dotMax)))
167 {
168 // copy the new max across all lanes of our max accumulator
169 max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0x4e));
170 max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0xb1));
171
172 dotMax = max;
173
174 // find first occurrence of that max
175 size_t test;
176 for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], max))); index++ ) // local_count must be a multiple of 4
177 {}
178 // record where it is.
179 maxIndex = 4*index + segment + indexTable[test];
180 }
181 }
182
183 // account for work we've already done
184 count -= segment;
185
186 // Deal with the last < STACK_ARRAY_COUNT vectors
187 max = dotMax;
188 index = 0;
189
190
191 if( btUnlikely( count > 16) )
192 {
193 for( ; index + 4 <= count / 4; index+=4 )
194 { // do four dot products at a time. Carefully avoid touching the w element.
195 float4 v0 = vertices[0];
196 float4 v1 = vertices[1];
197 float4 v2 = vertices[2];
198 float4 v3 = vertices[3]; vertices += 4;
199
200 float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
201 float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
202 float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
203 float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
204
205 lo0 = lo0*vLo;
206 lo1 = lo1*vLo;
207 float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
208 float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
209 float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
210 z = z*vHi;
211 x = x+y;
212 x = x+z;
213 stack_array[index] = x;
214 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
215
216 v0 = vertices[0];
217 v1 = vertices[1];
218 v2 = vertices[2];
219 v3 = vertices[3]; vertices += 4;
220
221 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
222 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
223 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
224 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
225
226 lo0 = lo0*vLo;
227 lo1 = lo1*vLo;
228 z = _mm_shuffle_ps(hi0, hi1, 0x88);
229 x = _mm_shuffle_ps(lo0, lo1, 0x88);
230 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
231 z = z*vHi;
232 x = x+y;
233 x = x+z;
234 stack_array[index+1] = x;
235 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
236
237 v0 = vertices[0];
238 v1 = vertices[1];
239 v2 = vertices[2];
240 v3 = vertices[3]; vertices += 4;
241
242 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
243 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
244 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
245 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
246
247 lo0 = lo0*vLo;
248 lo1 = lo1*vLo;
249 z = _mm_shuffle_ps(hi0, hi1, 0x88);
250 x = _mm_shuffle_ps(lo0, lo1, 0x88);
251 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
252 z = z*vHi;
253 x = x+y;
254 x = x+z;
255 stack_array[index+2] = x;
256 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
257
258 v0 = vertices[0];
259 v1 = vertices[1];
260 v2 = vertices[2];
261 v3 = vertices[3]; vertices += 4;
262
263 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
264 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
265 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
266 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
267
268 lo0 = lo0*vLo;
269 lo1 = lo1*vLo;
270 z = _mm_shuffle_ps(hi0, hi1, 0x88);
271 x = _mm_shuffle_ps(lo0, lo1, 0x88);
272 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
273 z = z*vHi;
274 x = x+y;
275 x = x+z;
276 stack_array[index+3] = x;
277 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
278
279 // It is too costly to keep the index of the max here. We will look for it again later. We save a lot of work this way.
280 }
281 }
282
283 size_t localCount = (count & -4L) - 4*index;
284 if( localCount )
285 {
286#ifdef __APPLE__
287 float4 t0, t1, t2, t3, t4;
288 float4 * sap = &stack_array[index + localCount / 4];
289 vertices += localCount; // counter the offset
290 size_t byteIndex = -(localCount) * sizeof(float);
291 //AT&T Code style assembly
292 asm volatile
293 ( ".align 4 \n\
294 0: movaps %[max], %[t2] // move max out of the way to avoid propagating NaNs in max \n\
295 movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\
296 movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\
297 movaps %[t0], %[max] // vertices[0] \n\
298 movlhps %[t1], %[max] // x0y0x1y1 \n\
299 movaps 32(%[vertices], %[byteIndex], 4), %[t3] // vertices[2] \n\
300 movaps 48(%[vertices], %[byteIndex], 4), %[t4] // vertices[3] \n\
301 mulps %[vLo], %[max] // x0y0x1y1 * vLo \n\
302 movhlps %[t0], %[t1] // z0w0z1w1 \n\
303 movaps %[t3], %[t0] // vertices[2] \n\
304 movlhps %[t4], %[t0] // x2y2x3y3 \n\
305 mulps %[vLo], %[t0] // x2y2x3y3 * vLo \n\
306 movhlps %[t3], %[t4] // z2w2z3w3 \n\
307 shufps $0x88, %[t4], %[t1] // z0z1z2z3 \n\
308 mulps %[vHi], %[t1] // z0z1z2z3 * vHi \n\
309 movaps %[max], %[t3] // x0y0x1y1 * vLo \n\
310 shufps $0x88, %[t0], %[max] // x0x1x2x3 * vLo.x \n\
311 shufps $0xdd, %[t0], %[t3] // y0y1y2y3 * vLo.y \n\
312 addps %[t3], %[max] // x + y \n\
313 addps %[t1], %[max] // x + y + z \n\
314 movaps %[max], (%[sap], %[byteIndex]) // record result for later scrutiny \n\
315 maxps %[t2], %[max] // record max, restore max \n\
316 add $16, %[byteIndex] // advance loop counter\n\
317 jnz 0b \n\
318 "
319 : [max] "+x" (max), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex)
320 : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap)
321 : "memory", "cc"
322 );
323 index += localCount/4;
324#else
325 {
326 for( unsigned int i=0; i<localCount/4; i++,index++)
327 { // do four dot products at a time. Carefully avoid touching the w element.
328 float4 v0 = vertices[0];
329 float4 v1 = vertices[1];
330 float4 v2 = vertices[2];
331 float4 v3 = vertices[3];
332 vertices += 4;
333
334 float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
335 float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
336 float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
337 float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
338
339 lo0 = lo0*vLo;
340 lo1 = lo1*vLo;
341 float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
342 float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
343 float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
344 z = z*vHi;
345 x = x+y;
346 x = x+z;
347 stack_array[index] = x;
348 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
349 }
350 }
351#endif //__APPLE__
352 }
353
354 // process the last few points
355 if( count & 3 )
356 {
357 float4 v0, v1, v2, x, y, z;
358 switch( count & 3 )
359 {
360 case 3:
361 {
362 v0 = vertices[0];
363 v1 = vertices[1];
364 v2 = vertices[2];
365
366 // Calculate 3 dot products, transpose, duplicate v2
367 float4 lo0 = _mm_movelh_ps( v0, v1); // xyxy.lo
368 float4 hi0 = _mm_movehl_ps( v1, v0); // z?z?.lo
369 lo0 = lo0*vLo;
370 z = _mm_shuffle_ps(hi0, v2, 0xa8 ); // z0z1z2z2
371 z = z*vHi;
372 float4 lo1 = _mm_movelh_ps(v2, v2); // xyxy
373 lo1 = lo1*vLo;
374 x = _mm_shuffle_ps(lo0, lo1, 0x88);
375 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
376 }
377 break;
378 case 2:
379 {
380 v0 = vertices[0];
381 v1 = vertices[1];
382 float4 xy = _mm_movelh_ps(v0, v1);
383 z = _mm_movehl_ps(v1, v0);
384 xy = xy*vLo;
385 z = _mm_shuffle_ps( z, z, 0xa8);
386 x = _mm_shuffle_ps( xy, xy, 0xa8);
387 y = _mm_shuffle_ps( xy, xy, 0xfd);
388 z = z*vHi;
389 }
390 break;
391 case 1:
392 {
393 float4 xy = vertices[0];
394 z = _mm_shuffle_ps( xy, xy, 0xaa);
395 xy = xy*vLo;
396 z = z*vHi;
397 x = _mm_shuffle_ps(xy, xy, 0);
398 y = _mm_shuffle_ps(xy, xy, 0x55);
399 }
400 break;
401 }
402 x = x+y;
403 x = x+z;
404 stack_array[index] = x;
405 max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan
406 index++;
407 }
408
409 // if we found a new max.
410 if( 0 == segment || 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(max, dotMax)))
411 { // we found a new max. Search for it
412 // find max across the max vector, place in all elements of max -- big latency hit here
413 max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0x4e));
414 max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0xb1));
415
416 // It is slightly faster to do this part in scalar code when count < 8. However, the common case for
417 // this where it actually makes a difference is handled in the early out at the top of the function,
418 // so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced
419 // complexity, and removed it.
420
421 dotMax = max;
422
423 // scan for the first occurence of max in the array
424 size_t test;
425 for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], max))); index++ ) // local_count must be a multiple of 4
426 {}
427 maxIndex = 4*index + segment + indexTable[test];
428 }
429
430 _mm_store_ss( dotResult, dotMax);
431 return maxIndex;
432}
433
434long _mindot_large( const float *vv, const float *vec, unsigned long count, float *dotResult );
435
436long _mindot_large( const float *vv, const float *vec, unsigned long count, float *dotResult )
437{
438 const float4 *vertices = (const float4*) vv;
439 static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
440 float4 dotmin = btAssign128( BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY );
441 float4 vvec = _mm_loadu_ps( vec );
442 float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa ));
443 float4 vLo = _mm_movelh_ps( vvec, vvec );
444
445 long minIndex = -1L;
446
447 size_t segment = 0;
448 float4 stack_array[ STACK_ARRAY_COUNT ];
449
450#if DEBUG
451 //memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
452#endif
453
454 size_t index;
455 float4 min;
456 // Faster loop without cleanup code for full tiles
457 for ( segment = 0; segment + STACK_ARRAY_COUNT*4 <= count; segment += STACK_ARRAY_COUNT*4 )
458 {
459 min = dotmin;
460
461 for( index = 0; index < STACK_ARRAY_COUNT; index+= 4 )
462 { // do four dot products at a time. Carefully avoid touching the w element.
463 float4 v0 = vertices[0];
464 float4 v1 = vertices[1];
465 float4 v2 = vertices[2];
466 float4 v3 = vertices[3]; vertices += 4;
467
468 float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
469 float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
470 float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
471 float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
472
473 lo0 = lo0*vLo;
474 lo1 = lo1*vLo;
475 float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
476 float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
477 float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
478 z = z*vHi;
479 x = x+y;
480 x = x+z;
481 stack_array[index] = x;
482 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
483
484 v0 = vertices[0];
485 v1 = vertices[1];
486 v2 = vertices[2];
487 v3 = vertices[3]; vertices += 4;
488
489 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
490 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
491 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
492 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
493
494 lo0 = lo0*vLo;
495 lo1 = lo1*vLo;
496 z = _mm_shuffle_ps(hi0, hi1, 0x88);
497 x = _mm_shuffle_ps(lo0, lo1, 0x88);
498 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
499 z = z*vHi;
500 x = x+y;
501 x = x+z;
502 stack_array[index+1] = x;
503 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
504
505 v0 = vertices[0];
506 v1 = vertices[1];
507 v2 = vertices[2];
508 v3 = vertices[3]; vertices += 4;
509
510 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
511 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
512 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
513 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
514
515 lo0 = lo0*vLo;
516 lo1 = lo1*vLo;
517 z = _mm_shuffle_ps(hi0, hi1, 0x88);
518 x = _mm_shuffle_ps(lo0, lo1, 0x88);
519 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
520 z = z*vHi;
521 x = x+y;
522 x = x+z;
523 stack_array[index+2] = x;
524 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
525
526 v0 = vertices[0];
527 v1 = vertices[1];
528 v2 = vertices[2];
529 v3 = vertices[3]; vertices += 4;
530
531 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
532 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
533 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
534 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
535
536 lo0 = lo0*vLo;
537 lo1 = lo1*vLo;
538 z = _mm_shuffle_ps(hi0, hi1, 0x88);
539 x = _mm_shuffle_ps(lo0, lo1, 0x88);
540 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
541 z = z*vHi;
542 x = x+y;
543 x = x+z;
544 stack_array[index+3] = x;
545 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
546
547 // It is too costly to keep the index of the min here. We will look for it again later. We save a lot of work this way.
548 }
549
550 // If we found a new min
551 if( 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(min, dotmin)))
552 {
553 // copy the new min across all lanes of our min accumulator
554 min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0x4e));
555 min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0xb1));
556
557 dotmin = min;
558
559 // find first occurrence of that min
560 size_t test;
561 for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], min))); index++ ) // local_count must be a multiple of 4
562 {}
563 // record where it is.
564 minIndex = 4*index + segment + indexTable[test];
565 }
566 }
567
568 // account for work we've already done
569 count -= segment;
570
571 // Deal with the last < STACK_ARRAY_COUNT vectors
572 min = dotmin;
573 index = 0;
574
575
576 if(btUnlikely( count > 16) )
577 {
578 for( ; index + 4 <= count / 4; index+=4 )
579 { // do four dot products at a time. Carefully avoid touching the w element.
580 float4 v0 = vertices[0];
581 float4 v1 = vertices[1];
582 float4 v2 = vertices[2];
583 float4 v3 = vertices[3]; vertices += 4;
584
585 float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
586 float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
587 float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
588 float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
589
590 lo0 = lo0*vLo;
591 lo1 = lo1*vLo;
592 float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
593 float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
594 float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
595 z = z*vHi;
596 x = x+y;
597 x = x+z;
598 stack_array[index] = x;
599 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
600
601 v0 = vertices[0];
602 v1 = vertices[1];
603 v2 = vertices[2];
604 v3 = vertices[3]; vertices += 4;
605
606 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
607 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
608 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
609 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
610
611 lo0 = lo0*vLo;
612 lo1 = lo1*vLo;
613 z = _mm_shuffle_ps(hi0, hi1, 0x88);
614 x = _mm_shuffle_ps(lo0, lo1, 0x88);
615 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
616 z = z*vHi;
617 x = x+y;
618 x = x+z;
619 stack_array[index+1] = x;
620 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
621
622 v0 = vertices[0];
623 v1 = vertices[1];
624 v2 = vertices[2];
625 v3 = vertices[3]; vertices += 4;
626
627 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
628 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
629 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
630 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
631
632 lo0 = lo0*vLo;
633 lo1 = lo1*vLo;
634 z = _mm_shuffle_ps(hi0, hi1, 0x88);
635 x = _mm_shuffle_ps(lo0, lo1, 0x88);
636 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
637 z = z*vHi;
638 x = x+y;
639 x = x+z;
640 stack_array[index+2] = x;
641 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
642
643 v0 = vertices[0];
644 v1 = vertices[1];
645 v2 = vertices[2];
646 v3 = vertices[3]; vertices += 4;
647
648 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
649 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
650 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
651 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
652
653 lo0 = lo0*vLo;
654 lo1 = lo1*vLo;
655 z = _mm_shuffle_ps(hi0, hi1, 0x88);
656 x = _mm_shuffle_ps(lo0, lo1, 0x88);
657 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
658 z = z*vHi;
659 x = x+y;
660 x = x+z;
661 stack_array[index+3] = x;
662 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
663
664 // It is too costly to keep the index of the min here. We will look for it again later. We save a lot of work this way.
665 }
666 }
667
668 size_t localCount = (count & -4L) - 4*index;
669 if( localCount )
670 {
671
672
673#ifdef __APPLE__
674 vertices += localCount; // counter the offset
675 float4 t0, t1, t2, t3, t4;
676 size_t byteIndex = -(localCount) * sizeof(float);
677 float4 * sap = &stack_array[index + localCount / 4];
678
679 asm volatile
680 ( ".align 4 \n\
681 0: movaps %[min], %[t2] // move min out of the way to avoid propagating NaNs in min \n\
682 movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\
683 movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\
684 movaps %[t0], %[min] // vertices[0] \n\
685 movlhps %[t1], %[min] // x0y0x1y1 \n\
686 movaps 32(%[vertices], %[byteIndex], 4), %[t3] // vertices[2] \n\
687 movaps 48(%[vertices], %[byteIndex], 4), %[t4] // vertices[3] \n\
688 mulps %[vLo], %[min] // x0y0x1y1 * vLo \n\
689 movhlps %[t0], %[t1] // z0w0z1w1 \n\
690 movaps %[t3], %[t0] // vertices[2] \n\
691 movlhps %[t4], %[t0] // x2y2x3y3 \n\
692 movhlps %[t3], %[t4] // z2w2z3w3 \n\
693 mulps %[vLo], %[t0] // x2y2x3y3 * vLo \n\
694 shufps $0x88, %[t4], %[t1] // z0z1z2z3 \n\
695 mulps %[vHi], %[t1] // z0z1z2z3 * vHi \n\
696 movaps %[min], %[t3] // x0y0x1y1 * vLo \n\
697 shufps $0x88, %[t0], %[min] // x0x1x2x3 * vLo.x \n\
698 shufps $0xdd, %[t0], %[t3] // y0y1y2y3 * vLo.y \n\
699 addps %[t3], %[min] // x + y \n\
700 addps %[t1], %[min] // x + y + z \n\
701 movaps %[min], (%[sap], %[byteIndex]) // record result for later scrutiny \n\
702 minps %[t2], %[min] // record min, restore min \n\
703 add $16, %[byteIndex] // advance loop counter\n\
704 jnz 0b \n\
705 "
706 : [min] "+x" (min), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex)
707 : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap)
708 : "memory", "cc"
709 );
710 index += localCount/4;
711#else
712 {
713 for( unsigned int i=0; i<localCount/4; i++,index++)
714 { // do four dot products at a time. Carefully avoid touching the w element.
715 float4 v0 = vertices[0];
716 float4 v1 = vertices[1];
717 float4 v2 = vertices[2];
718 float4 v3 = vertices[3];
719 vertices += 4;
720
721 float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1
722 float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1
723 float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3
724 float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3
725
726 lo0 = lo0*vLo;
727 lo1 = lo1*vLo;
728 float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
729 float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
730 float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
731 z = z*vHi;
732 x = x+y;
733 x = x+z;
734 stack_array[index] = x;
735 min = _mm_min_ps( x, min ); // control the order here so that max is never NaN even if x is nan
736 }
737 }
738
739#endif
740 }
741
742 // process the last few points
743 if( count & 3 )
744 {
745 float4 v0, v1, v2, x, y, z;
746 switch( count & 3 )
747 {
748 case 3:
749 {
750 v0 = vertices[0];
751 v1 = vertices[1];
752 v2 = vertices[2];
753
754 // Calculate 3 dot products, transpose, duplicate v2
755 float4 lo0 = _mm_movelh_ps( v0, v1); // xyxy.lo
756 float4 hi0 = _mm_movehl_ps( v1, v0); // z?z?.lo
757 lo0 = lo0*vLo;
758 z = _mm_shuffle_ps(hi0, v2, 0xa8 ); // z0z1z2z2
759 z = z*vHi;
760 float4 lo1 = _mm_movelh_ps(v2, v2); // xyxy
761 lo1 = lo1*vLo;
762 x = _mm_shuffle_ps(lo0, lo1, 0x88);
763 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
764 }
765 break;
766 case 2:
767 {
768 v0 = vertices[0];
769 v1 = vertices[1];
770 float4 xy = _mm_movelh_ps(v0, v1);
771 z = _mm_movehl_ps(v1, v0);
772 xy = xy*vLo;
773 z = _mm_shuffle_ps( z, z, 0xa8);
774 x = _mm_shuffle_ps( xy, xy, 0xa8);
775 y = _mm_shuffle_ps( xy, xy, 0xfd);
776 z = z*vHi;
777 }
778 break;
779 case 1:
780 {
781 float4 xy = vertices[0];
782 z = _mm_shuffle_ps( xy, xy, 0xaa);
783 xy = xy*vLo;
784 z = z*vHi;
785 x = _mm_shuffle_ps(xy, xy, 0);
786 y = _mm_shuffle_ps(xy, xy, 0x55);
787 }
788 break;
789 }
790 x = x+y;
791 x = x+z;
792 stack_array[index] = x;
793 min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan
794 index++;
795 }
796
797 // if we found a new min.
798 if( 0 == segment || 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(min, dotmin)))
799 { // we found a new min. Search for it
800 // find min across the min vector, place in all elements of min -- big latency hit here
801 min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0x4e));
802 min = _mm_min_ps(min, (float4) _mm_shuffle_ps( min, min, 0xb1));
803
804 // It is slightly faster to do this part in scalar code when count < 8. However, the common case for
805 // this where it actually makes a difference is handled in the early out at the top of the function,
806 // so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced
807 // complexity, and removed it.
808
809 dotmin = min;
810
811 // scan for the first occurence of min in the array
812 size_t test;
813 for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], min))); index++ ) // local_count must be a multiple of 4
814 {}
815 minIndex = 4*index + segment + indexTable[test];
816 }
817
818 _mm_store_ss( dotResult, dotmin);
819 return minIndex;
820}
821
822
823#elif defined BT_USE_NEON
824
825#define ARM_NEON_GCC_COMPATIBILITY 1
826#include <arm_neon.h>
827#include <sys/types.h>
828#include <sys/sysctl.h> //for sysctlbyname
829
830static long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult );
831static long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult );
832static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult );
833static long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult );
834static long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult );
835static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult );
836
837long (*_maxdot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _maxdot_large_sel;
838long (*_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = _mindot_large_sel;
839
840
841static inline uint32_t btGetCpuCapabilities( void )
842{
843 static uint32_t capabilities = 0;
844 static bool testedCapabilities = false;
845
846 if( 0 == testedCapabilities)
847 {
848 uint32_t hasFeature = 0;
849 size_t featureSize = sizeof( hasFeature );
850 int err = sysctlbyname( "hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0 );
851
852 if( 0 == err && hasFeature)
853 capabilities |= 0x2000;
854
855 testedCapabilities = true;
856 }
857
858 return capabilities;
859}
860
861
862
863
864static long _maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult )
865{
866
867 if( btGetCpuCapabilities() & 0x2000 )
868 _maxdot_large = _maxdot_large_v1;
869 else
870 _maxdot_large = _maxdot_large_v0;
871
872 return _maxdot_large(vv, vec, count, dotResult);
873}
874
875static long _mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult )
876{
877
878 if( btGetCpuCapabilities() & 0x2000 )
879 _mindot_large = _mindot_large_v1;
880 else
881 _mindot_large = _mindot_large_v0;
882
883 return _mindot_large(vv, vec, count, dotResult);
884}
885
886
887
888#if defined __arm__
889# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
890#else
891//support 64bit arm
892# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; })
893#endif
894
895
896long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )
897{
898 unsigned long i = 0;
899 float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
900 float32x2_t vLo = vget_low_f32(vvec);
901 float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
902 float32x2_t dotMaxLo = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
903 float32x2_t dotMaxHi = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
904 uint32x2_t indexLo = (uint32x2_t) {0, 1};
905 uint32x2_t indexHi = (uint32x2_t) {2, 3};
906 uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
907 uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
908 const uint32x2_t four = (uint32x2_t) {4,4};
909
910 for( ; i+8 <= count; i+= 8 )
911 {
912 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
913 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
914 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
915 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
916
917 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
918 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
919 float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
920 float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
921
922 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
923 float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
924 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
925 float32x2_t zHi = vmul_f32( z1.val[0], vHi);
926
927 float32x2_t rLo = vpadd_f32( xy0, xy1);
928 float32x2_t rHi = vpadd_f32( xy2, xy3);
929 rLo = vadd_f32(rLo, zLo);
930 rHi = vadd_f32(rHi, zHi);
931
932 uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
933 uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi );
934 dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
935 dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
936 iLo = vbsl_u32(maskLo, indexLo, iLo);
937 iHi = vbsl_u32(maskHi, indexHi, iHi);
938 indexLo = vadd_u32(indexLo, four);
939 indexHi = vadd_u32(indexHi, four);
940
941 v0 = vld1q_f32_aligned_postincrement( vv );
942 v1 = vld1q_f32_aligned_postincrement( vv );
943 v2 = vld1q_f32_aligned_postincrement( vv );
944 v3 = vld1q_f32_aligned_postincrement( vv );
945
946 xy0 = vmul_f32( vget_low_f32(v0), vLo);
947 xy1 = vmul_f32( vget_low_f32(v1), vLo);
948 xy2 = vmul_f32( vget_low_f32(v2), vLo);
949 xy3 = vmul_f32( vget_low_f32(v3), vLo);
950
951 z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
952 z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
953 zLo = vmul_f32( z0.val[0], vHi);
954 zHi = vmul_f32( z1.val[0], vHi);
955
956 rLo = vpadd_f32( xy0, xy1);
957 rHi = vpadd_f32( xy2, xy3);
958 rLo = vadd_f32(rLo, zLo);
959 rHi = vadd_f32(rHi, zHi);
960
961 maskLo = vcgt_f32( rLo, dotMaxLo );
962 maskHi = vcgt_f32( rHi, dotMaxHi );
963 dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
964 dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
965 iLo = vbsl_u32(maskLo, indexLo, iLo);
966 iHi = vbsl_u32(maskHi, indexHi, iHi);
967 indexLo = vadd_u32(indexLo, four);
968 indexHi = vadd_u32(indexHi, four);
969 }
970
971 for( ; i+4 <= count; i+= 4 )
972 {
973 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
974 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
975 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
976 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
977
978 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
979 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
980 float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
981 float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
982
983 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
984 float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
985 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
986 float32x2_t zHi = vmul_f32( z1.val[0], vHi);
987
988 float32x2_t rLo = vpadd_f32( xy0, xy1);
989 float32x2_t rHi = vpadd_f32( xy2, xy3);
990 rLo = vadd_f32(rLo, zLo);
991 rHi = vadd_f32(rHi, zHi);
992
993 uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
994 uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi );
995 dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
996 dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
997 iLo = vbsl_u32(maskLo, indexLo, iLo);
998 iHi = vbsl_u32(maskHi, indexHi, iHi);
999 indexLo = vadd_u32(indexLo, four);
1000 indexHi = vadd_u32(indexHi, four);
1001 }
1002
1003 switch( count & 3 )
1004 {
1005 case 3:
1006 {
1007 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1008 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1009 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1010
1011 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1012 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
1013 float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
1014
1015 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1016 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
1017 float32x2_t zHi = vmul_f32( vdup_lane_f32(vget_high_f32(v2), 0), vHi);
1018
1019 float32x2_t rLo = vpadd_f32( xy0, xy1);
1020 float32x2_t rHi = vpadd_f32( xy2, xy2);
1021 rLo = vadd_f32(rLo, zLo);
1022 rHi = vadd_f32(rHi, zHi);
1023
1024 uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
1025 uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi );
1026 dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
1027 dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi);
1028 iLo = vbsl_u32(maskLo, indexLo, iLo);
1029 iHi = vbsl_u32(maskHi, indexHi, iHi);
1030 }
1031 break;
1032 case 2:
1033 {
1034 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1035 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1036
1037 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1038 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
1039
1040 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1041 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
1042
1043 float32x2_t rLo = vpadd_f32( xy0, xy1);
1044 rLo = vadd_f32(rLo, zLo);
1045
1046 uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
1047 dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
1048 iLo = vbsl_u32(maskLo, indexLo, iLo);
1049 }
1050 break;
1051 case 1:
1052 {
1053 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1054 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1055 float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
1056 float32x2_t zLo = vmul_f32( z0, vHi);
1057 float32x2_t rLo = vpadd_f32( xy0, xy0);
1058 rLo = vadd_f32(rLo, zLo);
1059 uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo );
1060 dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo);
1061 iLo = vbsl_u32(maskLo, indexLo, iLo);
1062 }
1063 break;
1064
1065 default:
1066 break;
1067 }
1068
1069 // select best answer between hi and lo results
1070 uint32x2_t mask = vcgt_f32( dotMaxHi, dotMaxLo );
1071 dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
1072 iLo = vbsl_u32(mask, iHi, iLo);
1073
1074 // select best answer between even and odd results
1075 dotMaxHi = vdup_lane_f32(dotMaxLo, 1);
1076 iHi = vdup_lane_u32(iLo, 1);
1077 mask = vcgt_f32( dotMaxHi, dotMaxLo );
1078 dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
1079 iLo = vbsl_u32(mask, iHi, iLo);
1080
1081 *dotResult = vget_lane_f32( dotMaxLo, 0);
1082 return vget_lane_u32(iLo, 0);
1083}
1084
1085
1086long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult )
1087{
1088 float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
1089 float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
1090 float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
1091 const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
1092 uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
1093 uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) };
1094 float32x4_t maxDot = (float32x4_t) { -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY };
1095
1096 unsigned long i = 0;
1097 for( ; i + 8 <= count; i += 8 )
1098 {
1099 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1100 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1101 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1102 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
1103
1104 // the next two lines should resolve to a single vswp d, d
1105 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1106 float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
1107 // the next two lines should resolve to a single vswp d, d
1108 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1109 float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
1110
1111 xy0 = vmulq_f32(xy0, vLo);
1112 xy1 = vmulq_f32(xy1, vLo);
1113
1114 float32x4x2_t zb = vuzpq_f32( z0, z1);
1115 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1116 float32x4x2_t xy = vuzpq_f32( xy0, xy1);
1117 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1118 x = vaddq_f32(x, z);
1119
1120 uint32x4_t mask = vcgtq_f32(x, maxDot);
1121 maxDot = vbslq_f32( mask, x, maxDot);
1122 index = vbslq_u32(mask, local_index, index);
1123 local_index = vaddq_u32(local_index, four);
1124
1125 v0 = vld1q_f32_aligned_postincrement( vv );
1126 v1 = vld1q_f32_aligned_postincrement( vv );
1127 v2 = vld1q_f32_aligned_postincrement( vv );
1128 v3 = vld1q_f32_aligned_postincrement( vv );
1129
1130 // the next two lines should resolve to a single vswp d, d
1131 xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1132 xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
1133 // the next two lines should resolve to a single vswp d, d
1134 z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1135 z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
1136
1137 xy0 = vmulq_f32(xy0, vLo);
1138 xy1 = vmulq_f32(xy1, vLo);
1139
1140 zb = vuzpq_f32( z0, z1);
1141 z = vmulq_f32( zb.val[0], vHi);
1142 xy = vuzpq_f32( xy0, xy1);
1143 x = vaddq_f32(xy.val[0], xy.val[1]);
1144 x = vaddq_f32(x, z);
1145
1146 mask = vcgtq_f32(x, maxDot);
1147 maxDot = vbslq_f32( mask, x, maxDot);
1148 index = vbslq_u32(mask, local_index, index);
1149 local_index = vaddq_u32(local_index, four);
1150 }
1151
1152 for( ; i + 4 <= count; i += 4 )
1153 {
1154 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1155 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1156 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1157 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
1158
1159 // the next two lines should resolve to a single vswp d, d
1160 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1161 float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
1162 // the next two lines should resolve to a single vswp d, d
1163 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1164 float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
1165
1166 xy0 = vmulq_f32(xy0, vLo);
1167 xy1 = vmulq_f32(xy1, vLo);
1168
1169 float32x4x2_t zb = vuzpq_f32( z0, z1);
1170 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1171 float32x4x2_t xy = vuzpq_f32( xy0, xy1);
1172 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1173 x = vaddq_f32(x, z);
1174
1175 uint32x4_t mask = vcgtq_f32(x, maxDot);
1176 maxDot = vbslq_f32( mask, x, maxDot);
1177 index = vbslq_u32(mask, local_index, index);
1178 local_index = vaddq_u32(local_index, four);
1179 }
1180
1181 switch (count & 3) {
1182 case 3:
1183 {
1184 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1185 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1186 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1187
1188 // the next two lines should resolve to a single vswp d, d
1189 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1190 float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v2));
1191 // the next two lines should resolve to a single vswp d, d
1192 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1193 float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v2));
1194
1195 xy0 = vmulq_f32(xy0, vLo);
1196 xy1 = vmulq_f32(xy1, vLo);
1197
1198 float32x4x2_t zb = vuzpq_f32( z0, z1);
1199 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1200 float32x4x2_t xy = vuzpq_f32( xy0, xy1);
1201 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1202 x = vaddq_f32(x, z);
1203
1204 uint32x4_t mask = vcgtq_f32(x, maxDot);
1205 maxDot = vbslq_f32( mask, x, maxDot);
1206 index = vbslq_u32(mask, local_index, index);
1207 local_index = vaddq_u32(local_index, four);
1208 }
1209 break;
1210
1211 case 2:
1212 {
1213 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1214 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1215
1216 // the next two lines should resolve to a single vswp d, d
1217 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1218 // the next two lines should resolve to a single vswp d, d
1219 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1220
1221 xy0 = vmulq_f32(xy0, vLo);
1222
1223 float32x4x2_t zb = vuzpq_f32( z0, z0);
1224 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1225 float32x4x2_t xy = vuzpq_f32( xy0, xy0);
1226 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1227 x = vaddq_f32(x, z);
1228
1229 uint32x4_t mask = vcgtq_f32(x, maxDot);
1230 maxDot = vbslq_f32( mask, x, maxDot);
1231 index = vbslq_u32(mask, local_index, index);
1232 local_index = vaddq_u32(local_index, four);
1233 }
1234 break;
1235
1236 case 1:
1237 {
1238 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1239
1240 // the next two lines should resolve to a single vswp d, d
1241 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v0));
1242 // the next two lines should resolve to a single vswp d, d
1243 float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0);
1244
1245 xy0 = vmulq_f32(xy0, vLo);
1246
1247 z = vmulq_f32( z, vHi);
1248 float32x4x2_t xy = vuzpq_f32( xy0, xy0);
1249 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1250 x = vaddq_f32(x, z);
1251
1252 uint32x4_t mask = vcgtq_f32(x, maxDot);
1253 maxDot = vbslq_f32( mask, x, maxDot);
1254 index = vbslq_u32(mask, local_index, index);
1255 local_index = vaddq_u32(local_index, four);
1256 }
1257 break;
1258
1259 default:
1260 break;
1261 }
1262
1263
1264 // select best answer between hi and lo results
1265 uint32x2_t mask = vcgt_f32( vget_high_f32(maxDot), vget_low_f32(maxDot));
1266 float32x2_t maxDot2 = vbsl_f32(mask, vget_high_f32(maxDot), vget_low_f32(maxDot));
1267 uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
1268
1269 // select best answer between even and odd results
1270 float32x2_t maxDotO = vdup_lane_f32(maxDot2, 1);
1271 uint32x2_t indexHi = vdup_lane_u32(index2, 1);
1272 mask = vcgt_f32( maxDotO, maxDot2 );
1273 maxDot2 = vbsl_f32(mask, maxDotO, maxDot2);
1274 index2 = vbsl_u32(mask, indexHi, index2);
1275
1276 *dotResult = vget_lane_f32( maxDot2, 0);
1277 return vget_lane_u32(index2, 0);
1278
1279}
1280
1281long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )
1282{
1283 unsigned long i = 0;
1284 float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
1285 float32x2_t vLo = vget_low_f32(vvec);
1286 float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
1287 float32x2_t dotMinLo = (float32x2_t) { BT_INFINITY, BT_INFINITY };
1288 float32x2_t dotMinHi = (float32x2_t) { BT_INFINITY, BT_INFINITY };
1289 uint32x2_t indexLo = (uint32x2_t) {0, 1};
1290 uint32x2_t indexHi = (uint32x2_t) {2, 3};
1291 uint32x2_t iLo = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
1292 uint32x2_t iHi = (uint32x2_t) {static_cast<uint32_t>(-1), static_cast<uint32_t>(-1)};
1293 const uint32x2_t four = (uint32x2_t) {4,4};
1294
1295 for( ; i+8 <= count; i+= 8 )
1296 {
1297 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1298 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1299 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1300 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
1301
1302 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1303 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
1304 float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
1305 float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
1306
1307 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1308 float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
1309 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
1310 float32x2_t zHi = vmul_f32( z1.val[0], vHi);
1311
1312 float32x2_t rLo = vpadd_f32( xy0, xy1);
1313 float32x2_t rHi = vpadd_f32( xy2, xy3);
1314 rLo = vadd_f32(rLo, zLo);
1315 rHi = vadd_f32(rHi, zHi);
1316
1317 uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
1318 uint32x2_t maskHi = vclt_f32( rHi, dotMinHi );
1319 dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
1320 dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
1321 iLo = vbsl_u32(maskLo, indexLo, iLo);
1322 iHi = vbsl_u32(maskHi, indexHi, iHi);
1323 indexLo = vadd_u32(indexLo, four);
1324 indexHi = vadd_u32(indexHi, four);
1325
1326 v0 = vld1q_f32_aligned_postincrement( vv );
1327 v1 = vld1q_f32_aligned_postincrement( vv );
1328 v2 = vld1q_f32_aligned_postincrement( vv );
1329 v3 = vld1q_f32_aligned_postincrement( vv );
1330
1331 xy0 = vmul_f32( vget_low_f32(v0), vLo);
1332 xy1 = vmul_f32( vget_low_f32(v1), vLo);
1333 xy2 = vmul_f32( vget_low_f32(v2), vLo);
1334 xy3 = vmul_f32( vget_low_f32(v3), vLo);
1335
1336 z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1337 z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
1338 zLo = vmul_f32( z0.val[0], vHi);
1339 zHi = vmul_f32( z1.val[0], vHi);
1340
1341 rLo = vpadd_f32( xy0, xy1);
1342 rHi = vpadd_f32( xy2, xy3);
1343 rLo = vadd_f32(rLo, zLo);
1344 rHi = vadd_f32(rHi, zHi);
1345
1346 maskLo = vclt_f32( rLo, dotMinLo );
1347 maskHi = vclt_f32( rHi, dotMinHi );
1348 dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
1349 dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
1350 iLo = vbsl_u32(maskLo, indexLo, iLo);
1351 iHi = vbsl_u32(maskHi, indexHi, iHi);
1352 indexLo = vadd_u32(indexLo, four);
1353 indexHi = vadd_u32(indexHi, four);
1354 }
1355
1356 for( ; i+4 <= count; i+= 4 )
1357 {
1358 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1359 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1360 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1361 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
1362
1363 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1364 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
1365 float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
1366 float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo);
1367
1368 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1369 float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3));
1370 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
1371 float32x2_t zHi = vmul_f32( z1.val[0], vHi);
1372
1373 float32x2_t rLo = vpadd_f32( xy0, xy1);
1374 float32x2_t rHi = vpadd_f32( xy2, xy3);
1375 rLo = vadd_f32(rLo, zLo);
1376 rHi = vadd_f32(rHi, zHi);
1377
1378 uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
1379 uint32x2_t maskHi = vclt_f32( rHi, dotMinHi );
1380 dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
1381 dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
1382 iLo = vbsl_u32(maskLo, indexLo, iLo);
1383 iHi = vbsl_u32(maskHi, indexHi, iHi);
1384 indexLo = vadd_u32(indexLo, four);
1385 indexHi = vadd_u32(indexHi, four);
1386 }
1387 switch( count & 3 )
1388 {
1389 case 3:
1390 {
1391 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1392 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1393 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1394
1395 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1396 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
1397 float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo);
1398
1399 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1400 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
1401 float32x2_t zHi = vmul_f32( vdup_lane_f32(vget_high_f32(v2), 0), vHi);
1402
1403 float32x2_t rLo = vpadd_f32( xy0, xy1);
1404 float32x2_t rHi = vpadd_f32( xy2, xy2);
1405 rLo = vadd_f32(rLo, zLo);
1406 rHi = vadd_f32(rHi, zHi);
1407
1408 uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
1409 uint32x2_t maskHi = vclt_f32( rHi, dotMinHi );
1410 dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
1411 dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi);
1412 iLo = vbsl_u32(maskLo, indexLo, iLo);
1413 iHi = vbsl_u32(maskHi, indexHi, iHi);
1414 }
1415 break;
1416 case 2:
1417 {
1418 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1419 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1420
1421 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1422 float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo);
1423
1424 float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1));
1425 float32x2_t zLo = vmul_f32( z0.val[0], vHi);
1426
1427 float32x2_t rLo = vpadd_f32( xy0, xy1);
1428 rLo = vadd_f32(rLo, zLo);
1429
1430 uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
1431 dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
1432 iLo = vbsl_u32(maskLo, indexLo, iLo);
1433 }
1434 break;
1435 case 1:
1436 {
1437 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1438 float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo);
1439 float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
1440 float32x2_t zLo = vmul_f32( z0, vHi);
1441 float32x2_t rLo = vpadd_f32( xy0, xy0);
1442 rLo = vadd_f32(rLo, zLo);
1443 uint32x2_t maskLo = vclt_f32( rLo, dotMinLo );
1444 dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo);
1445 iLo = vbsl_u32(maskLo, indexLo, iLo);
1446 }
1447 break;
1448
1449 default:
1450 break;
1451 }
1452
1453 // select best answer between hi and lo results
1454 uint32x2_t mask = vclt_f32( dotMinHi, dotMinLo );
1455 dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
1456 iLo = vbsl_u32(mask, iHi, iLo);
1457
1458 // select best answer between even and odd results
1459 dotMinHi = vdup_lane_f32(dotMinLo, 1);
1460 iHi = vdup_lane_u32(iLo, 1);
1461 mask = vclt_f32( dotMinHi, dotMinLo );
1462 dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
1463 iLo = vbsl_u32(mask, iHi, iLo);
1464
1465 *dotResult = vget_lane_f32( dotMinLo, 0);
1466 return vget_lane_u32(iLo, 0);
1467}
1468
1469long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult )
1470{
1471 float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
1472 float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
1473 float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
1474 const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
1475 uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
1476 uint32x4_t index = (uint32x4_t) { static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1) };
1477 float32x4_t minDot = (float32x4_t) { BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY };
1478
1479 unsigned long i = 0;
1480 for( ; i + 8 <= count; i += 8 )
1481 {
1482 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1483 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1484 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1485 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
1486
1487 // the next two lines should resolve to a single vswp d, d
1488 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1489 float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
1490 // the next two lines should resolve to a single vswp d, d
1491 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1492 float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
1493
1494 xy0 = vmulq_f32(xy0, vLo);
1495 xy1 = vmulq_f32(xy1, vLo);
1496
1497 float32x4x2_t zb = vuzpq_f32( z0, z1);
1498 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1499 float32x4x2_t xy = vuzpq_f32( xy0, xy1);
1500 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1501 x = vaddq_f32(x, z);
1502
1503 uint32x4_t mask = vcltq_f32(x, minDot);
1504 minDot = vbslq_f32( mask, x, minDot);
1505 index = vbslq_u32(mask, local_index, index);
1506 local_index = vaddq_u32(local_index, four);
1507
1508 v0 = vld1q_f32_aligned_postincrement( vv );
1509 v1 = vld1q_f32_aligned_postincrement( vv );
1510 v2 = vld1q_f32_aligned_postincrement( vv );
1511 v3 = vld1q_f32_aligned_postincrement( vv );
1512
1513 // the next two lines should resolve to a single vswp d, d
1514 xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1515 xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
1516 // the next two lines should resolve to a single vswp d, d
1517 z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1518 z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
1519
1520 xy0 = vmulq_f32(xy0, vLo);
1521 xy1 = vmulq_f32(xy1, vLo);
1522
1523 zb = vuzpq_f32( z0, z1);
1524 z = vmulq_f32( zb.val[0], vHi);
1525 xy = vuzpq_f32( xy0, xy1);
1526 x = vaddq_f32(xy.val[0], xy.val[1]);
1527 x = vaddq_f32(x, z);
1528
1529 mask = vcltq_f32(x, minDot);
1530 minDot = vbslq_f32( mask, x, minDot);
1531 index = vbslq_u32(mask, local_index, index);
1532 local_index = vaddq_u32(local_index, four);
1533 }
1534
1535 for( ; i + 4 <= count; i += 4 )
1536 {
1537 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1538 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1539 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1540 float32x4_t v3 = vld1q_f32_aligned_postincrement( vv );
1541
1542 // the next two lines should resolve to a single vswp d, d
1543 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1544 float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3));
1545 // the next two lines should resolve to a single vswp d, d
1546 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1547 float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3));
1548
1549 xy0 = vmulq_f32(xy0, vLo);
1550 xy1 = vmulq_f32(xy1, vLo);
1551
1552 float32x4x2_t zb = vuzpq_f32( z0, z1);
1553 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1554 float32x4x2_t xy = vuzpq_f32( xy0, xy1);
1555 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1556 x = vaddq_f32(x, z);
1557
1558 uint32x4_t mask = vcltq_f32(x, minDot);
1559 minDot = vbslq_f32( mask, x, minDot);
1560 index = vbslq_u32(mask, local_index, index);
1561 local_index = vaddq_u32(local_index, four);
1562 }
1563
1564 switch (count & 3) {
1565 case 3:
1566 {
1567 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1568 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1569 float32x4_t v2 = vld1q_f32_aligned_postincrement( vv );
1570
1571 // the next two lines should resolve to a single vswp d, d
1572 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1573 float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v2));
1574 // the next two lines should resolve to a single vswp d, d
1575 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1576 float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v2));
1577
1578 xy0 = vmulq_f32(xy0, vLo);
1579 xy1 = vmulq_f32(xy1, vLo);
1580
1581 float32x4x2_t zb = vuzpq_f32( z0, z1);
1582 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1583 float32x4x2_t xy = vuzpq_f32( xy0, xy1);
1584 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1585 x = vaddq_f32(x, z);
1586
1587 uint32x4_t mask = vcltq_f32(x, minDot);
1588 minDot = vbslq_f32( mask, x, minDot);
1589 index = vbslq_u32(mask, local_index, index);
1590 local_index = vaddq_u32(local_index, four);
1591 }
1592 break;
1593
1594 case 2:
1595 {
1596 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1597 float32x4_t v1 = vld1q_f32_aligned_postincrement( vv );
1598
1599 // the next two lines should resolve to a single vswp d, d
1600 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1));
1601 // the next two lines should resolve to a single vswp d, d
1602 float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1));
1603
1604 xy0 = vmulq_f32(xy0, vLo);
1605
1606 float32x4x2_t zb = vuzpq_f32( z0, z0);
1607 float32x4_t z = vmulq_f32( zb.val[0], vHi);
1608 float32x4x2_t xy = vuzpq_f32( xy0, xy0);
1609 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1610 x = vaddq_f32(x, z);
1611
1612 uint32x4_t mask = vcltq_f32(x, minDot);
1613 minDot = vbslq_f32( mask, x, minDot);
1614 index = vbslq_u32(mask, local_index, index);
1615 local_index = vaddq_u32(local_index, four);
1616 }
1617 break;
1618
1619 case 1:
1620 {
1621 float32x4_t v0 = vld1q_f32_aligned_postincrement( vv );
1622
1623 // the next two lines should resolve to a single vswp d, d
1624 float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v0));
1625 // the next two lines should resolve to a single vswp d, d
1626 float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0);
1627
1628 xy0 = vmulq_f32(xy0, vLo);
1629
1630 z = vmulq_f32( z, vHi);
1631 float32x4x2_t xy = vuzpq_f32( xy0, xy0);
1632 float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
1633 x = vaddq_f32(x, z);
1634
1635 uint32x4_t mask = vcltq_f32(x, minDot);
1636 minDot = vbslq_f32( mask, x, minDot);
1637 index = vbslq_u32(mask, local_index, index);
1638 local_index = vaddq_u32(local_index, four);
1639 }
1640 break;
1641
1642 default:
1643 break;
1644 }
1645
1646
1647 // select best answer between hi and lo results
1648 uint32x2_t mask = vclt_f32( vget_high_f32(minDot), vget_low_f32(minDot));
1649 float32x2_t minDot2 = vbsl_f32(mask, vget_high_f32(minDot), vget_low_f32(minDot));
1650 uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
1651
1652 // select best answer between even and odd results
1653 float32x2_t minDotO = vdup_lane_f32(minDot2, 1);
1654 uint32x2_t indexHi = vdup_lane_u32(index2, 1);
1655 mask = vclt_f32( minDotO, minDot2 );
1656 minDot2 = vbsl_f32(mask, minDotO, minDot2);
1657 index2 = vbsl_u32(mask, indexHi, index2);
1658
1659 *dotResult = vget_lane_f32( minDot2, 0);
1660 return vget_lane_u32(index2, 0);
1661
1662}
1663
1664#else
1665 #error Unhandled __APPLE__ arch
1666#endif
1667
1668#endif /* __APPLE__ */
1669
1670
uint32_t
unsigned int uint32_t
Definition btConvexHullComputer.cpp:32
btMax
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:29
btUnlikely
#define btUnlikely(_c)
Definition btScalar.h:137
BT_INFINITY
#define BT_INFINITY
Definition btScalar.h:383
Generated by doxygen 1.10.0