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