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