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