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