Bullet Collision Detection & Physics Library
btMatrix3x3.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 #ifndef BT_MATRIX3x3_H
16 #define BT_MATRIX3x3_H
17 
18 #include "btVector3.h"
19 #include "btQuaternion.h"
20 #include <stdio.h>
21 
22 #ifdef BT_USE_SSE
23 //const __m128 ATTRIBUTE_ALIGNED16(v2220) = {2.0f, 2.0f, 2.0f, 0.0f};
24 //const __m128 ATTRIBUTE_ALIGNED16(vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f};
25 #define vMPPP (_mm_set_ps(+0.0f, +0.0f, +0.0f, -0.0f))
26 #endif
27 
28 #if defined(BT_USE_SSE)
29 #define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f))
30 #define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f))
31 #define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f))
32 #elif defined(BT_USE_NEON)
33 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
34 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
35 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
36 #endif
37 
38 #ifdef BT_USE_DOUBLE_PRECISION
39 #define btMatrix3x3Data btMatrix3x3DoubleData
40 #else
41 #define btMatrix3x3Data btMatrix3x3FloatData
42 #endif //BT_USE_DOUBLE_PRECISION
43 
48 {
50  btVector3 m_el[3];
51 
52 public:
55 
56  // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
57 
59  explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
60  /*
61  template <typename btScalar>
62  Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
63  {
64  setEulerYPR(yaw, pitch, roll);
65  }
66  */
68  btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
69  const btScalar& yx, const btScalar& yy, const btScalar& yz,
70  const btScalar& zx, const btScalar& zy, const btScalar& zz)
71  {
72  setValue(xx, xy, xz,
73  yx, yy, yz,
74  zx, zy, zz);
75  }
76 
77 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
78  SIMD_FORCE_INLINE btMatrix3x3(const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2)
79  {
80  m_el[0].mVec128 = v0;
81  m_el[1].mVec128 = v1;
82  m_el[2].mVec128 = v2;
83  }
84 
85  SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2)
86  {
87  m_el[0] = v0;
88  m_el[1] = v1;
89  m_el[2] = v2;
90  }
91 
92  // Copy constructor
94  {
95  m_el[0].mVec128 = rhs.m_el[0].mVec128;
96  m_el[1].mVec128 = rhs.m_el[1].mVec128;
97  m_el[2].mVec128 = rhs.m_el[2].mVec128;
98  }
99 
100  // Assignment Operator
101  SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m)
102  {
103  m_el[0].mVec128 = m.m_el[0].mVec128;
104  m_el[1].mVec128 = m.m_el[1].mVec128;
105  m_el[2].mVec128 = m.m_el[2].mVec128;
106 
107  return *this;
108  }
109 
110 #else
111 
114  {
115  m_el[0] = other.m_el[0];
116  m_el[1] = other.m_el[1];
117  m_el[2] = other.m_el[2];
118  }
119 
122  {
123  m_el[0] = other.m_el[0];
124  m_el[1] = other.m_el[1];
125  m_el[2] = other.m_el[2];
126  return *this;
127  }
128 
129  SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2)
130  {
131  m_el[0] = v0;
132  m_el[1] = v1;
133  m_el[2] = v2;
134  }
135 
136 #endif
137 
141  {
142  return btVector3(m_el[0][i], m_el[1][i], m_el[2][i]);
143  }
144 
147  SIMD_FORCE_INLINE const btVector3& getRow(int i) const
148  {
149  btFullAssert(0 <= i && i < 3);
150  return m_el[i];
151  }
152 
156  {
157  btFullAssert(0 <= i && i < 3);
158  return m_el[i];
159  }
160 
164  {
165  btFullAssert(0 <= i && i < 3);
166  return m_el[i];
167  }
168 
172  btMatrix3x3& operator*=(const btMatrix3x3& m);
173 
177  btMatrix3x3& operator+=(const btMatrix3x3& m);
178 
182  btMatrix3x3& operator-=(const btMatrix3x3& m);
183 
187  {
188  m_el[0].setValue(m[0], m[4], m[8]);
189  m_el[1].setValue(m[1], m[5], m[9]);
190  m_el[2].setValue(m[2], m[6], m[10]);
191  }
202  void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
203  const btScalar& yx, const btScalar& yy, const btScalar& yz,
204  const btScalar& zx, const btScalar& zy, const btScalar& zz)
205  {
206  m_el[0].setValue(xx, xy, xz);
207  m_el[1].setValue(yx, yy, yz);
208  m_el[2].setValue(zx, zy, zz);
209  }
210 
213  void setRotation(const btQuaternion& q)
214  {
215  btScalar d = q.length2();
216  btFullAssert(d != btScalar(0.0));
217  btScalar s = btScalar(2.0) / d;
218 
219 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
220  __m128 vs, Q = q.get128();
221  __m128i Qi = btCastfTo128i(Q);
222  __m128 Y, Z;
223  __m128 V1, V2, V3;
224  __m128 V11, V21, V31;
225  __m128 NQ = _mm_xor_ps(Q, btvMzeroMask);
226  __m128i NQi = btCastfTo128i(NQ);
227 
228  V1 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 0, 2, 3))); // Y X Z W
229  V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0, 0, 1, 3)); // -X -X Y W
230  V3 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(2, 1, 0, 3))); // Z Y X W
231  V1 = _mm_xor_ps(V1, vMPPP); // change the sign of the first element
232 
233  V11 = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 1, 0, 3))); // Y Y X W
234  V21 = _mm_unpackhi_ps(Q, Q); // Z Z W W
235  V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(0, 2, 0, 3)); // X Z -X -W
236 
237  V2 = V2 * V1; //
238  V1 = V1 * V11; //
239  V3 = V3 * V31; //
240 
241  V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2, 3, 1, 3)); // -Z -W Y W
242  V11 = V11 * V21; //
243  V21 = _mm_xor_ps(V21, vMPPP); // change the sign of the first element
244  V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3, 3, 1, 3)); // W W -Y -W
245  V31 = _mm_xor_ps(V31, vMPPP); // change the sign of the first element
246  Y = btCastiTo128f(_mm_shuffle_epi32(NQi, BT_SHUFFLE(3, 2, 0, 3))); // -W -Z -X -W
247  Z = btCastiTo128f(_mm_shuffle_epi32(Qi, BT_SHUFFLE(1, 0, 1, 3))); // Y X Y W
248 
249  vs = _mm_load_ss(&s);
250  V21 = V21 * Y;
251  V31 = V31 * Z;
252 
253  V1 = V1 + V11;
254  V2 = V2 + V21;
255  V3 = V3 + V31;
256 
257  vs = bt_splat3_ps(vs, 0);
258  // s ready
259  V1 = V1 * vs;
260  V2 = V2 * vs;
261  V3 = V3 * vs;
262 
263  V1 = V1 + v1000;
264  V2 = V2 + v0100;
265  V3 = V3 + v0010;
266 
267  m_el[0] = V1;
268  m_el[1] = V2;
269  m_el[2] = V3;
270 #else
271  btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
272  btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
273  btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
274  btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
275  setValue(
276  btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
277  xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
278  xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
279 #endif
280  }
281 
287  void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
288  {
289  setEulerZYX(roll, pitch, yaw);
290  }
291 
301  void setEulerZYX(btScalar eulerX, btScalar eulerY, btScalar eulerZ)
302  {
304  btScalar ci(btCos(eulerX));
305  btScalar cj(btCos(eulerY));
306  btScalar ch(btCos(eulerZ));
307  btScalar si(btSin(eulerX));
308  btScalar sj(btSin(eulerY));
309  btScalar sh(btSin(eulerZ));
310  btScalar cc = ci * ch;
311  btScalar cs = ci * sh;
312  btScalar sc = si * ch;
313  btScalar ss = si * sh;
314 
315  setValue(cj * ch, sj * sc - cs, sj * cc + ss,
316  cj * sh, sj * ss + cc, sj * cs - sc,
317  -sj, cj * si, cj * ci);
318  }
319 
321  void setIdentity()
322  {
323 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
324  m_el[0] = v1000;
325  m_el[1] = v0100;
326  m_el[2] = v0010;
327 #else
328  setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
329  btScalar(0.0), btScalar(1.0), btScalar(0.0),
330  btScalar(0.0), btScalar(0.0), btScalar(1.0));
331 #endif
332  }
333 
334  static const btMatrix3x3& getIdentity()
335  {
336 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
337  static const btMatrix3x3
338  identityMatrix(v1000, v0100, v0010);
339 #else
340  static const btMatrix3x3
341  identityMatrix(
342  btScalar(1.0), btScalar(0.0), btScalar(0.0),
343  btScalar(0.0), btScalar(1.0), btScalar(0.0),
344  btScalar(0.0), btScalar(0.0), btScalar(1.0));
345 #endif
346  return identityMatrix;
347  }
348 
351  void getOpenGLSubMatrix(btScalar * m) const
352  {
353 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
354  __m128 v0 = m_el[0].mVec128;
355  __m128 v1 = m_el[1].mVec128;
356  __m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2
357  __m128* vm = (__m128*)m;
358  __m128 vT;
359 
360  v2 = _mm_and_ps(v2, btvFFF0fMask); // x2 y2 z2 0
361 
362  vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * *
363  v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1
364 
365  v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3)); // y0 y1 y2 0
366  v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3)); // x0 x1 x2 0
367  v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0
368 
369  vm[0] = v0;
370  vm[1] = v1;
371  vm[2] = v2;
372 #elif defined(BT_USE_NEON)
373  // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
374  static const uint32x2_t zMask = (const uint32x2_t){static_cast<uint32_t>(-1), 0};
375  float32x4_t* vm = (float32x4_t*)m;
376  float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128); // {x0 x1 z0 z1}, {y0 y1 w0 w1}
377  float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f)); // {x2 0 }, {y2 0}
378  float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
379  float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
380  float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
381  float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q); // z0 z1 z2 0
382 
383  vm[0] = v0;
384  vm[1] = v1;
385  vm[2] = v2;
386 #else
387  m[0] = btScalar(m_el[0].x());
388  m[1] = btScalar(m_el[1].x());
389  m[2] = btScalar(m_el[2].x());
390  m[3] = btScalar(0.0);
391  m[4] = btScalar(m_el[0].y());
392  m[5] = btScalar(m_el[1].y());
393  m[6] = btScalar(m_el[2].y());
394  m[7] = btScalar(0.0);
395  m[8] = btScalar(m_el[0].z());
396  m[9] = btScalar(m_el[1].z());
397  m[10] = btScalar(m_el[2].z());
398  m[11] = btScalar(0.0);
399 #endif
400  }
401 
404  void getRotation(btQuaternion & q) const
405  {
406 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
407  btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
408  btScalar s, x;
409 
410  union {
411  btSimdFloat4 vec;
412  btScalar f[4];
413  } temp;
414 
415  if (trace > btScalar(0.0))
416  {
417  x = trace + btScalar(1.0);
418 
419  temp.f[0] = m_el[2].y() - m_el[1].z();
420  temp.f[1] = m_el[0].z() - m_el[2].x();
421  temp.f[2] = m_el[1].x() - m_el[0].y();
422  temp.f[3] = x;
423  //temp.f[3]= s * btScalar(0.5);
424  }
425  else
426  {
427  int i, j, k;
428  if (m_el[0].x() < m_el[1].y())
429  {
430  if (m_el[1].y() < m_el[2].z())
431  {
432  i = 2;
433  j = 0;
434  k = 1;
435  }
436  else
437  {
438  i = 1;
439  j = 2;
440  k = 0;
441  }
442  }
443  else
444  {
445  if (m_el[0].x() < m_el[2].z())
446  {
447  i = 2;
448  j = 0;
449  k = 1;
450  }
451  else
452  {
453  i = 0;
454  j = 1;
455  k = 2;
456  }
457  }
458 
459  x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0);
460 
461  temp.f[3] = (m_el[k][j] - m_el[j][k]);
462  temp.f[j] = (m_el[j][i] + m_el[i][j]);
463  temp.f[k] = (m_el[k][i] + m_el[i][k]);
464  temp.f[i] = x;
465  //temp.f[i] = s * btScalar(0.5);
466  }
467 
468  s = btSqrt(x);
469  q.set128(temp.vec);
470  s = btScalar(0.5) / s;
471 
472  q *= s;
473 #else
474  btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
475 
476  btScalar temp[4];
477 
478  if (trace > btScalar(0.0))
479  {
480  btScalar s = btSqrt(trace + btScalar(1.0));
481  temp[3] = (s * btScalar(0.5));
482  s = btScalar(0.5) / s;
483 
484  temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
485  temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
486  temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
487  }
488  else
489  {
490  int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
491  int j = (i + 1) % 3;
492  int k = (i + 2) % 3;
493 
494  btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
495  temp[i] = s * btScalar(0.5);
496  s = btScalar(0.5) / s;
497 
498  temp[3] = (m_el[k][j] - m_el[j][k]) * s;
499  temp[j] = (m_el[j][i] + m_el[i][j]) * s;
500  temp[k] = (m_el[k][i] + m_el[i][k]) * s;
501  }
502  q.setValue(temp[0], temp[1], temp[2], temp[3]);
503 #endif
504  }
505 
510  void getEulerYPR(btScalar & yaw, btScalar & pitch, btScalar & roll) const
511  {
512  // first use the normal calculus
513  yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
514  pitch = btScalar(btAsin(-m_el[2].x()));
515  roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
516 
517  // on pitch = +/-HalfPI
518  if (btFabs(pitch) == SIMD_HALF_PI)
519  {
520  if (yaw > 0)
521  yaw -= SIMD_PI;
522  else
523  yaw += SIMD_PI;
524 
525  if (roll > 0)
526  roll -= SIMD_PI;
527  else
528  roll += SIMD_PI;
529  }
530  };
531 
537  void getEulerZYX(btScalar & yaw, btScalar & pitch, btScalar & roll, unsigned int solution_number = 1) const
538  {
539  struct Euler
540  {
541  btScalar yaw;
542  btScalar pitch;
543  btScalar roll;
544  };
545 
546  Euler euler_out;
547  Euler euler_out2; //second solution
548  //get the pointer to the raw data
549 
550  // Check that pitch is not at a singularity
551  if (btFabs(m_el[2].x()) >= 1)
552  {
553  euler_out.yaw = 0;
554  euler_out2.yaw = 0;
555 
556  // From difference of angles formula
557  btScalar delta = btAtan2(m_el[0].x(), m_el[0].z());
558  if (m_el[2].x() > 0) //gimbal locked up
559  {
560  euler_out.pitch = SIMD_PI / btScalar(2.0);
561  euler_out2.pitch = SIMD_PI / btScalar(2.0);
562  euler_out.roll = euler_out.pitch + delta;
563  euler_out2.roll = euler_out.pitch + delta;
564  }
565  else // gimbal locked down
566  {
567  euler_out.pitch = -SIMD_PI / btScalar(2.0);
568  euler_out2.pitch = -SIMD_PI / btScalar(2.0);
569  euler_out.roll = -euler_out.pitch + delta;
570  euler_out2.roll = -euler_out.pitch + delta;
571  }
572  }
573  else
574  {
575  euler_out.pitch = -btAsin(m_el[2].x());
576  euler_out2.pitch = SIMD_PI - euler_out.pitch;
577 
578  euler_out.roll = btAtan2(m_el[2].y() / btCos(euler_out.pitch),
579  m_el[2].z() / btCos(euler_out.pitch));
580  euler_out2.roll = btAtan2(m_el[2].y() / btCos(euler_out2.pitch),
581  m_el[2].z() / btCos(euler_out2.pitch));
582 
583  euler_out.yaw = btAtan2(m_el[1].x() / btCos(euler_out.pitch),
584  m_el[0].x() / btCos(euler_out.pitch));
585  euler_out2.yaw = btAtan2(m_el[1].x() / btCos(euler_out2.pitch),
586  m_el[0].x() / btCos(euler_out2.pitch));
587  }
588 
589  if (solution_number == 1)
590  {
591  yaw = euler_out.yaw;
592  pitch = euler_out.pitch;
593  roll = euler_out.roll;
594  }
595  else
596  {
597  yaw = euler_out2.yaw;
598  pitch = euler_out2.pitch;
599  roll = euler_out2.roll;
600  }
601  }
602 
606  btMatrix3x3 scaled(const btVector3& s) const
607  {
608 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
609  return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
610 #else
611  return btMatrix3x3(
612  m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
613  m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
614  m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
615 #endif
616  }
617 
619  btScalar determinant() const;
621  btMatrix3x3 adjoint() const;
623  btMatrix3x3 absolute() const;
625  btMatrix3x3 transpose() const;
627  btMatrix3x3 inverse() const;
628 
632  btVector3 solve33(const btVector3& b) const
633  {
634  btVector3 col1 = getColumn(0);
635  btVector3 col2 = getColumn(1);
636  btVector3 col3 = getColumn(2);
637 
638  btScalar det = btDot(col1, btCross(col2, col3));
639  if (btFabs(det) > SIMD_EPSILON)
640  {
641  det = 1.0f / det;
642  }
643  btVector3 x;
644  x[0] = det * btDot(b, btCross(col2, col3));
645  x[1] = det * btDot(col1, btCross(b, col3));
646  x[2] = det * btDot(col1, btCross(col2, b));
647  return x;
648  }
649 
650  btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
651  btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
652 
654  {
655  return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
656  }
658  {
659  return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
660  }
662  {
663  return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
664  }
665 
672  SIMD_FORCE_INLINE void extractRotation(btQuaternion & q, btScalar tolerance = 1.0e-9, int maxIter = 100)
673  {
674  int iter = 0;
675  btScalar w;
676  const btMatrix3x3& A = *this;
677  for (iter = 0; iter < maxIter; iter++)
678  {
679  btMatrix3x3 R(q);
680  btVector3 omega = (R.getColumn(0).cross(A.getColumn(0)) + R.getColumn(1).cross(A.getColumn(1)) + R.getColumn(2).cross(A.getColumn(2))) * (btScalar(1.0) / btFabs(R.getColumn(0).dot(A.getColumn(0)) + R.getColumn(1).dot(A.getColumn(1)) + R.getColumn(2).dot(A.getColumn(2))) +
681  tolerance);
682  w = omega.norm();
683  if (w < tolerance)
684  break;
685  q = btQuaternion(btVector3((btScalar(1.0) / w) * omega), w) *
686  q;
687  q.normalize();
688  }
689  }
690 
700  void diagonalize(btMatrix3x3 & rot, btScalar threshold, int maxSteps)
701  {
702  rot.setIdentity();
703  for (int step = maxSteps; step > 0; step--)
704  {
705  // find off-diagonal element [p][q] with largest magnitude
706  int p = 0;
707  int q = 1;
708  int r = 2;
709  btScalar max = btFabs(m_el[0][1]);
710  btScalar v = btFabs(m_el[0][2]);
711  if (v > max)
712  {
713  q = 2;
714  r = 1;
715  max = v;
716  }
717  v = btFabs(m_el[1][2]);
718  if (v > max)
719  {
720  p = 1;
721  q = 2;
722  r = 0;
723  max = v;
724  }
725 
726  btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
727  if (max <= t)
728  {
729  if (max <= SIMD_EPSILON * t)
730  {
731  return;
732  }
733  step = 1;
734  }
735 
736  // compute Jacobi rotation J which leads to a zero for element [p][q]
737  btScalar mpq = m_el[p][q];
738  btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
739  btScalar theta2 = theta * theta;
740  btScalar cos;
741  btScalar sin;
742  if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
743  {
744  t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
745  : 1 / (theta - btSqrt(1 + theta2));
746  cos = 1 / btSqrt(1 + t * t);
747  sin = cos * t;
748  }
749  else
750  {
751  // approximation for large theta-value, i.e., a nearly diagonal matrix
752  t = 1 / (theta * (2 + btScalar(0.5) / theta2));
753  cos = 1 - btScalar(0.5) * t * t;
754  sin = cos * t;
755  }
756 
757  // apply rotation to matrix (this = J^T * this * J)
758  m_el[p][q] = m_el[q][p] = 0;
759  m_el[p][p] -= t * mpq;
760  m_el[q][q] += t * mpq;
761  btScalar mrp = m_el[r][p];
762  btScalar mrq = m_el[r][q];
763  m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
764  m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
765 
766  // apply rotation to rot (rot = rot * J)
767  for (int i = 0; i < 3; i++)
768  {
769  btVector3& row = rot[i];
770  mrp = row[p];
771  mrq = row[q];
772  row[p] = cos * mrp - sin * mrq;
773  row[q] = cos * mrq + sin * mrp;
774  }
775  }
776  }
777 
785  btScalar cofac(int r1, int c1, int r2, int c2) const
786  {
787  return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
788  }
789 
790  void serialize(struct btMatrix3x3Data & dataOut) const;
791 
792  void serializeFloat(struct btMatrix3x3FloatData & dataOut) const;
793 
794  void deSerialize(const struct btMatrix3x3Data& dataIn);
795 
796  void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn);
797 
798  void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn);
799 };
800 
803 {
804 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)
805  __m128 rv00, rv01, rv02;
806  __m128 rv10, rv11, rv12;
807  __m128 rv20, rv21, rv22;
808  __m128 mv0, mv1, mv2;
809 
810  rv02 = m_el[0].mVec128;
811  rv12 = m_el[1].mVec128;
812  rv22 = m_el[2].mVec128;
813 
814  mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask);
815  mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask);
816  mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask);
817 
818  // rv0
819  rv00 = bt_splat_ps(rv02, 0);
820  rv01 = bt_splat_ps(rv02, 1);
821  rv02 = bt_splat_ps(rv02, 2);
822 
823  rv00 = _mm_mul_ps(rv00, mv0);
824  rv01 = _mm_mul_ps(rv01, mv1);
825  rv02 = _mm_mul_ps(rv02, mv2);
826 
827  // rv1
828  rv10 = bt_splat_ps(rv12, 0);
829  rv11 = bt_splat_ps(rv12, 1);
830  rv12 = bt_splat_ps(rv12, 2);
831 
832  rv10 = _mm_mul_ps(rv10, mv0);
833  rv11 = _mm_mul_ps(rv11, mv1);
834  rv12 = _mm_mul_ps(rv12, mv2);
835 
836  // rv2
837  rv20 = bt_splat_ps(rv22, 0);
838  rv21 = bt_splat_ps(rv22, 1);
839  rv22 = bt_splat_ps(rv22, 2);
840 
841  rv20 = _mm_mul_ps(rv20, mv0);
842  rv21 = _mm_mul_ps(rv21, mv1);
843  rv22 = _mm_mul_ps(rv22, mv2);
844 
845  rv00 = _mm_add_ps(rv00, rv01);
846  rv10 = _mm_add_ps(rv10, rv11);
847  rv20 = _mm_add_ps(rv20, rv21);
848 
849  m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
850  m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
851  m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
852 
853 #elif defined(BT_USE_NEON)
854 
855  float32x4_t rv0, rv1, rv2;
856  float32x4_t v0, v1, v2;
857  float32x4_t mv0, mv1, mv2;
858 
859  v0 = m_el[0].mVec128;
860  v1 = m_el[1].mVec128;
861  v2 = m_el[2].mVec128;
862 
863  mv0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
864  mv1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
865  mv2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
866 
867  rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
868  rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
869  rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
870 
871  rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
872  rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
873  rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
874 
875  rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
876  rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
877  rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
878 
879  m_el[0].mVec128 = rv0;
880  m_el[1].mVec128 = rv1;
881  m_el[2].mVec128 = rv2;
882 #else
883  setValue(
884  m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
885  m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
886  m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
887 #endif
888  return *this;
889 }
890 
893 {
894 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
895  m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
896  m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
897  m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
898 #else
899  setValue(
900  m_el[0][0] + m.m_el[0][0],
901  m_el[0][1] + m.m_el[0][1],
902  m_el[0][2] + m.m_el[0][2],
903  m_el[1][0] + m.m_el[1][0],
904  m_el[1][1] + m.m_el[1][1],
905  m_el[1][2] + m.m_el[1][2],
906  m_el[2][0] + m.m_el[2][0],
907  m_el[2][1] + m.m_el[2][1],
908  m_el[2][2] + m.m_el[2][2]);
909 #endif
910  return *this;
911 }
912 
914 operator*(const btMatrix3x3& m, const btScalar& k)
915 {
916 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
917  __m128 vk = bt_splat_ps(_mm_load_ss((float*)&k), 0x80);
918  return btMatrix3x3(
919  _mm_mul_ps(m[0].mVec128, vk),
920  _mm_mul_ps(m[1].mVec128, vk),
921  _mm_mul_ps(m[2].mVec128, vk));
922 #elif defined(BT_USE_NEON)
923  return btMatrix3x3(
924  vmulq_n_f32(m[0].mVec128, k),
925  vmulq_n_f32(m[1].mVec128, k),
926  vmulq_n_f32(m[2].mVec128, k));
927 #else
928  return btMatrix3x3(
929  m[0].x() * k, m[0].y() * k, m[0].z() * k,
930  m[1].x() * k, m[1].y() * k, m[1].z() * k,
931  m[2].x() * k, m[2].y() * k, m[2].z() * k);
932 #endif
933 }
934 
936 operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
937 {
938 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
939  return btMatrix3x3(
940  m1[0].mVec128 + m2[0].mVec128,
941  m1[1].mVec128 + m2[1].mVec128,
942  m1[2].mVec128 + m2[2].mVec128);
943 #else
944  return btMatrix3x3(
945  m1[0][0] + m2[0][0],
946  m1[0][1] + m2[0][1],
947  m1[0][2] + m2[0][2],
948 
949  m1[1][0] + m2[1][0],
950  m1[1][1] + m2[1][1],
951  m1[1][2] + m2[1][2],
952 
953  m1[2][0] + m2[2][0],
954  m1[2][1] + m2[2][1],
955  m1[2][2] + m2[2][2]);
956 #endif
957 }
958 
960 operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
961 {
962 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
963  return btMatrix3x3(
964  m1[0].mVec128 - m2[0].mVec128,
965  m1[1].mVec128 - m2[1].mVec128,
966  m1[2].mVec128 - m2[2].mVec128);
967 #else
968  return btMatrix3x3(
969  m1[0][0] - m2[0][0],
970  m1[0][1] - m2[0][1],
971  m1[0][2] - m2[0][2],
972 
973  m1[1][0] - m2[1][0],
974  m1[1][1] - m2[1][1],
975  m1[1][2] - m2[1][2],
976 
977  m1[2][0] - m2[2][0],
978  m1[2][1] - m2[2][1],
979  m1[2][2] - m2[2][2]);
980 #endif
981 }
982 
985 {
986 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
987  m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
988  m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
989  m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
990 #else
991  setValue(
992  m_el[0][0] - m.m_el[0][0],
993  m_el[0][1] - m.m_el[0][1],
994  m_el[0][2] - m.m_el[0][2],
995  m_el[1][0] - m.m_el[1][0],
996  m_el[1][1] - m.m_el[1][1],
997  m_el[1][2] - m.m_el[1][2],
998  m_el[2][0] - m.m_el[2][0],
999  m_el[2][1] - m.m_el[2][1],
1000  m_el[2][2] - m.m_el[2][2]);
1001 #endif
1002  return *this;
1003 }
1004 
1007 {
1008  return btTriple((*this)[0], (*this)[1], (*this)[2]);
1009 }
1010 
1013 {
1014 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1015  return btMatrix3x3(
1016  _mm_and_ps(m_el[0].mVec128, btvAbsfMask),
1017  _mm_and_ps(m_el[1].mVec128, btvAbsfMask),
1018  _mm_and_ps(m_el[2].mVec128, btvAbsfMask));
1019 #elif defined(BT_USE_NEON)
1020  return btMatrix3x3(
1021  (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask),
1022  (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask),
1023  (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
1024 #else
1025  return btMatrix3x3(
1026  btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
1027  btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
1028  btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
1029 #endif
1030 }
1031 
1034 {
1035 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1036  __m128 v0 = m_el[0].mVec128;
1037  __m128 v1 = m_el[1].mVec128;
1038  __m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2
1039  __m128 vT;
1040 
1041  v2 = _mm_and_ps(v2, btvFFF0fMask); // x2 y2 z2 0
1042 
1043  vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * *
1044  v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1
1045 
1046  v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3)); // y0 y1 y2 0
1047  v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3)); // x0 x1 x2 0
1048  v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0
1049 
1050  return btMatrix3x3(v0, v1, v2);
1051 #elif defined(BT_USE_NEON)
1052  // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
1053  static const uint32x2_t zMask = (const uint32x2_t){static_cast<uint32_t>(-1), 0};
1054  float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128); // {x0 x1 z0 z1}, {y0 y1 w0 w1}
1055  float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f)); // {x2 0 }, {y2 0}
1056  float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
1057  float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
1058  float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
1059  float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q); // z0 z1 z2 0
1060  return btMatrix3x3(v0, v1, v2);
1061 #else
1062  return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
1063  m_el[0].y(), m_el[1].y(), m_el[2].y(),
1064  m_el[0].z(), m_el[1].z(), m_el[2].z());
1065 #endif
1066 }
1067 
1070 {
1071  return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
1072  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
1073  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
1074 }
1075 
1078 {
1079  btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
1080  btScalar det = (*this)[0].dot(co);
1081  //btFullAssert(det != btScalar(0.0));
1082  btAssert(det != btScalar(0.0));
1083  btScalar s = btScalar(1.0) / det;
1084  return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
1085  co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
1086  co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
1087 }
1088 
1091 {
1092 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1093  // zeros w
1094  // static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
1095  __m128 row = m_el[0].mVec128;
1096  __m128 m0 = _mm_and_ps(m.getRow(0).mVec128, btvFFF0fMask);
1097  __m128 m1 = _mm_and_ps(m.getRow(1).mVec128, btvFFF0fMask);
1098  __m128 m2 = _mm_and_ps(m.getRow(2).mVec128, btvFFF0fMask);
1099  __m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
1100  __m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
1101  __m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
1102  row = m_el[1].mVec128;
1103  r0 = _mm_add_ps(r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
1104  r1 = _mm_add_ps(r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
1105  r2 = _mm_add_ps(r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
1106  row = m_el[2].mVec128;
1107  r0 = _mm_add_ps(r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
1108  r1 = _mm_add_ps(r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
1109  r2 = _mm_add_ps(r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
1110  return btMatrix3x3(r0, r1, r2);
1111 
1112 #elif defined BT_USE_NEON
1113  // zeros w
1114  static const uint32x4_t xyzMask = (const uint32x4_t){static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), static_cast<uint32_t>(-1), 0};
1115  float32x4_t m0 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(0).mVec128, xyzMask);
1116  float32x4_t m1 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(1).mVec128, xyzMask);
1117  float32x4_t m2 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(2).mVec128, xyzMask);
1118  float32x4_t row = m_el[0].mVec128;
1119  float32x4_t r0 = vmulq_lane_f32(m0, vget_low_f32(row), 0);
1120  float32x4_t r1 = vmulq_lane_f32(m0, vget_low_f32(row), 1);
1121  float32x4_t r2 = vmulq_lane_f32(m0, vget_high_f32(row), 0);
1122  row = m_el[1].mVec128;
1123  r0 = vmlaq_lane_f32(r0, m1, vget_low_f32(row), 0);
1124  r1 = vmlaq_lane_f32(r1, m1, vget_low_f32(row), 1);
1125  r2 = vmlaq_lane_f32(r2, m1, vget_high_f32(row), 0);
1126  row = m_el[2].mVec128;
1127  r0 = vmlaq_lane_f32(r0, m2, vget_low_f32(row), 0);
1128  r1 = vmlaq_lane_f32(r1, m2, vget_low_f32(row), 1);
1129  r2 = vmlaq_lane_f32(r2, m2, vget_high_f32(row), 0);
1130  return btMatrix3x3(r0, r1, r2);
1131 #else
1132  return btMatrix3x3(
1133  m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
1134  m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
1135  m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
1136  m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
1137  m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
1138  m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
1139  m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
1140  m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
1141  m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
1142 #endif
1143 }
1144 
1147 {
1148 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1149  __m128 a0 = m_el[0].mVec128;
1150  __m128 a1 = m_el[1].mVec128;
1151  __m128 a2 = m_el[2].mVec128;
1152 
1153  btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
1154  __m128 mx = mT[0].mVec128;
1155  __m128 my = mT[1].mVec128;
1156  __m128 mz = mT[2].mVec128;
1157 
1158  __m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
1159  __m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
1160  __m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
1161  r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
1162  r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
1163  r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
1164  r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
1165  r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
1166  r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
1167  return btMatrix3x3(r0, r1, r2);
1168 
1169 #elif defined BT_USE_NEON
1170  float32x4_t a0 = m_el[0].mVec128;
1171  float32x4_t a1 = m_el[1].mVec128;
1172  float32x4_t a2 = m_el[2].mVec128;
1173 
1174  btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
1175  float32x4_t mx = mT[0].mVec128;
1176  float32x4_t my = mT[1].mVec128;
1177  float32x4_t mz = mT[2].mVec128;
1178 
1179  float32x4_t r0 = vmulq_lane_f32(mx, vget_low_f32(a0), 0);
1180  float32x4_t r1 = vmulq_lane_f32(mx, vget_low_f32(a1), 0);
1181  float32x4_t r2 = vmulq_lane_f32(mx, vget_low_f32(a2), 0);
1182  r0 = vmlaq_lane_f32(r0, my, vget_low_f32(a0), 1);
1183  r1 = vmlaq_lane_f32(r1, my, vget_low_f32(a1), 1);
1184  r2 = vmlaq_lane_f32(r2, my, vget_low_f32(a2), 1);
1185  r0 = vmlaq_lane_f32(r0, mz, vget_high_f32(a0), 0);
1186  r1 = vmlaq_lane_f32(r1, mz, vget_high_f32(a1), 0);
1187  r2 = vmlaq_lane_f32(r2, mz, vget_high_f32(a2), 0);
1188  return btMatrix3x3(r0, r1, r2);
1189 
1190 #else
1191  return btMatrix3x3(
1192  m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
1193  m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
1194  m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
1195 #endif
1196 }
1197 
1199 operator*(const btMatrix3x3& m, const btVector3& v)
1200 {
1201 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
1202  return v.dot3(m[0], m[1], m[2]);
1203 #else
1204  return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
1205 #endif
1206 }
1207 
1209 operator*(const btVector3& v, const btMatrix3x3& m)
1210 {
1211 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1212 
1213  const __m128 vv = v.mVec128;
1214 
1215  __m128 c0 = bt_splat_ps(vv, 0);
1216  __m128 c1 = bt_splat_ps(vv, 1);
1217  __m128 c2 = bt_splat_ps(vv, 2);
1218 
1219  c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask));
1220  c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask));
1221  c0 = _mm_add_ps(c0, c1);
1222  c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask));
1223 
1224  return btVector3(_mm_add_ps(c0, c2));
1225 #elif defined(BT_USE_NEON)
1226  const float32x4_t vv = v.mVec128;
1227  const float32x2_t vlo = vget_low_f32(vv);
1228  const float32x2_t vhi = vget_high_f32(vv);
1229 
1230  float32x4_t c0, c1, c2;
1231 
1232  c0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
1233  c1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
1234  c2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
1235 
1236  c0 = vmulq_lane_f32(c0, vlo, 0);
1237  c1 = vmulq_lane_f32(c1, vlo, 1);
1238  c2 = vmulq_lane_f32(c2, vhi, 0);
1239  c0 = vaddq_f32(c0, c1);
1240  c0 = vaddq_f32(c0, c2);
1241 
1242  return btVector3(c0);
1243 #else
1244  return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
1245 #endif
1246 }
1247 
1249 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
1250 {
1251 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1252 
1253  __m128 m10 = m1[0].mVec128;
1254  __m128 m11 = m1[1].mVec128;
1255  __m128 m12 = m1[2].mVec128;
1256 
1257  __m128 m2v = _mm_and_ps(m2[0].mVec128, btvFFF0fMask);
1258 
1259  __m128 c0 = bt_splat_ps(m10, 0);
1260  __m128 c1 = bt_splat_ps(m11, 0);
1261  __m128 c2 = bt_splat_ps(m12, 0);
1262 
1263  c0 = _mm_mul_ps(c0, m2v);
1264  c1 = _mm_mul_ps(c1, m2v);
1265  c2 = _mm_mul_ps(c2, m2v);
1266 
1267  m2v = _mm_and_ps(m2[1].mVec128, btvFFF0fMask);
1268 
1269  __m128 c0_1 = bt_splat_ps(m10, 1);
1270  __m128 c1_1 = bt_splat_ps(m11, 1);
1271  __m128 c2_1 = bt_splat_ps(m12, 1);
1272 
1273  c0_1 = _mm_mul_ps(c0_1, m2v);
1274  c1_1 = _mm_mul_ps(c1_1, m2v);
1275  c2_1 = _mm_mul_ps(c2_1, m2v);
1276 
1277  m2v = _mm_and_ps(m2[2].mVec128, btvFFF0fMask);
1278 
1279  c0 = _mm_add_ps(c0, c0_1);
1280  c1 = _mm_add_ps(c1, c1_1);
1281  c2 = _mm_add_ps(c2, c2_1);
1282 
1283  m10 = bt_splat_ps(m10, 2);
1284  m11 = bt_splat_ps(m11, 2);
1285  m12 = bt_splat_ps(m12, 2);
1286 
1287  m10 = _mm_mul_ps(m10, m2v);
1288  m11 = _mm_mul_ps(m11, m2v);
1289  m12 = _mm_mul_ps(m12, m2v);
1290 
1291  c0 = _mm_add_ps(c0, m10);
1292  c1 = _mm_add_ps(c1, m11);
1293  c2 = _mm_add_ps(c2, m12);
1294 
1295  return btMatrix3x3(c0, c1, c2);
1296 
1297 #elif defined(BT_USE_NEON)
1298 
1299  float32x4_t rv0, rv1, rv2;
1300  float32x4_t v0, v1, v2;
1301  float32x4_t mv0, mv1, mv2;
1302 
1303  v0 = m1[0].mVec128;
1304  v1 = m1[1].mVec128;
1305  v2 = m1[2].mVec128;
1306 
1307  mv0 = (float32x4_t)vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask);
1308  mv1 = (float32x4_t)vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask);
1309  mv2 = (float32x4_t)vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask);
1310 
1311  rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
1312  rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
1313  rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
1314 
1315  rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
1316  rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
1317  rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
1318 
1319  rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
1320  rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
1321  rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
1322 
1323  return btMatrix3x3(rv0, rv1, rv2);
1324 
1325 #else
1326  return btMatrix3x3(
1327  m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
1328  m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
1329  m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
1330 #endif
1331 }
1332 
1333 /*
1334 SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
1335 return btMatrix3x3(
1336 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
1337 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
1338 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
1339 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
1340 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
1341 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
1342 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
1343 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
1344 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
1345 }
1346 */
1347 
1351 {
1352 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
1353 
1354  __m128 c0, c1, c2;
1355 
1356  c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
1357  c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
1358  c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
1359 
1360  c0 = _mm_and_ps(c0, c1);
1361  c0 = _mm_and_ps(c0, c2);
1362 
1363  int m = _mm_movemask_ps((__m128)c0);
1364  return (0x7 == (m & 0x7));
1365 
1366 #else
1367  return (m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
1368  m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
1369  m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2]);
1370 #endif
1371 }
1372 
1375 {
1377 };
1378 
1381 {
1383 };
1384 
1386 {
1387  for (int i = 0; i < 3; i++)
1388  m_el[i].serialize(dataOut.m_el[i]);
1389 }
1390 
1392 {
1393  for (int i = 0; i < 3; i++)
1394  m_el[i].serializeFloat(dataOut.m_el[i]);
1395 }
1396 
1398 {
1399  for (int i = 0; i < 3; i++)
1400  m_el[i].deSerialize(dataIn.m_el[i]);
1401 }
1402 
1404 {
1405  for (int i = 0; i < 3; i++)
1406  m_el[i].deSerializeFloat(dataIn.m_el[i]);
1407 }
1408 
1410 {
1411  for (int i = 0; i < 3; i++)
1412  m_el[i].deSerializeDouble(dataIn.m_el[i]);
1413 }
1414 
1415 #endif //BT_MATRIX3x3_H
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btMatrix3x3DoubleData
for serialization
Definition: btMatrix3x3.h:1380
btMatrix3x3::operator=
btMatrix3x3 & operator=(const btMatrix3x3 &other)
Assignment Operator.
Definition: btMatrix3x3.h:121
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btMatrix3x3DoubleData::m_el
btVector3DoubleData m_el[3]
Definition: btMatrix3x3.h:1382
btMatrix3x3::m_el
btVector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: btMatrix3x3.h:50
btMatrix3x3::btMatrix3x3
btMatrix3x3(const btMatrix3x3 &other)
Copy constructor.
Definition: btMatrix3x3.h:113
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition: btScalar.h:528
btVector3::dot3
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:720
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1077
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btMatrix3x3::diagonalize
void diagonalize(btMatrix3x3 &rot, btScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: btMatrix3x3.h:700
btMatrix3x3::extractRotation
void extractRotation(btQuaternion &q, btScalar tolerance=1.0e-9, int maxIter=100)
extractRotation is from "A robust method to extract the rotational part of deformations" See http://d...
Definition: btMatrix3x3.h:672
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:526
btTriple
btScalar btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3)
Definition: btVector3.h:924
btQuadWord::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:149
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
btMatrix3x3::tdotx
btScalar tdotx(const btVector3 &v) const
Definition: btMatrix3x3.h:653
btMatrix3x3::timesTranspose
btMatrix3x3 timesTranspose(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1146
btMatrix3x3::solve33
btVector3 solve33(const btVector3 &b) const
Solve A * x = b, where b is a column vector.
Definition: btMatrix3x3.h:632
operator*
btMatrix3x3 operator*(const btMatrix3x3 &m, const btScalar &k)
Definition: btMatrix3x3.h:914
btMatrix3x3::tdotz
btScalar tdotz(const btVector3 &v) const
Definition: btMatrix3x3.h:661
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
btMatrix3x3::deSerializeFloat
void deSerializeFloat(const struct btMatrix3x3FloatData &dataIn)
Definition: btMatrix3x3.h:1403
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
btVector3FloatData
Definition: btVector3.h:1281
btQuaternion::length2
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:364
btMatrix3x3::tdoty
btScalar tdoty(const btVector3 &v) const
Definition: btMatrix3x3.h:657
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btMatrix3x3::getEulerYPR
void getEulerYPR(btScalar &yaw, btScalar &pitch, btScalar &roll) const
Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR.
Definition: btMatrix3x3.h:510
btMatrix3x3::getRow
const btVector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: btMatrix3x3.h:147
btMatrix3x3::getEulerZYX
void getEulerZYX(btScalar &yaw, btScalar &pitch, btScalar &roll, unsigned int solution_number=1) const
Get the matrix represented as euler angles around ZYX.
Definition: btMatrix3x3.h:537
btVector3.h
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:499
btMatrix3x3::cofac
btScalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: btMatrix3x3.h:785
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btVector3DoubleData
Definition: btVector3.h:1286
btMatrix3x3::btMatrix3x3
btMatrix3x3()
No initializaion constructor.
Definition: btMatrix3x3.h:54
btFullAssert
#define btFullAssert(x)
Definition: btScalar.h:156
operator+
btMatrix3x3 operator+(const btMatrix3x3 &m1, const btMatrix3x3 &m2)
Definition: btMatrix3x3.h:936
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:498
btMatrix3x3::operator+=
btMatrix3x3 & operator+=(const btMatrix3x3 &m)
Adds by the target matrix on the right.
Definition: btMatrix3x3.h:892
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btMatrix3x3::determinant
btScalar determinant() const
Return the determinant of the matrix.
Definition: btMatrix3x3.h:1006
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
btMatrix3x3::btMatrix3x3
btMatrix3x3(const btQuaternion &q)
Constructor from Quaternion.
Definition: btMatrix3x3.h:59
btQuaternion.h
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:140
btMatrix3x3::deSerialize
void deSerialize(const struct btMatrix3x3Data &dataIn)
Definition: btMatrix3x3.h:1397
btMatrix3x3::transposeTimes
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1090
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
btMatrix3x3::deSerializeDouble
void deSerializeDouble(const struct btMatrix3x3DoubleData &dataIn)
Definition: btMatrix3x3.h:1409
btMatrix3x3::operator[]
btVector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
Definition: btMatrix3x3.h:155
btMatrix3x3::operator*=
btMatrix3x3 & operator*=(const btMatrix3x3 &m)
Multiply by the target matrix on the right.
Definition: btMatrix3x3.h:802
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
btMatrix3x3::getIdentity
static const btMatrix3x3 & getIdentity()
Definition: btMatrix3x3.h:334
btMatrix3x3::setValue
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
Definition: btMatrix3x3.h:202
btMatrix3x3::setFromOpenGLSubMatrix
void setFromOpenGLSubMatrix(const btScalar *m)
Set from the rotational part of a 4x4 OpenGL matrix.
Definition: btMatrix3x3.h:186
btMatrix3x3::btMatrix3x3
btMatrix3x3(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Constructor with row major formatting.
Definition: btMatrix3x3.h:68
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btMatrix3x3::adjoint
btMatrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: btMatrix3x3.h:1069
operator-
btMatrix3x3 operator-(const btMatrix3x3 &m1, const btMatrix3x3 &m2)
Definition: btMatrix3x3.h:960
btMatrix3x3::getRotation
void getRotation(btQuaternion &q) const
Get the matrix represented as a quaternion.
Definition: btMatrix3x3.h:404
btMatrix3x3::scaled
btMatrix3x3 scaled(const btVector3 &s) const
Create a scaled copy of the matrix.
Definition: btMatrix3x3.h:606
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:321
btMatrix3x3::operator-=
btMatrix3x3 & operator-=(const btMatrix3x3 &m)
Substractss by the target matrix on the right.
Definition: btMatrix3x3.h:984
btMatrix3x3::setEulerYPR
void setEulerYPR(const btScalar &yaw, const btScalar &pitch, const btScalar &roll)
Set the matrix from euler angles using YPR around YXZ respectively.
Definition: btMatrix3x3.h:287
operator==
bool operator==(const btMatrix3x3 &m1, const btMatrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
Definition: btMatrix3x3.h:1350
btMatrix3x3::absolute
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:1012
btMatrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(btScalar *m) const
Fill the rotational part of an OpenGL matrix and clear the shear/perspective.
Definition: btMatrix3x3.h:351
btMatrix3x3Data
#define btMatrix3x3Data
Definition: btMatrix3x3.h:41
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btMatrix3x3::btMatrix3x3
btMatrix3x3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
Definition: btMatrix3x3.h:129
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
btMatrix3x3::setRotation
void setRotation(const btQuaternion &q)
Set the matrix from a quaternion.
Definition: btMatrix3x3.h:213
btMatrix3x3::setEulerZYX
void setEulerZYX(btScalar eulerX, btScalar eulerY, btScalar eulerZ)
Set the matrix from euler angles YPR around ZYX axes.
Definition: btMatrix3x3.h:301
btMatrix3x3::serializeFloat
void serializeFloat(struct btMatrix3x3FloatData &dataOut) const
Definition: btMatrix3x3.h:1391
btMatrix3x3FloatData
for serialization
Definition: btMatrix3x3.h:1374
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btMatrix3x3::serialize
void serialize(struct btMatrix3x3Data &dataOut) const
Definition: btMatrix3x3.h:1385
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btMatrix3x3FloatData::m_el
btVector3FloatData m_el[3]
Definition: btMatrix3x3.h:1376
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAsin
btScalar btAsin(btScalar x)
Definition: btScalar.h:509
btMatrix3x3::operator[]
const btVector3 & operator[](int i) const
Get a const reference to a row of the matrix as a vector.
Definition: btMatrix3x3.h:163