btScalar.h

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2003-2009 Erwin Coumans  http://bullet.googlecode.com
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 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.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
00013 */
00014 
00015 
00016 
00017 #ifndef BT_SCALAR_H
00018 #define BT_SCALAR_H
00019 
00020 #ifdef BT_MANAGED_CODE
00021 //Aligned data types not supported in managed code
00022 #pragma unmanaged
00023 #endif
00024 
00025 
00026 #include <math.h>
00027 #include <stdlib.h>//size_t for MSVC 6.0
00028 #include <float.h>
00029 
00030 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
00031 #define BT_BULLET_VERSION 281
00032 
00033 inline int      btGetVersion()
00034 {
00035         return BT_BULLET_VERSION;
00036 }
00037 
00038 #if defined(DEBUG) || defined (_DEBUG)
00039 #define BT_DEBUG
00040 #endif
00041 
00042 
00043 #ifdef _WIN32
00044 
00045                 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
00046 
00047                         #define SIMD_FORCE_INLINE inline
00048                         #define ATTRIBUTE_ALIGNED16(a) a
00049                         #define ATTRIBUTE_ALIGNED64(a) a
00050                         #define ATTRIBUTE_ALIGNED128(a) a
00051                 #else
00052                         //#define BT_HAS_ALIGNED_ALLOCATOR
00053                         #pragma warning(disable : 4324) // disable padding warning
00054 //                      #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
00055 //                      #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
00056 //                      #pragma warning(disable:4786) // Disable the "debug name too long" warning
00057 
00058                         #define SIMD_FORCE_INLINE __forceinline
00059                         #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
00060                         #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
00061                         #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
00062                 #ifdef _XBOX
00063                         #define BT_USE_VMX128
00064 
00065                         #include <ppcintrinsics.h>
00066                         #define BT_HAVE_NATIVE_FSEL
00067                         #define btFsel(a,b,c) __fsel((a),(b),(c))
00068                 #else
00069 
00070 #if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
00071                         #define BT_USE_SSE
00072                         #ifdef BT_USE_SSE
00073                         //BT_USE_SSE_IN_API is disabled under Windows by default, because 
00074                         //it makes it harder to integrate Bullet into your application under Windows 
00075                         //(structured embedding Bullet structs/classes need to be 16-byte aligned)
00076                         //with relatively little performance gain
00077                         //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
00078                         //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)
00079                         //#define BT_USE_SSE_IN_API
00080                         #endif //BT_USE_SSE
00081                         #include <emmintrin.h>
00082 #endif
00083 
00084                 #endif//_XBOX
00085 
00086                 #endif //__MINGW32__
00087 
00088 #ifdef BT_DEBUG
00089         #ifdef _MSC_VER
00090                 #include <stdio.h>
00091                 #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak();       }}
00092         #else//_MSC_VER
00093                 #include <assert.h>
00094                 #define btAssert assert
00095         #endif//_MSC_VER
00096 #else
00097                 #define btAssert(x)
00098 #endif
00099                 //btFullAssert is optional, slows down a lot
00100                 #define btFullAssert(x)
00101 
00102                 #define btLikely(_c)  _c
00103                 #define btUnlikely(_c) _c
00104 
00105 #else
00106         
00107 #if defined     (__CELLOS_LV2__)
00108                 #define SIMD_FORCE_INLINE inline __attribute__((always_inline))
00109                 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
00110                 #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
00111                 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
00112                 #ifndef assert
00113                 #include <assert.h>
00114                 #endif
00115 #ifdef BT_DEBUG
00116 #ifdef __SPU__
00117 #include <spu_printf.h>
00118 #define printf spu_printf
00119         #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
00120 #else
00121         #define btAssert assert
00122 #endif
00123         
00124 #else
00125                 #define btAssert(x)
00126 #endif
00127                 //btFullAssert is optional, slows down a lot
00128                 #define btFullAssert(x)
00129 
00130                 #define btLikely(_c)  _c
00131                 #define btUnlikely(_c) _c
00132 
00133 #else
00134 
00135 #ifdef USE_LIBSPE2
00136 
00137                 #define SIMD_FORCE_INLINE __inline
00138                 #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
00139                 #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
00140                 #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
00141                 #ifndef assert
00142                 #include <assert.h>
00143                 #endif
00144 #ifdef BT_DEBUG
00145                 #define btAssert assert
00146 #else
00147                 #define btAssert(x)
00148 #endif
00149                 //btFullAssert is optional, slows down a lot
00150                 #define btFullAssert(x)
00151 
00152 
00153                 #define btLikely(_c)   __builtin_expect((_c), 1)
00154                 #define btUnlikely(_c) __builtin_expect((_c), 0)
00155                 
00156 
00157 #else
00158         //non-windows systems
00159 
00160 #if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
00161     #if defined (__i386__) || defined (__x86_64__)
00162         #define BT_USE_SSE
00163                 //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
00164                 //if apps run into issues, we will disable the next line
00165                 #define BT_USE_SSE_IN_API
00166         #ifdef BT_USE_SSE
00167             // include appropriate SSE level
00168             #if defined (__SSE4_1__)
00169                 #include <smmintrin.h>
00170             #elif defined (__SSSE3__)
00171                 #include <tmmintrin.h>
00172             #elif defined (__SSE3__)
00173                 #include <pmmintrin.h>
00174             #else
00175                 #include <emmintrin.h>
00176             #endif
00177         #endif //BT_USE_SSE
00178     #elif defined( __armv7__ )
00179         #ifdef __clang__
00180             #define BT_USE_NEON 1
00181 
00182             #if defined BT_USE_NEON && defined (__clang__)
00183                 #include <arm_neon.h>
00184             #endif//BT_USE_NEON
00185        #endif //__clang__
00186     #endif//__arm__
00187 
00188         #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
00189 
00190         #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
00191         #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
00192         #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
00193         #ifndef assert
00194         #include <assert.h>
00195         #endif
00196 
00197         #if defined(DEBUG) || defined (_DEBUG)
00198          #if defined (__i386__) || defined (__x86_64__)
00199         #include <stdio.h>
00200          #define btAssert(x)\
00201         {\
00202         if(!(x))\
00203         {\
00204                 printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
00205                 asm volatile ("int3");\
00206         }\
00207         }
00208         #else//defined (__i386__) || defined (__x86_64__)
00209                 #define btAssert assert
00210         #endif//defined (__i386__) || defined (__x86_64__)
00211         #else//defined(DEBUG) || defined (_DEBUG)
00212                 #define btAssert(x)
00213         #endif//defined(DEBUG) || defined (_DEBUG)
00214 
00215         //btFullAssert is optional, slows down a lot
00216         #define btFullAssert(x)
00217         #define btLikely(_c)  _c
00218         #define btUnlikely(_c) _c
00219 
00220 #else
00221 
00222                 #define SIMD_FORCE_INLINE inline
00223 
00224 
00225 
00226 
00227                 #define ATTRIBUTE_ALIGNED16(a) a
00228                 #define ATTRIBUTE_ALIGNED64(a) a
00229                 #define ATTRIBUTE_ALIGNED128(a) a
00230                 #ifndef assert
00231                 #include <assert.h>
00232                 #endif
00233 
00234 #if defined(DEBUG) || defined (_DEBUG)
00235                 #define btAssert assert
00236 #else
00237                 #define btAssert(x)
00238 #endif
00239 
00240                 //btFullAssert is optional, slows down a lot
00241                 #define btFullAssert(x)
00242                 #define btLikely(_c)  _c
00243                 #define btUnlikely(_c) _c
00244 #endif //__APPLE__ 
00245 
00246 #endif // LIBSPE2
00247 
00248 #endif  //__CELLOS_LV2__
00249 #endif
00250 
00251 
00253 #if defined(BT_USE_DOUBLE_PRECISION)
00254 typedef double btScalar;
00255 //this number could be bigger in double precision
00256 #define BT_LARGE_FLOAT 1e30
00257 #else
00258 typedef float btScalar;
00259 //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
00260 #define BT_LARGE_FLOAT 1e18f
00261 #endif
00262 
00263 #ifdef BT_USE_SSE
00264 typedef __m128 btSimdFloat4;
00265 #endif//BT_USE_SSE
00266 
00267 #if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
00268 #ifdef _WIN32
00269 
00270 #ifndef BT_NAN
00271 static int btNanMask = 0x7F800001;
00272 #define BT_NAN (*(float*)&btNanMask)
00273 #endif
00274 
00275 #ifndef BT_INFINITY
00276 static  int btInfinityMask = 0x7F800000;
00277 #define BT_INFINITY (*(float*)&btInfinityMask)
00278 #endif
00279 
00280 inline __m128 operator + (const __m128 A, const __m128 B)
00281 {
00282     return _mm_add_ps(A, B);
00283 }
00284 
00285 inline __m128 operator - (const __m128 A, const __m128 B)
00286 {
00287     return _mm_sub_ps(A, B);
00288 }
00289 
00290 inline __m128 operator * (const __m128 A, const __m128 B)
00291 {
00292     return _mm_mul_ps(A, B);
00293 }
00294 
00295 #define btCastfTo128i(a) (_mm_castps_si128(a))
00296 #define btCastfTo128d(a) (_mm_castps_pd(a))
00297 #define btCastiTo128f(a) (_mm_castsi128_ps(a))
00298 #define btCastdTo128f(a) (_mm_castpd_ps(a))
00299 #define btCastdTo128i(a) (_mm_castpd_si128(a))
00300 #define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
00301 
00302 #else//_WIN32
00303 
00304 #define btCastfTo128i(a) ((__m128i)(a))
00305 #define btCastfTo128d(a) ((__m128d)(a))
00306 #define btCastiTo128f(a)  ((__m128) (a))
00307 #define btCastdTo128f(a) ((__m128) (a))
00308 #define btCastdTo128i(a) ((__m128i)(a))
00309 #define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
00310 #define BT_INFINITY INFINITY
00311 #define BT_NAN NAN
00312 #endif//_WIN32
00313 #endif //BT_USE_SSE_IN_API
00314 
00315 #ifdef BT_USE_NEON
00316 #include <arm_neon.h>
00317 
00318 typedef float32x4_t btSimdFloat4;
00319 #define BT_INFINITY INFINITY
00320 #define BT_NAN NAN
00321 #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
00322 #endif
00323 
00324 
00325 
00326 
00327 
00328 #define BT_DECLARE_ALIGNED_ALLOCATOR() \
00329    SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
00330    SIMD_FORCE_INLINE void  operator delete(void* ptr)         { btAlignedFree(ptr); }   \
00331    SIMD_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
00332    SIMD_FORCE_INLINE void  operator delete(void*, void*)      { }   \
00333    SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
00334    SIMD_FORCE_INLINE void  operator delete[](void* ptr)         { btAlignedFree(ptr); }   \
00335    SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
00336    SIMD_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
00337 
00338 
00339 
00340 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
00341                 
00342 SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
00343 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
00344 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
00345 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
00346 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
00347 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return acos(x); }
00348 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (x<btScalar(-1))     x=btScalar(-1); if (x>btScalar(1))      x=btScalar(1); return asin(x); }
00349 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
00350 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
00351 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
00352 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
00353 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
00354 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
00355 
00356 #else
00357                 
00358 SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) 
00359 { 
00360 #ifdef USE_APPROXIMATION
00361     double x, z, tempf;
00362     unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
00363 
00364         tempf = y;
00365         *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
00366         x =  tempf;
00367         z =  y*btScalar(0.5);
00368         x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
00369         x = (btScalar(1.5)*x)-(x*x)*(x*z);
00370         x = (btScalar(1.5)*x)-(x*x)*(x*z);
00371         x = (btScalar(1.5)*x)-(x*x)*(x*z);
00372         x = (btScalar(1.5)*x)-(x*x)*(x*z);
00373         return x*y;
00374 #else
00375         return sqrtf(y); 
00376 #endif
00377 }
00378 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
00379 SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
00380 SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
00381 SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
00382 SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { 
00383         if (x<btScalar(-1))     
00384                 x=btScalar(-1); 
00385         if (x>btScalar(1))      
00386                 x=btScalar(1);
00387         return acosf(x); 
00388 }
00389 SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { 
00390         if (x<btScalar(-1))     
00391                 x=btScalar(-1); 
00392         if (x>btScalar(1))      
00393                 x=btScalar(1);
00394         return asinf(x); 
00395 }
00396 SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
00397 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
00398 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
00399 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
00400 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
00401 SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
00402         
00403 #endif
00404 
00405 #define SIMD_2_PI         btScalar(6.283185307179586232)
00406 #define SIMD_PI           (SIMD_2_PI * btScalar(0.5))
00407 #define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
00408 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
00409 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
00410 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
00411 
00412 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))          /* reciprocal square root */
00413 
00414 
00415 #ifdef BT_USE_DOUBLE_PRECISION
00416 #define SIMD_EPSILON      DBL_EPSILON
00417 #define SIMD_INFINITY     DBL_MAX
00418 #else
00419 #define SIMD_EPSILON      FLT_EPSILON
00420 #define SIMD_INFINITY     FLT_MAX
00421 #endif
00422 
00423 SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) 
00424 {
00425         btScalar coeff_1 = SIMD_PI / 4.0f;
00426         btScalar coeff_2 = 3.0f * coeff_1;
00427         btScalar abs_y = btFabs(y);
00428         btScalar angle;
00429         if (x >= 0.0f) {
00430                 btScalar r = (x - abs_y) / (x + abs_y);
00431                 angle = coeff_1 - coeff_1 * r;
00432         } else {
00433                 btScalar r = (x + abs_y) / (abs_y - x);
00434                 angle = coeff_2 - coeff_1 * r;
00435         }
00436         return (y < 0.0f) ? -angle : angle;
00437 }
00438 
00439 SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
00440 
00441 SIMD_FORCE_INLINE bool  btEqual(btScalar a, btScalar eps) {
00442         return (((a) <= eps) && !((a) < -eps));
00443 }
00444 SIMD_FORCE_INLINE bool  btGreaterEqual (btScalar a, btScalar eps) {
00445         return (!((a) <= eps));
00446 }
00447 
00448 
00449 SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
00450     return x < btScalar(0.0) ? 1 : 0;
00451 }
00452 
00453 SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
00454 SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
00455 
00456 #define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
00457 
00458 #ifndef btFsel
00459 SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
00460 {
00461         return a >= 0 ? b : c;
00462 }
00463 #endif
00464 #define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
00465 
00466 
00467 SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
00468 {
00469    long int i = 1;
00470    const char *p = (const char *) &i;
00471    if (p[0] == 1)  // Lowest address contains the least significant byte
00472            return true;
00473    else
00474            return false;
00475 }
00476 
00477 
00478 
00481 SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
00482 {
00483     // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
00484     // Rely on positive value or'ed with its negative having sign bit on
00485     // and zero value or'ed with its negative (which is still zero) having sign bit off 
00486     // Use arithmetic shift right, shifting the sign bit through all 32 bits
00487     unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
00488     unsigned testEqz = ~testNz;
00489     return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); 
00490 }
00491 SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
00492 {
00493     unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
00494     unsigned testEqz = ~testNz; 
00495     return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
00496 }
00497 SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
00498 {
00499 #ifdef BT_HAVE_NATIVE_FSEL
00500     return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
00501 #else
00502     return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
00503 #endif
00504 }
00505 
00506 template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
00507 {
00508         T tmp = a;
00509         a = b;
00510         b = tmp;
00511 }
00512 
00513 
00514 //PCK: endian swapping functions
00515 SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
00516 {
00517         return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8)  | ((val & 0x000000ff) << 24));
00518 }
00519 
00520 SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
00521 {
00522         return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
00523 }
00524 
00525 SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
00526 {
00527         return btSwapEndian((unsigned)val);
00528 }
00529 
00530 SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
00531 {
00532         return btSwapEndian((unsigned short) val);
00533 }
00534 
00541 SIMD_FORCE_INLINE unsigned int  btSwapEndianFloat(float d)
00542 {
00543     unsigned int a = 0;
00544     unsigned char *dst = (unsigned char *)&a;
00545     unsigned char *src = (unsigned char *)&d;
00546 
00547     dst[0] = src[3];
00548     dst[1] = src[2];
00549     dst[2] = src[1];
00550     dst[3] = src[0];
00551     return a;
00552 }
00553 
00554 // unswap using char pointers
00555 SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) 
00556 {
00557     float d = 0.0f;
00558     unsigned char *src = (unsigned char *)&a;
00559     unsigned char *dst = (unsigned char *)&d;
00560 
00561     dst[0] = src[3];
00562     dst[1] = src[2];
00563     dst[2] = src[1];
00564     dst[3] = src[0];
00565 
00566     return d;
00567 }
00568 
00569 
00570 // swap using char pointers
00571 SIMD_FORCE_INLINE void  btSwapEndianDouble(double d, unsigned char* dst)
00572 {
00573     unsigned char *src = (unsigned char *)&d;
00574 
00575     dst[0] = src[7];
00576     dst[1] = src[6];
00577     dst[2] = src[5];
00578     dst[3] = src[4];
00579     dst[4] = src[3];
00580     dst[5] = src[2];
00581     dst[6] = src[1];
00582     dst[7] = src[0];
00583 
00584 }
00585 
00586 // unswap using char pointers
00587 SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) 
00588 {
00589     double d = 0.0;
00590     unsigned char *dst = (unsigned char *)&d;
00591 
00592     dst[0] = src[7];
00593     dst[1] = src[6];
00594     dst[2] = src[5];
00595     dst[3] = src[4];
00596     dst[4] = src[3];
00597     dst[5] = src[2];
00598     dst[6] = src[1];
00599     dst[7] = src[0];
00600 
00601         return d;
00602 }
00603 
00604 // returns normalized value in range [-SIMD_PI, SIMD_PI]
00605 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
00606 {
00607         angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
00608         if(angleInRadians < -SIMD_PI)
00609         {
00610                 return angleInRadians + SIMD_2_PI;
00611         }
00612         else if(angleInRadians > SIMD_PI)
00613         {
00614                 return angleInRadians - SIMD_2_PI;
00615         }
00616         else
00617         {
00618                 return angleInRadians;
00619         }
00620 }
00621 
00623 struct btTypedObject
00624 {
00625         btTypedObject(int objectType)
00626                 :m_objectType(objectType)
00627         {
00628         }
00629         int     m_objectType;
00630         inline int getObjectType() const
00631         {
00632                 return m_objectType;
00633         }
00634 };
00635 
00636 
00637   
00639 template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
00640 {
00641                 
00642         struct btConvertPointerSizeT
00643         {
00644                 union 
00645                 {
00646                                 T* ptr;
00647                                 size_t integer;
00648                 };
00649         };
00650     btConvertPointerSizeT converter;
00651     
00652     
00653         const size_t bit_mask = ~(alignment - 1);
00654     converter.ptr = unalignedPtr;
00655         converter.integer += alignment-1;
00656         converter.integer &= bit_mask;
00657         return converter.ptr;
00658 }
00659 
00660 #endif //BT_SCALAR_H