Bullet Collision Detection & Physics Library
btVector3.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
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_VECTOR3_H
18 #define BT_VECTOR3_H
19 
20 //#include <stdint.h>
21 #include "btScalar.h"
22 #include "btMinMax.h"
23 #include "btAlignedAllocator.h"
24 
25 #ifdef BT_USE_DOUBLE_PRECISION
26 #define btVector3Data btVector3DoubleData
27 #define btVector3DataName "btVector3DoubleData"
28 #else
29 #define btVector3Data btVector3FloatData
30 #define btVector3DataName "btVector3FloatData"
31 #endif //BT_USE_DOUBLE_PRECISION
32 
33 #if defined BT_USE_SSE
34 
35 //typedef uint32_t __m128i __attribute__ ((vector_size(16)));
36 
37 #ifdef _MSC_VER
38 #pragma warning(disable: 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
39 #endif
40 
41 
42 #define BT_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x))
43 //#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
44 #define bt_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) )
45 #define bt_splat3_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i, 3) )
46 #define bt_splat_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i,_i) )
47 
48 #define btv3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
49 #define btvAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
50 #define btvFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
51 #define btv3AbsfMask btCastiTo128f(btv3AbsiMask)
52 #define btvFFF0fMask btCastiTo128f(btvFFF0Mask)
53 #define btvxyzMaskf btvFFF0fMask
54 #define btvAbsfMask btCastiTo128f(btvAbsMask)
55 
56 
57 
58 const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
59 const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
60 const __m128 ATTRIBUTE_ALIGNED16(vHalf) = {0.5f, 0.5f, 0.5f, 0.5f};
61 const __m128 ATTRIBUTE_ALIGNED16(v1_5) = {1.5f, 1.5f, 1.5f, 1.5f};
62 
63 #endif
64 
65 #ifdef BT_USE_NEON
66 
67 const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
68 const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
69 const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
70 const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
71 
72 #endif
73 
78 ATTRIBUTE_ALIGNED16(class) btVector3
79 {
80 public:
81 
82  BT_DECLARE_ALIGNED_ALLOCATOR();
83 
84 #if defined (__SPU__) && defined (__CELLOS_LV2__)
85  btScalar m_floats[4];
86 public:
87  SIMD_FORCE_INLINE const vec_float4& get128() const
88  {
89  return *((const vec_float4*)&m_floats[0]);
90  }
91 public:
92 #else //__CELLOS_LV2__ __SPU__
93  #if defined (BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM
94  union {
95  btSimdFloat4 mVec128;
96  btScalar m_floats[4];
97  };
98  SIMD_FORCE_INLINE btSimdFloat4 get128() const
99  {
100  return mVec128;
101  }
102  SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
103  {
104  mVec128 = v128;
105  }
106  #else
107  btScalar m_floats[4];
108  #endif
109 #endif //__CELLOS_LV2__ __SPU__
110 
111  public:
112 
114  SIMD_FORCE_INLINE btVector3()
115  {
116 
117  }
118 
119 
120 
126  SIMD_FORCE_INLINE btVector3(const btScalar& _x, const btScalar& _y, const btScalar& _z)
127  {
128  m_floats[0] = _x;
129  m_floats[1] = _y;
130  m_floats[2] = _z;
131  m_floats[3] = btScalar(0.f);
132  }
133 
134 #if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) )|| defined (BT_USE_NEON)
135  // Set Vector
136  SIMD_FORCE_INLINE btVector3( btSimdFloat4 v)
137  {
138  mVec128 = v;
139  }
140 
141  // Copy constructor
142  SIMD_FORCE_INLINE btVector3(const btVector3& rhs)
143  {
144  mVec128 = rhs.mVec128;
145  }
146 
147  // Assignment Operator
148  SIMD_FORCE_INLINE btVector3&
149  operator=(const btVector3& v)
150  {
151  mVec128 = v.mVec128;
152 
153  return *this;
154  }
155 #endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
156 
159  SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
160  {
161 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
162  mVec128 = _mm_add_ps(mVec128, v.mVec128);
163 #elif defined(BT_USE_NEON)
164  mVec128 = vaddq_f32(mVec128, v.mVec128);
165 #else
166  m_floats[0] += v.m_floats[0];
167  m_floats[1] += v.m_floats[1];
168  m_floats[2] += v.m_floats[2];
169 #endif
170  return *this;
171  }
172 
173 
176  SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
177  {
178 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
179  mVec128 = _mm_sub_ps(mVec128, v.mVec128);
180 #elif defined(BT_USE_NEON)
181  mVec128 = vsubq_f32(mVec128, v.mVec128);
182 #else
183  m_floats[0] -= v.m_floats[0];
184  m_floats[1] -= v.m_floats[1];
185  m_floats[2] -= v.m_floats[2];
186 #endif
187  return *this;
188  }
189 
192  SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
193  {
194 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
195  __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
196  vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
197  mVec128 = _mm_mul_ps(mVec128, vs);
198 #elif defined(BT_USE_NEON)
199  mVec128 = vmulq_n_f32(mVec128, s);
200 #else
201  m_floats[0] *= s;
202  m_floats[1] *= s;
203  m_floats[2] *= s;
204 #endif
205  return *this;
206  }
207 
210  SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
211  {
212  btFullAssert(s != btScalar(0.0));
213 
214 #if 0 //defined(BT_USE_SSE_IN_API)
215 // this code is not faster !
216  __m128 vs = _mm_load_ss(&s);
217  vs = _mm_div_ss(v1110, vs);
218  vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
219 
220  mVec128 = _mm_mul_ps(mVec128, vs);
221 
222  return *this;
223 #else
224  return *this *= btScalar(1.0) / s;
225 #endif
226  }
227 
230  SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
231  {
232 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
233  __m128 vd = _mm_mul_ps(mVec128, v.mVec128);
234  __m128 z = _mm_movehl_ps(vd, vd);
235  __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
236  vd = _mm_add_ss(vd, y);
237  vd = _mm_add_ss(vd, z);
238  return _mm_cvtss_f32(vd);
239 #elif defined(BT_USE_NEON)
240  float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
241  float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
242  x = vadd_f32(x, vget_high_f32(vd));
243  return vget_lane_f32(x, 0);
244 #else
245  return m_floats[0] * v.m_floats[0] +
246  m_floats[1] * v.m_floats[1] +
247  m_floats[2] * v.m_floats[2];
248 #endif
249  }
250 
252  SIMD_FORCE_INLINE btScalar length2() const
253  {
254  return dot(*this);
255  }
256 
258  SIMD_FORCE_INLINE btScalar length() const
259  {
260  return btSqrt(length2());
261  }
262 
265  SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
266 
269  SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
270 
271  SIMD_FORCE_INLINE btVector3& safeNormalize()
272  {
273  btVector3 absVec = this->absolute();
274  int maxIndex = absVec.maxAxis();
275  if (absVec[maxIndex]>0)
276  {
277  *this /= absVec[maxIndex];
278  return *this /= length();
279  }
280  setValue(1,0,0);
281  return *this;
282  }
283 
286  SIMD_FORCE_INLINE btVector3& normalize()
287  {
288 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
289  // dot product first
290  __m128 vd = _mm_mul_ps(mVec128, mVec128);
291  __m128 z = _mm_movehl_ps(vd, vd);
292  __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
293  vd = _mm_add_ss(vd, y);
294  vd = _mm_add_ss(vd, z);
295 
296  #if 0
297  vd = _mm_sqrt_ss(vd);
298  vd = _mm_div_ss(v1110, vd);
299  vd = bt_splat_ps(vd, 0x80);
300  mVec128 = _mm_mul_ps(mVec128, vd);
301  #else
302 
303  // NR step 1/sqrt(x) - vd is x, y is output
304  y = _mm_rsqrt_ss(vd); // estimate
305 
306  // one step NR
307  z = v1_5;
308  vd = _mm_mul_ss(vd, vHalf); // vd * 0.5
309  //x2 = vd;
310  vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
311  vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
312  z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0
313 
314  y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0)
315 
316  y = bt_splat_ps(y, 0x80);
317  mVec128 = _mm_mul_ps(mVec128, y);
318 
319  #endif
320 
321 
322  return *this;
323 #else
324  return *this /= length();
325 #endif
326  }
327 
329  SIMD_FORCE_INLINE btVector3 normalized() const;
330 
334  SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
335 
338  SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
339  {
340  btScalar s = btSqrt(length2() * v.length2());
341  btFullAssert(s != btScalar(0.0));
342  return btAcos(dot(v) / s);
343  }
344 
346  SIMD_FORCE_INLINE btVector3 absolute() const
347  {
348 
349 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
350  return btVector3(_mm_and_ps(mVec128, btv3AbsfMask));
351 #elif defined(BT_USE_NEON)
352  return btVector3(vabsq_f32(mVec128));
353 #else
354  return btVector3(
355  btFabs(m_floats[0]),
356  btFabs(m_floats[1]),
357  btFabs(m_floats[2]));
358 #endif
359  }
360 
363  SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
364  {
365 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
366  __m128 T, V;
367 
368  T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
369  V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
370 
371  V = _mm_mul_ps(V, mVec128);
372  T = _mm_mul_ps(T, v.mVec128);
373  V = _mm_sub_ps(V, T);
374 
375  V = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3));
376  return btVector3(V);
377 #elif defined(BT_USE_NEON)
378  float32x4_t T, V;
379  // form (Y, Z, X, _) of mVec128 and v.mVec128
380  float32x2_t Tlow = vget_low_f32(mVec128);
381  float32x2_t Vlow = vget_low_f32(v.mVec128);
382  T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
383  V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
384 
385  V = vmulq_f32(V, mVec128);
386  T = vmulq_f32(T, v.mVec128);
387  V = vsubq_f32(V, T);
388  Vlow = vget_low_f32(V);
389  // form (Y, Z, X, _);
390  V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
391  V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask);
392 
393  return btVector3(V);
394 #else
395  return btVector3(
396  m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
397  m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
398  m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
399 #endif
400  }
401 
402  SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
403  {
404 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
405  // cross:
406  __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
407  __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
408 
409  V = _mm_mul_ps(V, v1.mVec128);
410  T = _mm_mul_ps(T, v2.mVec128);
411  V = _mm_sub_ps(V, T);
412 
413  V = _mm_shuffle_ps(V, V, BT_SHUFFLE(1, 2, 0, 3));
414 
415  // dot:
416  V = _mm_mul_ps(V, mVec128);
417  __m128 z = _mm_movehl_ps(V, V);
418  __m128 y = _mm_shuffle_ps(V, V, 0x55);
419  V = _mm_add_ss(V, y);
420  V = _mm_add_ss(V, z);
421  return _mm_cvtss_f32(V);
422 
423 #elif defined(BT_USE_NEON)
424  // cross:
425  float32x4_t T, V;
426  // form (Y, Z, X, _) of mVec128 and v.mVec128
427  float32x2_t Tlow = vget_low_f32(v1.mVec128);
428  float32x2_t Vlow = vget_low_f32(v2.mVec128);
429  T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
430  V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
431 
432  V = vmulq_f32(V, v1.mVec128);
433  T = vmulq_f32(T, v2.mVec128);
434  V = vsubq_f32(V, T);
435  Vlow = vget_low_f32(V);
436  // form (Y, Z, X, _);
437  V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
438 
439  // dot:
440  V = vmulq_f32(mVec128, V);
441  float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
442  x = vadd_f32(x, vget_high_f32(V));
443  return vget_lane_f32(x, 0);
444 #else
445  return
446  m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
447  m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
448  m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
449 #endif
450  }
451 
454  SIMD_FORCE_INLINE int minAxis() const
455  {
456  return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
457  }
458 
461  SIMD_FORCE_INLINE int maxAxis() const
462  {
463  return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
464  }
465 
466  SIMD_FORCE_INLINE int furthestAxis() const
467  {
468  return absolute().minAxis();
469  }
470 
471  SIMD_FORCE_INLINE int closestAxis() const
472  {
473  return absolute().maxAxis();
474  }
475 
476 
477  SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
478  {
479 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
480  __m128 vrt = _mm_load_ss(&rt); // (rt 0 0 0)
481  btScalar s = btScalar(1.0) - rt;
482  __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
483  vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
484  __m128 r0 = _mm_mul_ps(v0.mVec128, vs);
485  vrt = bt_pshufd_ps(vrt, 0x80); // (rt rt rt 0.0)
486  __m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
487  __m128 tmp3 = _mm_add_ps(r0,r1);
488  mVec128 = tmp3;
489 #elif defined(BT_USE_NEON)
490  mVec128 = vsubq_f32(v1.mVec128, v0.mVec128);
491  mVec128 = vmulq_n_f32(mVec128, rt);
492  mVec128 = vaddq_f32(mVec128, v0.mVec128);
493 #else
494  btScalar s = btScalar(1.0) - rt;
495  m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
496  m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
497  m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
498  //don't do the unused w component
499  // m_co[3] = s * v0[3] + rt * v1[3];
500 #endif
501  }
502 
506  SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
507  {
508 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
509  __m128 vt = _mm_load_ss(&t); // (t 0 0 0)
510  vt = bt_pshufd_ps(vt, 0x80); // (rt rt rt 0.0)
511  __m128 vl = _mm_sub_ps(v.mVec128, mVec128);
512  vl = _mm_mul_ps(vl, vt);
513  vl = _mm_add_ps(vl, mVec128);
514 
515  return btVector3(vl);
516 #elif defined(BT_USE_NEON)
517  float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
518  vl = vmulq_n_f32(vl, t);
519  vl = vaddq_f32(vl, mVec128);
520 
521  return btVector3(vl);
522 #else
523  return
524  btVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
525  m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
526  m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
527 #endif
528  }
529 
532  SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
533  {
534 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
535  mVec128 = _mm_mul_ps(mVec128, v.mVec128);
536 #elif defined(BT_USE_NEON)
537  mVec128 = vmulq_f32(mVec128, v.mVec128);
538 #else
539  m_floats[0] *= v.m_floats[0];
540  m_floats[1] *= v.m_floats[1];
541  m_floats[2] *= v.m_floats[2];
542 #endif
543  return *this;
544  }
545 
547  SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
549  SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
551  SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
553  SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
555  SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;};
557  SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
559  SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
561  SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
563  SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
565  SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
567  SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
568 
569  //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
570  //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
572  SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
573  SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
574 
575  SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
576  {
577 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
578  return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
579 #else
580  return ((m_floats[3]==other.m_floats[3]) &&
581  (m_floats[2]==other.m_floats[2]) &&
582  (m_floats[1]==other.m_floats[1]) &&
583  (m_floats[0]==other.m_floats[0]));
584 #endif
585  }
586 
587  SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
588  {
589  return !(*this == other);
590  }
591 
595  SIMD_FORCE_INLINE void setMax(const btVector3& other)
596  {
597 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
598  mVec128 = _mm_max_ps(mVec128, other.mVec128);
599 #elif defined(BT_USE_NEON)
600  mVec128 = vmaxq_f32(mVec128, other.mVec128);
601 #else
602  btSetMax(m_floats[0], other.m_floats[0]);
603  btSetMax(m_floats[1], other.m_floats[1]);
604  btSetMax(m_floats[2], other.m_floats[2]);
605  btSetMax(m_floats[3], other.w());
606 #endif
607  }
608 
612  SIMD_FORCE_INLINE void setMin(const btVector3& other)
613  {
614 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
615  mVec128 = _mm_min_ps(mVec128, other.mVec128);
616 #elif defined(BT_USE_NEON)
617  mVec128 = vminq_f32(mVec128, other.mVec128);
618 #else
619  btSetMin(m_floats[0], other.m_floats[0]);
620  btSetMin(m_floats[1], other.m_floats[1]);
621  btSetMin(m_floats[2], other.m_floats[2]);
622  btSetMin(m_floats[3], other.w());
623 #endif
624  }
625 
626  SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
627  {
628  m_floats[0]=_x;
629  m_floats[1]=_y;
630  m_floats[2]=_z;
631  m_floats[3] = btScalar(0.f);
632  }
633 
634  void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
635  {
636 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
637 
638  __m128 V = _mm_and_ps(mVec128, btvFFF0fMask);
639  __m128 V0 = _mm_xor_ps(btvMzeroMask, V);
640  __m128 V2 = _mm_movelh_ps(V0, V);
641 
642  __m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
643 
644  V0 = _mm_shuffle_ps(V0, V, 0xDB);
645  V2 = _mm_shuffle_ps(V2, V, 0xF9);
646 
647  v0->mVec128 = V0;
648  v1->mVec128 = V1;
649  v2->mVec128 = V2;
650 #else
651  v0->setValue(0. ,-z() ,y());
652  v1->setValue(z() ,0. ,-x());
653  v2->setValue(-y() ,x() ,0.);
654 #endif
655  }
656 
657  void setZero()
658  {
659 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
660  mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
661 #elif defined(BT_USE_NEON)
662  int32x4_t vi = vdupq_n_s32(0);
663  mVec128 = vreinterpretq_f32_s32(vi);
664 #else
665  setValue(btScalar(0.),btScalar(0.),btScalar(0.));
666 #endif
667  }
668 
669  SIMD_FORCE_INLINE bool isZero() const
670  {
671  return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
672  }
673 
674  SIMD_FORCE_INLINE bool fuzzyZero() const
675  {
676  return length2() < SIMD_EPSILON;
677  }
678 
679  SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
680 
681  SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
682 
683  SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const;
684 
685  SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
686 
687  SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const;
688 
689  SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
690 
695  SIMD_FORCE_INLINE long maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
696 
701  SIMD_FORCE_INLINE long minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
702 
703  /* create a vector as btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 )) */
704  SIMD_FORCE_INLINE btVector3 dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const
705  {
706 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
707 
708  __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
709  __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
710  __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 );
711  __m128 b0 = _mm_unpacklo_ps( a0, a1 );
712  __m128 b1 = _mm_unpackhi_ps( a0, a1 );
713  __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
714  __m128 r = _mm_movelh_ps( b0, b2 );
715  r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
716  a2 = _mm_and_ps( a2, btvxyzMaskf);
717  r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
718  return btVector3(r);
719 
720 #elif defined(BT_USE_NEON)
721  static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
722  float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
723  float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
724  float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
725  float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1));
726  a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
727  float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
728  float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
729  return btVector3( vcombine_f32(b0, b1) );
730 #else
731  return btVector3( dot(v0), dot(v1), dot(v2));
732 #endif
733  }
734 };
735 
737 SIMD_FORCE_INLINE btVector3
738 operator+(const btVector3& v1, const btVector3& v2)
739 {
740 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
741  return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
742 #elif defined(BT_USE_NEON)
743  return btVector3(vaddq_f32(v1.mVec128, v2.mVec128));
744 #else
745  return btVector3(
746  v1.m_floats[0] + v2.m_floats[0],
747  v1.m_floats[1] + v2.m_floats[1],
748  v1.m_floats[2] + v2.m_floats[2]);
749 #endif
750 }
751 
753 SIMD_FORCE_INLINE btVector3
754 operator*(const btVector3& v1, const btVector3& v2)
755 {
756 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
757  return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
758 #elif defined(BT_USE_NEON)
759  return btVector3(vmulq_f32(v1.mVec128, v2.mVec128));
760 #else
761  return btVector3(
762  v1.m_floats[0] * v2.m_floats[0],
763  v1.m_floats[1] * v2.m_floats[1],
764  v1.m_floats[2] * v2.m_floats[2]);
765 #endif
766 }
767 
769 SIMD_FORCE_INLINE btVector3
770 operator-(const btVector3& v1, const btVector3& v2)
771 {
772 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
773 
774  // without _mm_and_ps this code causes slowdown in Concave moving
775  __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
776  return btVector3(_mm_and_ps(r, btvFFF0fMask));
777 #elif defined(BT_USE_NEON)
778  float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
779  return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
780 #else
781  return btVector3(
782  v1.m_floats[0] - v2.m_floats[0],
783  v1.m_floats[1] - v2.m_floats[1],
784  v1.m_floats[2] - v2.m_floats[2]);
785 #endif
786 }
787 
789 SIMD_FORCE_INLINE btVector3
790 operator-(const btVector3& v)
791 {
792 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
793  __m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
794  return btVector3(_mm_and_ps(r, btvFFF0fMask));
795 #elif defined(BT_USE_NEON)
796  return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
797 #else
798  return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
799 #endif
800 }
801 
803 SIMD_FORCE_INLINE btVector3
804 operator*(const btVector3& v, const btScalar& s)
805 {
806 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
807  __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
808  vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
809  return btVector3(_mm_mul_ps(v.mVec128, vs));
810 #elif defined(BT_USE_NEON)
811  float32x4_t r = vmulq_n_f32(v.mVec128, s);
812  return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
813 #else
814  return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
815 #endif
816 }
817 
819 SIMD_FORCE_INLINE btVector3
820 operator*(const btScalar& s, const btVector3& v)
821 {
822  return v * s;
823 }
824 
826 SIMD_FORCE_INLINE btVector3
827 operator/(const btVector3& v, const btScalar& s)
828 {
829  btFullAssert(s != btScalar(0.0));
830 #if 0 //defined(BT_USE_SSE_IN_API)
831 // this code is not faster !
832  __m128 vs = _mm_load_ss(&s);
833  vs = _mm_div_ss(v1110, vs);
834  vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
835 
836  return btVector3(_mm_mul_ps(v.mVec128, vs));
837 #else
838  return v * (btScalar(1.0) / s);
839 #endif
840 }
841 
843 SIMD_FORCE_INLINE btVector3
844 operator/(const btVector3& v1, const btVector3& v2)
845 {
846 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
847  __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
848  vec = _mm_and_ps(vec, btvFFF0fMask);
849  return btVector3(vec);
850 #elif defined(BT_USE_NEON)
851  float32x4_t x, y, v, m;
852 
853  x = v1.mVec128;
854  y = v2.mVec128;
855 
856  v = vrecpeq_f32(y); // v ~ 1/y
857  m = vrecpsq_f32(y, v); // m = (2-v*y)
858  v = vmulq_f32(v, m); // vv = v*m ~~ 1/y
859  m = vrecpsq_f32(y, v); // mm = (2-vv*y)
860  v = vmulq_f32(v, x); // x*vv
861  v = vmulq_f32(v, m); // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
862 
863  return btVector3(v);
864 #else
865  return btVector3(
866  v1.m_floats[0] / v2.m_floats[0],
867  v1.m_floats[1] / v2.m_floats[1],
868  v1.m_floats[2] / v2.m_floats[2]);
869 #endif
870 }
871 
873 SIMD_FORCE_INLINE btScalar
874 btDot(const btVector3& v1, const btVector3& v2)
875 {
876  return v1.dot(v2);
877 }
878 
879 
881 SIMD_FORCE_INLINE btScalar
882 btDistance2(const btVector3& v1, const btVector3& v2)
883 {
884  return v1.distance2(v2);
885 }
886 
887 
889 SIMD_FORCE_INLINE btScalar
890 btDistance(const btVector3& v1, const btVector3& v2)
891 {
892  return v1.distance(v2);
893 }
894 
896 SIMD_FORCE_INLINE btScalar
897 btAngle(const btVector3& v1, const btVector3& v2)
898 {
899  return v1.angle(v2);
900 }
901 
903 SIMD_FORCE_INLINE btVector3
904 btCross(const btVector3& v1, const btVector3& v2)
905 {
906  return v1.cross(v2);
907 }
908 
909 SIMD_FORCE_INLINE btScalar
910 btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
911 {
912  return v1.triple(v2, v3);
913 }
914 
919 SIMD_FORCE_INLINE btVector3
920 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
921 {
922  return v1.lerp(v2, t);
923 }
924 
925 
926 
927 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
928 {
929  return (v - *this).length2();
930 }
931 
932 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
933 {
934  return (v - *this).length();
935 }
936 
937 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
938 {
939 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
940  btVector3 norm = *this;
941 
942  return norm.normalize();
943 #else
944  return *this / length();
945 #endif
946 }
947 
948 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
949 {
950  // wAxis must be a unit lenght vector
951 
952 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
953 
954  __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
955  btScalar ssin = btSin( _angle );
956  __m128 C = wAxis.cross( mVec128 ).mVec128;
957  O = _mm_and_ps(O, btvFFF0fMask);
958  btScalar scos = btCos( _angle );
959 
960  __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
961  __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
962 
963  __m128 Y = bt_pshufd_ps(O, 0xC9); // (Y Z X 0)
964  __m128 Z = bt_pshufd_ps(O, 0xD2); // (Z X Y 0)
965  O = _mm_add_ps(O, Y);
966  vsin = bt_pshufd_ps(vsin, 0x80); // (S S S 0)
967  O = _mm_add_ps(O, Z);
968  vcos = bt_pshufd_ps(vcos, 0x80); // (S S S 0)
969 
970  vsin = vsin * C;
971  O = O * wAxis.mVec128;
972  __m128 X = mVec128 - O;
973 
974  O = O + vsin;
975  vcos = vcos * X;
976  O = O + vcos;
977 
978  return btVector3(O);
979 #else
980  btVector3 o = wAxis * wAxis.dot( *this );
981  btVector3 _x = *this - o;
982  btVector3 _y;
983 
984  _y = wAxis.cross( *this );
985 
986  return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) );
987 #endif
988 }
989 
990 SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
991 {
992 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
993  #if defined _WIN32 || defined (BT_USE_SSE)
994  const long scalar_cutoff = 10;
995  long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
996  #elif defined BT_USE_NEON
997  const long scalar_cutoff = 4;
998  extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
999  #endif
1000  if( array_count < scalar_cutoff )
1001 #endif
1002  {
1003  btScalar maxDot = -SIMD_INFINITY;
1004  int i = 0;
1005  int ptIndex = -1;
1006  for( i = 0; i < array_count; i++ )
1007  {
1008  btScalar dot = array[i].dot(*this);
1009 
1010  if( dot > maxDot )
1011  {
1012  maxDot = dot;
1013  ptIndex = i;
1014  }
1015  }
1016 
1017  dotOut = maxDot;
1018  return ptIndex;
1019  }
1020 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1021  return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1022 #endif
1023 }
1024 
1025 SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
1026 {
1027 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1028  #if defined BT_USE_SSE
1029  const long scalar_cutoff = 10;
1030  long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1031  #elif defined BT_USE_NEON
1032  const long scalar_cutoff = 4;
1033  extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1034  #else
1035  #error unhandled arch!
1036  #endif
1037 
1038  if( array_count < scalar_cutoff )
1039 #endif
1040  {
1041  btScalar minDot = SIMD_INFINITY;
1042  int i = 0;
1043  int ptIndex = -1;
1044 
1045  for( i = 0; i < array_count; i++ )
1046  {
1047  btScalar dot = array[i].dot(*this);
1048 
1049  if( dot < minDot )
1050  {
1051  minDot = dot;
1052  ptIndex = i;
1053  }
1054  }
1055 
1056  dotOut = minDot;
1057 
1058  return ptIndex;
1059  }
1060 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1061  return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1062 #endif//BT_USE_SIMD_VECTOR3
1063 }
1064 
1065 
1066 class btVector4 : public btVector3
1067 {
1068 public:
1069 
1070  SIMD_FORCE_INLINE btVector4() {}
1071 
1072 
1073  SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1074  : btVector3(_x,_y,_z)
1075  {
1076  m_floats[3] = _w;
1077  }
1078 
1079 #if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON)
1080  SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec)
1081  {
1082  mVec128 = vec;
1083  }
1084 
1085  SIMD_FORCE_INLINE btVector4(const btVector3& rhs)
1086  {
1087  mVec128 = rhs.mVec128;
1088  }
1089 
1090  SIMD_FORCE_INLINE btVector4&
1091  operator=(const btVector4& v)
1092  {
1093  mVec128 = v.mVec128;
1094  return *this;
1095  }
1096 #endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1097 
1098  SIMD_FORCE_INLINE btVector4 absolute4() const
1099  {
1100 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
1101  return btVector4(_mm_and_ps(mVec128, btvAbsfMask));
1102 #elif defined(BT_USE_NEON)
1103  return btVector4(vabsq_f32(mVec128));
1104 #else
1105  return btVector4(
1106  btFabs(m_floats[0]),
1107  btFabs(m_floats[1]),
1108  btFabs(m_floats[2]),
1109  btFabs(m_floats[3]));
1110 #endif
1111  }
1112 
1113 
1114  btScalar getW() const { return m_floats[3];}
1115 
1116 
1117  SIMD_FORCE_INLINE int maxAxis4() const
1118  {
1119  int maxIndex = -1;
1120  btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
1121  if (m_floats[0] > maxVal)
1122  {
1123  maxIndex = 0;
1124  maxVal = m_floats[0];
1125  }
1126  if (m_floats[1] > maxVal)
1127  {
1128  maxIndex = 1;
1129  maxVal = m_floats[1];
1130  }
1131  if (m_floats[2] > maxVal)
1132  {
1133  maxIndex = 2;
1134  maxVal =m_floats[2];
1135  }
1136  if (m_floats[3] > maxVal)
1137  {
1138  maxIndex = 3;
1139  maxVal = m_floats[3];
1140  }
1141 
1142  return maxIndex;
1143  }
1144 
1145 
1146  SIMD_FORCE_INLINE int minAxis4() const
1147  {
1148  int minIndex = -1;
1149  btScalar minVal = btScalar(BT_LARGE_FLOAT);
1150  if (m_floats[0] < minVal)
1151  {
1152  minIndex = 0;
1153  minVal = m_floats[0];
1154  }
1155  if (m_floats[1] < minVal)
1156  {
1157  minIndex = 1;
1158  minVal = m_floats[1];
1159  }
1160  if (m_floats[2] < minVal)
1161  {
1162  minIndex = 2;
1163  minVal =m_floats[2];
1164  }
1165  if (m_floats[3] < minVal)
1166  {
1167  minIndex = 3;
1168  minVal = m_floats[3];
1169  }
1170 
1171  return minIndex;
1172  }
1173 
1174 
1175  SIMD_FORCE_INLINE int closestAxis4() const
1176  {
1177  return absolute4().maxAxis4();
1178  }
1179 
1180 
1181 
1182 
1190 /* void getValue(btScalar *m) const
1191  {
1192  m[0] = m_floats[0];
1193  m[1] = m_floats[1];
1194  m[2] =m_floats[2];
1195  }
1196 */
1203  SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1204  {
1205  m_floats[0]=_x;
1206  m_floats[1]=_y;
1207  m_floats[2]=_z;
1208  m_floats[3]=_w;
1209  }
1210 
1211 
1212 };
1213 
1214 
1216 SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
1217 {
1218  #ifdef BT_USE_DOUBLE_PRECISION
1219  unsigned char* dest = (unsigned char*) &destVal;
1220  unsigned char* src = (unsigned char*) &sourceVal;
1221  dest[0] = src[7];
1222  dest[1] = src[6];
1223  dest[2] = src[5];
1224  dest[3] = src[4];
1225  dest[4] = src[3];
1226  dest[5] = src[2];
1227  dest[6] = src[1];
1228  dest[7] = src[0];
1229 #else
1230  unsigned char* dest = (unsigned char*) &destVal;
1231  unsigned char* src = (unsigned char*) &sourceVal;
1232  dest[0] = src[3];
1233  dest[1] = src[2];
1234  dest[2] = src[1];
1235  dest[3] = src[0];
1236 #endif //BT_USE_DOUBLE_PRECISION
1237 }
1239 SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
1240 {
1241  for (int i=0;i<4;i++)
1242  {
1243  btSwapScalarEndian(sourceVec[i],destVec[i]);
1244  }
1245 
1246 }
1247 
1249 SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
1250 {
1251 
1252  btVector3 swappedVec;
1253  for (int i=0;i<4;i++)
1254  {
1255  btSwapScalarEndian(vector[i],swappedVec[i]);
1256  }
1257  vector = swappedVec;
1258 }
1259 
1260 template <class T>
1261 SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
1262 {
1263  if (btFabs(n[2]) > SIMDSQRT12) {
1264  // choose p in y-z plane
1265  btScalar a = n[1]*n[1] + n[2]*n[2];
1266  btScalar k = btRecipSqrt (a);
1267  p[0] = 0;
1268  p[1] = -n[2]*k;
1269  p[2] = n[1]*k;
1270  // set q = n x p
1271  q[0] = a*k;
1272  q[1] = -n[0]*p[2];
1273  q[2] = n[0]*p[1];
1274  }
1275  else {
1276  // choose p in x-y plane
1277  btScalar a = n[0]*n[0] + n[1]*n[1];
1278  btScalar k = btRecipSqrt (a);
1279  p[0] = -n[1]*k;
1280  p[1] = n[0]*k;
1281  p[2] = 0;
1282  // set q = n x p
1283  q[0] = -n[2]*p[1];
1284  q[1] = n[2]*p[0];
1285  q[2] = a*k;
1286  }
1287 }
1288 
1289 
1290 struct btVector3FloatData
1291 {
1292  float m_floats[4];
1293 };
1294 
1295 struct btVector3DoubleData
1296 {
1297  double m_floats[4];
1298 
1299 };
1300 
1301 SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
1302 {
1304  for (int i=0;i<4;i++)
1305  dataOut.m_floats[i] = float(m_floats[i]);
1306 }
1307 
1308 SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
1309 {
1310  for (int i=0;i<4;i++)
1311  m_floats[i] = btScalar(dataIn.m_floats[i]);
1312 }
1313 
1314 
1315 SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
1316 {
1318  for (int i=0;i<4;i++)
1319  dataOut.m_floats[i] = double(m_floats[i]);
1320 }
1321 
1322 SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
1323 {
1324  for (int i=0;i<4;i++)
1325  m_floats[i] = btScalar(dataIn.m_floats[i]);
1326 }
1327 
1328 
1329 SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
1330 {
1332  for (int i=0;i<4;i++)
1333  dataOut.m_floats[i] = m_floats[i];
1334 }
1335 
1336 SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
1337 {
1338  for (int i=0;i<4;i++)
1339  m_floats[i] = dataIn.m_floats[i];
1340 }
1341 
1342 #endif //BT_VECTOR3_H
Generated on Mon Mar 18 2013 14:12:58 for Bullet Collision Detection & Physics Library by   doxygen 1.8.3.1