Bullet Collision Detection & Physics Library
btQuaternion.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_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 
49 const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
50 const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
51 
52 #endif
53 
55 class btQuaternion : public btQuadWord {
56 public:
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 
354  btScalar length() const
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 
609  SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn);
610 
611  SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const;
612 
614 
615 };
616 
617 
618 
619 
620 
623 operator*(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 
711 operator*(const btQuaternion& q, const btVector3& w)
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 
794 operator*(const btVector3& w, const btQuaternion& q)
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 
878 dot(const btQuaternion& q1, const btQuaternion& q2)
879 {
880  return q1.dot(q2);
881 }
882 
883 
887 {
888  return q.length();
889 }
890 
893 btAngle(const btQuaternion& q1, const btQuaternion& q2)
894 {
895  return q1.angle(q2);
896 }
897 
901 {
902  return q.inverse();
903 }
904 
911 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
912 {
913  return q1.slerp(q2, t);
914 }
915 
917 quatRotate(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 
931 shortestArcQuat(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 
static T sum(const btAlignedObjectArray< T > &items)
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btScalar length() const
Return the length of the quaternion.
Definition: btQuaternion.h:354
#define SIMD_EPSILON
Definition: btScalar.h:521
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
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
Return the negative of this quaternion This simply negates each element.
Definition: btQuaternion.h:525
const btScalar & getZ() const
Return the z value.
Definition: btQuadWord.h:106
btQuaternion & operator*=(const btQuaternion &q)
Multiply this quaternion by q on the right.
Definition: btQuaternion.h:238
void serializeDouble(struct btQuaternionDoubleData &dataOut) const
Definition: btQuaternion.h:985
btQuaternion(const btScalar &yaw, const btScalar &pitch, const btScalar &roll)
Constructor from Euler angles.
Definition: btQuaternion.h:101
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
btScalar btSin(btScalar x)
Definition: btScalar.h:477
void getEulerZYX(btScalar &yawZ, btScalar &pitchY, btScalar &rollX) const
Get the euler angles from this quaternion.
Definition: btQuaternion.h:165
static const btQuaternion & getIdentity()
Definition: btQuaternion.h:595
btQuaternion & safeNormalize()
Definition: btQuaternion.h:358
void deSerializeDouble(const struct btQuaternionDoubleData &dataIn)
Definition: btQuaternion.h:992
void deSerializeFloat(const struct btQuaternionFloatData &dataIn)
Definition: btQuaternion.h:978
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1283
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define btAssert(x)
Definition: btScalar.h:131
btQuaternion operator*(const btQuaternion &q1, const btQuaternion &q2)
Return the product of two quaternions.
Definition: btQuaternion.h:623
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
btQuaternion & operator/=(const btScalar &s)
Inversely scale this quaternion.
Definition: btQuaternion.h:419
btQuaternion & operator-=(const btQuaternion &q)
Subtract out a quaternion.
Definition: btQuaternion.h:201
btScalar dot(const btQuaternion &q) const
Return the dot product between this quaternion and another.
Definition: btQuaternion.h:321
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:900
btQuaternion operator/(const btScalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: btQuaternion.h:411
btQuaternion operator*(const btScalar &s) const
Return a scaled version of this quaternion.
Definition: btQuaternion.h:395
btQuaternion & operator+=(const btQuaternion &q)
Add two quaternions.
Definition: btQuaternion.h:184
const btScalar & getW() const
Definition: btQuaternion.h:601
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:482
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
#define SIMD_PI
Definition: btScalar.h:504
btQuaternion operator-(const btQuaternion &q2) const
Return the difference between this quaternion and the other.
Definition: btQuaternion.h:511
btQuaternion farthest(const btQuaternion &qd) const
Definition: btQuaternion.h:537
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:931
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
void setRotation(const btVector3 &axis, const btScalar &_angle)
Set the rotation using axis angle notation.
Definition: btQuaternion.h:112
void serializeFloat(struct btQuaternionFloatData &dataOut) const
Definition: btQuaternion.h:971
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:369
btQuaternion shortestArcQuatNormalize2(btVector3 &v0, btVector3 &v1)
Definition: btQuaternion.h:950
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: btQuadWord.h:104
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:496
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:152
btQuaternion()
No initialization constructor.
Definition: btQuaternion.h:58
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
The btQuadWord class is base class for btVector3 and btQuaternion.
Definition: btQuadWord.h:34
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btQuaternion normalized() const
Return a normalized version of this quaternion.
Definition: btQuaternion.h:426
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btScalar m_floats[4]
Definition: btQuadWord.h:69
btVector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: btQuaternion.h:471
void deSerialize(const struct btQuaternionData &dataIn)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
btQuaternion(const btScalar &_x, const btScalar &_y, const btScalar &_z, const btScalar &_w)
Constructor from scalars.
Definition: btQuaternion.h:87
btScalar btAcos(btScalar x)
Definition: btScalar.h:479
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:452
void setEulerZYX(const btScalar &yawZ, const btScalar &pitchY, const btScalar &rollX)
Set the quaternion using euler angles.
Definition: btQuaternion.h:144
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:548
btQuaternion & operator*=(const btScalar &s)
Scale this quaternion.
Definition: btQuaternion.h:218
btScalar angle(const btQuaternion &q) const
Return the half angle between this quaternion and the other.
Definition: btQuaternion.h:432
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:348
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btScalar angleShortestPath(const btQuaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: btQuaternion.h:441
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btScalar btAsin(btScalar x)
Definition: btScalar.h:487
#define btQuaternionData
Definition: btQuaternion.h:29
void setEuler(const btScalar &yaw, const btScalar &pitch, const btScalar &roll)
Set the quaternion using Euler angles.
Definition: btQuaternion.h:124
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
const btScalar & getX() const
Return the x value.
Definition: btQuadWord.h:102
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
Definition: btQuaternion.h:893
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
void serialize(struct btQuaternionData &dataOut) const
Definition: btQuaternion.h:999
btQuaternion operator+(const btQuaternion &q2) const
Return the sum of this quaternion and the other.
Definition: btQuaternion.h:496
btQuaternion(const btVector3 &_axis, const btScalar &_angle)
Axis angle Constructor.
Definition: btQuaternion.h:93
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btCos(btScalar x)
Definition: btScalar.h:476
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar btFabs(btScalar x)
Definition: btScalar.h:475