bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / LinearMath / btScalar.h
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 281
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( __armv7__ )
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 ///@todo: check out alignment methods for other platforms/compilers
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                 ///@todo: check out alignment methods for other platforms/compilers
230                 ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
231                 ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
232                 ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
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
258 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
259 #if defined(BT_USE_DOUBLE_PRECISION)
260 typedef double btScalar;
261 //this number could be bigger in double precision
262 #define BT_LARGE_FLOAT 1e30
263 #else
264 typedef float btScalar;
265 //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
266 #define BT_LARGE_FLOAT 1e18f
267 #endif
268
269 #ifdef BT_USE_SSE
270 typedef __m128 btSimdFloat4;
271 #endif//BT_USE_SSE
272
273 #if defined (BT_USE_SSE)
274 //#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
275 #ifdef _WIN32
276
277 #ifndef BT_NAN
278 static int btNanMask = 0x7F800001;
279 #define BT_NAN (*(float*)&btNanMask)
280 #endif
281
282 #ifndef BT_INFINITY
283 static  int btInfinityMask = 0x7F800000;
284 #define BT_INFINITY (*(float*)&btInfinityMask)
285 #endif
286
287 inline __m128 operator + (const __m128 A, const __m128 B)
288 {
289     return _mm_add_ps(A, B);
290 }
291
292 inline __m128 operator - (const __m128 A, const __m128 B)
293 {
294     return _mm_sub_ps(A, B);
295 }
296
297 inline __m128 operator * (const __m128 A, const __m128 B)
298 {
299     return _mm_mul_ps(A, B);
300 }
301
302 #define btCastfTo128i(a) (_mm_castps_si128(a))
303 #define btCastfTo128d(a) (_mm_castps_pd(a))
304 #define btCastiTo128f(a) (_mm_castsi128_ps(a))
305 #define btCastdTo128f(a) (_mm_castpd_ps(a))
306 #define btCastdTo128i(a) (_mm_castpd_si128(a))
307 #define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
308
309 #else//_WIN32
310
311 #define btCastfTo128i(a) ((__m128i)(a))
312 #define btCastfTo128d(a) ((__m128d)(a))
313 #define btCastiTo128f(a)  ((__m128) (a))
314 #define btCastdTo128f(a) ((__m128) (a))
315 #define btCastdTo128i(a) ((__m128i)(a))
316 #define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
317 #define BT_INFINITY INFINITY
318 #define BT_NAN NAN
319 #endif//_WIN32
320 #endif //BT_USE_SSE_IN_API
321
322 #ifdef BT_USE_NEON
323 #include <arm_neon.h>
324
325 typedef float32x4_t btSimdFloat4;
326 #define BT_INFINITY INFINITY
327 #define BT_NAN NAN
328 #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
329 #endif
330
331
332
333
334
335 #define BT_DECLARE_ALIGNED_ALLOCATOR() \
336    SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
337    SIMD_FORCE_INLINE void  operator delete(void* ptr)         { btAlignedFree(ptr); }   \
338    SIMD_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
339    SIMD_FORCE_INLINE void  operator delete(void*, void*)      { }   \
340    SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
341    SIMD_FORCE_INLINE void  operator delete[](void* ptr)         { btAlignedFree(ptr); }   \
342    SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
343    SIMD_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
344
345
346
347 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
348                 
349 SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
350 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
351 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
352 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
353 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
354 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return acos(x); }
355 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return asin(x); }
356 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
357 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
358 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
359 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
360 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
361 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
362
363 #else
364                 
365 SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) 
366
367 #ifdef USE_APPROXIMATION
368     double x, z, tempf;
369     unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
370
371         tempf = y;
372         *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
373         x =  tempf;
374         z =  y*btScalar(0.5);
375         x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
376         x = (btScalar(1.5)*x)-(x*x)*(x*z);
377         x = (btScalar(1.5)*x)-(x*x)*(x*z);
378         x = (btScalar(1.5)*x)-(x*x)*(x*z);
379         x = (btScalar(1.5)*x)-(x*x)*(x*z);
380         return x*y;
381 #else
382         return sqrtf(y); 
383 #endif
384 }
385 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
386 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
387 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
388 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
389 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { 
390         if (x<btScalar(-1))     
391                 x=btScalar(-1); 
392         if (x>btScalar(1))      
393                 x=btScalar(1);
394         return acosf(x); 
395 }
396 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { 
397         if (x<btScalar(-1))     
398                 x=btScalar(-1); 
399         if (x>btScalar(1))      
400                 x=btScalar(1);
401         return asinf(x); 
402 }
403 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
404 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
405 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
406 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
407 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
408 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
409         
410 #endif
411
412 #define SIMD_2_PI         btScalar(6.283185307179586232)
413 #define SIMD_PI           (SIMD_2_PI * btScalar(0.5))
414 #define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
415 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
416 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
417 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
418
419 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
420
421
422 #ifdef BT_USE_DOUBLE_PRECISION
423 #define SIMD_EPSILON      DBL_EPSILON
424 #define SIMD_INFINITY     DBL_MAX
425 #else
426 #define SIMD_EPSILON      FLT_EPSILON
427 #define SIMD_INFINITY     FLT_MAX
428 #endif
429
430 SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) 
431 {
432         btScalar coeff_1 = SIMD_PI / 4.0f;
433         btScalar coeff_2 = 3.0f * coeff_1;
434         btScalar abs_y = btFabs(y);
435         btScalar angle;
436         if (x >= 0.0f) {
437                 btScalar r = (x - abs_y) / (x + abs_y);
438                 angle = coeff_1 - coeff_1 * r;
439         } else {
440                 btScalar r = (x + abs_y) / (abs_y - x);
441                 angle = coeff_2 - coeff_1 * r;
442         }
443         return (y < 0.0f) ? -angle : angle;
444 }
445
446 SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
447
448 SIMD_FORCE_INLINE bool  btEqual(btScalar a, btScalar eps) {
449         return (((a) <= eps) && !((a) < -eps));
450 }
451 SIMD_FORCE_INLINE bool  btGreaterEqual (btScalar a, btScalar eps) {
452         return (!((a) <= eps));
453 }
454
455
456 SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
457     return x < btScalar(0.0) ? 1 : 0;
458 }
459
460 SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
461 SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
462
463 #define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
464
465 #ifndef btFsel
466 SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
467 {
468         return a >= 0 ? b : c;
469 }
470 #endif
471 #define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
472
473
474 SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
475 {
476    long int i = 1;
477    const char *p = (const char *) &i;
478    if (p[0] == 1)  // Lowest address contains the least significant byte
479            return true;
480    else
481            return false;
482 }
483
484
485
486 ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
487 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
488 SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
489 {
490     // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
491     // Rely on positive value or'ed with its negative having sign bit on
492     // and zero value or'ed with its negative (which is still zero) having sign bit off 
493     // Use arithmetic shift right, shifting the sign bit through all 32 bits
494     unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
495     unsigned testEqz = ~testNz;
496     return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); 
497 }
498 SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
499 {
500     unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
501     unsigned testEqz = ~testNz; 
502     return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
503 }
504 SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
505 {
506 #ifdef BT_HAVE_NATIVE_FSEL
507     return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
508 #else
509     return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
510 #endif
511 }
512
513 template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
514 {
515         T tmp = a;
516         a = b;
517         b = tmp;
518 }
519
520
521 //PCK: endian swapping functions
522 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
523 {
524         return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8)  | ((val & 0x000000ff) << 24));
525 }
526
527 SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
528 {
529         return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
530 }
531
532 SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
533 {
534         return btSwapEndian((unsigned)val);
535 }
536
537 SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
538 {
539         return btSwapEndian((unsigned short) val);
540 }
541
542 ///btSwapFloat uses using char pointers to swap the endianness
543 ////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
544 ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. 
545 ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. 
546 ///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. 
547 ///so instead of returning a float/double, we return integer/long long integer
548 SIMD_FORCE_INLINE unsigned int  btSwapEndianFloat(float d)
549 {
550     unsigned int a = 0;
551     unsigned char *dst = (unsigned char *)&a;
552     unsigned char *src = (unsigned char *)&d;
553
554     dst[0] = src[3];
555     dst[1] = src[2];
556     dst[2] = src[1];
557     dst[3] = src[0];
558     return a;
559 }
560
561 // unswap using char pointers
562 SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) 
563 {
564     float d = 0.0f;
565     unsigned char *src = (unsigned char *)&a;
566     unsigned char *dst = (unsigned char *)&d;
567
568     dst[0] = src[3];
569     dst[1] = src[2];
570     dst[2] = src[1];
571     dst[3] = src[0];
572
573     return d;
574 }
575
576
577 // swap using char pointers
578 SIMD_FORCE_INLINE void  btSwapEndianDouble(double d, unsigned char* dst)
579 {
580     unsigned char *src = (unsigned char *)&d;
581
582     dst[0] = src[7];
583     dst[1] = src[6];
584     dst[2] = src[5];
585     dst[3] = src[4];
586     dst[4] = src[3];
587     dst[5] = src[2];
588     dst[6] = src[1];
589     dst[7] = src[0];
590
591 }
592
593 // unswap using char pointers
594 SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) 
595 {
596     double d = 0.0;
597     unsigned char *dst = (unsigned char *)&d;
598
599     dst[0] = src[7];
600     dst[1] = src[6];
601     dst[2] = src[5];
602     dst[3] = src[4];
603     dst[4] = src[3];
604     dst[5] = src[2];
605     dst[6] = src[1];
606     dst[7] = src[0];
607
608         return d;
609 }
610
611 // returns normalized value in range [-SIMD_PI, SIMD_PI]
612 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
613 {
614         angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
615         if(angleInRadians < -SIMD_PI)
616         {
617                 return angleInRadians + SIMD_2_PI;
618         }
619         else if(angleInRadians > SIMD_PI)
620         {
621                 return angleInRadians - SIMD_2_PI;
622         }
623         else
624         {
625                 return angleInRadians;
626         }
627 }
628
629 ///rudimentary class to provide type info
630 struct btTypedObject
631 {
632         btTypedObject(int objectType)
633                 :m_objectType(objectType)
634         {
635         }
636         int     m_objectType;
637         inline int getObjectType() const
638         {
639                 return m_objectType;
640         }
641 };
642
643
644   
645 ///align a pointer to the provided alignment, upwards
646 template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
647 {
648                 
649         struct btConvertPointerSizeT
650         {
651                 union 
652                 {
653                                 T* ptr;
654                                 size_t integer;
655                 };
656         };
657     btConvertPointerSizeT converter;
658     
659     
660         const size_t bit_mask = ~(alignment - 1);
661     converter.ptr = unalignedPtr;
662         converter.integer += alignment-1;
663         converter.integer &= bit_mask;
664         return converter.ptr;
665 }
666
667 #endif //BT_SCALAR_H