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
b3GetVersion()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
b3Sqrt(b3Scalar x)357 B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
358 {
359 return sqrt(x);
360 }
b3Fabs(b3Scalar x)361 B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
b3Cos(b3Scalar x)362 B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
b3Sin(b3Scalar x)363 B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
b3Tan(b3Scalar x)364 B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
b3Acos(b3Scalar 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 }
b3Asin(b3Scalar x)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 }
b3Atan(b3Scalar x)377 B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
b3Atan2(b3Scalar x,b3Scalar y)378 B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
b3Exp(b3Scalar x)379 B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
b3Log(b3Scalar x)380 B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
b3Pow(b3Scalar x,b3Scalar y)381 B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return pow(x, y); }
b3Fmod(b3Scalar x,b3Scalar y)382 B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmod(x, y); }
383
384 #else
385
b3Sqrt(b3Scalar y)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 }
b3Fabs(b3Scalar x)406 B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
b3Cos(b3Scalar x)407 B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
b3Sin(b3Scalar x)408 B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
b3Tan(b3Scalar x)409 B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
b3Acos(b3Scalar 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 }
b3Asin(b3Scalar x)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 }
b3Atan(b3Scalar x)426 B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
b3Atan2(b3Scalar x,b3Scalar y)427 B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
b3Exp(b3Scalar x)428 B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
b3Log(b3Scalar x)429 B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
b3Pow(b3Scalar x,b3Scalar y)430 B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return powf(x, y); }
b3Fmod(b3Scalar x,b3Scalar 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
b3Atan2Fast(b3Scalar y,b3Scalar x)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
b3FuzzyZero(b3Scalar x)471 B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
472
b3Equal(b3Scalar a,b3Scalar eps)473 B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
474 {
475 return (((a) <= eps) && !((a) < -eps));
476 }
b3GreaterEqual(b3Scalar a,b3Scalar eps)477 B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
478 {
479 return (!((a) <= eps));
480 }
481
b3IsNegative(b3Scalar x)482 B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
483 {
484 return x < b3Scalar(0.0) ? 1 : 0;
485 }
486
b3Radians(b3Scalar x)487 B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
b3Degrees(b3Scalar x)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
b3Fsel(b3Scalar a,b3Scalar b,b3Scalar c)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
b3MachineIsLittleEndian()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
b3Select(unsigned condition,unsigned valueIfConditionNonZero,unsigned valueIfConditionZero)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 }
b3Select(unsigned condition,int valueIfConditionNonZero,int valueIfConditionZero)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 }
b3Select(unsigned condition,float valueIfConditionNonZero,float valueIfConditionZero)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>
b3Swap(T & a,T & b)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
b3SwapEndian(unsigned val)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
b3SwapEndian(unsigned short val)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
b3SwapEndian(int val)560 B3_FORCE_INLINE unsigned b3SwapEndian(int val)
561 {
562 return b3SwapEndian((unsigned)val);
563 }
564
b3SwapEndian(short val)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
b3SwapEndianFloat(float d)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
b3UnswapEndianFloat(unsigned int a)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
b3SwapEndianDouble(double d,unsigned char * dst)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
b3UnswapEndianDouble(const unsigned char * src)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]
b3NormalizeAngle(b3Scalar angleInRadians)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 {
b3TypedObjectb3TypedObject658 b3TypedObject(int objectType)
659 : m_objectType(objectType)
660 {
661 }
662 int m_objectType;
getObjectTypeb3TypedObject663 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>
b3AlignPointer(T * unalignedPtr,size_t alignment)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
690