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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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(!fuzzyZero());
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 float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128);
505 vl = vmulq_n_f32(vl, rt);
506 mVec128 = vaddq_f32(vl, 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
689 SIMD_FORCE_INLINE bool fuzzyZero() const
690 {
691 return length2() < SIMD_EPSILON*SIMD_EPSILON;
692 }
693
694 SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
695
696 SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
697
698 SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const;
699
700 SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
701
702 SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const;
703
704 SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
705
710 SIMD_FORCE_INLINE long maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
711
716 SIMD_FORCE_INLINE long minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const;
717
718 /* create a vector as btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 )) */
719 SIMD_FORCE_INLINE btVector3 dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const
720 {
721 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
722
723 __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
724 __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
725 __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 );
726 __m128 b0 = _mm_unpacklo_ps( a0, a1 );
727 __m128 b1 = _mm_unpackhi_ps( a0, a1 );
728 __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
729 __m128 r = _mm_movelh_ps( b0, b2 );
730 r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
731 a2 = _mm_and_ps( a2, btvxyzMaskf);
732 r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
733 return btVector3(r);
734
735 #elif defined(BT_USE_NEON)
736 static const uint32x4_t xyzMask = (const uint32x4_t){ static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0 };
737 float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
738 float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
739 float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
740 float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1));
741 a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
742 float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
743 float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
744 return btVector3( vcombine_f32(b0, b1) );
745 #else
746 return btVector3( dot(v0), dot(v1), dot(v2));
747 #endif
748 }
749 };
750
752 SIMD_FORCE_INLINE btVector3
753 operator+(const btVector3& v1, const btVector3& v2)
754 {
755 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
756 return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
757 #elif defined(BT_USE_NEON)
758 return btVector3(vaddq_f32(v1.mVec128, v2.mVec128));
759 #else
760 return btVector3(
761 v1.m_floats[0] + v2.m_floats[0],
762 v1.m_floats[1] + v2.m_floats[1],
763 v1.m_floats[2] + v2.m_floats[2]);
764 #endif
765 }
766
768 SIMD_FORCE_INLINE btVector3
769 operator*(const btVector3& v1, const btVector3& v2)
770 {
771 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
772 return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
773 #elif defined(BT_USE_NEON)
774 return btVector3(vmulq_f32(v1.mVec128, v2.mVec128));
775 #else
776 return btVector3(
777 v1.m_floats[0] * v2.m_floats[0],
778 v1.m_floats[1] * v2.m_floats[1],
779 v1.m_floats[2] * v2.m_floats[2]);
780 #endif
781 }
782
784 SIMD_FORCE_INLINE btVector3
785 operator-(const btVector3& v1, const btVector3& v2)
786 {
787 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
788
789 // without _mm_and_ps this code causes slowdown in Concave moving
790 __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
791 return btVector3(_mm_and_ps(r, btvFFF0fMask));
792 #elif defined(BT_USE_NEON)
793 float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
794 return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
795 #else
796 return btVector3(
797 v1.m_floats[0] - v2.m_floats[0],
798 v1.m_floats[1] - v2.m_floats[1],
799 v1.m_floats[2] - v2.m_floats[2]);
800 #endif
801 }
802
804 SIMD_FORCE_INLINE btVector3
805 operator-(const btVector3& v)
806 {
807 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
808 __m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
809 return btVector3(_mm_and_ps(r, btvFFF0fMask));
810 #elif defined(BT_USE_NEON)
811 return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
812 #else
813 return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
814 #endif
815 }
816
818 SIMD_FORCE_INLINE btVector3
819 operator*(const btVector3& v, const btScalar& s)
820 {
821 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
822 __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
823 vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
824 return btVector3(_mm_mul_ps(v.mVec128, vs));
825 #elif defined(BT_USE_NEON)
826 float32x4_t r = vmulq_n_f32(v.mVec128, s);
827 return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
828 #else
829 return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
830 #endif
831 }
832
834 SIMD_FORCE_INLINE btVector3
835 operator*(const btScalar& s, const btVector3& v)
836 {
837 return v * s;
838 }
839
841 SIMD_FORCE_INLINE btVector3
842 operator/(const btVector3& v, const btScalar& s)
843 {
844 btFullAssert(s != btScalar(0.0));
845 #if 0 //defined(BT_USE_SSE_IN_API)
846 // this code is not faster !
847 __m128 vs = _mm_load_ss(&s);
848 vs = _mm_div_ss(v1110, vs);
849 vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
850
851 return btVector3(_mm_mul_ps(v.mVec128, vs));
852 #else
853 return v * (btScalar(1.0) / s);
854 #endif
855 }
856
858 SIMD_FORCE_INLINE btVector3
859 operator/(const btVector3& v1, const btVector3& v2)
860 {
861 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
862 __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
863 vec = _mm_and_ps(vec, btvFFF0fMask);
864 return btVector3(vec);
865 #elif defined(BT_USE_NEON)
866 float32x4_t x, y, v, m;
867
868 x = v1.mVec128;
869 y = v2.mVec128;
870
871 v = vrecpeq_f32(y); // v ~ 1/y
872 m = vrecpsq_f32(y, v); // m = (2-v*y)
873 v = vmulq_f32(v, m); // vv = v*m ~~ 1/y
874 m = vrecpsq_f32(y, v); // mm = (2-vv*y)
875 v = vmulq_f32(v, x); // x*vv
876 v = vmulq_f32(v, m); // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
877
878 return btVector3(v);
879 #else
880 return btVector3(
881 v1.m_floats[0] / v2.m_floats[0],
882 v1.m_floats[1] / v2.m_floats[1],
883 v1.m_floats[2] / v2.m_floats[2]);
884 #endif
885 }
886
888 SIMD_FORCE_INLINE btScalar
889 btDot(const btVector3& v1, const btVector3& v2)
890 {
891 return v1.dot(v2);
892 }
893
894
896 SIMD_FORCE_INLINE btScalar
897 btDistance2(const btVector3& v1, const btVector3& v2)
898 {
899 return v1.distance2(v2);
900 }
901
902
904 SIMD_FORCE_INLINE btScalar
905 btDistance(const btVector3& v1, const btVector3& v2)
906 {
907 return v1.distance(v2);
908 }
909
911 SIMD_FORCE_INLINE btScalar
912 btAngle(const btVector3& v1, const btVector3& v2)
913 {
914 return v1.angle(v2);
915 }
916
918 SIMD_FORCE_INLINE btVector3
919 btCross(const btVector3& v1, const btVector3& v2)
920 {
921 return v1.cross(v2);
922 }
923
924 SIMD_FORCE_INLINE btScalar
925 btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
926 {
927 return v1.triple(v2, v3);
928 }
929
934 SIMD_FORCE_INLINE btVector3
935 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
936 {
937 return v1.lerp(v2, t);
938 }
939
940
941
942 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
943 {
944 return (v - *this).length2();
945 }
946
947 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
948 {
949 return (v - *this).length();
950 }
951
952 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
953 {
954 btVector3 nrm = *this;
955
956 return nrm.normalize();
957 }
958
959 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
960 {
961 // wAxis must be a unit lenght vector
962
963 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
964
965 __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
966 btScalar ssin = btSin( _angle );
967 __m128 C = wAxis.cross( mVec128 ).mVec128;
968 O = _mm_and_ps(O, btvFFF0fMask);
969 btScalar scos = btCos( _angle );
970
971 __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
972 __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
973
974 __m128 Y = bt_pshufd_ps(O, 0xC9); // (Y Z X 0)
975 __m128 Z = bt_pshufd_ps(O, 0xD2); // (Z X Y 0)
976 O = _mm_add_ps(O, Y);
977 vsin = bt_pshufd_ps(vsin, 0x80); // (S S S 0)
978 O = _mm_add_ps(O, Z);
979 vcos = bt_pshufd_ps(vcos, 0x80); // (S S S 0)
980
981 vsin = vsin * C;
982 O = O * wAxis.mVec128;
983 __m128 X = mVec128 - O;
984
985 O = O + vsin;
986 vcos = vcos * X;
987 O = O + vcos;
988
989 return btVector3(O);
990 #else
991 btVector3 o = wAxis * wAxis.dot( *this );
992 btVector3 _x = *this - o;
993 btVector3 _y;
994
995 _y = wAxis.cross( *this );
996
997 return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) );
998 #endif
999 }
1000
1001 SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
1002 {
1003 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1004 #if defined _WIN32 || defined (BT_USE_SSE)
1005 const long scalar_cutoff = 10;
1006 long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1007 #elif defined BT_USE_NEON
1008 const long scalar_cutoff = 4;
1009 extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1010 #endif
1011 if( array_count < scalar_cutoff )
1012 #endif
1013 {
1014 btScalar maxDot1 = -SIMD_INFINITY;
1015 int i = 0;
1016 int ptIndex = -1;
1017 for( i = 0; i < array_count; i++ )
1018 {
1019 btScalar dot = array[i].dot(*this);
1020
1021 if( dot > maxDot1 )
1022 {
1023 maxDot1 = dot;
1024 ptIndex = i;
1025 }
1026 }
1027
1028 dotOut = maxDot1;
1029 return ptIndex;
1030 }
1031 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1032 return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1033 #endif
1034 }
1035
1036 SIMD_FORCE_INLINE long btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
1037 {
1038 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1039 #if defined BT_USE_SSE
1040 const long scalar_cutoff = 10;
1041 long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1042 #elif defined BT_USE_NEON
1043 const long scalar_cutoff = 4;
1044 extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1045 #else
1046 #error unhandled arch!
1047 #endif
1048
1049 if( array_count < scalar_cutoff )
1050 #endif
1051 {
1052 btScalar minDot = SIMD_INFINITY;
1053 int i = 0;
1054 int ptIndex = -1;
1055
1056 for( i = 0; i < array_count; i++ )
1057 {
1058 btScalar dot = array[i].dot(*this);
1059
1060 if( dot < minDot )
1061 {
1062 minDot = dot;
1063 ptIndex = i;
1064 }
1065 }
1066
1067 dotOut = minDot;
1068
1069 return ptIndex;
1070 }
1071 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1072 return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1073 #endif//BT_USE_SIMD_VECTOR3
1074 }
1075
1076
1077 class btVector4 : public btVector3
1078 {
1079 public:
1080
1081 SIMD_FORCE_INLINE btVector4() {}
1082
1083
1084 SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1085 : btVector3(_x,_y,_z)
1086 {
1087 m_floats[3] = _w;
1088 }
1089
1090 #if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON)
1091 SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec)
1092 {
1093 mVec128 = vec;
1094 }
1095
1096 SIMD_FORCE_INLINE btVector4(const btVector3& rhs)
1097 {
1098 mVec128 = rhs.mVec128;
1099 }
1100
1101 SIMD_FORCE_INLINE btVector4&
1102 operator=(const btVector4& v)
1103 {
1104 mVec128 = v.mVec128;
1105 return *this;
1106 }
1107 #endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1108
1109 SIMD_FORCE_INLINE btVector4 absolute4() const
1110 {
1111 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
1112 return btVector4(_mm_and_ps(mVec128, btvAbsfMask));
1113 #elif defined(BT_USE_NEON)
1114 return btVector4(vabsq_f32(mVec128));
1115 #else
1116 return btVector4(
1117 btFabs(m_floats[0]),
1118 btFabs(m_floats[1]),
1119 btFabs(m_floats[2]),
1120 btFabs(m_floats[3]));
1121 #endif
1122 }
1123
1124
1125 btScalar getW() const { return m_floats[3];}
1126
1127
1128 SIMD_FORCE_INLINE int maxAxis4() const
1129 {
1130 int maxIndex = -1;
1131 btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
1132 if (m_floats[0] > maxVal)
1133 {
1134 maxIndex = 0;
1135 maxVal = m_floats[0];
1136 }
1137 if (m_floats[1] > maxVal)
1138 {
1139 maxIndex = 1;
1140 maxVal = m_floats[1];
1141 }
1142 if (m_floats[2] > maxVal)
1143 {
1144 maxIndex = 2;
1145 maxVal =m_floats[2];
1146 }
1147 if (m_floats[3] > maxVal)
1148 {
1149 maxIndex = 3;
1150 maxVal = m_floats[3];
1151 }
1152
1153 return maxIndex;
1154 }
1155
1156
1157 SIMD_FORCE_INLINE int minAxis4() const
1158 {
1159 int minIndex = -1;
1160 btScalar minVal = btScalar(BT_LARGE_FLOAT);
1161 if (m_floats[0] < minVal)
1162 {
1163 minIndex = 0;
1164 minVal = m_floats[0];
1165 }
1166 if (m_floats[1] < minVal)
1167 {
1168 minIndex = 1;
1169 minVal = m_floats[1];
1170 }
1171 if (m_floats[2] < minVal)
1172 {
1173 minIndex = 2;
1174 minVal =m_floats[2];
1175 }
1176 if (m_floats[3] < minVal)
1177 {
1178 minIndex = 3;
1179 minVal = m_floats[3];
1180 }
1181
1182 return minIndex;
1183 }
1184
1185
1186 SIMD_FORCE_INLINE int closestAxis4() const
1187 {
1188 return absolute4().maxAxis4();
1189 }
1190
1191
1192
1193
1201 /* void getValue(btScalar *m) const
1202 {
1203 m[0] = m_floats[0];
1204 m[1] = m_floats[1];
1205 m[2] =m_floats[2];
1206 }
1207 */
1214 SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1215 {
1216 m_floats[0]=_x;
1217 m_floats[1]=_y;
1218 m_floats[2]=_z;
1219 m_floats[3]=_w;
1220 }
1221
1222
1223 };
1224
1225
1227 SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
1228 {
1229 #ifdef BT_USE_DOUBLE_PRECISION
1230 unsigned char* dest = (unsigned char*) &destVal;
1231 unsigned char* src = (unsigned char*) &sourceVal;
1232 dest[0] = src[7];
1233 dest[1] = src[6];
1234 dest[2] = src[5];
1235 dest[3] = src[4];
1236 dest[4] = src[3];
1237 dest[5] = src[2];
1238 dest[6] = src[1];
1239 dest[7] = src[0];
1240 #else
1241 unsigned char* dest = (unsigned char*) &destVal;
1242 unsigned char* src = (unsigned char*) &sourceVal;
1243 dest[0] = src[3];
1244 dest[1] = src[2];
1245 dest[2] = src[1];
1246 dest[3] = src[0];
1247 #endif //BT_USE_DOUBLE_PRECISION
1248 }
1250 SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
1251 {
1252 for (int i=0;i<4;i++)
1253 {
1254 btSwapScalarEndian(sourceVec[i],destVec[i]);
1255 }
1256
1257 }
1258
1260 SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
1261 {
1262
1263 btVector3 swappedVec;
1264 for (int i=0;i<4;i++)
1265 {
1266 btSwapScalarEndian(vector[i],swappedVec[i]);
1267 }
1268 vector = swappedVec;
1269 }
1270
1271 template <class T>
1272 SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
1273 {
1274 if (btFabs(n[2]) > SIMDSQRT12) {
1275 // choose p in y-z plane
1276 btScalar a = n[1]*n[1] + n[2]*n[2];
1277 btScalar k = btRecipSqrt (a);
1278 p[0] = 0;
1279 p[1] = -n[2]*k;
1280 p[2] = n[1]*k;
1281 // set q = n x p
1282 q[0] = a*k;
1283 q[1] = -n[0]*p[2];
1284 q[2] = n[0]*p[1];
1285 }
1286 else {
1287 // choose p in x-y plane
1288 btScalar a = n[0]*n[0] + n[1]*n[1];
1289 btScalar k = btRecipSqrt (a);
1290 p[0] = -n[1]*k;
1291 p[1] = n[0]*k;
1292 p[2] = 0;
1293 // set q = n x p
1294 q[0] = -n[2]*p[1];
1295 q[1] = n[2]*p[0];
1296 q[2] = a*k;
1297 }
1298 }
1299
1300
1301 struct btVector3FloatData
1302 {
1303 float m_floats[4];
1304 };
1305
1306 struct btVector3DoubleData
1307 {
1308 double m_floats[4];
1309
1310 };
1311
1312 SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
1313 {
1315 for (int i=0;i<4;i++)
1316 dataOut.m_floats[i] = float(m_floats[i]);
1317 }
1318
1319 SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
1320 {
1321 for (int i=0;i<4;i++)
1322 m_floats[i] = btScalar(dataIn.m_floats[i]);
1323 }
1324
1325
1326 SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
1327 {
1329 for (int i=0;i<4;i++)
1330 dataOut.m_floats[i] = double(m_floats[i]);
1331 }
1332
1333 SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
1334 {
1335 for (int i=0;i<4;i++)
1336 m_floats[i] = btScalar(dataIn.m_floats[i]);
1337 }
1338
1339
1340 SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
1341 {
1343 for (int i=0;i<4;i++)
1344 dataOut.m_floats[i] = m_floats[i];
1345 }
1346
1347 SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
1348 {
1349 for (int i=0;i<4;i++)
1350 m_floats[i] = dataIn.m_floats[i];
1351 }
1352
1353 #endif //BT_VECTOR3_H
btVector3::angle
btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:352
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:853
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
btVector3::operator!=
bool operator!=(const btVector3 &other) const
Definition: btVector3.h:601
btVector3::operator*=
btVector3 & operator*=(const btVector3 &v)
Elementwise multiply this vector by the other.
Definition: btVector3.h:546
btVector3::deSerializeDouble
void deSerializeDouble(const struct btVector3DoubleData &dataIn)
Definition: btVector3.h:1333
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::operator+=
btVector3 & operator+=(const btVector3 &v)
Add a vector to this one.
Definition: btVector3.h:164
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:947
btVector3::operator==
bool operator==(const btVector3 &other) const
Definition: btVector3.h:589
operator*
btVector3 operator*(const btVector3 &v1, const btVector3 &v2)
Return the elementwise product of two vectors.
Definition: btVector3.h:769
btAngle
btScalar btAngle(const btVector3 &v1, const btVector3 &v2)
Return the angle between two vectors.
Definition: btVector3.h:912
btVector3::m_floats
btScalar m_floats[4]
Definition: btVector3.h:112
btVector3DoubleData::m_floats
double m_floats[4]
Definition: btVector3.h:1308
btVector4::getW
btScalar getW() const
Definition: btVector3.h:1125
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:451
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btVector3::deSerialize
void deSerialize(const struct btVector3Data &dataIn)
Definition: btVector3.h:1347
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btVector3::serializeDouble
void serializeDouble(struct btVector3DoubleData &dataOut) const
Definition: btVector3.h:1326
btVector4::btVector4
btVector4(const btScalar &_x, const btScalar &_y, const btScalar &_z, const btScalar &_w)
Definition: btVector3.h:1084
uint32_t
unsigned int uint32_t
Definition: btConvexHullComputer.cpp:32
btVector3::absolute
btVector3 absolute() const
Return a vector will the absolute values of each element.
Definition: btVector3.h:360
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:1001
btVector3::furthestAxis
int furthestAxis() const
Definition: btVector3.h:480
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
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:1227
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:942
btVector3FloatData::m_floats
float m_floats[4]
Definition: btVector3.h:1303
btFullAssert
#define btFullAssert(x)
Definition: btScalar.h:116
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3::safeNormalize
btVector3 & safeNormalize()
Definition: btVector3.h:282
btVector3::lerp
btVector3 lerp(const btVector3 &v, const btScalar &t) const
Return the linear interpolation between this and another vector.
Definition: btVector3.h:520
btVector3::operator/=
btVector3 & operator/=(const btScalar &s)
Inversely scale the vector.
Definition: btVector3.h:215
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:1036
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
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:919
SIMDSQRT12
#define SIMDSQRT12
Definition: btScalar.h:481
btDistance
btScalar btDistance(const btVector3 &v1, const btVector3 &v2)
Return the distance between two vectors.
Definition: btVector3.h:905
btVector4
Definition: btVector3.h:1077
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
btVector3::rotate
btVector3 rotate(const btVector3 &wAxis, const btScalar angle) const
Return a rotated version of this vector.
Definition: btVector3.h:959
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:495
btVector3::triple
btScalar triple(const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:416
btVector3::w
const btScalar & w() const
Return the w value.
Definition: btVector3.h:581
btVector4::btVector4
btVector4()
Definition: btVector3.h:1081
btVector3::setW
void setW(btScalar _w)
Set the w value.
Definition: btVector3.h:573
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btVector3::serialize
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1340
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btRecipSqrt
#define btRecipSqrt(x)
Definition: btScalar.h:483
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btUnSwapVector3Endian
void btUnSwapVector3Endian(btVector3 &vector)
btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization ...
Definition: btVector3.h:1260
btVector3::fuzzyZero
bool fuzzyZero() const
Definition: btVector3.h:689
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3::serializeFloat
void serializeFloat(struct btVector3FloatData &dataOut) const
Definition: btVector3.h:1312
btMinMax.h
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
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:64
btVector3::isZero
bool isZero() const
Definition: btVector3.h:683
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btAcos
btScalar btAcos(btScalar x)
Definition: btScalar.h:453
btVector3::closestAxis
int closestAxis() const
Definition: btVector3.h:485
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
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:753
btVector4::closestAxis4
int closestAxis4() const
Definition: btVector3.h:1186
BT_DECLARE_ALIGNED_ALLOCATOR
#define BT_DECLARE_ALIGNED_ALLOCATOR()
Definition: btScalar.h:388
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:468
btVector3::dot3
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:719
btVector4::maxAxis4
int maxAxis4() const
Definition: btVector3.h:1128
btVector4::minAxis4
int minAxis4() const
Definition: btVector3.h:1157
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:845
btVector3FloatData
Definition: btVector3.h:1301
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:889
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:609
btVector3::deSerializeFloat
void deSerializeFloat(const struct btVector3FloatData &dataIn)
Definition: btVector3.h:1319
btDistance2
btScalar btDistance2(const btVector3 &v1, const btVector3 &v2)
Return the distance squared between two vectors.
Definition: btVector3.h:897
operator/
btVector3 operator/(const btVector3 &v, const btScalar &s)
Return the vector inversely scaled by s.
Definition: btVector3.h:842
operator-
btVector3 operator-(const btVector3 &v1, const btVector3 &v2)
Return the difference between two vectors.
Definition: btVector3.h:785
btVector3::setInterpolate3
void setInterpolate3(const btVector3 &v0, const btVector3 &v1, btScalar rt)
Definition: btVector3.h:491
btTriple
btScalar btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3)
Definition: btVector3.h:925
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:1250
lerp
btVector3 lerp(const btVector3 &v1, const btVector3 &v2, const btScalar &t)
Return the linear interpolation between two vectors.
Definition: btVector3.h:935
btVector3::getSkewSymmetricMatrix
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
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:626
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btVector3DoubleData
Definition: btVector3.h:1306
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:450
btVector4::absolute4
btVector4 absolute4() const
Definition: btVector3.h:1109
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:475
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:1214
btScalar.h
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579