1 /*
2 Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9
10 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.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15
16
17 #ifndef BT_SCALAR_H
18 #define BT_SCALAR_H
19
20 #ifdef BT_MANAGED_CODE
21 //Aligned data types not supported in managed code
22 #pragma unmanaged
23 #endif
24
25
26 #include <math.h>
27 #include <stdlib.h>//size_t for MSVC 6.0
28 #include <float.h>
29
30 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
31 #define BT_BULLET_VERSION 283
32
btGetVersion()33 inline int btGetVersion()
34 {
35 return BT_BULLET_VERSION;
36 }
37
38 #if defined(DEBUG) || defined (_DEBUG)
39 #define BT_DEBUG
40 #endif
41
42
43 #ifdef _WIN32
44
45 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
46
47 #define SIMD_FORCE_INLINE inline
48 #define ATTRIBUTE_ALIGNED16(a) a
49 #define ATTRIBUTE_ALIGNED64(a) a
50 #define ATTRIBUTE_ALIGNED128(a) a
51 #elif (_M_ARM)
52 #define SIMD_FORCE_INLINE __forceinline
53 #define ATTRIBUTE_ALIGNED16(a) __declspec() a
54 #define ATTRIBUTE_ALIGNED64(a) __declspec() a
55 #define ATTRIBUTE_ALIGNED128(a) __declspec () a
56 #else
57 //#define BT_HAS_ALIGNED_ALLOCATOR
58 #pragma warning(disable : 4324) // disable padding warning
59 // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
60 // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
61 // #pragma warning(disable:4786) // Disable the "debug name too long" warning
62
63 #define SIMD_FORCE_INLINE __forceinline
64 #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
65 #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
66 #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
67 #ifdef _XBOX
68 #define BT_USE_VMX128
69
70 #include <ppcintrinsics.h>
71 #define BT_HAVE_NATIVE_FSEL
72 #define btFsel(a,b,c) __fsel((a),(b),(c))
73 #else
74
75 #if defined (_M_ARM)
76 //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however)
77 #elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
78 #if _MSC_VER>1400
79 #define BT_USE_SIMD_VECTOR3
80 #endif
81
82 #define BT_USE_SSE
83 #ifdef BT_USE_SSE
84
85 #if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default)
86 #define BT_ALLOW_SSE4
87 #endif //(_MSC_FULL_VER >= 160040219)
88
89 //BT_USE_SSE_IN_API is disabled under Windows by default, because
90 //it makes it harder to integrate Bullet into your application under Windows
91 //(structured embedding Bullet structs/classes need to be 16-byte aligned)
92 //with relatively little performance gain
93 //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
94 //you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
95 //#define BT_USE_SSE_IN_API
96 #endif //BT_USE_SSE
97 #include <emmintrin.h>
98 #endif
99
100 #endif//_XBOX
101
102 #endif //__MINGW32__
103
104 #ifdef BT_DEBUG
105 #ifdef _MSC_VER
106 #include <stdio.h>
107 #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }}
108 #else//_MSC_VER
109 #include <assert.h>
110 #define btAssert assert
111 #endif//_MSC_VER
112 #else
113 #define btAssert(x)
114 #endif
115 //btFullAssert is optional, slows down a lot
116 #define btFullAssert(x)
117
118 #define btLikely(_c) _c
119 #define btUnlikely(_c) _c
120
121 #else
122
123 #if defined (__CELLOS_LV2__)
124 #define SIMD_FORCE_INLINE inline __attribute__((always_inline))
125 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
126 #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
127 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
128 #ifndef assert
129 #include <assert.h>
130 #endif
131 #ifdef BT_DEBUG
132 #ifdef __SPU__
133 #include <spu_printf.h>
134 #define printf spu_printf
135 #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
136 #else
137 #define btAssert assert
138 #endif
139
140 #else
141 #define btAssert(x)
142 #endif
143 //btFullAssert is optional, slows down a lot
144 #define btFullAssert(x)
145
146 #define btLikely(_c) _c
147 #define btUnlikely(_c) _c
148
149 #else
150
151 #ifdef USE_LIBSPE2
152
153 #define SIMD_FORCE_INLINE __inline
154 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
155 #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
156 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
157 #ifndef assert
158 #include <assert.h>
159 #endif
160 #ifdef BT_DEBUG
161 #define btAssert assert
162 #else
163 #define btAssert(x)
164 #endif
165 //btFullAssert is optional, slows down a lot
166 #define btFullAssert(x)
167
168
169 #define btLikely(_c) __builtin_expect((_c), 1)
170 #define btUnlikely(_c) __builtin_expect((_c), 0)
171
172
173 #else
174 //non-windows systems
175
176 #if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
177 #if defined (__i386__) || defined (__x86_64__)
178 #define BT_USE_SIMD_VECTOR3
179 #define BT_USE_SSE
180 //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
181 //if apps run into issues, we will disable the next line
182 #define BT_USE_SSE_IN_API
183 #ifdef BT_USE_SSE
184 // include appropriate SSE level
185 #if defined (__SSE4_1__)
186 #include <smmintrin.h>
187 #elif defined (__SSSE3__)
188 #include <tmmintrin.h>
189 #elif defined (__SSE3__)
190 #include <pmmintrin.h>
191 #else
192 #include <emmintrin.h>
193 #endif
194 #endif //BT_USE_SSE
195 #elif defined( __ARM_NEON__ )
196 #ifdef __clang__
197 #define BT_USE_NEON 1
198 #define BT_USE_SIMD_VECTOR3
199
200 #if defined BT_USE_NEON && defined (__clang__)
201 #include <arm_neon.h>
202 #endif//BT_USE_NEON
203 #endif //__clang__
204 #endif//__arm__
205
206 #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
207 ///@todo: check out alignment methods for other platforms/compilers
208 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
209 #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
210 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
211 #ifndef assert
212 #include <assert.h>
213 #endif
214
215 #if defined(DEBUG) || defined (_DEBUG)
216 #if defined (__i386__) || defined (__x86_64__)
217 #include <stdio.h>
218 #define btAssert(x)\
219 {\
220 if(!(x))\
221 {\
222 printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
223 asm volatile ("int3");\
224 }\
225 }
226 #else//defined (__i386__) || defined (__x86_64__)
227 #define btAssert assert
228 #endif//defined (__i386__) || defined (__x86_64__)
229 #else//defined(DEBUG) || defined (_DEBUG)
230 #define btAssert(x)
231 #endif//defined(DEBUG) || defined (_DEBUG)
232
233 //btFullAssert is optional, slows down a lot
234 #define btFullAssert(x)
235 #define btLikely(_c) _c
236 #define btUnlikely(_c) _c
237
238 #else
239
240 #define SIMD_FORCE_INLINE inline
241 ///@todo: check out alignment methods for other platforms/compilers
242 ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
243 ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
244 ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
245 #define ATTRIBUTE_ALIGNED16(a) a
246 #define ATTRIBUTE_ALIGNED64(a) a
247 #define ATTRIBUTE_ALIGNED128(a) a
248 #ifndef assert
249 #include <assert.h>
250 #endif
251
252 #if defined(DEBUG) || defined (_DEBUG)
253 #define btAssert assert
254 #else
255 #define btAssert(x)
256 #endif
257
258 //btFullAssert is optional, slows down a lot
259 #define btFullAssert(x)
260 #define btLikely(_c) _c
261 #define btUnlikely(_c) _c
262 #endif //__APPLE__
263
264 #endif // LIBSPE2
265
266 #endif //__CELLOS_LV2__
267 #endif
268
269
270 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
271 #if defined(BT_USE_DOUBLE_PRECISION)
272
273 #define btScalar double
274 //this number could be bigger in double precision
275 #define BT_LARGE_FLOAT 1e30
276 #else
277
278 #define btScalar float
279 //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
280 #define BT_LARGE_FLOAT 1e18f
281 #endif
282
283 #ifdef BT_USE_SSE
284 typedef __m128 btSimdFloat4;
285 #endif//BT_USE_SSE
286
287 #if defined (BT_USE_SSE)
288 //#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
289 #ifdef _WIN32
290
291 #ifndef BT_NAN
292 static int btNanMask = 0x7F800001;
293 #define BT_NAN (*(float*)&btNanMask)
294 #endif
295
296 #ifndef BT_INFINITY
297 static int btInfinityMask = 0x7F800000;
298 #define BT_INFINITY (*(float*)&btInfinityMask)
btGetInfinityMask()299 inline int btGetInfinityMask()//suppress stupid compiler warning
300 {
301 return btInfinityMask;
302 }
303 #endif
304
305 //use this, in case there are clashes (such as xnamath.h)
306 #ifndef BT_NO_SIMD_OPERATOR_OVERLOADS
307 inline __m128 operator + (const __m128 A, const __m128 B)
308 {
309 return _mm_add_ps(A, B);
310 }
311
312 inline __m128 operator - (const __m128 A, const __m128 B)
313 {
314 return _mm_sub_ps(A, B);
315 }
316
317 inline __m128 operator * (const __m128 A, const __m128 B)
318 {
319 return _mm_mul_ps(A, B);
320 }
321 #endif //BT_NO_SIMD_OPERATOR_OVERLOADS
322
323 #define btCastfTo128i(a) (_mm_castps_si128(a))
324 #define btCastfTo128d(a) (_mm_castps_pd(a))
325 #define btCastiTo128f(a) (_mm_castsi128_ps(a))
326 #define btCastdTo128f(a) (_mm_castpd_ps(a))
327 #define btCastdTo128i(a) (_mm_castpd_si128(a))
328 #define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
329
330 #else//_WIN32
331
332 #define btCastfTo128i(a) ((__m128i)(a))
333 #define btCastfTo128d(a) ((__m128d)(a))
334 #define btCastiTo128f(a) ((__m128) (a))
335 #define btCastdTo128f(a) ((__m128) (a))
336 #define btCastdTo128i(a) ((__m128i)(a))
337 #define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
338 #define BT_INFINITY INFINITY
339 #define BT_NAN NAN
340 #endif//_WIN32
341 #else
342
343 #ifdef BT_USE_NEON
344 #include <arm_neon.h>
345
346 typedef float32x4_t btSimdFloat4;
347 #define BT_INFINITY INFINITY
348 #define BT_NAN NAN
349 #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
350 #else//BT_USE_NEON
351
352 #ifndef BT_INFINITY
353 struct btInfMaskConverter
354 {
355 union {
356 float mask;
357 int intmask;
358 };
359 btInfMaskConverter(int mask=0x7F800000)
intmaskbtInfMaskConverter360 :intmask(mask)
361 {
362 }
363 };
364 static btInfMaskConverter btInfinityMask = 0x7F800000;
365 #define BT_INFINITY (btInfinityMask.mask)
btGetInfinityMask()366 inline int btGetInfinityMask()//suppress stupid compiler warning
367 {
368 return btInfinityMask.intmask;
369 }
370 #endif
371 #endif//BT_USE_NEON
372
373 #endif //BT_USE_SSE
374
375 #ifdef BT_USE_NEON
376 #include <arm_neon.h>
377
378 typedef float32x4_t btSimdFloat4;
379 #define BT_INFINITY INFINITY
380 #define BT_NAN NAN
381 #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
382 #endif
383
384
385
386
387
388 #define BT_DECLARE_ALIGNED_ALLOCATOR() \
389 SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
390 SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \
391 SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
392 SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
393 SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
394 SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \
395 SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
396 SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
397
398
399
400 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
401
btSqrt(btScalar x)402 SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
btFabs(btScalar x)403 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
btCos(btScalar x)404 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
btSin(btScalar x)405 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
btTan(btScalar x)406 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
btAcos(btScalar x)407 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1)) x=btScalar(-1); if (x>btScalar(1)) x=btScalar(1); return acos(x); }
btAsin(btScalar x)408 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1)) x=btScalar(-1); if (x>btScalar(1)) x=btScalar(1); return asin(x); }
btAtan(btScalar x)409 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
btAtan2(btScalar x,btScalar y)410 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
btExp(btScalar x)411 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
btLog(btScalar x)412 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
btPow(btScalar x,btScalar y)413 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
btFmod(btScalar x,btScalar y)414 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
415
416 #else
417
btSqrt(btScalar y)418 SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
419 {
420 #ifdef USE_APPROXIMATION
421 #ifdef __LP64__
422 float xhalf = 0.5f*y;
423 int i = *(int*)&y;
424 i = 0x5f375a86 - (i>>1);
425 y = *(float*)&i;
426 y = y*(1.5f - xhalf*y*y);
427 y = y*(1.5f - xhalf*y*y);
428 y = y*(1.5f - xhalf*y*y);
429 y=1/y;
430 return y;
431 #else
432 double x, z, tempf;
433 unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
434 tempf = y;
435 *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
436 x = tempf;
437 z = y*btScalar(0.5);
438 x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */
439 x = (btScalar(1.5)*x)-(x*x)*(x*z);
440 x = (btScalar(1.5)*x)-(x*x)*(x*z);
441 x = (btScalar(1.5)*x)-(x*x)*(x*z);
442 x = (btScalar(1.5)*x)-(x*x)*(x*z);
443 return x*y;
444 #endif
445 #else
446 return sqrtf(y);
447 #endif
448 }
btFabs(btScalar x)449 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
btCos(btScalar x)450 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
btSin(btScalar x)451 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
btTan(btScalar x)452 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
btAcos(btScalar x)453 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
454 if (x<btScalar(-1))
455 x=btScalar(-1);
456 if (x>btScalar(1))
457 x=btScalar(1);
458 return acosf(x);
459 }
btAsin(btScalar x)460 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
461 if (x<btScalar(-1))
462 x=btScalar(-1);
463 if (x>btScalar(1))
464 x=btScalar(1);
465 return asinf(x);
466 }
btAtan(btScalar x)467 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
btAtan2(btScalar x,btScalar y)468 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
btExp(btScalar x)469 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
btLog(btScalar x)470 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
btPow(btScalar x,btScalar y)471 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
btFmod(btScalar x,btScalar y)472 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
473
474 #endif
475
476 #define SIMD_PI btScalar(3.1415926535897932384626433832795029)
477 #define SIMD_2_PI (btScalar(2.0) * SIMD_PI)
478 #define SIMD_HALF_PI (SIMD_PI * btScalar(0.5))
479 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
480 #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
481 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
482
483 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */
484 #define btRecip(x) (btScalar(1.0)/btScalar(x))
485
486 #ifdef BT_USE_DOUBLE_PRECISION
487 #define SIMD_EPSILON DBL_EPSILON
488 #define SIMD_INFINITY DBL_MAX
489 #define BT_ONE 1.0
490 #define BT_ZERO 0.0
491 #define BT_TWO 2.0
492 #define BT_HALF 0.5
493 #else
494 #define SIMD_EPSILON FLT_EPSILON
495 #define SIMD_INFINITY FLT_MAX
496 #define BT_ONE 1.0f
497 #define BT_ZERO 0.0f
498 #define BT_TWO 2.0f
499 #define BT_HALF 0.5f
500 #endif
501
btAtan2Fast(btScalar y,btScalar x)502 SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
503 {
504 btScalar coeff_1 = SIMD_PI / 4.0f;
505 btScalar coeff_2 = 3.0f * coeff_1;
506 btScalar abs_y = btFabs(y);
507 btScalar angle;
508 if (x >= 0.0f) {
509 btScalar r = (x - abs_y) / (x + abs_y);
510 angle = coeff_1 - coeff_1 * r;
511 } else {
512 btScalar r = (x + abs_y) / (abs_y - x);
513 angle = coeff_2 - coeff_1 * r;
514 }
515 return (y < 0.0f) ? -angle : angle;
516 }
517
btFuzzyZero(btScalar x)518 SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
519
btEqual(btScalar a,btScalar eps)520 SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) {
521 return (((a) <= eps) && !((a) < -eps));
522 }
btGreaterEqual(btScalar a,btScalar eps)523 SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) {
524 return (!((a) <= eps));
525 }
526
527
btIsNegative(btScalar x)528 SIMD_FORCE_INLINE int btIsNegative(btScalar x) {
529 return x < btScalar(0.0) ? 1 : 0;
530 }
531
btRadians(btScalar x)532 SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
btDegrees(btScalar x)533 SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
534
535 #define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
536
537 #ifndef btFsel
btFsel(btScalar a,btScalar b,btScalar c)538 SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
539 {
540 return a >= 0 ? b : c;
541 }
542 #endif
543 #define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
544
545
btMachineIsLittleEndian()546 SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
547 {
548 long int i = 1;
549 const char *p = (const char *) &i;
550 if (p[0] == 1) // Lowest address contains the least significant byte
551 return true;
552 else
553 return false;
554 }
555
556
557
558 ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
559 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
btSelect(unsigned condition,unsigned valueIfConditionNonZero,unsigned valueIfConditionZero)560 SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
561 {
562 // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
563 // Rely on positive value or'ed with its negative having sign bit on
564 // and zero value or'ed with its negative (which is still zero) having sign bit off
565 // Use arithmetic shift right, shifting the sign bit through all 32 bits
566 unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
567 unsigned testEqz = ~testNz;
568 return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
569 }
btSelect(unsigned condition,int valueIfConditionNonZero,int valueIfConditionZero)570 SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
571 {
572 unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
573 unsigned testEqz = ~testNz;
574 return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
575 }
btSelect(unsigned condition,float valueIfConditionNonZero,float valueIfConditionZero)576 SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
577 {
578 #ifdef BT_HAVE_NATIVE_FSEL
579 return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
580 #else
581 return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
582 #endif
583 }
584
btSwap(T & a,T & b)585 template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
586 {
587 T tmp = a;
588 a = b;
589 b = tmp;
590 }
591
592
593 //PCK: endian swapping functions
btSwapEndian(unsigned val)594 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
595 {
596 return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
597 }
598
btSwapEndian(unsigned short val)599 SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
600 {
601 return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
602 }
603
btSwapEndian(int val)604 SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
605 {
606 return btSwapEndian((unsigned)val);
607 }
608
btSwapEndian(short val)609 SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
610 {
611 return btSwapEndian((unsigned short) val);
612 }
613
614 ///btSwapFloat uses using char pointers to swap the endianness
615 ////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
616 ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
617 ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
618 ///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
619 ///so instead of returning a float/double, we return integer/long long integer
btSwapEndianFloat(float d)620 SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
621 {
622 unsigned int a = 0;
623 unsigned char *dst = (unsigned char *)&a;
624 unsigned char *src = (unsigned char *)&d;
625
626 dst[0] = src[3];
627 dst[1] = src[2];
628 dst[2] = src[1];
629 dst[3] = src[0];
630 return a;
631 }
632
633 // unswap using char pointers
btUnswapEndianFloat(unsigned int a)634 SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
635 {
636 float d = 0.0f;
637 unsigned char *src = (unsigned char *)&a;
638 unsigned char *dst = (unsigned char *)&d;
639
640 dst[0] = src[3];
641 dst[1] = src[2];
642 dst[2] = src[1];
643 dst[3] = src[0];
644
645 return d;
646 }
647
648
649 // swap using char pointers
btSwapEndianDouble(double d,unsigned char * dst)650 SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
651 {
652 unsigned char *src = (unsigned char *)&d;
653
654 dst[0] = src[7];
655 dst[1] = src[6];
656 dst[2] = src[5];
657 dst[3] = src[4];
658 dst[4] = src[3];
659 dst[5] = src[2];
660 dst[6] = src[1];
661 dst[7] = src[0];
662
663 }
664
665 // unswap using char pointers
btUnswapEndianDouble(const unsigned char * src)666 SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
667 {
668 double d = 0.0;
669 unsigned char *dst = (unsigned char *)&d;
670
671 dst[0] = src[7];
672 dst[1] = src[6];
673 dst[2] = src[5];
674 dst[3] = src[4];
675 dst[4] = src[3];
676 dst[5] = src[2];
677 dst[6] = src[1];
678 dst[7] = src[0];
679
680 return d;
681 }
682
683 template<typename T>
btSetZero(T * a,int n)684 SIMD_FORCE_INLINE void btSetZero(T* a, int n)
685 {
686 T* acurr = a;
687 size_t ncurr = n;
688 while (ncurr > 0)
689 {
690 *(acurr++) = 0;
691 --ncurr;
692 }
693 }
694
695
btLargeDot(const btScalar * a,const btScalar * b,int n)696 SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
697 {
698 btScalar p0,q0,m0,p1,q1,m1,sum;
699 sum = 0;
700 n -= 2;
701 while (n >= 0) {
702 p0 = a[0]; q0 = b[0];
703 m0 = p0 * q0;
704 p1 = a[1]; q1 = b[1];
705 m1 = p1 * q1;
706 sum += m0;
707 sum += m1;
708 a += 2;
709 b += 2;
710 n -= 2;
711 }
712 n += 2;
713 while (n > 0) {
714 sum += (*a) * (*b);
715 a++;
716 b++;
717 n--;
718 }
719 return sum;
720 }
721
722
723 // returns normalized value in range [-SIMD_PI, SIMD_PI]
btNormalizeAngle(btScalar angleInRadians)724 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
725 {
726 angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
727 if(angleInRadians < -SIMD_PI)
728 {
729 return angleInRadians + SIMD_2_PI;
730 }
731 else if(angleInRadians > SIMD_PI)
732 {
733 return angleInRadians - SIMD_2_PI;
734 }
735 else
736 {
737 return angleInRadians;
738 }
739 }
740
741
742
743 ///rudimentary class to provide type info
744 struct btTypedObject
745 {
btTypedObjectbtTypedObject746 btTypedObject(int objectType)
747 :m_objectType(objectType)
748 {
749 }
750 int m_objectType;
getObjectTypebtTypedObject751 inline int getObjectType() const
752 {
753 return m_objectType;
754 }
755 };
756
757
758
759 ///align a pointer to the provided alignment, upwards
btAlignPointer(T * unalignedPtr,size_t alignment)760 template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
761 {
762
763 struct btConvertPointerSizeT
764 {
765 union
766 {
767 T* ptr;
768 size_t integer;
769 };
770 };
771 btConvertPointerSizeT converter;
772
773
774 const size_t bit_mask = ~(alignment - 1);
775 converter.ptr = unalignedPtr;
776 converter.integer += alignment-1;
777 converter.integer &= bit_mask;
778 return converter.ptr;
779 }
780
781
782 #endif //BT_SCALAR_H
783