| /* | |
| Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 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. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #ifndef SIMD___SCALAR_H | |
| #define SIMD___SCALAR_H | |
| #include <math.h> | |
| #include <stdlib.h>//size_t for MSVC 6.0 | |
| #include <cstdlib> | |
| #include <cfloat> | |
| #include <float.h> | |
| /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ | |
| #define BT_BULLET_VERSION 275 | |
| inline int btGetVersion() | |
| { | |
| return BT_BULLET_VERSION; | |
| } | |
| #if defined(DEBUG) || defined (_DEBUG) | |
| #define BT_DEBUG | |
| #endif | |
| #ifdef WIN32 | |
| #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) | |
| #define SIMD_FORCE_INLINE inline | |
| #define ATTRIBUTE_ALIGNED16(a) a | |
| #define ATTRIBUTE_ALIGNED128(a) a | |
| #else | |
| //#define BT_HAS_ALIGNED_ALLOCATOR | |
| #pragma warning(disable : 4324) // disable padding warning | |
| // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. | |
| // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines | |
| // #pragma warning(disable:4786) // Disable the "debug name too long" warning | |
| #define SIMD_FORCE_INLINE __forceinline | |
| #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a | |
| #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a | |
| #ifdef _XBOX | |
| #define BT_USE_VMX128 | |
| #include <ppcintrinsics.h> | |
| #define BT_HAVE_NATIVE_FSEL | |
| #define btFsel(a,b,c) __fsel((a),(b),(c)) | |
| #else | |
| #if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) | |
| #define BT_USE_SSE | |
| #include <emmintrin.h> | |
| #endif | |
| #endif//_XBOX | |
| #endif //__MINGW32__ | |
| #include <assert.h> | |
| #ifdef BT_DEBUG | |
| #define btAssert assert | |
| #else | |
| #define btAssert(x) | |
| #endif | |
| //btFullAssert is optional, slows down a lot | |
| #define btFullAssert(x) | |
| #define btLikely(_c) _c | |
| #define btUnlikely(_c) _c | |
| #else | |
| #if defined (__CELLOS_LV2__) | |
| #define SIMD_FORCE_INLINE inline | |
| #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) | |
| #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) | |
| #ifndef assert | |
| #include <assert.h> | |
| #endif | |
| #ifdef BT_DEBUG | |
| #define btAssert assert | |
| #else | |
| #define btAssert(x) | |
| #endif | |
| //btFullAssert is optional, slows down a lot | |
| #define btFullAssert(x) | |
| #define btLikely(_c) _c | |
| #define btUnlikely(_c) _c | |
| #else | |
| #ifdef USE_LIBSPE2 | |
| #define SIMD_FORCE_INLINE __inline | |
| #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) | |
| #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) | |
| #ifndef assert | |
| #include <assert.h> | |
| #endif | |
| #ifdef BT_DEBUG | |
| #define btAssert assert | |
| #else | |
| #define btAssert(x) | |
| #endif | |
| //btFullAssert is optional, slows down a lot | |
| #define btFullAssert(x) | |
| #define btLikely(_c) __builtin_expect((_c), 1) | |
| #define btUnlikely(_c) __builtin_expect((_c), 0) | |
| #else | |
| //non-windows systems | |
| #if (defined (__APPLE__) && defined (__i386__) && (!defined (BT_USE_DOUBLE_PRECISION))) | |
| #define BT_USE_SSE | |
| #include <emmintrin.h> | |
| #define SIMD_FORCE_INLINE inline | |
| ///@todo: check out alignment methods for other platforms/compilers | |
| #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) | |
| #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) | |
| #ifndef assert | |
| #include <assert.h> | |
| #endif | |
| #if defined(DEBUG) || defined (_DEBUG) | |
| #define btAssert assert | |
| #else | |
| #define btAssert(x) | |
| #endif | |
| //btFullAssert is optional, slows down a lot | |
| #define btFullAssert(x) | |
| #define btLikely(_c) _c | |
| #define btUnlikely(_c) _c | |
| #else | |
| #define SIMD_FORCE_INLINE inline | |
| ///@todo: check out alignment methods for other platforms/compilers | |
| ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) | |
| ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) | |
| #define ATTRIBUTE_ALIGNED16(a) a | |
| #define ATTRIBUTE_ALIGNED128(a) a | |
| #ifndef assert | |
| #include <assert.h> | |
| #endif | |
| #if defined(DEBUG) || defined (_DEBUG) | |
| #define btAssert assert | |
| #else | |
| #define btAssert(x) | |
| #endif | |
| //btFullAssert is optional, slows down a lot | |
| #define btFullAssert(x) | |
| #define btLikely(_c) _c | |
| #define btUnlikely(_c) _c | |
| #endif //__APPLE__ | |
| #endif // LIBSPE2 | |
| #endif //__CELLOS_LV2__ | |
| #endif | |
| ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. | |
| #if defined(BT_USE_DOUBLE_PRECISION) | |
| typedef double btScalar; | |
| //this number could be bigger in double precision | |
| #define BT_LARGE_FLOAT 1e30 | |
| #else | |
| typedef float btScalar; | |
| //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX | |
| #define BT_LARGE_FLOAT 1e18f | |
| #endif | |
| #define BT_DECLARE_ALIGNED_ALLOCATOR() \ | |
| SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ | |
| SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ | |
| SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ | |
| SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ | |
| SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ | |
| SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \ | |
| SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ | |
| SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ | |
| #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) | |
| SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } | |
| SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } | |
| SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } | |
| SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } | |
| SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } | |
| SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); } | |
| SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); } | |
| SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } | |
| SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } | |
| SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } | |
| SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } | |
| SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } | |
| SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } | |
| #else | |
| SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) | |
| { | |
| #ifdef USE_APPROXIMATION | |
| double x, z, tempf; | |
| unsigned long *tfptr = ((unsigned long *)&tempf) + 1; | |
| tempf = y; | |
| *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ | |
| x = tempf; | |
| z = y*btScalar(0.5); /* hoist out the “/2” */ | |
| x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ | |
| x = (btScalar(1.5)*x)-(x*x)*(x*z); | |
| x = (btScalar(1.5)*x)-(x*x)*(x*z); | |
| x = (btScalar(1.5)*x)-(x*x)*(x*z); | |
| x = (btScalar(1.5)*x)-(x*x)*(x*z); | |
| return x*y; | |
| #else | |
| return sqrtf(y); | |
| #endif | |
| } | |
| SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } | |
| SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } | |
| SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } | |
| SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } | |
| SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { | |
| btAssert(x <= btScalar(1.)); | |
| return acosf(x); | |
| } | |
| SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } | |
| SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } | |
| SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } | |
| SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } | |
| SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } | |
| SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } | |
| SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } | |
| #endif | |
| #define SIMD_2_PI btScalar(6.283185307179586232) | |
| #define SIMD_PI (SIMD_2_PI * btScalar(0.5)) | |
| #define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) | |
| #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) | |
| #define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) | |
| #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) | |
| #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ | |
| #ifdef BT_USE_DOUBLE_PRECISION | |
| #define SIMD_EPSILON DBL_EPSILON | |
| #define SIMD_INFINITY DBL_MAX | |
| #else | |
| #define SIMD_EPSILON FLT_EPSILON | |
| #define SIMD_INFINITY FLT_MAX | |
| #endif | |
| SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) | |
| { | |
| btScalar coeff_1 = SIMD_PI / 4.0f; | |
| btScalar coeff_2 = 3.0f * coeff_1; | |
| btScalar abs_y = btFabs(y); | |
| btScalar angle; | |
| if (x >= 0.0f) { | |
| btScalar r = (x - abs_y) / (x + abs_y); | |
| angle = coeff_1 - coeff_1 * r; | |
| } else { | |
| btScalar r = (x + abs_y) / (abs_y - x); | |
| angle = coeff_2 - coeff_1 * r; | |
| } | |
| return (y < 0.0f) ? -angle : angle; | |
| } | |
| SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } | |
| SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) { | |
| return (((a) <= eps) && !((a) < -eps)); | |
| } | |
| SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { | |
| return (!((a) <= eps)); | |
| } | |
| SIMD_FORCE_INLINE int btIsNegative(btScalar x) { | |
| return x < btScalar(0.0) ? 1 : 0; | |
| } | |
| SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } | |
| SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } | |
| #define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name | |
| #ifndef btFsel | |
| SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) | |
| { | |
| return a >= 0 ? b : c; | |
| } | |
| #endif | |
| #define btFsels(a,b,c) (btScalar)btFsel(a,b,c) | |
| SIMD_FORCE_INLINE bool btMachineIsLittleEndian() | |
| { | |
| long int i = 1; | |
| const char *p = (const char *) &i; | |
| if (p[0] == 1) // Lowest address contains the least significant byte | |
| return true; | |
| else | |
| return false; | |
| } | |
| ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 | |
| ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html | |
| SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) | |
| { | |
| // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero | |
| // Rely on positive value or'ed with its negative having sign bit on | |
| // and zero value or'ed with its negative (which is still zero) having sign bit off | |
| // Use arithmetic shift right, shifting the sign bit through all 32 bits | |
| unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); | |
| unsigned testEqz = ~testNz; | |
| return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); | |
| } | |
| SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) | |
| { | |
| unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); | |
| unsigned testEqz = ~testNz; | |
| return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); | |
| } | |
| SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) | |
| { | |
| #ifdef BT_HAVE_NATIVE_FSEL | |
| return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); | |
| #else | |
| return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; | |
| #endif | |
| } | |
| template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b) | |
| { | |
| T tmp = a; | |
| a = b; | |
| b = tmp; | |
| } | |
| //PCK: endian swapping functions | |
| SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) | |
| { | |
| return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); | |
| } | |
| SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) | |
| { | |
| return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); | |
| } | |
| SIMD_FORCE_INLINE unsigned btSwapEndian(int val) | |
| { | |
| return btSwapEndian((unsigned)val); | |
| } | |
| SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) | |
| { | |
| return btSwapEndian((unsigned short) val); | |
| } | |
| ///btSwapFloat uses using char pointers to swap the endianness | |
| ////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values | |
| ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. | |
| ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. | |
| ///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. | |
| ///so instead of returning a float/double, we return integer/long long integer | |
| SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) | |
| { | |
| unsigned int a = 0; | |
| unsigned char *dst = (unsigned char *)&a; | |
| unsigned char *src = (unsigned char *)&d; | |
| dst[0] = src[3]; | |
| dst[1] = src[2]; | |
| dst[2] = src[1]; | |
| dst[3] = src[0]; | |
| return a; | |
| } | |
| // unswap using char pointers | |
| SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) | |
| { | |
| float d = 0.0f; | |
| unsigned char *src = (unsigned char *)&a; | |
| unsigned char *dst = (unsigned char *)&d; | |
| dst[0] = src[3]; | |
| dst[1] = src[2]; | |
| dst[2] = src[1]; | |
| dst[3] = src[0]; | |
| return d; | |
| } | |
| // swap using char pointers | |
| SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst) | |
| { | |
| unsigned char *src = (unsigned char *)&d; | |
| dst[0] = src[7]; | |
| dst[1] = src[6]; | |
| dst[2] = src[5]; | |
| dst[3] = src[4]; | |
| dst[4] = src[3]; | |
| dst[5] = src[2]; | |
| dst[6] = src[1]; | |
| dst[7] = src[0]; | |
| } | |
| // unswap using char pointers | |
| SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) | |
| { | |
| double d = 0.0; | |
| unsigned char *dst = (unsigned char *)&d; | |
| dst[0] = src[7]; | |
| dst[1] = src[6]; | |
| dst[2] = src[5]; | |
| dst[3] = src[4]; | |
| dst[4] = src[3]; | |
| dst[5] = src[2]; | |
| dst[6] = src[1]; | |
| dst[7] = src[0]; | |
| return d; | |
| } | |
| // returns normalized value in range [-SIMD_PI, SIMD_PI] | |
| SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) | |
| { | |
| angleInRadians = btFmod(angleInRadians, SIMD_2_PI); | |
| if(angleInRadians < -SIMD_PI) | |
| { | |
| return angleInRadians + SIMD_2_PI; | |
| } | |
| else if(angleInRadians > SIMD_PI) | |
| { | |
| return angleInRadians - SIMD_2_PI; | |
| } | |
| else | |
| { | |
| return angleInRadians; | |
| } | |
| } | |
| ///rudimentary class to provide type info | |
| struct btTypedObject | |
| { | |
| btTypedObject(int objectType) | |
| :m_objectType(objectType) | |
| { | |
| } | |
| int m_objectType; | |
| inline int getObjectType() const | |
| { | |
| return m_objectType; | |
| } | |
| }; | |
| #endif //SIMD___SCALAR_H |