Bullet Collision Detection & Physics Library
btQuaternion.h
Go to the documentation of this file.
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15
16
17#ifndef BT_SIMD__QUATERNION_H_
18#define BT_SIMD__QUATERNION_H_
19
20
21#include "btVector3.h"
22#include "btQuadWord.h"
23
24
25#ifdef BT_USE_DOUBLE_PRECISION
26#define btQuaternionData btQuaternionDoubleData
27#define btQuaternionDataName "btQuaternionDoubleData"
28#else
29#define btQuaternionData btQuaternionFloatData
30#define btQuaternionDataName "btQuaternionFloatData"
31#endif //BT_USE_DOUBLE_PRECISION
32
33
34
35#ifdef BT_USE_SSE
36
37//const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
38#define vOnes (_mm_set_ps(1.0f, 1.0f, 1.0f, 1.0f))
39
40#endif
41
42#if defined(BT_USE_SSE)
43
44#define vQInv (_mm_set_ps(+0.0f, -0.0f, -0.0f, -0.0f))
45#define vPPPM (_mm_set_ps(-0.0f, +0.0f, +0.0f, +0.0f))
46
47#elif defined(BT_USE_NEON)
48
49const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
50const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
51
52#endif
53
55class btQuaternion : public btQuadWord {
56public:
59
60#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON)
61 // Set Vector
62 SIMD_FORCE_INLINE btQuaternion(const btSimdFloat4 vec)
63 {
64 mVec128 = vec;
65 }
66
67 // Copy constructor
69 {
70 mVec128 = rhs.mVec128;
71 }
72
73 // Assignment Operator
75 operator=(const btQuaternion& v)
76 {
77 mVec128 = v.mVec128;
78
79 return *this;
80 }
81
82#endif
83
84 // template <typename btScalar>
85 // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
87 btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
88 : btQuadWord(_x, _y, _z, _w)
89 {}
93 btQuaternion(const btVector3& _axis, const btScalar& _angle)
94 {
95 setRotation(_axis, _angle);
96 }
101 btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
102 {
103#ifndef BT_EULER_DEFAULT_ZYX
104 setEuler(yaw, pitch, roll);
105#else
106 setEulerZYX(yaw, pitch, roll);
107#endif
108 }
112 void setRotation(const btVector3& axis, const btScalar& _angle)
113 {
114 btScalar d = axis.length();
115 btAssert(d != btScalar(0.0));
116 btScalar s = btSin(_angle * btScalar(0.5)) / d;
117 setValue(axis.x() * s, axis.y() * s, axis.z() * s,
118 btCos(_angle * btScalar(0.5)));
119 }
124 void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
125 {
126 btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
127 btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
128 btScalar halfRoll = btScalar(roll) * btScalar(0.5);
129 btScalar cosYaw = btCos(halfYaw);
130 btScalar sinYaw = btSin(halfYaw);
131 btScalar cosPitch = btCos(halfPitch);
132 btScalar sinPitch = btSin(halfPitch);
133 btScalar cosRoll = btCos(halfRoll);
134 btScalar sinRoll = btSin(halfRoll);
135 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
136 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
137 sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
138 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
139 }
144 void setEulerZYX(const btScalar& yawZ, const btScalar& pitchY, const btScalar& rollX)
145 {
146 btScalar halfYaw = btScalar(yawZ) * btScalar(0.5);
147 btScalar halfPitch = btScalar(pitchY) * btScalar(0.5);
148 btScalar halfRoll = btScalar(rollX) * btScalar(0.5);
149 btScalar cosYaw = btCos(halfYaw);
150 btScalar sinYaw = btSin(halfYaw);
151 btScalar cosPitch = btCos(halfPitch);
152 btScalar sinPitch = btSin(halfPitch);
153 btScalar cosRoll = btCos(halfRoll);
154 btScalar sinRoll = btSin(halfRoll);
155 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
156 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
157 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
158 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
159 }
160
165 void getEulerZYX(btScalar& yawZ, btScalar& pitchY, btScalar& rollX) const
166 {
167 btScalar squ;
168 btScalar sqx;
169 btScalar sqy;
170 btScalar sqz;
171 btScalar sarg;
172 sqx = m_floats[0] * m_floats[0];
173 sqy = m_floats[1] * m_floats[1];
174 sqz = m_floats[2] * m_floats[2];
175 squ = m_floats[3] * m_floats[3];
176 rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
177 sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
178 pitchY = sarg <= btScalar(-1.0) ? btScalar(-0.5) * SIMD_PI: (sarg >= btScalar(1.0) ? btScalar(0.5) * SIMD_PI : btAsin(sarg));
179 yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
180 }
181
185 {
186#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
187 mVec128 = _mm_add_ps(mVec128, q.mVec128);
188#elif defined(BT_USE_NEON)
189 mVec128 = vaddq_f32(mVec128, q.mVec128);
190#else
191 m_floats[0] += q.x();
192 m_floats[1] += q.y();
193 m_floats[2] += q.z();
194 m_floats[3] += q.m_floats[3];
195#endif
196 return *this;
197 }
198
202 {
203#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
204 mVec128 = _mm_sub_ps(mVec128, q.mVec128);
205#elif defined(BT_USE_NEON)
206 mVec128 = vsubq_f32(mVec128, q.mVec128);
207#else
208 m_floats[0] -= q.x();
209 m_floats[1] -= q.y();
210 m_floats[2] -= q.z();
211 m_floats[3] -= q.m_floats[3];
212#endif
213 return *this;
214 }
215
219 {
220#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
221 __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
222 vs = bt_pshufd_ps(vs, 0); // (S S S S)
223 mVec128 = _mm_mul_ps(mVec128, vs);
224#elif defined(BT_USE_NEON)
225 mVec128 = vmulq_n_f32(mVec128, s);
226#else
227 m_floats[0] *= s;
228 m_floats[1] *= s;
229 m_floats[2] *= s;
230 m_floats[3] *= s;
231#endif
232 return *this;
233 }
234
239 {
240#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
241 __m128 vQ2 = q.get128();
242
243 __m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0));
244 __m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));
245
246 A1 = A1 * B1;
247
248 __m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1));
249 __m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
250
251 A2 = A2 * B2;
252
253 B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2));
254 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
255
256 B1 = B1 * B2; // A3 *= B3
257
258 mVec128 = bt_splat_ps(mVec128, 3); // A0
259 mVec128 = mVec128 * vQ2; // A0 * B0
260
261 A1 = A1 + A2; // AB12
262 mVec128 = mVec128 - B1; // AB03 = AB0 - AB3
263 A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
264 mVec128 = mVec128+ A1; // AB03 + AB12
265
266#elif defined(BT_USE_NEON)
267
268 float32x4_t vQ1 = mVec128;
269 float32x4_t vQ2 = q.get128();
270 float32x4_t A0, A1, B1, A2, B2, A3, B3;
271 float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
272
273 {
274 float32x2x2_t tmp;
275 tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
276 vQ1zx = tmp.val[0];
277
278 tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
279 vQ2zx = tmp.val[0];
280 }
281 vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
282
283 vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
284
285 vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
286 vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
287
288 A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
289 B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
290
291 A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
292 B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
293
294 A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
295 B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
296
297 A1 = vmulq_f32(A1, B1);
298 A2 = vmulq_f32(A2, B2);
299 A3 = vmulq_f32(A3, B3); // A3 *= B3
300 A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
301
302 A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
303 A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
304
305 // change the sign of the last element
306 A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
307 A0 = vaddq_f32(A0, A1); // AB03 + AB12
308
309 mVec128 = A0;
310#else
311 setValue(
312 m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
313 m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
314 m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
315 m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
316#endif
317 return *this;
318 }
321 btScalar dot(const btQuaternion& q) const
322 {
323#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
324 __m128 vd;
325
326 vd = _mm_mul_ps(mVec128, q.mVec128);
327
328 __m128 t = _mm_movehl_ps(vd, vd);
329 vd = _mm_add_ps(vd, t);
330 t = _mm_shuffle_ps(vd, vd, 0x55);
331 vd = _mm_add_ss(vd, t);
332
333 return _mm_cvtss_f32(vd);
334#elif defined(BT_USE_NEON)
335 float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
336 float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
337 x = vpadd_f32(x, x);
338 return vget_lane_f32(x, 0);
339#else
340 return m_floats[0] * q.x() +
341 m_floats[1] * q.y() +
342 m_floats[2] * q.z() +
343 m_floats[3] * q.m_floats[3];
344#endif
345 }
346
349 {
350 return dot(*this);
351 }
352
355 {
356 return btSqrt(length2());
357 }
359 {
360 btScalar l2 = length2();
361 if (l2>SIMD_EPSILON)
362 {
363 normalize();
364 }
365 return *this;
366 }
370 {
371#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
372 __m128 vd;
373
374 vd = _mm_mul_ps(mVec128, mVec128);
375
376 __m128 t = _mm_movehl_ps(vd, vd);
377 vd = _mm_add_ps(vd, t);
378 t = _mm_shuffle_ps(vd, vd, 0x55);
379 vd = _mm_add_ss(vd, t);
380
381 vd = _mm_sqrt_ss(vd);
382 vd = _mm_div_ss(vOnes, vd);
383 vd = bt_pshufd_ps(vd, 0); // splat
384 mVec128 = _mm_mul_ps(mVec128, vd);
385
386 return *this;
387#else
388 return *this /= length();
389#endif
390 }
391
395 operator*(const btScalar& s) const
396 {
397#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
398 __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
399 vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
400
401 return btQuaternion(_mm_mul_ps(mVec128, vs));
402#elif defined(BT_USE_NEON)
403 return btQuaternion(vmulq_n_f32(mVec128, s));
404#else
405 return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
406#endif
407 }
408
412 {
413 btAssert(s != btScalar(0.0));
414 return *this * (btScalar(1.0) / s);
415 }
416
420 {
421 btAssert(s != btScalar(0.0));
422 return *this *= btScalar(1.0) / s;
423 }
424
427 {
428 return *this / length();
429 }
432 btScalar angle(const btQuaternion& q) const
433 {
434 btScalar s = btSqrt(length2() * q.length2());
435 btAssert(s != btScalar(0.0));
436 return btAcos(dot(q) / s);
437 }
438
442 {
443 btScalar s = btSqrt(length2() * q.length2());
444 btAssert(s != btScalar(0.0));
445 if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
446 return btAcos(dot(-q) / s) * btScalar(2.0);
447 else
448 return btAcos(dot(q) / s) * btScalar(2.0);
449 }
450
453 {
454 btScalar s = btScalar(2.) * btAcos(m_floats[3]);
455 return s;
456 }
457
460 {
461 btScalar s;
462 if (m_floats[3] >= 0)
463 s = btScalar(2.) * btAcos(m_floats[3]);
464 else
465 s = btScalar(2.) * btAcos(-m_floats[3]);
466 return s;
467 }
468
469
472 {
473 btScalar s_squared = 1.f-m_floats[3]*m_floats[3];
474
475 if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
476 return btVector3(1.0, 0.0, 0.0); // Arbitrary
477 btScalar s = 1.f/btSqrt(s_squared);
478 return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
479 }
480
483 {
484#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
485 return btQuaternion(_mm_xor_ps(mVec128, vQInv));
486#elif defined(BT_USE_NEON)
487 return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
488#else
489 return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
490#endif
491 }
492
496 operator+(const btQuaternion& q2) const
497 {
498#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
499 return btQuaternion(_mm_add_ps(mVec128, q2.mVec128));
500#elif defined(BT_USE_NEON)
501 return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
502#else
503 const btQuaternion& q1 = *this;
504 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
505#endif
506 }
507
511 operator-(const btQuaternion& q2) const
512 {
513#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
514 return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128));
515#elif defined(BT_USE_NEON)
516 return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
517#else
518 const btQuaternion& q1 = *this;
519 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
520#endif
521 }
522
526 {
527#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
528 return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask));
529#elif defined(BT_USE_NEON)
530 return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
531#else
532 const btQuaternion& q2 = *this;
533 return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
534#endif
535 }
538 {
539 btQuaternion diff,sum;
540 diff = *this - qd;
541 sum = *this + qd;
542 if( diff.dot(diff) > sum.dot(sum) )
543 return qd;
544 return (-qd);
545 }
546
549 {
550 btQuaternion diff,sum;
551 diff = *this - qd;
552 sum = *this + qd;
553 if( diff.dot(diff) < sum.dot(sum) )
554 return qd;
555 return (-qd);
556 }
557
558
563 btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
564 {
565
566 const btScalar magnitude = btSqrt(length2() * q.length2());
567 btAssert(magnitude > btScalar(0));
568
569 const btScalar product = dot(q) / magnitude;
570 const btScalar absproduct = btFabs(product);
571
572 if(absproduct < btScalar(1.0 - SIMD_EPSILON))
573 {
574 // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
575 const btScalar theta = btAcos(absproduct);
576 const btScalar d = btSin(theta);
577 btAssert(d > btScalar(0));
578
579 const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
580 const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d;
581 const btScalar s1 = btSin(sign * t * theta) / d;
582
583 return btQuaternion(
584 (m_floats[0] * s0 + q.x() * s1),
585 (m_floats[1] * s0 + q.y() * s1),
586 (m_floats[2] * s0 + q.z() * s1),
587 (m_floats[3] * s0 + q.w() * s1));
588 }
589 else
590 {
591 return *this;
592 }
593 }
594
595 static const btQuaternion& getIdentity()
596 {
597 static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
598 return identityQuat;
599 }
600
601 SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
602
603 SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const;
604
605 SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionData& dataIn);
606
607 SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const;
608
610
611 SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const;
612
614
615};
616
617
618
619
620
623operator*(const btQuaternion& q1, const btQuaternion& q2)
624{
625#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
626 __m128 vQ1 = q1.get128();
627 __m128 vQ2 = q2.get128();
628 __m128 A0, A1, B1, A2, B2;
629
630 A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x // vtrn
631 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X // vdup vext
632
633 A1 = A1 * B1;
634
635 A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z X Y // vext
636 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup
637
638 A2 = A2 * B2;
639
640 B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext
641 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn
642
643 B1 = B1 * B2; // A3 *= B3
644
645 A0 = bt_splat_ps(vQ1, 3); // A0
646 A0 = A0 * vQ2; // A0 * B0
647
648 A1 = A1 + A2; // AB12
649 A0 = A0 - B1; // AB03 = AB0 - AB3
650
651 A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
652 A0 = A0 + A1; // AB03 + AB12
653
654 return btQuaternion(A0);
655
656#elif defined(BT_USE_NEON)
657
658 float32x4_t vQ1 = q1.get128();
659 float32x4_t vQ2 = q2.get128();
660 float32x4_t A0, A1, B1, A2, B2, A3, B3;
661 float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
662
663 {
664 float32x2x2_t tmp;
665 tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
666 vQ1zx = tmp.val[0];
667
668 tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
669 vQ2zx = tmp.val[0];
670 }
671 vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
672
673 vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
674
675 vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
676 vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
677
678 A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
679 B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
680
681 A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
682 B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
683
684 A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
685 B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
686
687 A1 = vmulq_f32(A1, B1);
688 A2 = vmulq_f32(A2, B2);
689 A3 = vmulq_f32(A3, B3); // A3 *= B3
690 A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
691
692 A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
693 A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
694
695 // change the sign of the last element
696 A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
697 A0 = vaddq_f32(A0, A1); // AB03 + AB12
698
699 return btQuaternion(A0);
700
701#else
702 return btQuaternion(
703 q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
704 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
705 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
706 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
707#endif
708}
709
712{
713#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
714 __m128 vQ1 = q.get128();
715 __m128 vQ2 = w.get128();
716 __m128 A1, B1, A2, B2, A3, B3;
717
718 A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0));
719 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0));
720
721 A1 = A1 * B1;
722
723 A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
724 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
725
726 A2 = A2 * B2;
727
728 A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
729 B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
730
731 A3 = A3 * B3; // A3 *= B3
732
733 A1 = A1 + A2; // AB12
734 A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
735 A1 = A1 - A3; // AB123 = AB12 - AB3
736
737 return btQuaternion(A1);
738
739#elif defined(BT_USE_NEON)
740
741 float32x4_t vQ1 = q.get128();
742 float32x4_t vQ2 = w.get128();
743 float32x4_t A1, B1, A2, B2, A3, B3;
744 float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
745
746 vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
747 {
748 float32x2x2_t tmp;
749
750 tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
751 vQ2zx = tmp.val[0];
752
753 tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
754 vQ1zx = tmp.val[0];
755 }
756
757 vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
758
759 vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
760 vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
761
762 A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X
763 B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x
764
765 A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
766 B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
767
768 A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
769 B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
770
771 A1 = vmulq_f32(A1, B1);
772 A2 = vmulq_f32(A2, B2);
773 A3 = vmulq_f32(A3, B3); // A3 *= B3
774
775 A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
776
777 // change the sign of the last element
778 A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
779
780 A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
781
782 return btQuaternion(A1);
783
784#else
785 return btQuaternion(
786 q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
787 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
788 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
789 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
790#endif
791}
792
795{
796#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
797 __m128 vQ1 = w.get128();
798 __m128 vQ2 = q.get128();
799 __m128 A1, B1, A2, B2, A3, B3;
800
801 A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x
802 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X
803
804 A1 = A1 * B1;
805
806 A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
807 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
808
809 A2 = A2 *B2;
810
811 A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
812 B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
813
814 A3 = A3 * B3; // A3 *= B3
815
816 A1 = A1 + A2; // AB12
817 A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
818 A1 = A1 - A3; // AB123 = AB12 - AB3
819
820 return btQuaternion(A1);
821
822#elif defined(BT_USE_NEON)
823
824 float32x4_t vQ1 = w.get128();
825 float32x4_t vQ2 = q.get128();
826 float32x4_t A1, B1, A2, B2, A3, B3;
827 float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
828
829 {
830 float32x2x2_t tmp;
831
832 tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
833 vQ1zx = tmp.val[0];
834
835 tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
836 vQ2zx = tmp.val[0];
837 }
838 vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
839
840 vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
841
842 vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
843 vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
844
845 A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
846 B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
847
848 A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
849 B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
850
851 A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
852 B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
853
854 A1 = vmulq_f32(A1, B1);
855 A2 = vmulq_f32(A2, B2);
856 A3 = vmulq_f32(A3, B3); // A3 *= B3
857
858 A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
859
860 // change the sign of the last element
861 A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
862
863 A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
864
865 return btQuaternion(A1);
866
867#else
868 return btQuaternion(
869 +w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
870 +w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
871 +w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
872 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
873#endif
874}
875
878dot(const btQuaternion& q1, const btQuaternion& q2)
879{
880 return q1.dot(q2);
881}
882
883
887{
888 return q.length();
889}
890
893btAngle(const btQuaternion& q1, const btQuaternion& q2)
894{
895 return q1.angle(q2);
896}
897
901{
902 return q.inverse();
903}
904
911slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
912{
913 return q1.slerp(q2, t);
914}
915
917quatRotate(const btQuaternion& rotation, const btVector3& v)
918{
919 btQuaternion q = rotation * v;
920 q *= rotation.inverse();
921#if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
922 return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
923#elif defined(BT_USE_NEON)
924 return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
925#else
926 return btVector3(q.getX(),q.getY(),q.getZ());
927#endif
928}
929
931shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
932{
933 btVector3 c = v0.cross(v1);
934 btScalar d = v0.dot(v1);
935
936 if (d < -1.0 + SIMD_EPSILON)
937 {
938 btVector3 n,unused;
939 btPlaneSpace1(v0,n,unused);
940 return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
941 }
942
943 btScalar s = btSqrt((1.0f + d) * 2.0f);
944 btScalar rs = 1.0f / s;
945
946 return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
947}
948
951{
952 v0.normalize();
953 v1.normalize();
954 return shortestArcQuat(v0,v1);
955}
956
957
958
959
961{
962 float m_floats[4];
963};
964
966{
967 double m_floats[4];
968
969};
970
972{
974 for (int i=0;i<4;i++)
975 dataOut.m_floats[i] = float(m_floats[i]);
976}
977
979{
980 for (int i=0;i<4;i++)
981 m_floats[i] = btScalar(dataIn.m_floats[i]);
982}
983
984
986{
988 for (int i=0;i<4;i++)
989 dataOut.m_floats[i] = double(m_floats[i]);
990}
991
993{
994 for (int i=0;i<4;i++)
995 m_floats[i] = btScalar(dataIn.m_floats[i]);
996}
997
998
1000{
1002 for (int i=0;i<4;i++)
1003 dataOut.m_floats[i] = m_floats[i];
1004}
1005
1007{
1008 for (int i=0;i<4;i++)
1009 m_floats[i] = dataIn.m_floats[i];
1010}
1011
1012
1013#endif //BT_SIMD__QUATERNION_H_
1014
1015
1016
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btQuaternion slerp(const btQuaternion &q1, const btQuaternion &q2, const btScalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: btQuaternion.h:911
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:931
btQuaternion shortestArcQuatNormalize2(btVector3 &v0, btVector3 &v1)
Definition: btQuaternion.h:950
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:900
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
btQuaternion operator*(const btQuaternion &q1, const btQuaternion &q2)
Return the product of two quaternions.
Definition: btQuaternion.h:623
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
Definition: btQuaternion.h:893
#define btQuaternionData
Definition: btQuaternion.h:29
#define SIMD_PI
Definition: btScalar.h:504
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:496
btScalar btSin(btScalar x)
Definition: btScalar.h:477
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
btScalar btCos(btScalar x)
Definition: btScalar.h:476
#define SIMD_EPSILON
Definition: btScalar.h:521
btScalar btAcos(btScalar x)
Definition: btScalar.h:479
btScalar btAsin(btScalar x)
Definition: btScalar.h:487
#define btAssert(x)
Definition: btScalar.h:131
static T sum(const btAlignedObjectArray< T > &items)
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1283
The btQuadWord class is base class for btVector3 and btQuaternion.
Definition: btQuadWord.h:38
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
const btScalar & getY() const
Return the y value.
Definition: btQuadWord.h:104
const btScalar & getX() const
Return the x value.
Definition: btQuadWord.h:102
btScalar m_floats[4]
Definition: btQuadWord.h:69
const btScalar & getZ() const
Return the z value.
Definition: btQuadWord.h:106
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:152
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btQuaternion operator-(const btQuaternion &q2) const
Return the difference between this quaternion and the other.
Definition: btQuaternion.h:511
btQuaternion operator/(const btScalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: btQuaternion.h:411
btQuaternion slerp(const btQuaternion &q, const btScalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
Definition: btQuaternion.h:563
btQuaternion operator*(const btScalar &s) const
Return a scaled version of this quaternion.
Definition: btQuaternion.h:395
void getEulerZYX(btScalar &yawZ, btScalar &pitchY, btScalar &rollX) const
Get the euler angles from this quaternion.
Definition: btQuaternion.h:165
btScalar angleShortestPath(const btQuaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: btQuaternion.h:441
void deSerializeFloat(const struct btQuaternionFloatData &dataIn)
Definition: btQuaternion.h:978
static const btQuaternion & getIdentity()
Definition: btQuaternion.h:595
btQuaternion & operator*=(const btQuaternion &q)
Multiply this quaternion by q on the right.
Definition: btQuaternion.h:238
btScalar length() const
Return the length of the quaternion.
Definition: btQuaternion.h:354
btQuaternion(const btVector3 &_axis, const btScalar &_angle)
Axis angle Constructor.
Definition: btQuaternion.h:93
btQuaternion farthest(const btQuaternion &qd) const
Definition: btQuaternion.h:537
void deSerialize(const struct btQuaternionData &dataIn)
btQuaternion()
No initialization constructor.
Definition: btQuaternion.h:58
const btScalar & getW() const
Definition: btQuaternion.h:601
btScalar dot(const btQuaternion &q) const
Return the dot product between this quaternion and another.
Definition: btQuaternion.h:321
void serialize(struct btQuaternionData &dataOut) const
Definition: btQuaternion.h:999
void deSerializeDouble(const struct btQuaternionDoubleData &dataIn)
Definition: btQuaternion.h:992
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:452
void serializeDouble(struct btQuaternionDoubleData &dataOut) const
Definition: btQuaternion.h:985
btQuaternion operator+(const btQuaternion &q2) const
Return the sum of this quaternion and the other.
Definition: btQuaternion.h:496
btQuaternion(const btScalar &_x, const btScalar &_y, const btScalar &_z, const btScalar &_w)
Constructor from scalars.
Definition: btQuaternion.h:87
btScalar angle(const btQuaternion &q) const
Return the half angle between this quaternion and the other.
Definition: btQuaternion.h:432
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:548
btQuaternion & operator*=(const btScalar &s)
Scale this quaternion.
Definition: btQuaternion.h:218
btQuaternion(const btScalar &yaw, const btScalar &pitch, const btScalar &roll)
Constructor from Euler angles.
Definition: btQuaternion.h:101
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:348
btQuaternion normalized() const
Return a normalized version of this quaternion.
Definition: btQuaternion.h:426
btQuaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: btQuaternion.h:525
btQuaternion & safeNormalize()
Definition: btQuaternion.h:358
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:482
btQuaternion & operator/=(const btScalar &s)
Inversely scale this quaternion.
Definition: btQuaternion.h:419
btScalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path.
Definition: btQuaternion.h:459
btQuaternion & operator-=(const btQuaternion &q)
Subtract out a quaternion.
Definition: btQuaternion.h:201
btVector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: btQuaternion.h:471
void setRotation(const btVector3 &axis, const btScalar &_angle)
Set the rotation using axis angle notation.
Definition: btQuaternion.h:112
void setEulerZYX(const btScalar &yawZ, const btScalar &pitchY, const btScalar &rollX)
Set the quaternion using euler angles.
Definition: btQuaternion.h:144
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:369
void setEuler(const btScalar &yaw, const btScalar &pitch, const btScalar &roll)
Set the quaternion using Euler angles.
Definition: btQuaternion.h:124
btQuaternion & operator+=(const btQuaternion &q)
Add two quaternions.
Definition: btQuaternion.h:184
void serializeFloat(struct btQuaternionFloatData &dataOut) const
Definition: btQuaternion.h:971
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589