dali-physics/third-party/bullet3/src/Bullet3Common/b3Scalar.h

   1 /*
   2 Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
   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 B3_SCALAR_H
  16 #define B3_SCALAR_H
  17
  18 #ifdef B3_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 //Original repository is at http://github.com/erwincoumans/bullet3
  28 #define B3_BULLET_VERSION 300
  29
  30 inline int b3GetVersion()
  31 {
  32         return B3_BULLET_VERSION;
  33 }
  34
  35 #if defined(DEBUG) || defined(_DEBUG)
  36 #define B3_DEBUG
  37 #endif
  38
  39 #include "b3Logging.h"  //for b3Error
  40
  41 #ifdef _WIN32
  42
  43 #if  defined(__GNUC__)  // it should handle both MINGW and CYGWIN
  44 #define B3_FORCE_INLINE             __inline__ __attribute__((always_inline))
  45 #define B3_ATTRIBUTE_ALIGNED16(a)   a __attribute__((aligned(16)))
  46 #define B3_ATTRIBUTE_ALIGNED64(a)   a __attribute__((aligned(64)))
  47 #define B3_ATTRIBUTE_ALIGNED128(a)  a __attribute__((aligned(128)))
  48 #elif ( defined(_MSC_VER) && _MSC_VER < 1300 )
  49 #define B3_FORCE_INLINE inline
  50 #define B3_ATTRIBUTE_ALIGNED16(a) a
  51 #define B3_ATTRIBUTE_ALIGNED64(a) a
  52 #define B3_ATTRIBUTE_ALIGNED128(a) a
  53 #else
  54 //#define B3_HAS_ALIGNED_ALLOCATOR
  55 #pragma warning(disable : 4324)  // disable padding warning
  56 //                      #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
  57 #pragma warning(disable : 4996)  //Turn off warnings about deprecated C routines
  58 //                      #pragma warning(disable:4786) // Disable the "debug name too long" warning
  59
  60 #define B3_FORCE_INLINE __forceinline
  61 #define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
  62 #define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
  63 #define B3_ATTRIBUTE_ALIGNED128(a) __declspec(align(128)) a
  64 #ifdef _XBOX
  65 #define B3_USE_VMX128
  66
  67 #include <ppcintrinsics.h>
  68 #define B3_HAVE_NATIVE_FSEL
  69 #define b3Fsel(a, b, c) __fsel((a), (b), (c))
  70 #else
  71
  72 #if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(B3_USE_DOUBLE_PRECISION))
  73 #if (defined(_M_IX86) || defined(_M_X64))
  74
  75
  76 #ifdef __clang__
  77 //#define B3_NO_SIMD_OPERATOR_OVERLOADS
  78 #define B3_DISABLE_SSE
  79 #endif //__clang__
  80
  81 #ifndef B3_DISABLE_SSE
  82 #define B3_USE_SSE
  83 #endif //B3_DISABLE_SSE
  84
  85 #ifdef B3_USE_SSE
  86 //B3_USE_SSE_IN_API is disabled under Windows by default, because
  87 //it makes it harder to integrate Bullet into your application under Windows
  88 //(structured embedding Bullet structs/classes need to be 16-byte aligned)
  89 //with relatively little performance gain
  90 //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
  91 //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)
  92 //#define B3_USE_SSE_IN_API
  93 #endif  //B3_USE_SSE
  94 #include <emmintrin.h>
  95 #endif
  96 #endif
  97
  98 #endif  //_XBOX
  99
 100 #endif  //__MINGW32__
 101
 102 #ifdef B3_DEBUG
 103 #ifdef _MSC_VER
 104 #include <stdio.h>
 105 #define b3Assert(x) { if(!(x)){b3Error("Assert " __FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak();   }}
 106 #else  //_MSC_VER
 107 #include <assert.h>
 108 #define b3Assert assert
 109 #endif  //_MSC_VER
 110 #else
 111 #define b3Assert(x)
 112 #endif
 113 //b3FullAssert is optional, slows down a lot
 114 #define b3FullAssert(x)
 115
 116 #define b3Likely(_c) _c
 117 #define b3Unlikely(_c) _c
 118
 119 #else
 120
 121 #if defined(__CELLOS_LV2__)
 122 #define B3_FORCE_INLINE inline __attribute__((always_inline))
 123 #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
 124 #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
 125 #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
 126 #ifndef assert
 127 #include <assert.h>
 128 #endif
 129 #ifdef B3_DEBUG
 130 #ifdef __SPU__
 131 #include <spu_printf.h>
 132 #define printf spu_printf
 133 #define b3Assert(x)               \
 134         {                             \
 135                 if (!(x))                 \
 136                 {                         \
 137                         b3Error(              \
 138                                 "Assert "__FILE__ \
 139                                 ":%u (" #x ")\n", \
 140                                 __LINE__);        \
 141                         spu_hcmpeq(0, 0);     \
 142                 }                         \
 143         }
 144 #else
 145 #define b3Assert assert
 146 #endif
 147
 148 #else
 149 #define b3Assert(x)
 150 #endif
 151 //b3FullAssert is optional, slows down a lot
 152 #define b3FullAssert(x)
 153
 154 #define b3Likely(_c) _c
 155 #define b3Unlikely(_c) _c
 156
 157 #else
 158
 159 #ifdef USE_LIBSPE2
 160
 161 #define B3_FORCE_INLINE __inline
 162 #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
 163 #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
 164 #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
 165 #ifndef assert
 166 #include <assert.h>
 167 #endif
 168 #ifdef B3_DEBUG
 169 #define b3Assert assert
 170 #else
 171 #define b3Assert(x)
 172 #endif
 173 //b3FullAssert is optional, slows down a lot
 174 #define b3FullAssert(x)
 175
 176 #define b3Likely(_c) __builtin_expect((_c), 1)
 177 #define b3Unlikely(_c) __builtin_expect((_c), 0)
 178
 179 #else
 180 //non-windows systems
 181
 182 #if (defined(__APPLE__) && (!defined(B3_USE_DOUBLE_PRECISION)))
 183 #if defined(__i386__) || defined(__x86_64__)
 184 #define B3_USE_SSE
 185 //B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
 186 //if apps run into issues, we will disable the next line
 187 #define B3_USE_SSE_IN_API
 188 #ifdef B3_USE_SSE
 189 // include appropriate SSE level
 190 #if defined(__SSE4_1__)
 191 #include <smmintrin.h>
 192 #elif defined(__SSSE3__)
 193 #include <tmmintrin.h>
 194 #elif defined(__SSE3__)
 195 #include <pmmintrin.h>
 196 #else
 197 #include <emmintrin.h>
 198 #endif
 199 #endif  //B3_USE_SSE
 200 #elif defined(__armv7__)
 201 #ifdef __clang__
 202 #define B3_USE_NEON 1
 203
 204 #if defined B3_USE_NEON && defined(__clang__)
 205 #include <arm_neon.h>
 206 #endif  //B3_USE_NEON
 207 #endif  //__clang__
 208 #endif  //__arm__
 209
 210 #define B3_FORCE_INLINE inline __attribute__((always_inline))
 211 ///@todo: check out alignment methods for other platforms/compilers
 212 #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
 213 #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
 214 #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
 215 #ifndef assert
 216 #include <assert.h>
 217 #endif
 218
 219 #if defined(DEBUG) || defined(_DEBUG)
 220 #if defined(__i386__) || defined(__x86_64__)
 221 #include <stdio.h>
 222 #define b3Assert(x)                                                             \
 223         {                                                                           \
 224                 if (!(x))                                                               \
 225                 {                                                                       \
 226                         b3Error("Assert %s in line %d, file %s\n", #x, __LINE__, __FILE__); \
 227                         asm volatile("int3");                                               \
 228                 }                                                                       \
 229         }
 230 #else  //defined (__i386__) || defined (__x86_64__)
 231 #define b3Assert assert
 232 #endif  //defined (__i386__) || defined (__x86_64__)
 233 #else   //defined(DEBUG) || defined (_DEBUG)
 234 #define b3Assert(x)
 235 #endif  //defined(DEBUG) || defined (_DEBUG)
 236
 237 //b3FullAssert is optional, slows down a lot
 238 #define b3FullAssert(x)
 239 #define b3Likely(_c) _c
 240 #define b3Unlikely(_c) _c
 241
 242 #else
 243
 244 #define B3_FORCE_INLINE inline
 245 ///@todo: check out alignment methods for other platforms/compilers
 246 #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
 247 #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
 248 #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
 249 ///#define B3_ATTRIBUTE_ALIGNED16(a) a
 250 ///#define B3_ATTRIBUTE_ALIGNED64(a) a
 251 ///#define B3_ATTRIBUTE_ALIGNED128(a) a
 252 #ifndef assert
 253 #include <assert.h>
 254 #endif
 255
 256 #if defined(DEBUG) || defined(_DEBUG)
 257 #define b3Assert assert
 258 #else
 259 #define b3Assert(x)
 260 #endif
 261
 262 //b3FullAssert is optional, slows down a lot
 263 #define b3FullAssert(x)
 264 #define b3Likely(_c) _c
 265 #define b3Unlikely(_c) _c
 266 #endif  //__APPLE__
 267
 268 #endif  // LIBSPE2
 269
 270 #endif  //__CELLOS_LV2__
 271 #endif
 272
 273 ///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
 274 #if defined(B3_USE_DOUBLE_PRECISION)
 275 typedef double b3Scalar;
 276 //this number could be bigger in double precision
 277 #define B3_LARGE_FLOAT 1e30
 278 #else
 279 typedef float b3Scalar;
 280 //keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
 281 #define B3_LARGE_FLOAT 1e18f
 282 #endif
 283
 284 #ifdef B3_USE_SSE
 285 typedef __m128 b3SimdFloat4;
 286 #endif  //B3_USE_SSE
 287
 288 #if defined B3_USE_SSE_IN_API && defined(B3_USE_SSE)
 289 #ifdef _WIN32
 290
 291 #ifndef B3_NAN
 292 static int b3NanMask = 0x7F800001;
 293 #define B3_NAN (*(float *)&b3NanMask)
 294 #endif
 295
 296 #ifndef B3_INFINITY_MASK
 297 static int b3InfinityMask = 0x7F800000;
 298 #define B3_INFINITY_MASK (*(float *)&b3InfinityMask)
 299 #endif
 300 #ifndef B3_NO_SIMD_OPERATOR_OVERLOADS
 301 inline __m128 operator+(const __m128 A, const __m128 B)
 302 {
 303         return _mm_add_ps(A, B);
 304 }
 305
 306 inline __m128 operator-(const __m128 A, const __m128 B)
 307 {
 308         return _mm_sub_ps(A, B);
 309 }
 310
 311 inline __m128 operator*(const __m128 A, const __m128 B)
 312 {
 313         return _mm_mul_ps(A, B);
 314 }
 315 #endif //B3_NO_SIMD_OPERATOR_OVERLOADS
 316 #define b3CastfTo128i(a) (_mm_castps_si128(a))
 317 #define b3CastfTo128d(a) (_mm_castps_pd(a))
 318 #define b3CastiTo128f(a) (_mm_castsi128_ps(a))
 319 #define b3CastdTo128f(a) (_mm_castpd_ps(a))
 320 #define b3CastdTo128i(a) (_mm_castpd_si128(a))
 321 #define b3Assign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)
 322
 323 #else  //_WIN32
 324
 325 #define b3CastfTo128i(a) ((__m128i)(a))
 326 #define b3CastfTo128d(a) ((__m128d)(a))
 327 #define b3CastiTo128f(a) ((__m128)(a))
 328 #define b3CastdTo128f(a) ((__m128)(a))
 329 #define b3CastdTo128i(a) ((__m128i)(a))
 330 #define b3Assign128(r0, r1, r2, r3) \
 331         (__m128) { r0, r1, r2, r3 }
 332 #endif  //_WIN32
 333 #endif  //B3_USE_SSE_IN_API
 334
 335 #ifdef B3_USE_NEON
 336 #include <arm_neon.h>
 337
 338 typedef float32x4_t b3SimdFloat4;
 339 #define B3_INFINITY INFINITY
 340 #define B3_NAN NAN
 341 #define b3Assign128(r0, r1, r2, r3) \
 342         (float32x4_t) { r0, r1, r2, r3 }
 343 #endif
 344
 345 #define B3_DECLARE_ALIGNED_ALLOCATOR()                                                                   \
 346         B3_FORCE_INLINE void *operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); }   \
 347         B3_FORCE_INLINE void operator delete(void *ptr) { b3AlignedFree(ptr); }                              \
 348         B3_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; }                                \
 349         B3_FORCE_INLINE void operator delete(void *, void *) {}                                              \
 350         B3_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
 351         B3_FORCE_INLINE void operator delete[](void *ptr) { b3AlignedFree(ptr); }                            \
 352         B3_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; }                              \
 353         B3_FORCE_INLINE void operator delete[](void *, void *) {}
 354
 355 #if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
 356
 357 B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
 358 {
 359         return sqrt(x);
 360 }
 361 B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
 362 B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
 363 B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
 364 B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
 365 B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
 366 {
 367         if (x < b3Scalar(-1)) x = b3Scalar(-1);
 368         if (x > b3Scalar(1)) x = b3Scalar(1);
 369         return acos(x);
 370 }
 371 B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
 372 {
 373         if (x < b3Scalar(-1)) x = b3Scalar(-1);
 374         if (x > b3Scalar(1)) x = b3Scalar(1);
 375         return asin(x);
 376 }
 377 B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
 378 B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
 379 B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
 380 B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
 381 B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return pow(x, y); }
 382 B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmod(x, y); }
 383
 384 #else
 385
 386 B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
 387 {
 388 #ifdef USE_APPROXIMATION
 389         double x, z, tempf;
 390         unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
 391
 392         tempf = y;
 393         *tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
 394         x = tempf;
 395         z = y * b3Scalar(0.5);
 396         x = (b3Scalar(1.5) * x) - (x * x) * (x * z); /* iteration formula     */
 397         x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
 398         x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
 399         x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
 400         x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
 401         return x * y;
 402 #else
 403         return sqrtf(y);
 404 #endif
 405 }
 406 B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
 407 B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
 408 B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
 409 B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
 410 B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
 411 {
 412         if (x < b3Scalar(-1))
 413                 x = b3Scalar(-1);
 414         if (x > b3Scalar(1))
 415                 x = b3Scalar(1);
 416         return acosf(x);
 417 }
 418 B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
 419 {
 420         if (x < b3Scalar(-1))
 421                 x = b3Scalar(-1);
 422         if (x > b3Scalar(1))
 423                 x = b3Scalar(1);
 424         return asinf(x);
 425 }
 426 B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
 427 B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
 428 B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
 429 B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
 430 B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return powf(x, y); }
 431 B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmodf(x, y); }
 432
 433 #endif
 434
 435 #define B3_2_PI b3Scalar(6.283185307179586232)
 436 #define B3_PI (B3_2_PI * b3Scalar(0.5))
 437 #define B3_HALF_PI (B3_2_PI * b3Scalar(0.25))
 438 #define B3_RADS_PER_DEG (B3_2_PI / b3Scalar(360.0))
 439 #define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI)
 440 #define B3_SQRT12 b3Scalar(0.7071067811865475244008443621048490)
 441
 442 #define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0) / b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
 443
 444 #ifdef B3_USE_DOUBLE_PRECISION
 445 #define B3_EPSILON DBL_EPSILON
 446 #define B3_INFINITY DBL_MAX
 447 #else
 448 #define B3_EPSILON FLT_EPSILON
 449 #define B3_INFINITY FLT_MAX
 450 #endif
 451
 452 B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
 453 {
 454         b3Scalar coeff_1 = B3_PI / 4.0f;
 455         b3Scalar coeff_2 = 3.0f * coeff_1;
 456         b3Scalar abs_y = b3Fabs(y);
 457         b3Scalar angle;
 458         if (x >= 0.0f)
 459         {
 460                 b3Scalar r = (x - abs_y) / (x + abs_y);
 461                 angle = coeff_1 - coeff_1 * r;
 462         }
 463         else
 464         {
 465                 b3Scalar r = (x + abs_y) / (abs_y - x);
 466                 angle = coeff_2 - coeff_1 * r;
 467         }
 468         return (y < 0.0f) ? -angle : angle;
 469 }
 470
 471 B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
 472
 473 B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
 474 {
 475         return (((a) <= eps) && !((a) < -eps));
 476 }
 477 B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
 478 {
 479         return (!((a) <= eps));
 480 }
 481
 482 B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
 483 {
 484         return x < b3Scalar(0.0) ? 1 : 0;
 485 }
 486
 487 B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
 488 B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; }
 489
 490 #define B3_DECLARE_HANDLE(name) \
 491         typedef struct name##__     \
 492         {                           \
 493                 int unused;             \
 494         } * name
 495
 496 #ifndef b3Fsel
 497 B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
 498 {
 499         return a >= 0 ? b : c;
 500 }
 501 #endif
 502 #define b3Fsels(a, b, c) (b3Scalar) b3Fsel(a, b, c)
 503
 504 B3_FORCE_INLINE bool b3MachineIsLittleEndian()
 505 {
 506         long int i = 1;
 507         const char *p = (const char *)&i;
 508         if (p[0] == 1)  // Lowest address contains the least significant byte
 509                 return true;
 510         else
 511                 return false;
 512 }
 513
 514 ///b3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
 515 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
 516 B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
 517 {
 518         // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
 519         // Rely on positive value or'ed with its negative having sign bit on
 520         // and zero value or'ed with its negative (which is still zero) having sign bit off
 521         // Use arithmetic shift right, shifting the sign bit through all 32 bits
 522         unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
 523         unsigned testEqz = ~testNz;
 524         return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
 525 }
 526 B3_FORCE_INLINE int b3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
 527 {
 528         unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
 529         unsigned testEqz = ~testNz;
 530         return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
 531 }
 532 B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
 533 {
 534 #ifdef B3_HAVE_NATIVE_FSEL
 535         return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
 536 #else
 537         return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
 538 #endif
 539 }
 540
 541 template <typename T>
 542 B3_FORCE_INLINE void b3Swap(T &a, T &b)
 543 {
 544         T tmp = a;
 545         a = b;
 546         b = tmp;
 547 }
 548
 549 //PCK: endian swapping functions
 550 B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
 551 {
 552         return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
 553 }
 554
 555 B3_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val)
 556 {
 557         return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
 558 }
 559
 560 B3_FORCE_INLINE unsigned b3SwapEndian(int val)
 561 {
 562         return b3SwapEndian((unsigned)val);
 563 }
 564
 565 B3_FORCE_INLINE unsigned short b3SwapEndian(short val)
 566 {
 567         return b3SwapEndian((unsigned short)val);
 568 }
 569
 570 ///b3SwapFloat uses using char pointers to swap the endianness
 571 ////b3SwapFloat/b3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
 572 ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
 573 ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
 574 ///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.
 575 ///so instead of returning a float/double, we return integer/long long integer
 576 B3_FORCE_INLINE unsigned int b3SwapEndianFloat(float d)
 577 {
 578         unsigned int a = 0;
 579         unsigned char *dst = (unsigned char *)&a;
 580         unsigned char *src = (unsigned char *)&d;
 581
 582         dst[0] = src[3];
 583         dst[1] = src[2];
 584         dst[2] = src[1];
 585         dst[3] = src[0];
 586         return a;
 587 }
 588
 589 // unswap using char pointers
 590 B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
 591 {
 592         float d = 0.0f;
 593         unsigned char *src = (unsigned char *)&a;
 594         unsigned char *dst = (unsigned char *)&d;
 595
 596         dst[0] = src[3];
 597         dst[1] = src[2];
 598         dst[2] = src[1];
 599         dst[3] = src[0];
 600
 601         return d;
 602 }
 603
 604 // swap using char pointers
 605 B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char *dst)
 606 {
 607         unsigned char *src = (unsigned char *)&d;
 608
 609         dst[0] = src[7];
 610         dst[1] = src[6];
 611         dst[2] = src[5];
 612         dst[3] = src[4];
 613         dst[4] = src[3];
 614         dst[5] = src[2];
 615         dst[6] = src[1];
 616         dst[7] = src[0];
 617 }
 618
 619 // unswap using char pointers
 620 B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src)
 621 {
 622         double d = 0.0;
 623         unsigned char *dst = (unsigned char *)&d;
 624
 625         dst[0] = src[7];
 626         dst[1] = src[6];
 627         dst[2] = src[5];
 628         dst[3] = src[4];
 629         dst[4] = src[3];
 630         dst[5] = src[2];
 631         dst[6] = src[1];
 632         dst[7] = src[0];
 633
 634         return d;
 635 }
 636
 637 // returns normalized value in range [-B3_PI, B3_PI]
 638 B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
 639 {
 640         angleInRadians = b3Fmod(angleInRadians, B3_2_PI);
 641         if (angleInRadians < -B3_PI)
 642         {
 643                 return angleInRadians + B3_2_PI;
 644         }
 645         else if (angleInRadians > B3_PI)
 646         {
 647                 return angleInRadians - B3_2_PI;
 648         }
 649         else
 650         {
 651                 return angleInRadians;
 652         }
 653 }
 654
 655 ///rudimentary class to provide type info
 656 struct b3TypedObject
 657 {
 658         b3TypedObject(int objectType)
 659                 : m_objectType(objectType)
 660         {
 661         }
 662         int m_objectType;
 663         inline int getObjectType() const
 664         {
 665                 return m_objectType;
 666         }
 667 };
 668
 669 ///align a pointer to the provided alignment, upwards
 670 template <typename T>
 671 T *b3AlignPointer(T *unalignedPtr, size_t alignment)
 672 {
 673         struct b3ConvertPointerSizeT
 674         {
 675                 union {
 676                         T *ptr;
 677                         size_t integer;
 678                 };
 679         };
 680         b3ConvertPointerSizeT converter;
 681
 682         const size_t bit_mask = ~(alignment - 1);
 683         converter.ptr = unalignedPtr;
 684         converter.integer += alignment - 1;
 685         converter.integer &= bit_mask;
 686         return converter.ptr;
 687 }
 688
 689 #endif  //B3_SCALAR_H