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