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