tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
Scalar.h
Go to the documentation of this file.
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 TF2_SCALAR_H
18 #define TF2_SCALAR_H
19 
20 #ifdef TF2_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 <cstdlib>
29 #include <cfloat>
30 #include <float.h>
31 
32 #if defined(DEBUG) || defined (_DEBUG)
33 #define TF2_DEBUG
34 #endif
35 
36 
37 #ifdef _WIN32
38 
39  #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
40 
41  #define TF2SIMD_FORCE_INLINE inline
42  #define ATTRIBUTE_ALIGNED16(a) a
43  #define ATTRIBUTE_ALIGNED64(a) a
44  #define ATTRIBUTE_ALIGNED128(a) a
45  #else
46  //#define TF2_HAS_ALIGNED_ALLOCATOR
47  #pragma warning(disable : 4324) // disable padding warning
48 // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
49 // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
50 // #pragma warning(disable:4786) // Disable the "debug name too long" warning
51 
52  #define TF2SIMD_FORCE_INLINE __forceinline
53  #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
54  #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
55  #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
56  #ifdef _XBOX
57  #define TF2_USE_VMX128
58 
59  #include <ppcintrinsics.h>
60  #define TF2_HAVE_NATIVE_FSEL
61  #define tf2Fsel(a,b,c) __fsel((a),(b),(c))
62  #else
63 
64 
65  #endif//_XBOX
66 
67  #endif //__MINGW32__
68 
69  #include <assert.h>
70 #ifdef TF2_DEBUG
71  #define tf2Assert assert
72 #else
73  #define tf2Assert(x)
74 #endif
75  //tf2FullAssert is optional, slows down a lot
76  #define tf2FullAssert(x)
77 
78  #define tf2Likely(_c) _c
79  #define tf2Unlikely(_c) _c
80 
81 #else
82 
83 #if defined (__CELLOS_LV2__)
84  #define TF2SIMD_FORCE_INLINE inline
85  #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
86  #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
87  #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
88  #ifndef assert
89  #include <assert.h>
90  #endif
91 #ifdef TF2_DEBUG
92  #define tf2Assert assert
93 #else
94  #define tf2Assert(x)
95 #endif
96  //tf2FullAssert is optional, slows down a lot
97  #define tf2FullAssert(x)
98 
99  #define tf2Likely(_c) _c
100  #define tf2Unlikely(_c) _c
101 
102 #else
103 
104 #ifdef USE_LIBSPE2
105 
106  #define TF2SIMD_FORCE_INLINE __inline
107  #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
108  #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
109  #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
110  #ifndef assert
111  #include <assert.h>
112  #endif
113 #ifdef TF2_DEBUG
114  #define tf2Assert assert
115 #else
116  #define tf2Assert(x)
117 #endif
118  //tf2FullAssert is optional, slows down a lot
119  #define tf2FullAssert(x)
120 
121 
122  #define tf2Likely(_c) __builtin_expect((_c), 1)
123  #define tf2Unlikely(_c) __builtin_expect((_c), 0)
124 
125 
126 #else
127  //non-windows systems
128 
129  #define TF2SIMD_FORCE_INLINE inline
130  #define ATTRIBUTE_ALIGNED16(a) a
135  #define ATTRIBUTE_ALIGNED64(a) a
136  #define ATTRIBUTE_ALIGNED128(a) a
137  #ifndef assert
138  #include <assert.h>
139  #endif
140 
141 #if defined(DEBUG) || defined (_DEBUG)
142  #define tf2Assert assert
143 #else
144  #define tf2Assert(x)
145 #endif
146 
147  //tf2FullAssert is optional, slows down a lot
148  #define tf2FullAssert(x)
149  #define tf2Likely(_c) _c
150  #define tf2Unlikely(_c) _c
151 
152 #endif // LIBSPE2
153 
154 #endif //__CELLOS_LV2__
155 #endif
156 
157 
159 typedef double tf2Scalar;
160 //this number could be bigger in double precision
161 #define TF2_LARGE_FLOAT 1e30
162 
163 
164 #define TF2_DECLARE_ALIGNED_ALLOCATOR() \
165  TF2SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \
166  TF2SIMD_FORCE_INLINE void operator delete(void* ptr) { tf2AlignedFree(ptr); } \
167  TF2SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
168  TF2SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
169  TF2SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \
170  TF2SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf2AlignedFree(ptr); } \
171  TF2SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
172  TF2SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
173 
174 
175 
176 
182 TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x) { if (x<tf2Scalar(-1)) x=tf2Scalar(-1); if (x>tf2Scalar(1)) x=tf2Scalar(1); return acos(x); }
183 TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x) { if (x<tf2Scalar(-1)) x=tf2Scalar(-1); if (x>tf2Scalar(1)) x=tf2Scalar(1); return asin(x); }
190 
191 
192 #define TF2SIMD_2_PI tf2Scalar(6.283185307179586232)
193 #define TF2SIMD_PI (TF2SIMD_2_PI * tf2Scalar(0.5))
194 #define TF2SIMD_HALF_PI (TF2SIMD_2_PI * tf2Scalar(0.25))
195 #define TF2SIMD_RADS_PER_DEG (TF2SIMD_2_PI / tf2Scalar(360.0))
196 #define TF2SIMD_DEGS_PER_RAD (tf2Scalar(360.0) / TF2SIMD_2_PI)
197 #define TF2SIMDSQRT12 tf2Scalar(0.7071067811865475244008443621048490)
198 
199 #define tf2RecipSqrt(x) ((tf2Scalar)(tf2Scalar(1.0)/tf2Sqrt(tf2Scalar(x)))) /* reciprocal square root */
200 
201 
202 #define TF2SIMD_EPSILON DBL_EPSILON
203 #define TF2SIMD_INFINITY DBL_MAX
204 
206 {
207  tf2Scalar coeff_1 = TF2SIMD_PI / 4.0f;
208  tf2Scalar coeff_2 = 3.0f * coeff_1;
209  tf2Scalar abs_y = tf2Fabs(y);
211  if (x >= 0.0f) {
212  tf2Scalar r = (x - abs_y) / (x + abs_y);
213  angle = coeff_1 - coeff_1 * r;
214  } else {
215  tf2Scalar r = (x + abs_y) / (abs_y - x);
216  angle = coeff_2 - coeff_1 * r;
217  }
218  return (y < 0.0f) ? -angle : angle;
219 }
220 
222 
224  return (((a) <= eps) && !((a) < -eps));
225 }
227  return (!((a) <= eps));
228 }
229 
230 
232  return x < tf2Scalar(0.0) ? 1 : 0;
233 }
234 
237 
238 #define TF2_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
239 
240 #ifndef tf2Fsel
242 {
243  return a >= 0 ? b : c;
244 }
245 #endif
246 #define tf2Fsels(a,b,c) (tf2Scalar)tf2Fsel(a,b,c)
247 
248 
250 {
251  long int i = 1;
252  const char *p = (const char *) &i;
253  if (p[0] == 1) // Lowest address contains the least significant byte
254  return true;
255  else
256  return false;
257 }
258 
259 
260 
263 TF2SIMD_FORCE_INLINE unsigned tf2Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
264 {
265  // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
266  // Rely on positive value or'ed with its negative having sign bit on
267  // and zero value or'ed with its negative (which is still zero) having sign bit off
268  // Use arithmetic shift right, shifting the sign bit through all 32 bits
269  unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
270  unsigned testEqz = ~testNz;
271  return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
272 }
273 TF2SIMD_FORCE_INLINE int tf2Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
274 {
275  unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
276  unsigned testEqz = ~testNz;
277  return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
278 }
279 TF2SIMD_FORCE_INLINE float tf2Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
280 {
281 #ifdef TF2_HAVE_NATIVE_FSEL
282  return (float)tf2Fsel((tf2Scalar)condition - tf2Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
283 #else
284  return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
285 #endif
286 }
287 
288 template<typename T> TF2SIMD_FORCE_INLINE void tf2Swap(T& a, T& b)
289 {
290  T tmp = a;
291  a = b;
292  b = tmp;
293 }
294 
295 
296 //PCK: endian swapping functions
297 TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(unsigned val)
298 {
299  return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
300 }
301 
302 TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(unsigned short val)
303 {
304  return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
305 }
306 
308 {
309  return tf2SwapEndian((unsigned)val);
310 }
311 
312 TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(short val)
313 {
314  return tf2SwapEndian((unsigned short) val);
315 }
316 
324 {
325  unsigned int a = 0;
326  unsigned char *dst = (unsigned char *)&a;
327  unsigned char *src = (unsigned char *)&d;
328 
329  dst[0] = src[3];
330  dst[1] = src[2];
331  dst[2] = src[1];
332  dst[3] = src[0];
333  return a;
334 }
335 
336 // unswap using char pointers
338 {
339  float d = 0.0f;
340  unsigned char *src = (unsigned char *)&a;
341  unsigned char *dst = (unsigned char *)&d;
342 
343  dst[0] = src[3];
344  dst[1] = src[2];
345  dst[2] = src[1];
346  dst[3] = src[0];
347 
348  return d;
349 }
350 
351 
352 // swap using char pointers
353 TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char* dst)
354 {
355  unsigned char *src = (unsigned char *)&d;
356 
357  dst[0] = src[7];
358  dst[1] = src[6];
359  dst[2] = src[5];
360  dst[3] = src[4];
361  dst[4] = src[3];
362  dst[5] = src[2];
363  dst[6] = src[1];
364  dst[7] = src[0];
365 
366 }
367 
368 // unswap using char pointers
369 TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src)
370 {
371  double d = 0.0;
372  unsigned char *dst = (unsigned char *)&d;
373 
374  dst[0] = src[7];
375  dst[1] = src[6];
376  dst[2] = src[5];
377  dst[3] = src[4];
378  dst[4] = src[3];
379  dst[5] = src[2];
380  dst[6] = src[1];
381  dst[7] = src[0];
382 
383  return d;
384 }
385 
386 // returns normalized value in range [-TF2SIMD_PI, TF2SIMD_PI]
388 {
389  angleInRadians = tf2Fmod(angleInRadians, TF2SIMD_2_PI);
390  if(angleInRadians < -TF2SIMD_PI)
391  {
392  return angleInRadians + TF2SIMD_2_PI;
393  }
394  else if(angleInRadians > TF2SIMD_PI)
395  {
396  return angleInRadians - TF2SIMD_2_PI;
397  }
398  else
399  {
400  return angleInRadians;
401  }
402 }
403 
406 {
407  tf2TypedObject(int objectType)
408  :m_objectType(objectType)
409  {
410  }
412  inline int getObjectType() const
413  {
414  return m_objectType;
415  }
416 };
417 #endif //TF2SIMD___SCALAR_H
tf2Asin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
Definition: Scalar.h:183
tf2Atan2
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:185
TF2SIMD_2_PI
#define TF2SIMD_2_PI
Definition: Scalar.h:192
tf2MachineIsLittleEndian
TF2SIMD_FORCE_INLINE bool tf2MachineIsLittleEndian()
Definition: Scalar.h:249
TF2SIMD_EPSILON
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
tf2Radians
TF2SIMD_FORCE_INLINE tf2Scalar tf2Radians(tf2Scalar x)
Definition: Scalar.h:235
tf2Equal
TF2SIMD_FORCE_INLINE bool tf2Equal(tf2Scalar a, tf2Scalar eps)
Definition: Scalar.h:223
tf2SwapEndian
TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(unsigned val)
Definition: Scalar.h:297
tf2Sin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
tf2Fmod
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:189
tf2Swap
TF2SIMD_FORCE_INLINE void tf2Swap(T &a, T &b)
Definition: Scalar.h:288
tf2Sqrt
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
TF2SIMD_RADS_PER_DEG
#define TF2SIMD_RADS_PER_DEG
Definition: Scalar.h:195
tf2Degrees
TF2SIMD_FORCE_INLINE tf2Scalar tf2Degrees(tf2Scalar x)
Definition: Scalar.h:236
tf2Acos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x)
Definition: Scalar.h:182
tf2UnswapEndianFloat
TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a)
Definition: Scalar.h:337
tf2NormalizeAngle
TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians)
Definition: Scalar.h:387
tf2UnswapEndianDouble
TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src)
Definition: Scalar.h:369
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
tf2TypedObject::getObjectType
int getObjectType() const
Definition: Scalar.h:412
tf2Select
TF2SIMD_FORCE_INLINE unsigned tf2Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
Definition: Scalar.h:263
tf2FuzzyZero
TF2SIMD_FORCE_INLINE bool tf2FuzzyZero(tf2Scalar x)
Definition: Scalar.h:221
tf2Atan
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan(tf2Scalar x)
Definition: Scalar.h:184
tf2TypedObject::tf2TypedObject
tf2TypedObject(int objectType)
Definition: Scalar.h:407
tf2Tan
TF2SIMD_FORCE_INLINE tf2Scalar tf2Tan(tf2Scalar x)
Definition: Scalar.h:181
tf2Cos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
tf2Atan2Fast
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x)
Definition: Scalar.h:205
tf2Fsel
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fsel(tf2Scalar a, tf2Scalar b, tf2Scalar c)
Definition: Scalar.h:241
tf2SwapEndianFloat
TF2SIMD_FORCE_INLINE unsigned int tf2SwapEndianFloat(float d)
tf2SwapFloat uses using char pointers to swap the endianness
Definition: Scalar.h:323
tf2SwapEndianDouble
TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char *dst)
Definition: Scalar.h:353
tf2Fabs
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x)
Definition: Scalar.h:178
tf2GreaterEqual
TF2SIMD_FORCE_INLINE bool tf2GreaterEqual(tf2Scalar a, tf2Scalar eps)
Definition: Scalar.h:226
tf2::angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
Definition: Quaternion.h:410
tf2Log
TF2SIMD_FORCE_INLINE tf2Scalar tf2Log(tf2Scalar x)
Definition: Scalar.h:187
tf2TypedObject
rudimentary class to provide type info
Definition: Scalar.h:405
tf2Scalar
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
TF2SIMD_PI
#define TF2SIMD_PI
Definition: Scalar.h:193
TF2SIMD_DEGS_PER_RAD
#define TF2SIMD_DEGS_PER_RAD
Definition: Scalar.h:196
tf2Pow
TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:188
tf2IsNegative
TF2SIMD_FORCE_INLINE int tf2IsNegative(tf2Scalar x)
Definition: Scalar.h:231
tf2Exp
TF2SIMD_FORCE_INLINE tf2Scalar tf2Exp(tf2Scalar x)
Definition: Scalar.h:186
tf2TypedObject::m_objectType
int m_objectType
Definition: Scalar.h:411