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  sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
177 
178  // If the pitch angle is PI/2 or -PI/2, we can only compute
179  // the sum roll + yaw. However, any combination that gives
180  // the right sum will produce the correct orientation, so we
181  // set rollX = 0 and compute yawZ.
182  if (sarg <= -btScalar(0.99999))
183  {
184  pitchY = btScalar(-0.5)*SIMD_PI;
185  rollX = 0;
186  yawZ = btScalar(2) * btAtan2(m_floats[0],-m_floats[1]);
187  } else if (sarg >= btScalar(0.99999))
188  {
189  pitchY = btScalar(0.5)*SIMD_PI;
190  rollX = 0;
191  yawZ = btScalar(2) * btAtan2(-m_floats[0], m_floats[1]);
192  } else
193  {
194  pitchY = btAsin(sarg);
195  rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
196  yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
197  }
198  }
199 
203  {
204 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
205  mVec128 = _mm_add_ps(mVec128, q.mVec128);
206 #elif defined(BT_USE_NEON)
207  mVec128 = vaddq_f32(mVec128, q.mVec128);
208 #else
209  m_floats[0] += q.x();
210  m_floats[1] += q.y();
211  m_floats[2] += q.z();
212  m_floats[3] += q.m_floats[3];
213 #endif
214  return *this;
215  }
216 
220  {
221 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
222  mVec128 = _mm_sub_ps(mVec128, q.mVec128);
223 #elif defined(BT_USE_NEON)
224  mVec128 = vsubq_f32(mVec128, q.mVec128);
225 #else
226  m_floats[0] -= q.x();
227  m_floats[1] -= q.y();
228  m_floats[2] -= q.z();
229  m_floats[3] -= q.m_floats[3];
230 #endif
231  return *this;
232  }
233 
237  {
238 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
239  __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
240  vs = bt_pshufd_ps(vs, 0); // (S S S S)
241  mVec128 = _mm_mul_ps(mVec128, vs);
242 #elif defined(BT_USE_NEON)
243  mVec128 = vmulq_n_f32(mVec128, s);
244 #else
245  m_floats[0] *= s;
246  m_floats[1] *= s;
247  m_floats[2] *= s;
248  m_floats[3] *= s;
249 #endif
250  return *this;
251  }
252 
257  {
258 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
259  __m128 vQ2 = q.get128();
260 
261  __m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0));
262  __m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));
263 
264  A1 = A1 * B1;
265 
266  __m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1));
267  __m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
268 
269  A2 = A2 * B2;
270 
271  B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2));
272  B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
273 
274  B1 = B1 * B2; // A3 *= B3
275 
276  mVec128 = bt_splat_ps(mVec128, 3); // A0
277  mVec128 = mVec128 * vQ2; // A0 * B0
278 
279  A1 = A1 + A2; // AB12
280  mVec128 = mVec128 - B1; // AB03 = AB0 - AB3
281  A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
282  mVec128 = mVec128+ A1; // AB03 + AB12
283 
284 #elif defined(BT_USE_NEON)
285 
286  float32x4_t vQ1 = mVec128;
287  float32x4_t vQ2 = q.get128();
288  float32x4_t A0, A1, B1, A2, B2, A3, B3;
289  float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
290 
291  {
292  float32x2x2_t tmp;
293  tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
294  vQ1zx = tmp.val[0];
295 
296  tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
297  vQ2zx = tmp.val[0];
298  }
299  vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
300 
301  vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
302 
303  vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
304  vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
305 
306  A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
307  B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
308 
309  A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
310  B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
311 
312  A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
313  B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
314 
315  A1 = vmulq_f32(A1, B1);
316  A2 = vmulq_f32(A2, B2);
317  A3 = vmulq_f32(A3, B3); // A3 *= B3
318  A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
319 
320  A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
321  A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
322 
323  // change the sign of the last element
324  A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
325  A0 = vaddq_f32(A0, A1); // AB03 + AB12
326 
327  mVec128 = A0;
328 #else
329  setValue(
330  m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
331  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
332  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
333  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
334 #endif
335  return *this;
336  }
339  btScalar dot(const btQuaternion& q) const
340  {
341 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
342  __m128 vd;
343 
344  vd = _mm_mul_ps(mVec128, q.mVec128);
345 
346  __m128 t = _mm_movehl_ps(vd, vd);
347  vd = _mm_add_ps(vd, t);
348  t = _mm_shuffle_ps(vd, vd, 0x55);
349  vd = _mm_add_ss(vd, t);
350 
351  return _mm_cvtss_f32(vd);
352 #elif defined(BT_USE_NEON)
353  float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
354  float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
355  x = vpadd_f32(x, x);
356  return vget_lane_f32(x, 0);
357 #else
358  return m_floats[0] * q.x() +
359  m_floats[1] * q.y() +
360  m_floats[2] * q.z() +
361  m_floats[3] * q.m_floats[3];
362 #endif
363  }
364 
367  {
368  return dot(*this);
369  }
370 
372  btScalar length() const
373  {
374  return btSqrt(length2());
375  }
377  {
378  btScalar l2 = length2();
379  if (l2>SIMD_EPSILON)
380  {
381  normalize();
382  }
383  return *this;
384  }
388  {
389 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
390  __m128 vd;
391 
392  vd = _mm_mul_ps(mVec128, mVec128);
393 
394  __m128 t = _mm_movehl_ps(vd, vd);
395  vd = _mm_add_ps(vd, t);
396  t = _mm_shuffle_ps(vd, vd, 0x55);
397  vd = _mm_add_ss(vd, t);
398 
399  vd = _mm_sqrt_ss(vd);
400  vd = _mm_div_ss(vOnes, vd);
401  vd = bt_pshufd_ps(vd, 0); // splat
402  mVec128 = _mm_mul_ps(mVec128, vd);
403 
404  return *this;
405 #else
406  return *this /= length();
407 #endif
408  }
409 
413  operator*(const btScalar& s) const
414  {
415 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
416  __m128 vs = _mm_load_ss(&s); // (S 0 0 0)
417  vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
418 
419  return btQuaternion(_mm_mul_ps(mVec128, vs));
420 #elif defined(BT_USE_NEON)
421  return btQuaternion(vmulq_n_f32(mVec128, s));
422 #else
423  return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
424 #endif
425  }
426 
430  {
431  btAssert(s != btScalar(0.0));
432  return *this * (btScalar(1.0) / s);
433  }
434 
438  {
439  btAssert(s != btScalar(0.0));
440  return *this *= btScalar(1.0) / s;
441  }
442 
445  {
446  return *this / length();
447  }
450  btScalar angle(const btQuaternion& q) const
451  {
452  btScalar s = btSqrt(length2() * q.length2());
453  btAssert(s != btScalar(0.0));
454  return btAcos(dot(q) / s);
455  }
456 
460  {
461  btScalar s = btSqrt(length2() * q.length2());
462  btAssert(s != btScalar(0.0));
463  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
464  return btAcos(dot(-q) / s) * btScalar(2.0);
465  else
466  return btAcos(dot(q) / s) * btScalar(2.0);
467  }
468 
471  {
472  btScalar s = btScalar(2.) * btAcos(m_floats[3]);
473  return s;
474  }
475 
478  {
479  btScalar s;
480  if (m_floats[3] >= 0)
481  s = btScalar(2.) * btAcos(m_floats[3]);
482  else
483  s = btScalar(2.) * btAcos(-m_floats[3]);
484  return s;
485  }
486 
487 
490  {
491  btScalar s_squared = 1.f-m_floats[3]*m_floats[3];
492 
493  if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
494  return btVector3(1.0, 0.0, 0.0); // Arbitrary
495  btScalar s = 1.f/btSqrt(s_squared);
496  return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
497  }
498 
501  {
502 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
503  return btQuaternion(_mm_xor_ps(mVec128, vQInv));
504 #elif defined(BT_USE_NEON)
505  return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
506 #else
507  return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
508 #endif
509  }
510 
514  operator+(const btQuaternion& q2) const
515  {
516 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
517  return btQuaternion(_mm_add_ps(mVec128, q2.mVec128));
518 #elif defined(BT_USE_NEON)
519  return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
520 #else
521  const btQuaternion& q1 = *this;
522  return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
523 #endif
524  }
525 
529  operator-(const btQuaternion& q2) const
530  {
531 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
532  return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128));
533 #elif defined(BT_USE_NEON)
534  return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
535 #else
536  const btQuaternion& q1 = *this;
537  return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
538 #endif
539  }
540 
544  {
545 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
546  return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask));
547 #elif defined(BT_USE_NEON)
548  return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
549 #else
550  const btQuaternion& q2 = *this;
551  return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
552 #endif
553  }
556  {
557  btQuaternion diff,sum;
558  diff = *this - qd;
559  sum = *this + qd;
560  if( diff.dot(diff) > sum.dot(sum) )
561  return qd;
562  return (-qd);
563  }
564 
567  {
568  btQuaternion diff,sum;
569  diff = *this - qd;
570  sum = *this + qd;
571  if( diff.dot(diff) < sum.dot(sum) )
572  return qd;
573  return (-qd);
574  }
575 
576 
581  btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
582  {
583 
584  const btScalar magnitude = btSqrt(length2() * q.length2());
585  btAssert(magnitude > btScalar(0));
586 
587  const btScalar product = dot(q) / magnitude;
588  const btScalar absproduct = btFabs(product);
589 
590  if(absproduct < btScalar(1.0 - SIMD_EPSILON))
591  {
592  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
593  const btScalar theta = btAcos(absproduct);
594  const btScalar d = btSin(theta);
595  btAssert(d > btScalar(0));
596 
597  const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
598  const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d;
599  const btScalar s1 = btSin(sign * t * theta) / d;
600 
601  return btQuaternion(
602  (m_floats[0] * s0 + q.x() * s1),
603  (m_floats[1] * s0 + q.y() * s1),
604  (m_floats[2] * s0 + q.z() * s1),
605  (m_floats[3] * s0 + q.w() * s1));
606  }
607  else
608  {
609  return *this;
610  }
611  }
612 
613  static const btQuaternion& getIdentity()
614  {
615  static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
616  return identityQuat;
617  }
618 
619  SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
620 
621  SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const;
622 
623  SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionFloatData& dataIn);
624 
625  SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionDoubleData& dataIn);
626 
627  SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const;
628 
629  SIMD_FORCE_INLINE void deSerializeFloat(const struct btQuaternionFloatData& dataIn);
630 
631  SIMD_FORCE_INLINE void serializeDouble(struct btQuaternionDoubleData& dataOut) const;
632 
634 
635 };
636 
637 
638 
639 
640 
643 operator*(const btQuaternion& q1, const btQuaternion& q2)
644 {
645 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
646  __m128 vQ1 = q1.get128();
647  __m128 vQ2 = q2.get128();
648  __m128 A0, A1, B1, A2, B2;
649 
650  A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x // vtrn
651  B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X // vdup vext
652 
653  A1 = A1 * B1;
654 
655  A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z X Y // vext
656  B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup
657 
658  A2 = A2 * B2;
659 
660  B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext
661  B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn
662 
663  B1 = B1 * B2; // A3 *= B3
664 
665  A0 = bt_splat_ps(vQ1, 3); // A0
666  A0 = A0 * vQ2; // A0 * B0
667 
668  A1 = A1 + A2; // AB12
669  A0 = A0 - B1; // AB03 = AB0 - AB3
670 
671  A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
672  A0 = A0 + A1; // AB03 + AB12
673 
674  return btQuaternion(A0);
675 
676 #elif defined(BT_USE_NEON)
677 
678  float32x4_t vQ1 = q1.get128();
679  float32x4_t vQ2 = q2.get128();
680  float32x4_t A0, A1, B1, A2, B2, A3, B3;
681  float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
682 
683  {
684  float32x2x2_t tmp;
685  tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
686  vQ1zx = tmp.val[0];
687 
688  tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
689  vQ2zx = tmp.val[0];
690  }
691  vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
692 
693  vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
694 
695  vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
696  vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
697 
698  A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
699  B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
700 
701  A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
702  B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
703 
704  A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
705  B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
706 
707  A1 = vmulq_f32(A1, B1);
708  A2 = vmulq_f32(A2, B2);
709  A3 = vmulq_f32(A3, B3); // A3 *= B3
710  A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
711 
712  A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
713  A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
714 
715  // change the sign of the last element
716  A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
717  A0 = vaddq_f32(A0, A1); // AB03 + AB12
718 
719  return btQuaternion(A0);
720 
721 #else
722  return btQuaternion(
723  q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
724  q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
725  q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
726  q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
727 #endif
728 }
729 
732 {
733 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
734  __m128 vQ1 = q.get128();
735  __m128 vQ2 = w.get128();
736  __m128 A1, B1, A2, B2, A3, B3;
737 
738  A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0));
739  B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0));
740 
741  A1 = A1 * B1;
742 
743  A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
744  B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
745 
746  A2 = A2 * B2;
747 
748  A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
749  B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
750 
751  A3 = A3 * B3; // A3 *= B3
752 
753  A1 = A1 + A2; // AB12
754  A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
755  A1 = A1 - A3; // AB123 = AB12 - AB3
756 
757  return btQuaternion(A1);
758 
759 #elif defined(BT_USE_NEON)
760 
761  float32x4_t vQ1 = q.get128();
762  float32x4_t vQ2 = w.get128();
763  float32x4_t A1, B1, A2, B2, A3, B3;
764  float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
765 
766  vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
767  {
768  float32x2x2_t tmp;
769 
770  tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
771  vQ2zx = tmp.val[0];
772 
773  tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
774  vQ1zx = tmp.val[0];
775  }
776 
777  vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
778 
779  vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
780  vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
781 
782  A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X
783  B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x
784 
785  A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
786  B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
787 
788  A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
789  B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
790 
791  A1 = vmulq_f32(A1, B1);
792  A2 = vmulq_f32(A2, B2);
793  A3 = vmulq_f32(A3, B3); // A3 *= B3
794 
795  A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
796 
797  // change the sign of the last element
798  A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
799 
800  A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
801 
802  return btQuaternion(A1);
803 
804 #else
805  return btQuaternion(
806  q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
807  q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
808  q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
809  -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
810 #endif
811 }
812 
815 {
816 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
817  __m128 vQ1 = w.get128();
818  __m128 vQ2 = q.get128();
819  __m128 A1, B1, A2, B2, A3, B3;
820 
821  A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x
822  B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X
823 
824  A1 = A1 * B1;
825 
826  A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
827  B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
828 
829  A2 = A2 *B2;
830 
831  A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
832  B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
833 
834  A3 = A3 * B3; // A3 *= B3
835 
836  A1 = A1 + A2; // AB12
837  A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
838  A1 = A1 - A3; // AB123 = AB12 - AB3
839 
840  return btQuaternion(A1);
841 
842 #elif defined(BT_USE_NEON)
843 
844  float32x4_t vQ1 = w.get128();
845  float32x4_t vQ2 = q.get128();
846  float32x4_t A1, B1, A2, B2, A3, B3;
847  float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
848 
849  {
850  float32x2x2_t tmp;
851 
852  tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
853  vQ1zx = tmp.val[0];
854 
855  tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
856  vQ2zx = tmp.val[0];
857  }
858  vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
859 
860  vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
861 
862  vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
863  vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
864 
865  A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
866  B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
867 
868  A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
869  B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
870 
871  A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
872  B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
873 
874  A1 = vmulq_f32(A1, B1);
875  A2 = vmulq_f32(A2, B2);
876  A3 = vmulq_f32(A3, B3); // A3 *= B3
877 
878  A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
879 
880  // change the sign of the last element
881  A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
882 
883  A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
884 
885  return btQuaternion(A1);
886 
887 #else
888  return btQuaternion(
889  +w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
890  +w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
891  +w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
892  -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
893 #endif
894 }
895 
898 dot(const btQuaternion& q1, const btQuaternion& q2)
899 {
900  return q1.dot(q2);
901 }
902 
903 
907 {
908  return q.length();
909 }
910 
913 btAngle(const btQuaternion& q1, const btQuaternion& q2)
914 {
915  return q1.angle(q2);
916 }
917 
921 {
922  return q.inverse();
923 }
924 
931 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
932 {
933  return q1.slerp(q2, t);
934 }
935 
937 quatRotate(const btQuaternion& rotation, const btVector3& v)
938 {
939  btQuaternion q = rotation * v;
940  q *= rotation.inverse();
941 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
942  return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
943 #elif defined(BT_USE_NEON)
944  return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
945 #else
946  return btVector3(q.getX(),q.getY(),q.getZ());
947 #endif
948 }
949 
951 shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
952 {
953  btVector3 c = v0.cross(v1);
954  btScalar d = v0.dot(v1);
955 
956  if (d < -1.0 + SIMD_EPSILON)
957  {
958  btVector3 n,unused;
959  btPlaneSpace1(v0,n,unused);
960  return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
961  }
962 
963  btScalar s = btSqrt((1.0f + d) * 2.0f);
964  btScalar rs = 1.0f / s;
965 
966  return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
967 }
968 
971 {
972  v0.normalize();
973  v1.normalize();
974  return shortestArcQuat(v0,v1);
975 }
976 
977 
978 
979 
981 {
982  float m_floats[4];
983 };
984 
986 {
987  double m_floats[4];
988 
989 };
990 
992 {
994  for (int i=0;i<4;i++)
995  dataOut.m_floats[i] = float(m_floats[i]);
996 }
997 
999 {
1000  for (int i=0;i<4;i++)
1001  m_floats[i] = btScalar(dataIn.m_floats[i]);
1002 }
1003 
1004 
1006 {
1008  for (int i=0;i<4;i++)
1009  dataOut.m_floats[i] = double(m_floats[i]);
1010 }
1011 
1013 {
1014  for (int i=0;i<4;i++)
1015  m_floats[i] = btScalar(dataIn.m_floats[i]);
1016 }
1017 
1018 
1020 {
1022  for (int i=0;i<4;i++)
1023  dataOut.m_floats[i] = m_floats[i];
1024 }
1025 
1027 {
1028  for (int i = 0; i<4; i++)
1029  m_floats[i] = (btScalar)dataIn.m_floats[i];
1030 }
1031 
1033 {
1034  for (int i=0;i<4;i++)
1035  m_floats[i] = (btScalar)dataIn.m_floats[i];
1036 }
1037 
1038 
1039 #endif //BT_SIMD__QUATERNION_H_
1040 
1041 
1042 
static T sum(const btAlignedObjectArray< T > &items)
#define SIMD_EPSILON
Definition: btScalar.h:521
void deSerialize(const struct btQuaternionFloatData &dataIn)
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:470
btQuaternion & operator*=(const btQuaternion &q)
Multiply this quaternion by q on the right.
Definition: btQuaternion.h:256
btQuaternion farthest(const btQuaternion &qd) const
Definition: btQuaternion.h:555
btQuaternion(const btScalar &yaw, const btScalar &pitch, const btScalar &roll)
Constructor from Euler angles.
Definition: btQuaternion.h:101
void serialize(struct btQuaternionData &dataOut) const
btScalar btSin(btScalar x)
Definition: btScalar.h:477
static const btQuaternion & getIdentity()
Definition: btQuaternion.h:613
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btQuaternion & safeNormalize()
Definition: btQuaternion.h:376
void serializeDouble(struct btQuaternionDoubleData &dataOut) const
void deSerializeDouble(const struct btQuaternionDoubleData &dataIn)
void deSerializeFloat(const struct btQuaternionFloatData &dataIn)
Definition: btQuaternion.h:998
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1284
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define btAssert(x)
Definition: btScalar.h:131
btScalar dot(const btQuaternion &q) const
Return the dot product between this quaternion and another.
Definition: btQuaternion.h:339
const btScalar & getW() const
Definition: btQuaternion.h:619
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
btQuaternion & operator/=(const btScalar &s)
Inversely scale this quaternion.
Definition: btQuaternion.h:437
btQuaternion operator-(const btQuaternion &q2) const
Return the difference between this quaternion and the other.
Definition: btQuaternion.h:529
btQuaternion & operator-=(const btQuaternion &q)
Subtract out a quaternion.
Definition: btQuaternion.h:219
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btVector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: btQuaternion.h:489
btQuaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: btQuaternion.h:543
const btScalar & getY() const
Return the y value.
Definition: btQuadWord.h:104
const btScalar & getX() const
Return the x value.
Definition: btQuadWord.h:102
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btQuaternion & operator+=(const btQuaternion &q)
Add two quaternions.
Definition: btQuaternion.h:202
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:566
btScalar length() const
Return the length of the quaternion.
Definition: btQuaternion.h:372
btQuaternion operator*(const btScalar &s) const
Return a scaled version of this quaternion.
Definition: btQuaternion.h:413
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:937
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
void serializeFloat(struct btQuaternionFloatData &dataOut) const
Definition: btQuaternion.h:991
#define SIMD_PI
Definition: btScalar.h:504
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:951
void setRotation(const btVector3 &axis, const btScalar &_angle)
Set the rotation using axis angle notation.
Definition: btQuaternion.h:112
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:387
btQuaternion shortestArcQuatNormalize2(btVector3 &v0, btVector3 &v1)
Definition: btQuaternion.h:970
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
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btQuaternion()
No initialization constructor.
Definition: btQuaternion.h:58
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
The btQuadWord class is base class for btVector3 and btQuaternion.
Definition: btQuadWord.h:34
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:500
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar m_floats[4]
Definition: btQuadWord.h:69
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:366
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btScalar angleShortestPath(const btQuaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: btQuaternion.h:459
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
void getEulerZYX(btScalar &yawZ, btScalar &pitchY, btScalar &rollX) const
Get the euler angles from this quaternion.
Definition: btQuaternion.h:165
btScalar btAcos(btScalar x)
Definition: btScalar.h:479
void setEulerZYX(const btScalar &yawZ, const btScalar &pitchY, const btScalar &rollX)
Set the quaternion using euler angles.
Definition: btQuaternion.h:144
btQuaternion normalized() const
Return a normalized version of this quaternion.
Definition: btQuaternion.h:444
btQuaternion & operator*=(const btScalar &s)
Scale this quaternion.
Definition: btQuaternion.h:236
btScalar angle(const btQuaternion &q) const
Return the ***half*** angle between this quaternion and the other.
Definition: btQuaternion.h:450
btScalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path...
Definition: btQuaternion.h:477
btQuaternion operator+(const btQuaternion &q2) const
Return the sum of this quaternion and the other.
Definition: btQuaternion.h:514
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btQuaternion operator/(const btScalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: btQuaternion.h:429
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
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
Definition: btQuaternion.h:913
btQuaternion(const btVector3 &_axis, const btScalar &_angle)
Axis angle Constructor.
Definition: btQuaternion.h:93
const btScalar & getZ() const
Return the z value.
Definition: btQuadWord.h:106
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:581
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 btFabs(btScalar x)
Definition: btScalar.h:475
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591