Bullet Collision Detection & Physics Library: btVector3.h Source File

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
275 SIMD_FORCE_INLINE btScalar safeNorm() const
276 {
277 btScalar d = length2();
278 //workaround for some clang/gcc issue of sqrtf(tiny number) = -INF
279 if (d>SIMD_EPSILON)
280 return btSqrt(d);
281 return btScalar(0);
282 }
283
286 SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
287
290 SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
291
292 SIMD_FORCE_INLINE btVector3& safeNormalize()
293 {
294 btScalar l2 = length2();
295 //triNormal.normalize();
296 if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
297 {
298 (*this) /= btSqrt(l2);
299 }
300 else
301 {
302 setValue(1, 0, 0);
303 }
304 return *this;
305 }
306
309 SIMD_FORCE_INLINE btVector3& normalize()
310 {
311
312 btAssert(!fuzzyZero());
313
314 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
315 // dot product first
316 __m128 vd = _mm_mul_ps(mVec128, mVec128);
317 __m128 z = _mm_movehl_ps(vd, vd);
318 __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
319 vd = _mm_add_ss(vd, y);
320 vd = _mm_add_ss(vd, z);
321
322 #if 0
323 vd = _mm_sqrt_ss(vd);
324 vd = _mm_div_ss(v1110, vd);
325 vd = bt_splat_ps(vd, 0x80);
326 mVec128 = _mm_mul_ps(mVec128, vd);
327 #else
328
329 // NR step 1/sqrt(x) - vd is x, y is output
330 y = _mm_rsqrt_ss(vd); // estimate
331
332 // one step NR
333 z = v1_5;
334 vd = _mm_mul_ss(vd, vHalf); // vd * 0.5
335 //x2 = vd;
336 vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
337 vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
338 z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0
339
340 y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0)
341
342 y = bt_splat_ps(y, 0x80);
343 mVec128 = _mm_mul_ps(mVec128, y);
344
345 #endif
346
347
348 return *this;
349 #else
350 return *this /= length();
351 #endif
352 }
353
355 SIMD_FORCE_INLINE btVector3 normalized() const;
356
360 SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
361
364 SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
365 {
366 btScalar s = btSqrt(length2() * v.length2());
367 btFullAssert(s != btScalar(0.0));
368 return btAcos(dot(v) / s);
369 }
370
372 SIMD_FORCE_INLINE btVector3 absolute() const
373 {
374
375 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
376 return btVector3(_mm_and_ps(mVec128, btv3AbsfMask));
377 #elif defined(BT_USE_NEON)
378 return btVector3(vabsq_f32(mVec128));
379 #else
380 return btVector3(
381 btFabs(m_floats[0]),
382 btFabs(m_floats[1]),
383 btFabs(m_floats[2]));
384 #endif
385 }
386
389 SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
390 {
391 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
392 __m128 T, V;
393
394 T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
395 V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
396
397 V = _mm_mul_ps(V, mVec128);
398 T = _mm_mul_ps(T, v.mVec128);
399 V = _mm_sub_ps(V, T);
400
401 V = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3));
402 return btVector3(V);
403 #elif defined(BT_USE_NEON)
404 float32x4_t T, V;
405 // form (Y, Z, X, _) of mVec128 and v.mVec128
406 float32x2_t Tlow = vget_low_f32(mVec128);
407 float32x2_t Vlow = vget_low_f32(v.mVec128);
408 T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
409 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
410
411 V = vmulq_f32(V, mVec128);
412 T = vmulq_f32(T, v.mVec128);
413 V = vsubq_f32(V, T);
414 Vlow = vget_low_f32(V);
415 // form (Y, Z, X, _);
416 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
417 V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask);
418
419 return btVector3(V);
420 #else
421 return btVector3(
422 m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
423 m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
424 m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
425 #endif
426 }
427
428 SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
429 {
430 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
431 // cross:
432 __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
433 __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
434
435 V = _mm_mul_ps(V, v1.mVec128);
436 T = _mm_mul_ps(T, v2.mVec128);
437 V = _mm_sub_ps(V, T);
438
439 V = _mm_shuffle_ps(V, V, BT_SHUFFLE(1, 2, 0, 3));
440
441 // dot:
442 V = _mm_mul_ps(V, mVec128);
443 __m128 z = _mm_movehl_ps(V, V);
444 __m128 y = _mm_shuffle_ps(V, V, 0x55);
445 V = _mm_add_ss(V, y);
446 V = _mm_add_ss(V, z);
447 return _mm_cvtss_f32(V);
448
449 #elif defined(BT_USE_NEON)
450 // cross:
451 float32x4_t T, V;
452 // form (Y, Z, X, _) of mVec128 and v.mVec128
453 float32x2_t Tlow = vget_low_f32(v1.mVec128);
454 float32x2_t Vlow = vget_low_f32(v2.mVec128);
455 T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
456 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
457
458 V = vmulq_f32(V, v1.mVec128);
459 T = vmulq_f32(T, v2.mVec128);
460 V = vsubq_f32(V, T);
461 Vlow = vget_low_f32(V);
462 // form (Y, Z, X, _);
463 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
464
465 // dot:
466 V = vmulq_f32(mVec128, V);
467 float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
468 x = vadd_f32(x, vget_high_f32(V));
469 return vget_lane_f32(x, 0);
470 #else
471 return
472 m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
473 m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
474 m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
475 #endif
476 }
477
480 SIMD_FORCE_INLINE int minAxis() const
481 {
482 return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
483 }
484
487 SIMD_FORCE_INLINE int maxAxis() const
488 {
489 return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
490 }
491
492 SIMD_FORCE_INLINE int furthestAxis() const
493 {
494 return absolute().minAxis();
495 }
496
497 SIMD_FORCE_INLINE int closestAxis() const
498 {
499 return absolute().maxAxis();
500 }
501
502
503 SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
504 {
505 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
506 __m128 vrt = _mm_load_ss(&rt); // (rt 0 0 0)
507 btScalar s = btScalar(1.0) - rt;
508 __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
509 vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
510 __m128 r0 = _mm_mul_ps(v0.mVec128, vs);
511 vrt = bt_pshufd_ps(vrt, 0x80); // (rt rt rt 0.0)
512 __m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
513 __m128 tmp3 = _mm_add_ps(r0,r1);
514 mVec128 = tmp3;
515 #elif defined(BT_USE_NEON)
516 float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128);
517 vl = vmulq_n_f32(vl, rt);
518 mVec128 = vaddq_f32(vl, v0.mVec128);
519 #else
520 btScalar s = btScalar(1.0) - rt;
521 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
522 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
523 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
524 //don't do the unused w component
525 // m_co[3] = s * v0[3] + rt * v1[3];
526 #endif
527 }
528
532 SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
533 {
534 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
535 __m128 vt = _mm_load_ss(&t); // (t 0 0 0)
536 vt = bt_pshufd_ps(vt, 0x80); // (rt rt rt 0.0)
537 __m128 vl = _mm_sub_ps(v.mVec128, mVec128);
538 vl = _mm_mul_ps(vl, vt);
539 vl = _mm_add_ps(vl, mVec128);
540
541 return btVector3(vl);
542 #elif defined(BT_USE_NEON)
543 float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
544 vl = vmulq_n_f32(vl, t);
545 vl = vaddq_f32(vl, mVec128);
546
547 return btVector3(vl);
548 #else
549 return
550 btVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
551 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
552 m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
553 #endif
554 }
555
558 SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
559 {
560 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
561 mVec128 = _mm_mul_ps(mVec128, v.mVec128);
562 #elif defined(BT_USE_NEON)
563 mVec128 = vmulq_f32(mVec128, v.mVec128);
564 #else
565 m_floats[0] *= v.m_floats[0];
566 m_floats[1] *= v.m_floats[1];
567 m_floats[2] *= v.m_floats[2];
568 #endif
569 return *this;
570 }
571
573 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
575 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
577 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
579 SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
581 SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;};
583 SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
585 SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
587 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
589 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
591 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
593 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
594
595 //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
596 //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
598 SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
599 SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
600
601 SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
602 {
603 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
604 return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
605 #else
606 return ((m_floats[3]==other.m_floats[3]) &&
607 (m_floats[2]==other.m_floats[2]) &&
608 (m_floats[1]==other.m_floats[1]) &&
609 (m_floats[0]==other.m_floats[0]));
610 #endif
611 }
612
613 SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
614 {
615 return !(*this == other);
616 }
617
621 SIMD_FORCE_INLINE void setMax(const btVector3& other)
622 {
623 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
624 mVec128 = _mm_max_ps(mVec128, other.mVec128);
625 #elif defined(BT_USE_NEON)
626 mVec128 = vmaxq_f32(mVec128, other.mVec128);
627 #else
628 btSetMax(m_floats[0], other.m_floats[0]);
629 btSetMax(m_floats[1], other.m_floats[1]);
630 btSetMax(m_floats[2], other.m_floats[2]);
631 btSetMax(m_floats[3], other.w());
632 #endif
633 }
634
638 SIMD_FORCE_INLINE void setMin(const btVector3& other)
639 {
640 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
641 mVec128 = _mm_min_ps(mVec128, other.mVec128);
642 #elif defined(BT_USE_NEON)
643 mVec128 = vminq_f32(mVec128, other.mVec128);
644 #else
645 btSetMin(m_floats[0], other.m_floats[0]);
646 btSetMin(m_floats[1], other.m_floats[1]);
647 btSetMin(m_floats[2], other.m_floats[2]);
648 btSetMin(m_floats[3], other.w());
649 #endif
650 }
651
652 SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
653 {
654 m_floats[0]=_x;
655 m_floats[1]=_y;
656 m_floats[2]=_z;
657 m_floats[3] = btScalar(0.f);
658 }
659
660 void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
661 {
662 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
663
664 __m128 V = _mm_and_ps(mVec128, btvFFF0fMask);
665 __m128 V0 = _mm_xor_ps(btvMzeroMask, V);
666 __m128 V2 = _mm_movelh_ps(V0, V);
667
668 __m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
669
670 V0 = _mm_shuffle_ps(V0, V, 0xDB);
671 V2 = _mm_shuffle_ps(V2, V, 0xF9);
672
673 v0->mVec128 = V0;
674 v1->mVec128 = V1;
675 v2->mVec128 = V2;
676 #else
677 v0->setValue(0. ,-z() ,y());
678 v1->setValue(z() ,0. ,-x());
679 v2->setValue(-y() ,x() ,0.);
680 #endif
681 }
682
683 void setZero()
684 {
685 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
686 mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
687 #elif defined(BT_USE_NEON)
688 int32x4_t vi = vdupq_n_s32(0);
689 mVec128 = vreinterpretq_f32_s32(vi);
690 #else
691 setValue(btScalar(0.),btScalar(0.),btScalar(0.));
692 #endif
693 }
694
695 SIMD_FORCE_INLINE bool isZero() const
696 {
697 return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
698 }
699
700
701 SIMD_FORCE_INLINE bool fuzzyZero() const
702 {
703 return length2() < SIMD_EPSILON*SIMD_EPSILON;
704 }
705
706 SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
707
708 SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
709
710 SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const;
711
712 SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
713
714 SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const;
715
716 SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
717
722 SIMD_FORCE_INLINE long maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
723
728 SIMD_FORCE_INLINE long minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
729
730 /* create a vector as btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 )) */
731 SIMD_FORCE_INLINE btVector3 dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const
732 {
733 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
734
735 __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
736 __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
737 __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 );
738 __m128 b0 = _mm_unpacklo_ps( a0, a1 );
739 __m128 b1 = _mm_unpackhi_ps( a0, a1 );
740 __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
741 __m128 r = _mm_movelh_ps( b0, b2 );
742 r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
743 a2 = _mm_and_ps( a2, btvxyzMaskf);
744 r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
745 return btVector3(r);
746
747 #elif defined(BT_USE_NEON)
748 static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 };
749 float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
750 float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
751 float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
752 float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1));
753 a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
754 float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
755 float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
756 return btVector3( vcombine_f32(b0, b1) );
757 #else
758 return btVector3( dot(v0), dot(v1), dot(v2));
759 #endif
760 }
761 };
762
764 SIMD_FORCE_INLINE btVector3
765 operator+(const btVector3& v1, const btVector3& v2)
766 {
767 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
768 return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
769 #elif defined(BT_USE_NEON)
770 return btVector3(vaddq_f32(v1.mVec128, v2.mVec128));
771 #else
772 return btVector3(
773 v1.m_floats[0] + v2.m_floats[0],
774 v1.m_floats[1] + v2.m_floats[1],
775 v1.m_floats[2] + v2.m_floats[2]);
776 #endif
777 }
778
780 SIMD_FORCE_INLINE btVector3
781 operator*(const btVector3& v1, const btVector3& v2)
782 {
783 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
784 return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
785 #elif defined(BT_USE_NEON)
786 return btVector3(vmulq_f32(v1.mVec128, v2.mVec128));
787 #else
788 return btVector3(
789 v1.m_floats[0] * v2.m_floats[0],
790 v1.m_floats[1] * v2.m_floats[1],
791 v1.m_floats[2] * v2.m_floats[2]);
792 #endif
793 }
794
796 SIMD_FORCE_INLINE btVector3
797 operator-(const btVector3& v1, const btVector3& v2)
798 {
799 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
800
801 // without _mm_and_ps this code causes slowdown in Concave moving
802 __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
803 return btVector3(_mm_and_ps(r, btvFFF0fMask));
804 #elif defined(BT_USE_NEON)
805 float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
806 return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
807 #else
808 return btVector3(
809 v1.m_floats[0] - v2.m_floats[0],
810 v1.m_floats[1] - v2.m_floats[1],
811 v1.m_floats[2] - v2.m_floats[2]);
812 #endif
813 }
814
816 SIMD_FORCE_INLINE btVector3
817 operator-(const btVector3& v)
818 {
819 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
820 __m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
821 return btVector3(_mm_and_ps(r, btvFFF0fMask));
822 #elif defined(BT_USE_NEON)
823 return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
824 #else
825 return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
826 #endif
827 }
828
830 SIMD_FORCE_INLINE btVector3
831 operator*(const btVector3& v, const btScalar& s)
832 {
833 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
834 __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
835 vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
836 return btVector3(_mm_mul_ps(v.mVec128, vs));
837 #elif defined(BT_USE_NEON)
838 float32x4_t r = vmulq_n_f32(v.mVec128, s);
839 return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
840 #else
841 return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
842 #endif
843 }
844
846 SIMD_FORCE_INLINE btVector3
847 operator*(const btScalar& s, const btVector3& v)
848 {
849 return v * s;
850 }
851
853 SIMD_FORCE_INLINE btVector3
854 operator/(const btVector3& v, const btScalar& s)
855 {
856 btFullAssert(s != btScalar(0.0));
857 #if 0 //defined(BT_USE_SSE_IN_API)
858 // this code is not faster !
859 __m128 vs = _mm_load_ss(&s);
860 vs = _mm_div_ss(v1110, vs);
861 vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
862
863 return btVector3(_mm_mul_ps(v.mVec128, vs));
864 #else
865 return v * (btScalar(1.0) / s);
866 #endif
867 }
868
870 SIMD_FORCE_INLINE btVector3
871 operator/(const btVector3& v1, const btVector3& v2)
872 {
873 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
874 __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
875 vec = _mm_and_ps(vec, btvFFF0fMask);
876 return btVector3(vec);
877 #elif defined(BT_USE_NEON)
878 float32x4_t x, y, v, m;
879
880 x = v1.mVec128;
881 y = v2.mVec128;
882
883 v = vrecpeq_f32(y); // v ~ 1/y
884 m = vrecpsq_f32(y, v); // m = (2-v*y)
885 v = vmulq_f32(v, m); // vv = v*m ~~ 1/y
886 m = vrecpsq_f32(y, v); // mm = (2-vv*y)
887 v = vmulq_f32(v, x); // x*vv
888 v = vmulq_f32(v, m); // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
889
890 return btVector3(v);
891 #else
892 return btVector3(
893 v1.m_floats[0] / v2.m_floats[0],
894 v1.m_floats[1] / v2.m_floats[1],
895 v1.m_floats[2] / v2.m_floats[2]);
896 #endif
897 }
898
900 SIMD_FORCE_INLINE btScalar
901 btDot(const btVector3& v1, const btVector3& v2)
902 {
903 return v1.dot(v2);
904 }
905
906
908 SIMD_FORCE_INLINE btScalar
909 btDistance2(const btVector3& v1, const btVector3& v2)
910 {
911 return v1.distance2(v2);
912 }
913
914
916 SIMD_FORCE_INLINE btScalar
917 btDistance(const btVector3& v1, const btVector3& v2)
918 {
919 return v1.distance(v2);
920 }
921
923 SIMD_FORCE_INLINE btScalar
924 btAngle(const btVector3& v1, const btVector3& v2)
925 {
926 return v1.angle(v2);
927 }
928
930 SIMD_FORCE_INLINE btVector3
931 btCross(const btVector3& v1, const btVector3& v2)
932 {
933 return v1.cross(v2);
934 }
935
936 SIMD_FORCE_INLINE btScalar
937 btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
938 {
939 return v1.triple(v2, v3);
940 }
941
946 SIMD_FORCE_INLINE btVector3
947 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
948 {
949 return v1.lerp(v2, t);
950 }
951
952
953
954 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
955 {
956 return (v - *this).length2();
957 }
958
959 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
960 {
961 return (v - *this).length();
962 }
963
964 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
965 {
966 btVector3 nrm = *this;
967
968 return nrm.normalize();
969 }
970
971 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
972 {
973 // wAxis must be a unit lenght vector
974
975 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
976
977 __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
978 btScalar ssin = btSin( _angle );
979 __m128 C = wAxis.cross( mVec128 ).mVec128;
980 O = _mm_and_ps(O, btvFFF0fMask);
981 btScalar scos = btCos( _angle );
982
983 __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
984 __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
985
986 __m128 Y = bt_pshufd_ps(O, 0xC9); // (Y Z X 0)
987 __m128 Z = bt_pshufd_ps(O, 0xD2); // (Z X Y 0)
988 O = _mm_add_ps(O, Y);
989 vsin = bt_pshufd_ps(vsin, 0x80); // (S S S 0)
990 O = _mm_add_ps(O, Z);
991 vcos = bt_pshufd_ps(vcos, 0x80); // (S S S 0)
992
993 vsin = vsin * C;
994 O = O * wAxis.mVec128;
995 __m128 X = mVec128 - O;
996
997 O = O + vsin;
998 vcos = vcos * X;
999 O = O + vcos;
1000
1001 return btVector3(O);
1002 #else
1003 btVector3 o = wAxis * wAxis.dot( *this );
1004 btVector3 _x = *this - o;
1005 btVector3 _y;
1006
1007 _y = wAxis.cross( *this );
1008
1009 return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) );
1010 #endif
1011 }
1012
1013 SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
1014 {
1015 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1016 #if defined _WIN32 || defined (BT_USE_SSE)
1017 const long scalar_cutoff = 10;
1018 long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1019 #elif defined BT_USE_NEON
1020 const long scalar_cutoff = 4;
1021 extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1022 #endif
1023 if( array_count < scalar_cutoff )
1024 #endif
1025 {
1026 btScalar maxDot1 = -SIMD_INFINITY;
1027 int i = 0;
1028 int ptIndex = -1;
1029 for( i = 0; i < array_count; i++ )
1030 {
1031 btScalar dot = array[i].dot(*this);
1032
1033 if( dot > maxDot1 )
1034 {
1035 maxDot1 = dot;
1036 ptIndex = i;
1037 }
1038 }
1039
1040 dotOut = maxDot1;
1041 return ptIndex;
1042 }
1043 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1044 return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1045 #endif
1046 }
1047
1048 SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
1049 {
1050 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1051 #if defined BT_USE_SSE
1052 const long scalar_cutoff = 10;
1053 long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1054 #elif defined BT_USE_NEON
1055 const long scalar_cutoff = 4;
1056 extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1057 #else
1058 #error unhandled arch!
1059 #endif
1060
1061 if( array_count < scalar_cutoff )
1062 #endif
1063 {
1064 btScalar minDot = SIMD_INFINITY;
1065 int i = 0;
1066 int ptIndex = -1;
1067
1068 for( i = 0; i < array_count; i++ )
1069 {
1070 btScalar dot = array[i].dot(*this);
1071
1072 if( dot < minDot )
1073 {
1074 minDot = dot;
1075 ptIndex = i;
1076 }
1077 }
1078
1079 dotOut = minDot;
1080
1081 return ptIndex;
1082 }
1083 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1084 return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1085 #endif//BT_USE_SIMD_VECTOR3
1086 }
1087
1088
1089 class btVector4 : public btVector3
1090 {
1091 public:
1092
1093 SIMD_FORCE_INLINE btVector4() {}
1094
1095
1096 SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1097 : btVector3(_x,_y,_z)
1098 {
1099 m_floats[3] = _w;
1100 }
1101
1102 #if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON)
1103 SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec)
1104 {
1105 mVec128 = vec;
1106 }
1107
1108 SIMD_FORCE_INLINE btVector4(const btVector3& rhs)
1109 {
1110 mVec128 = rhs.mVec128;
1111 }
1112
1113 SIMD_FORCE_INLINE btVector4&
1114 operator=(const btVector4& v)
1115 {
1116 mVec128 = v.mVec128;
1117 return *this;
1118 }
1119 #endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1120
1121 SIMD_FORCE_INLINE btVector4 absolute4() const
1122 {
1123 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
1124 return btVector4(_mm_and_ps(mVec128, btvAbsfMask));
1125 #elif defined(BT_USE_NEON)
1126 return btVector4(vabsq_f32(mVec128));
1127 #else
1128 return btVector4(
1129 btFabs(m_floats[0]),
1130 btFabs(m_floats[1]),
1131 btFabs(m_floats[2]),
1132 btFabs(m_floats[3]));
1133 #endif
1134 }
1135
1136
1137 btScalar getW() const { return m_floats[3];}
1138
1139
1140 SIMD_FORCE_INLINE int maxAxis4() const
1141 {
1142 int maxIndex = -1;
1143 btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
1144 if (m_floats[0] > maxVal)
1145 {
1146 maxIndex = 0;
1147 maxVal = m_floats[0];
1148 }
1149 if (m_floats[1] > maxVal)
1150 {
1151 maxIndex = 1;
1152 maxVal = m_floats[1];
1153 }
1154 if (m_floats[2] > maxVal)
1155 {
1156 maxIndex = 2;
1157 maxVal =m_floats[2];
1158 }
1159 if (m_floats[3] > maxVal)
1160 {
1161 maxIndex = 3;
1162 }
1163
1164 return maxIndex;
1165 }
1166
1167
1168 SIMD_FORCE_INLINE int minAxis4() const
1169 {
1170 int minIndex = -1;
1171 btScalar minVal = btScalar(BT_LARGE_FLOAT);
1172 if (m_floats[0] < minVal)
1173 {
1174 minIndex = 0;
1175 minVal = m_floats[0];
1176 }
1177 if (m_floats[1] < minVal)
1178 {
1179 minIndex = 1;
1180 minVal = m_floats[1];
1181 }
1182 if (m_floats[2] < minVal)
1183 {
1184 minIndex = 2;
1185 minVal =m_floats[2];
1186 }
1187 if (m_floats[3] < minVal)
1188 {
1189 minIndex = 3;
1190 minVal = m_floats[3];
1191 }
1192
1193 return minIndex;
1194 }
1195
1196
1197 SIMD_FORCE_INLINE int closestAxis4() const
1198 {
1199 return absolute4().maxAxis4();
1200 }
1201
1202
1203
1204
1212 /* void getValue(btScalar *m) const
1213 {
1214 m[0] = m_floats[0];
1215 m[1] = m_floats[1];
1216 m[2] =m_floats[2];
1217 }
1218 */
1225 SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1226 {
1227 m_floats[0]=_x;
1228 m_floats[1]=_y;
1229 m_floats[2]=_z;
1230 m_floats[3]=_w;
1231 }
1232
1233
1234 };
1235
1236
1238 SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
1239 {
1240 #ifdef BT_USE_DOUBLE_PRECISION
1241 unsigned char* dest = (unsigned char*) &destVal;
1242 unsigned char* src = (unsigned char*) &sourceVal;
1243 dest[0] = src[7];
1244 dest[1] = src[6];
1245 dest[2] = src[5];
1246 dest[3] = src[4];
1247 dest[4] = src[3];
1248 dest[5] = src[2];
1249 dest[6] = src[1];
1250 dest[7] = src[0];
1251 #else
1252 unsigned char* dest = (unsigned char*) &destVal;
1253 unsigned char* src = (unsigned char*) &sourceVal;
1254 dest[0] = src[3];
1255 dest[1] = src[2];
1256 dest[2] = src[1];
1257 dest[3] = src[0];
1258 #endif //BT_USE_DOUBLE_PRECISION
1259 }
1261 SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
1262 {
1263 for (int i=0;i<4;i++)
1264 {
1265 btSwapScalarEndian(sourceVec[i],destVec[i]);
1266 }
1267
1268 }
1269
1271 SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
1272 {
1273
1274 btVector3 swappedVec;
1275 for (int i=0;i<4;i++)
1276 {
1277 btSwapScalarEndian(vector[i],swappedVec[i]);
1278 }
1279 vector = swappedVec;
1280 }
1281
1282 template <class T>
1283 SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
1284 {
1285 if (btFabs(n[2]) > SIMDSQRT12) {
1286 // choose p in y-z plane
1287 btScalar a = n[1]*n[1] + n[2]*n[2];
1288 btScalar k = btRecipSqrt (a);
1289 p[0] = 0;
1290 p[1] = -n[2]*k;
1291 p[2] = n[1]*k;
1292 // set q = n x p
1293 q[0] = a*k;
1294 q[1] = -n[0]*p[2];
1295 q[2] = n[0]*p[1];
1296 }
1297 else {
1298 // choose p in x-y plane
1299 btScalar a = n[0]*n[0] + n[1]*n[1];
1300 btScalar k = btRecipSqrt (a);
1301 p[0] = -n[1]*k;
1302 p[1] = n[0]*k;
1303 p[2] = 0;
1304 // set q = n x p
1305 q[0] = -n[2]*p[1];
1306 q[1] = n[2]*p[0];
1307 q[2] = a*k;
1308 }
1309 }
1310
1311
1312 struct btVector3FloatData
1313 {
1314 float m_floats[4];
1315 };
1316
1317 struct btVector3DoubleData
1318 {
1319 double m_floats[4];
1320
1321 };
1322
1323 SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
1324 {
1326 for (int i=0;i<4;i++)
1327 dataOut.m_floats[i] = float(m_floats[i]);
1328 }
1329
1330 SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
1331 {
1332 for (int i=0;i<4;i++)
1333 m_floats[i] = btScalar(dataIn.m_floats[i]);
1334 }
1335
1336
1337 SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
1338 {
1340 for (int i=0;i<4;i++)
1341 dataOut.m_floats[i] = double(m_floats[i]);
1342 }
1343
1344 SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
1345 {
1346 for (int i=0;i<4;i++)
1347 m_floats[i] = btScalar(dataIn.m_floats[i]);
1348 }
1349
1350
1351 SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
1352 {
1354 for (int i=0;i<4;i++)
1355 dataOut.m_floats[i] = m_floats[i];
1356 }
1357
1358 SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
1359 {
1360 for (int i=0;i<4;i++)
1361 m_floats[i] = dataIn.m_floats[i];
1362 }
1363
1364 #endif //BT_VECTOR3_H
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btVector3::operator*=
btVector3 & operator*=(const btVector3 &v)
Elementwise multiply this vector by the other.
Definition: btVector3.h:558
btVector3::deSerializeDouble
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1344
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btVector3::operator+=
btVector3 & operator+=(const btVector3 &v)
Add a vector to this one.
Definition: btVector3.h:164
btVector3::furthestAxis
int furthestAxis() const
Definition: btVector3.h:492
btVector3::dot3
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:731
operator*
btVector3 operator*(const btVector3 &v1, const btVector3 &v2)
Return the elementwise product of two vectors.
Definition: btVector3.h:781
btAngle
btScalar btAngle(const btVector3 &v1, const btVector3 &v2)
Return the angle between two vectors.
Definition: btVector3.h:924
btVector3::m_floats
btScalar m_floats[4]
Definition: btVector3.h:112
btVector3DoubleData::m_floats
double m_floats[4]
Definition: btVector3.h:1319
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:477
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:583
btVector3::deSerialize
void deSerialize(const struct btVector3Data &dataIn)
Definition: btVector3.h:1358
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1283
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btVector3::serializeFloat
void serializeFloat(struct btVector3FloatData &dataOut) const
Definition: btVector3.h:1323
btVector4::btVector4
btVector4(const btScalar &_x, const btScalar &_y, const btScalar &_z, const btScalar &_w)
Definition: btVector3.h:1096
uint32_t
unsigned int uint32_t
Definition: btConvexHullComputer.cpp:32
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
btVector3::minDot
long minDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of minimum dot product between this and vectors in array[]
Definition: btVector3.h:1048
btVector3Data
#define btVector3Data
Definition: btVector3.h:29
btAlignedAllocator.h
btSwapScalarEndian
void btSwapScalarEndian(const btScalar &sourceVal, btScalar &destVal)
btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization ...
Definition: btVector3.h:1238
btVector3FloatData::m_floats
float m_floats[4]
Definition: btVector3.h:1314
btFullAssert
#define btFullAssert(x)
Definition: btScalar.h:134
btVector3::safeNormalize
btVector3 & safeNormalize()
Definition: btVector3.h:292
btVector3::operator/=
btVector3 & operator/=(const btScalar &s)
Inversely scale the vector.
Definition: btVector3.h:215
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
btVector3::fuzzyZero
bool fuzzyZero() const
Definition: btVector3.h:701
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
btVector3::closestAxis
int closestAxis() const
Definition: btVector3.h:497
btVector3::serializeDouble
void serializeDouble(struct btVector3DoubleData &dataOut) const
Definition: btVector3.h:1337
btSetMin
void btSetMin(T &a, const T &b)
Definition: btMinMax.h:41
int32_t
int int32_t
Definition: btConvexHullComputer.cpp:30
btVector3::btVector3
btVector3()
No initialization constructor.
Definition: btVector3.h:119
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:931
SIMDSQRT12
#define SIMDSQRT12
Definition: btScalar.h:509
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
btVector3::maxDot
long maxDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of maximum dot product between this and vectors in array[]
Definition: btVector3.h:1013
btDistance
btScalar btDistance(const btVector3 &v1, const btVector3 &v2)
Return the distance between two vectors.
Definition: btVector3.h:917
btVector4
Definition: btVector3.h:1089
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:579
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:522
btVector3::minAxis
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z...
Definition: btVector3.h:480
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btVector3::distance2
btScalar distance2(const btVector3 &v) const
Return the distance squared between the ends of this and another vector This is symantically treating...
Definition: btVector3.h:954
btVector4::btVector4
btVector4()
Definition: btVector3.h:1093
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btVector3::getSkewSymmetricMatrix
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:660
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3::setW
void setW(btScalar _w)
Set the w value.
Definition: btVector3.h:585
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:581
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btRecipSqrt
#define btRecipSqrt(x)
Definition: btScalar.h:510
btVector3::setZero
void setZero()
Definition: btVector3.h:683
btUnSwapVector3Endian
void btUnSwapVector3Endian(btVector3 &vector)
btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization ...
Definition: btVector3.h:1271
btVector4::closestAxis4
int closestAxis4() const
Definition: btVector3.h:1197
btVector4::minAxis4
int minAxis4() const
Definition: btVector3.h:1168
btVector3::w
const btScalar & w() const
Return the w value.
Definition: btVector3.h:593
btVector3::rotate
btVector3 rotate(const btVector3 &wAxis, const btScalar angle) const
Return a rotated version of this vector.
Definition: btVector3.h:971
btMinMax.h
btSetMax
void btSetMax(T &a, const T &b)
Definition: btMinMax.h:50
btVector3::operator*=
btVector3 & operator*=(const btScalar &s)
Scale the vector.
Definition: btVector3.h:197
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
btAcos
btScalar btAcos(btScalar x)
Definition: btScalar.h:479
btVector3::absolute
btVector3 absolute() const
Return a vector will the absolute values of each element.
Definition: btVector3.h:372
btVector3::serialize
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1351
btVector4::absolute4
btVector4 absolute4() const
Definition: btVector3.h:1121
btVector3::angle
btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:364
btVector3::btVector3
btVector3(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Constructor from scalars.
Definition: btVector3.h:131
operator+
btVector3 operator+(const btVector3 &v1, const btVector3 &v2)
Return the sum of two vectors (Point symantics)
Definition: btVector3.h:765
btVector3::distance
btScalar distance(const btVector3 &v) const
Return the distance between the ends of this and another vector This is symantically treating the vec...
Definition: btVector3.h:959
btVector3::isZero
bool isZero() const
Definition: btVector3.h:695
btVector3::operator!=
bool operator!=(const btVector3 &other) const
Definition: btVector3.h:613
BT_DECLARE_ALIGNED_ALLOCATOR
#define BT_DECLARE_ALIGNED_ALLOCATOR()
Definition: btScalar.h:403
btVector4::maxAxis4
int maxAxis4() const
Definition: btVector3.h:1140
btVector3::maxAxis
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:487
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btVector3FloatData
Definition: btVector3.h:1312
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:901
btVector3::setMax
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
btVector3::deSerializeFloat
void deSerializeFloat(const struct btVector3FloatData &dataIn)
Definition: btVector3.h:1330
btDistance2
btScalar btDistance2(const btVector3 &v1, const btVector3 &v2)
Return the distance squared between two vectors.
Definition: btVector3.h:909
operator/
btVector3 operator/(const btVector3 &v, const btScalar &s)
Return the vector inversely scaled by s.
Definition: btVector3.h:854
operator-
btVector3 operator-(const btVector3 &v1, const btVector3 &v2)
Return the difference between two vectors.
Definition: btVector3.h:797
btVector3::setInterpolate3
void setInterpolate3(const btVector3 &v0, const btVector3 &v1, btScalar rt)
Definition: btVector3.h:503
btTriple
btScalar btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3)
Definition: btVector3.h:937
btVector3::operator==
bool operator==(const btVector3 &other) const
Definition: btVector3.h:601
btVector3::lerp
btVector3 lerp(const btVector3 &v, const btScalar &t) const
Return the linear interpolation between this and another vector.
Definition: btVector3.h:532
btVector3::triple
btScalar triple(const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:428
btVector3::operator-=
btVector3 & operator-=(const btVector3 &v)
Subtract a vector from this one.
Definition: btVector3.h:181
btSwapVector3Endian
void btSwapVector3Endian(const btVector3 &sourceVec, btVector3 &destVec)
btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization ...
Definition: btVector3.h:1261
lerp
btVector3 lerp(const btVector3 &v1, const btVector3 &v2, const btScalar &t)
Return the linear interpolation between two vectors.
Definition: btVector3.h:947
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
btVector3::setMin
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:638
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btVector3DoubleData
Definition: btVector3.h:1317
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:476
btVector3::safeNorm
btScalar safeNorm() const
Return the norm (length) of the vector.
Definition: btVector3.h:275
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btVector4::getW
btScalar getW() const
Definition: btVector3.h:1137
btVector4::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z, const btScalar &_w)
Set x,y,z and zero w.
Definition: btVector3.h:1225
btScalar.h
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:475