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