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