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 #include <TargetConditionals.h>
178 #if (defined (__i386__) || defined (__x86_64__)) && (!(TARGET_IPHONE_SIMULATOR))
179 //#define BT_USE_SIMD_VECTOR3
180 //#define BT_USE_SSE
181 //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
182 //if apps run into issues, we will disable the next line
183 //#define BT_USE_SSE_IN_API
184 #ifdef BT_USE_SSE
185 // include appropriate SSE level
186 #if defined (__SSE4_1__)
187 #include <smmintrin.h>
188 #elif defined (__SSSE3__)
189 #include <tmmintrin.h>
190 #elif defined (__SSE3__)
191 #include <pmmintrin.h>
192 #else
193 #include <emmintrin.h>
194 #endif
195 #endif //BT_USE_SSE
196 #elif defined( __ARM_NEON__ )
197 #ifdef __clang__
198 #define BT_USE_NEON 1
199 #define BT_USE_SIMD_VECTOR3
200
201 #if defined BT_USE_NEON && defined (__clang__)
202 #include <arm_neon.h>
203 #endif//BT_USE_NEON
204 #endif //__clang__
205 #endif//__arm__
206
207 #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
208 ///@todo: check out alignment methods for other platforms/compilers
209 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
210 #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
211 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
212 #ifndef assert
213 #include <assert.h>
214 #endif
215
216 #if defined(DEBUG) || defined (_DEBUG)
217 #if defined (__i386__) || defined (__x86_64__)
218 #include <stdio.h>
219 #define btAssert(x)\
220 {\
221 if(!(x))\
222 {\
223 printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
224 asm volatile ("int3");\
225 }\
226 }
227 #else//defined (__i386__) || defined (__x86_64__)
228 #define btAssert assert
229 #endif//defined (__i386__) || defined (__x86_64__)
230 #else//defined(DEBUG) || defined (_DEBUG)
231 #define btAssert(x)
232 #endif//defined(DEBUG) || defined (_DEBUG)
233
234 //btFullAssert is optional, slows down a lot
235 #define btFullAssert(x)
236 #define btLikely(_c) _c
237 #define btUnlikely(_c) _c
238
239 #else
240
241 #define SIMD_FORCE_INLINE inline
242 ///@todo: check out alignment methods for other platforms/compilers
243 ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
244 ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
245 ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
246 #define ATTRIBUTE_ALIGNED16(a) a
247 #define ATTRIBUTE_ALIGNED64(a) a
248 #define ATTRIBUTE_ALIGNED128(a) a
249 #ifndef assert
250 #include <assert.h>
251 #endif
252
253 #if defined(DEBUG) || defined (_DEBUG)
254 #define btAssert assert
255 #else
256 #define btAssert(x)
257 #endif
258
259 //btFullAssert is optional, slows down a lot
260 #define btFullAssert(x)
261 #define btLikely(_c) _c
262 #define btUnlikely(_c) _c
263 #endif //__APPLE__
264
265 #endif // LIBSPE2
266
267 #endif //__CELLOS_LV2__
268 #endif
269
270
271 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
272 #if defined(BT_USE_DOUBLE_PRECISION)
273
274 typedef double btScalar;
275 //this number could be bigger in double precision
276 #define BT_LARGE_FLOAT 1e30
277 #else
278
279 typedef float btScalar;
280 //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
281 #define BT_LARGE_FLOAT 1e18f
282 #endif
283
284 #ifdef BT_USE_SSE
285 typedef __m128 btSimdFloat4;
286 #endif//BT_USE_SSE
287
288 #if defined (BT_USE_SSE)
289 //#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
290 #ifdef _WIN32
291
292 #ifndef BT_NAN
293 static int btNanMask = 0x7F800001;
294 #define BT_NAN (*(float*)&btNanMask)
295 #endif
296
297 #ifndef BT_INFINITY
298 static int btInfinityMask = 0x7F800000;
299 #define BT_INFINITY (*(float*)&btInfinityMask)
btGetInfinityMask()300 inline int btGetInfinityMask()//suppress stupid compiler warning
301 {
302 return btInfinityMask;
303 }
304 #endif
305
306 //use this, in case there are clashes (such as xnamath.h)
307 #ifndef BT_NO_SIMD_OPERATOR_OVERLOADS
308 inline __m128 operator + (const __m128 A, const __m128 B)
309 {
310 return _mm_add_ps(A, B);
311 }
312
313 inline __m128 operator - (const __m128 A, const __m128 B)
314 {
315 return _mm_sub_ps(A, B);
316 }
317
318 inline __m128 operator * (const __m128 A, const __m128 B)
319 {
320 return _mm_mul_ps(A, B);
321 }
322 #endif //BT_NO_SIMD_OPERATOR_OVERLOADS
323
324 #define btCastfTo128i(a) (_mm_castps_si128(a))
325 #define btCastfTo128d(a) (_mm_castps_pd(a))
326 #define btCastiTo128f(a) (_mm_castsi128_ps(a))
327 #define btCastdTo128f(a) (_mm_castpd_ps(a))
328 #define btCastdTo128i(a) (_mm_castpd_si128(a))
329 #define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
330
331 #else//_WIN32
332
333 #define btCastfTo128i(a) ((__m128i)(a))
334 #define btCastfTo128d(a) ((__m128d)(a))
335 #define btCastiTo128f(a) ((__m128) (a))
336 #define btCastdTo128f(a) ((__m128) (a))
337 #define btCastdTo128i(a) ((__m128i)(a))
338 #define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
339 #define BT_INFINITY INFINITY
340 #define BT_NAN NAN
341 #endif//_WIN32
342 #else
343
344 #ifdef BT_USE_NEON
345 #include <arm_neon.h>
346
347 typedef float32x4_t btSimdFloat4;
348 #define BT_INFINITY INFINITY
349 #define BT_NAN NAN
350 #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
351 #else//BT_USE_NEON
352
353 #ifndef BT_INFINITY
354 struct btInfMaskConverter
355 {
356 union {
357 float mask;
358 int intmask;
359 };
360 btInfMaskConverter(int mask=0x7F800000)
intmaskbtInfMaskConverter361 :intmask(mask)
362 {
363 }
364 };
365 static btInfMaskConverter btInfinityMask = 0x7F800000;
366 #define BT_INFINITY (btInfinityMask.mask)
btGetInfinityMask()367 inline int btGetInfinityMask()//suppress stupid compiler warning
368 {
369 return btInfinityMask.intmask;
370 }
371 #endif
372 #endif//BT_USE_NEON
373
374 #endif //BT_USE_SSE
375
376 #ifdef BT_USE_NEON
377 #include <arm_neon.h>
378
379 typedef float32x4_t btSimdFloat4;
380 #define BT_INFINITY INFINITY
381 #define BT_NAN NAN
382 #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
383 #endif
384
385
386
387
388
389 #define BT_DECLARE_ALIGNED_ALLOCATOR() \
390 SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
391 SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \
392 SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
393 SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
394 SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
395 SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \
396 SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
397 SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
398
399
400
401 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
402
btSqrt(btScalar x)403 SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
btFabs(btScalar x)404 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
btCos(btScalar x)405 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
btSin(btScalar x)406 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
btTan(btScalar x)407 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
btAcos(btScalar x)408 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)409 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)410 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
btAtan2(btScalar x,btScalar y)411 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
btExp(btScalar x)412 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
btLog(btScalar x)413 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
btPow(btScalar x,btScalar y)414 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
btFmod(btScalar x,btScalar y)415 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
416
417 #else
418
btSqrt(btScalar y)419 SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
420 {
421 #ifdef USE_APPROXIMATION
422 #ifdef __LP64__
423 float xhalf = 0.5f*y;
424 int i = *(int*)&y;
425 i = 0x5f375a86 - (i>>1);
426 y = *(float*)&i;
427 y = y*(1.5f - xhalf*y*y);
428 y = y*(1.5f - xhalf*y*y);
429 y = y*(1.5f - xhalf*y*y);
430 y=1/y;
431 return y;
432 #else
433 double x, z, tempf;
434 unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
435 tempf = y;
436 *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
437 x = tempf;
438 z = y*btScalar(0.5);
439 x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */
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 x = (btScalar(1.5)*x)-(x*x)*(x*z);
444 return x*y;
445 #endif
446 #else
447 return sqrtf(y);
448 #endif
449 }
btFabs(btScalar x)450 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
btCos(btScalar x)451 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
btSin(btScalar x)452 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
btTan(btScalar x)453 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
btAcos(btScalar x)454 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
455 if (x<btScalar(-1))
456 x=btScalar(-1);
457 if (x>btScalar(1))
458 x=btScalar(1);
459 return acosf(x);
460 }
btAsin(btScalar x)461 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
462 if (x<btScalar(-1))
463 x=btScalar(-1);
464 if (x>btScalar(1))
465 x=btScalar(1);
466 return asinf(x);
467 }
btAtan(btScalar x)468 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
btAtan2(btScalar x,btScalar y)469 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
btExp(btScalar x)470 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
btLog(btScalar x)471 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
btPow(btScalar x,btScalar y)472 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
btFmod(btScalar x,btScalar y)473 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
474
475 #endif
476
477 #define SIMD_PI btScalar(3.1415926535897932384626433832795029)
478 #define SIMD_2_PI (btScalar(2.0) * SIMD_PI)
479 #define SIMD_HALF_PI (SIMD_PI * btScalar(0.5))
480 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
481 #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
482 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
483
484 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */
485 #define btRecip(x) (btScalar(1.0)/btScalar(x))
486
487 #ifdef BT_USE_DOUBLE_PRECISION
488 #define SIMD_EPSILON DBL_EPSILON
489 #define SIMD_INFINITY DBL_MAX
490 #define BT_ONE 1.0
491 #define BT_ZERO 0.0
492 #define BT_TWO 2.0
493 #define BT_HALF 0.5
494 #else
495 #define SIMD_EPSILON FLT_EPSILON
496 #define SIMD_INFINITY FLT_MAX
497 #define BT_ONE 1.0f
498 #define BT_ZERO 0.0f
499 #define BT_TWO 2.0f
500 #define BT_HALF 0.5f
501 #endif
502
btAtan2Fast(btScalar y,btScalar x)503 SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
504 {
505 btScalar coeff_1 = SIMD_PI / 4.0f;
506 btScalar coeff_2 = 3.0f * coeff_1;
507 btScalar abs_y = btFabs(y);
508 btScalar angle;
509 if (x >= 0.0f) {
510 btScalar r = (x - abs_y) / (x + abs_y);
511 angle = coeff_1 - coeff_1 * r;
512 } else {
513 btScalar r = (x + abs_y) / (abs_y - x);
514 angle = coeff_2 - coeff_1 * r;
515 }
516 return (y < 0.0f) ? -angle : angle;
517 }
518
btFuzzyZero(btScalar x)519 SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
520
btEqual(btScalar a,btScalar eps)521 SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) {
522 return (((a) <= eps) && !((a) < -eps));
523 }
btGreaterEqual(btScalar a,btScalar eps)524 SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) {
525 return (!((a) <= eps));
526 }
527
528
btIsNegative(btScalar x)529 SIMD_FORCE_INLINE int btIsNegative(btScalar x) {
530 return x < btScalar(0.0) ? 1 : 0;
531 }
532
btRadians(btScalar x)533 SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
btDegrees(btScalar x)534 SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
535
536 #define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
537
538 #ifndef btFsel
btFsel(btScalar a,btScalar b,btScalar c)539 SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
540 {
541 return a >= 0 ? b : c;
542 }
543 #endif
544 #define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
545
546
btMachineIsLittleEndian()547 SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
548 {
549 long int i = 1;
550 const char *p = (const char *) &i;
551 if (p[0] == 1) // Lowest address contains the least significant byte
552 return true;
553 else
554 return false;
555 }
556
557
558
559 ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
560 ///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)561 SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
562 {
563 // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
564 // Rely on positive value or'ed with its negative having sign bit on
565 // and zero value or'ed with its negative (which is still zero) having sign bit off
566 // Use arithmetic shift right, shifting the sign bit through all 32 bits
567 unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
568 unsigned testEqz = ~testNz;
569 return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
570 }
btSelect(unsigned condition,int valueIfConditionNonZero,int valueIfConditionZero)571 SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
572 {
573 unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
574 unsigned testEqz = ~testNz;
575 return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
576 }
btSelect(unsigned condition,float valueIfConditionNonZero,float valueIfConditionZero)577 SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
578 {
579 #ifdef BT_HAVE_NATIVE_FSEL
580 return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
581 #else
582 return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
583 #endif
584 }
585
btSwap(T & a,T & b)586 template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
587 {
588 T tmp = a;
589 a = b;
590 b = tmp;
591 }
592
593
594 //PCK: endian swapping functions
btSwapEndian(unsigned val)595 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
596 {
597 return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
598 }
599
btSwapEndian(unsigned short val)600 SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
601 {
602 return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
603 }
604
btSwapEndian(int val)605 SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
606 {
607 return btSwapEndian((unsigned)val);
608 }
609
btSwapEndian(short val)610 SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
611 {
612 return btSwapEndian((unsigned short) val);
613 }
614
615 ///btSwapFloat uses using char pointers to swap the endianness
616 ////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
617 ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
618 ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
619 ///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.
620 ///so instead of returning a float/double, we return integer/long long integer
btSwapEndianFloat(float d)621 SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
622 {
623 unsigned int a = 0;
624 unsigned char *dst = (unsigned char *)&a;
625 unsigned char *src = (unsigned char *)&d;
626
627 dst[0] = src[3];
628 dst[1] = src[2];
629 dst[2] = src[1];
630 dst[3] = src[0];
631 return a;
632 }
633
634 // unswap using char pointers
btUnswapEndianFloat(unsigned int a)635 SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
636 {
637 float d = 0.0f;
638 unsigned char *src = (unsigned char *)&a;
639 unsigned char *dst = (unsigned char *)&d;
640
641 dst[0] = src[3];
642 dst[1] = src[2];
643 dst[2] = src[1];
644 dst[3] = src[0];
645
646 return d;
647 }
648
649
650 // swap using char pointers
btSwapEndianDouble(double d,unsigned char * dst)651 SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
652 {
653 unsigned char *src = (unsigned char *)&d;
654
655 dst[0] = src[7];
656 dst[1] = src[6];
657 dst[2] = src[5];
658 dst[3] = src[4];
659 dst[4] = src[3];
660 dst[5] = src[2];
661 dst[6] = src[1];
662 dst[7] = src[0];
663
664 }
665
666 // unswap using char pointers
btUnswapEndianDouble(const unsigned char * src)667 SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
668 {
669 double d = 0.0;
670 unsigned char *dst = (unsigned char *)&d;
671
672 dst[0] = src[7];
673 dst[1] = src[6];
674 dst[2] = src[5];
675 dst[3] = src[4];
676 dst[4] = src[3];
677 dst[5] = src[2];
678 dst[6] = src[1];
679 dst[7] = src[0];
680
681 return d;
682 }
683
684 template<typename T>
btSetZero(T * a,int n)685 SIMD_FORCE_INLINE void btSetZero(T* a, int n)
686 {
687 T* acurr = a;
688 size_t ncurr = n;
689 while (ncurr > 0)
690 {
691 *(acurr++) = 0;
692 --ncurr;
693 }
694 }
695
696
btLargeDot(const btScalar * a,const btScalar * b,int n)697 SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
698 {
699 btScalar p0,q0,m0,p1,q1,m1,sum;
700 sum = 0;
701 n -= 2;
702 while (n >= 0) {
703 p0 = a[0]; q0 = b[0];
704 m0 = p0 * q0;
705 p1 = a[1]; q1 = b[1];
706 m1 = p1 * q1;
707 sum += m0;
708 sum += m1;
709 a += 2;
710 b += 2;
711 n -= 2;
712 }
713 n += 2;
714 while (n > 0) {
715 sum += (*a) * (*b);
716 a++;
717 b++;
718 n--;
719 }
720 return sum;
721 }
722
723
724 // returns normalized value in range [-SIMD_PI, SIMD_PI]
btNormalizeAngle(btScalar angleInRadians)725 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
726 {
727 angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
728 if(angleInRadians < -SIMD_PI)
729 {
730 return angleInRadians + SIMD_2_PI;
731 }
732 else if(angleInRadians > SIMD_PI)
733 {
734 return angleInRadians - SIMD_2_PI;
735 }
736 else
737 {
738 return angleInRadians;
739 }
740 }
741
742
743
744 ///rudimentary class to provide type info
745 struct btTypedObject
746 {
btTypedObjectbtTypedObject747 btTypedObject(int objectType)
748 :m_objectType(objectType)
749 {
750 }
751 int m_objectType;
getObjectTypebtTypedObject752 inline int getObjectType() const
753 {
754 return m_objectType;
755 }
756 };
757
758
759
760 ///align a pointer to the provided alignment, upwards
btAlignPointer(T * unalignedPtr,size_t alignment)761 template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
762 {
763
764 struct btConvertPointerSizeT
765 {
766 union
767 {
768 T* ptr;
769 size_t integer;
770 };
771 };
772 btConvertPointerSizeT converter;
773
774
775 const size_t bit_mask = ~(alignment - 1);
776 converter.ptr = unalignedPtr;
777 converter.integer += alignment-1;
778 converter.integer &= bit_mask;
779 return converter.ptr;
780 }
781
782
783 #endif //BT_SCALAR_H
784