Matrix3x3.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 #ifndef TF2_MATRIX3x3_H
17 #define TF2_MATRIX3x3_H
18 
19 #include "Vector3.h"
20 #include "Quaternion.h"
21 
22 namespace tf2
23 {
24 
25 
26 #define Matrix3x3Data Matrix3x3DoubleData
27 
28 
31 class Matrix3x3 {
32 
34  Vector3 m_el[3];
35 
36 public:
38  Matrix3x3 () {}
39 
40  // explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); }
41 
43  explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
44  /*
45  template <typename tf2Scalar>
46  Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
47  {
48  setEulerYPR(yaw, pitch, roll);
49  }
50  */
52  Matrix3x3(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
53  const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
54  const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
55  {
56  setValue(xx, xy, xz,
57  yx, yy, yz,
58  zx, zy, zz);
59  }
62  {
63  m_el[0] = other.m_el[0];
64  m_el[1] = other.m_el[1];
65  m_el[2] = other.m_el[2];
66  }
67 
68 
71  {
72  m_el[0] = other.m_el[0];
73  m_el[1] = other.m_el[1];
74  m_el[2] = other.m_el[2];
75  return *this;
76  }
77 
78 
81  TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
82  {
83  return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
84  }
85 
86 
89  TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const
90  {
91  tf2FullAssert(0 <= i && i < 3);
92  return m_el[i];
93  }
94 
98  {
99  tf2FullAssert(0 <= i && i < 3);
100  return m_el[i];
101  }
102 
105  TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const
106  {
107  tf2FullAssert(0 <= i && i < 3);
108  return m_el[i];
109  }
110 
114  Matrix3x3& operator*=(const Matrix3x3& m);
115 
119  {
120  m_el[0].setValue(m[0],m[4],m[8]);
121  m_el[1].setValue(m[1],m[5],m[9]);
122  m_el[2].setValue(m[2],m[6],m[10]);
123 
124  }
135  void setValue(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
136  const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
137  const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
138  {
139  m_el[0].setValue(xx,xy,xz);
140  m_el[1].setValue(yx,yy,yz);
141  m_el[2].setValue(zx,zy,zz);
142  }
143 
146  void setRotation(const Quaternion& q)
147  {
148  tf2Scalar d = q.length2();
149  tf2FullAssert(d != tf2Scalar(0.0));
150  tf2Scalar s = tf2Scalar(2.0) / d;
151  tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
152  tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
153  tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
154  tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
155  setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
156  xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx,
157  xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy));
158  }
159 
160 
166  void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated))
167  {
168  setEulerYPR(yaw, pitch, roll);
169  }
170 
180  void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) {
181  tf2Scalar ci ( tf2Cos(eulerX));
182  tf2Scalar cj ( tf2Cos(eulerY));
183  tf2Scalar ch ( tf2Cos(eulerZ));
184  tf2Scalar si ( tf2Sin(eulerX));
185  tf2Scalar sj ( tf2Sin(eulerY));
186  tf2Scalar sh ( tf2Sin(eulerZ));
187  tf2Scalar cc = ci * ch;
188  tf2Scalar cs = ci * sh;
189  tf2Scalar sc = si * ch;
190  tf2Scalar ss = si * sh;
191 
192  setValue(cj * ch, sj * sc - cs, sj * cc + ss,
193  cj * sh, sj * ss + cc, sj * cs - sc,
194  -sj, cj * si, cj * ci);
195  }
196 
204  setEulerYPR(yaw, pitch, roll);
205  }
206 
208  void setIdentity()
209  {
210  setValue(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0),
211  tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0),
212  tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0));
213  }
214 
215  static const Matrix3x3& getIdentity()
216  {
217  static const Matrix3x3 identityMatrix(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0),
218  tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0),
219  tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0));
220  return identityMatrix;
221  }
222 
226  {
227  m[0] = tf2Scalar(m_el[0].x());
228  m[1] = tf2Scalar(m_el[1].x());
229  m[2] = tf2Scalar(m_el[2].x());
230  m[3] = tf2Scalar(0.0);
231  m[4] = tf2Scalar(m_el[0].y());
232  m[5] = tf2Scalar(m_el[1].y());
233  m[6] = tf2Scalar(m_el[2].y());
234  m[7] = tf2Scalar(0.0);
235  m[8] = tf2Scalar(m_el[0].z());
236  m[9] = tf2Scalar(m_el[1].z());
237  m[10] = tf2Scalar(m_el[2].z());
238  m[11] = tf2Scalar(0.0);
239  }
240 
243  void getRotation(Quaternion& q) const
244  {
245  tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
246  tf2Scalar temp[4];
247 
248  if (trace > tf2Scalar(0.0))
249  {
250  tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0));
251  temp[3]=(s * tf2Scalar(0.5));
252  s = tf2Scalar(0.5) / s;
253 
254  temp[0]=((m_el[2].y() - m_el[1].z()) * s);
255  temp[1]=((m_el[0].z() - m_el[2].x()) * s);
256  temp[2]=((m_el[1].x() - m_el[0].y()) * s);
257  }
258  else
259  {
260  int i = m_el[0].x() < m_el[1].y() ?
261  (m_el[1].y() < m_el[2].z() ? 2 : 1) :
262  (m_el[0].x() < m_el[2].z() ? 2 : 0);
263  int j = (i + 1) % 3;
264  int k = (i + 2) % 3;
265 
266  tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0));
267  temp[i] = s * tf2Scalar(0.5);
268  s = tf2Scalar(0.5) / s;
269 
270  temp[3] = (m_el[k][j] - m_el[j][k]) * s;
271  temp[j] = (m_el[j][i] + m_el[i][j]) * s;
272  temp[k] = (m_el[k][i] + m_el[i][k]) * s;
273  }
274  q.setValue(temp[0],temp[1],temp[2],temp[3]);
275  }
276 
282  __attribute__((deprecated)) void getEulerZYX(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
283  {
284  getEulerYPR(yaw, pitch, roll, solution_number);
285  };
286 
287 
292  void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
293  {
294  struct Euler
295  {
296  tf2Scalar yaw;
298  tf2Scalar roll;
299  };
300 
301  Euler euler_out;
302  Euler euler_out2; //second solution
303  //get the pointer to the raw data
304 
305  // Check that pitch is not at a singularity
306  // Check that pitch is not at a singularity
307  if (tf2Fabs(m_el[2].x()) >= 1)
308  {
309  euler_out.yaw = 0;
310  euler_out2.yaw = 0;
311 
312  // From difference of angles formula
313  tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z());
314  if (m_el[2].x() < 0) //gimbal locked down
315  {
316  euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0);
317  euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0);
318  euler_out.roll = delta;
319  euler_out2.roll = delta;
320  }
321  else // gimbal locked up
322  {
323  euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
324  euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
325  euler_out.roll = delta;
326  euler_out2.roll = delta;
327  }
328  }
329  else
330  {
331  euler_out.pitch = - tf2Asin(m_el[2].x());
332  euler_out2.pitch = TF2SIMD_PI - euler_out.pitch;
333 
334  euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch),
335  m_el[2].z()/tf2Cos(euler_out.pitch));
336  euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch),
337  m_el[2].z()/tf2Cos(euler_out2.pitch));
338 
339  euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch),
340  m_el[0].x()/tf2Cos(euler_out.pitch));
341  euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch),
342  m_el[0].x()/tf2Cos(euler_out2.pitch));
343  }
344 
345  if (solution_number == 1)
346  {
347  yaw = euler_out.yaw;
348  pitch = euler_out.pitch;
349  roll = euler_out.roll;
350  }
351  else
352  {
353  yaw = euler_out2.yaw;
354  pitch = euler_out2.pitch;
355  roll = euler_out2.roll;
356  }
357  }
358 
364  void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const
365  {
366  getEulerYPR(yaw, pitch, roll, solution_number);
367  }
368 
372  Matrix3x3 scaled(const Vector3& s) const
373  {
374  return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
375  m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
376  m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
377  }
378 
380  tf2Scalar determinant() const;
382  Matrix3x3 adjoint() const;
384  Matrix3x3 absolute() const;
386  Matrix3x3 transpose() const;
388  Matrix3x3 inverse() const;
389 
390  Matrix3x3 transposeTimes(const Matrix3x3& m) const;
391  Matrix3x3 timesTranspose(const Matrix3x3& m) const;
392 
393  TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3& v) const
394  {
395  return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
396  }
397  TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3& v) const
398  {
399  return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
400  }
401  TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3& v) const
402  {
403  return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
404  }
405 
406 
416  void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps)
417  {
418  rot.setIdentity();
419  for (int step = maxSteps; step > 0; step--)
420  {
421  // find off-diagonal element [p][q] with largest magnitude
422  int p = 0;
423  int q = 1;
424  int r = 2;
425  tf2Scalar max = tf2Fabs(m_el[0][1]);
426  tf2Scalar v = tf2Fabs(m_el[0][2]);
427  if (v > max)
428  {
429  q = 2;
430  r = 1;
431  max = v;
432  }
433  v = tf2Fabs(m_el[1][2]);
434  if (v > max)
435  {
436  p = 1;
437  q = 2;
438  r = 0;
439  max = v;
440  }
441 
442  tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2]));
443  if (max <= t)
444  {
445  if (max <= TF2SIMD_EPSILON * t)
446  {
447  return;
448  }
449  step = 1;
450  }
451 
452  // compute Jacobi rotation J which leads to a zero for element [p][q]
453  tf2Scalar mpq = m_el[p][q];
454  tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
455  tf2Scalar theta2 = theta * theta;
456  tf2Scalar cos;
457  tf2Scalar sin;
458  if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON))
459  {
460  t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2))
461  : 1 / (theta - tf2Sqrt(1 + theta2));
462  cos = 1 / tf2Sqrt(1 + t * t);
463  sin = cos * t;
464  }
465  else
466  {
467  // approximation for large theta-value, i.e., a nearly diagonal matrix
468  t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2));
469  cos = 1 - tf2Scalar(0.5) * t * t;
470  sin = cos * t;
471  }
472 
473  // apply rotation to matrix (this = J^T * this * J)
474  m_el[p][q] = m_el[q][p] = 0;
475  m_el[p][p] -= t * mpq;
476  m_el[q][q] += t * mpq;
477  tf2Scalar mrp = m_el[r][p];
478  tf2Scalar mrq = m_el[r][q];
479  m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
480  m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
481 
482  // apply rotation to rot (rot = rot * J)
483  for (int i = 0; i < 3; i++)
484  {
485  Vector3& row = rot[i];
486  mrp = row[p];
487  mrq = row[q];
488  row[p] = cos * mrp - sin * mrq;
489  row[q] = cos * mrq + sin * mrp;
490  }
491  }
492  }
493 
494 
495 
496 
504  tf2Scalar cofac(int r1, int c1, int r2, int c2) const
505  {
506  return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
507  }
508 
509  void serialize(struct Matrix3x3Data& dataOut) const;
510 
511  void serializeFloat(struct Matrix3x3FloatData& dataOut) const;
512 
513  void deSerialize(const struct Matrix3x3Data& dataIn);
514 
515  void deSerializeFloat(const struct Matrix3x3FloatData& dataIn);
516 
517  void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn);
518 
519 };
520 
521 
524 {
525  setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
526  m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
527  m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
528  return *this;
529 }
530 
533 {
534  return tf2Triple((*this)[0], (*this)[1], (*this)[2]);
535 }
536 
537 
540 {
541  return Matrix3x3(
542  tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()),
543  tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()),
544  tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z()));
545 }
546 
549 {
550  return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
551  m_el[0].y(), m_el[1].y(), m_el[2].y(),
552  m_el[0].z(), m_el[1].z(), m_el[2].z());
553 }
554 
557 {
558  return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
559  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
560  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
561 }
562 
565 {
566  Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
567  tf2Scalar det = (*this)[0].dot(co);
568  tf2FullAssert(det != tf2Scalar(0.0));
569  tf2Scalar s = tf2Scalar(1.0) / det;
570  return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
571  co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
572  co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
573 }
574 
577 {
578  return Matrix3x3(
579  m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
580  m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
581  m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
582  m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
583  m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
584  m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
585  m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
586  m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
587  m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
588 }
589 
592 {
593  return Matrix3x3(
594  m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
595  m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
596  m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
597 
598 }
599 
600 TF2SIMD_FORCE_INLINE Vector3
601 operator*(const Matrix3x3& m, const Vector3& v)
602 {
603  return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
604 }
605 
606 
607 TF2SIMD_FORCE_INLINE Vector3
608 operator*(const Vector3& v, const Matrix3x3& m)
609 {
610  return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
611 }
612 
614 operator*(const Matrix3x3& m1, const Matrix3x3& m2)
615 {
616  return Matrix3x3(
617  m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
618  m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
619  m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
620 }
621 
622 /*
623 TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) {
624 return Matrix3x3(
625 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
626 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
627 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
628 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
629 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
630 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
631 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
632 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
633 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
634 }
635 */
636 
640 {
641  return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
642  m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
643  m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
644 }
645 
648 {
650 };
651 
654 {
656 };
657 
658 
659 
660 
662 {
663  for (int i=0;i<3;i++)
664  m_el[i].serialize(dataOut.m_el[i]);
665 }
666 
668 {
669  for (int i=0;i<3;i++)
670  m_el[i].serializeFloat(dataOut.m_el[i]);
671 }
672 
673 
675 {
676  for (int i=0;i<3;i++)
677  m_el[i].deSerialize(dataIn.m_el[i]);
678 }
679 
681 {
682  for (int i=0;i<3;i++)
683  m_el[i].deSerializeFloat(dataIn.m_el[i]);
684 }
685 
687 {
688  for (int i=0;i<3;i++)
689  m_el[i].deSerializeDouble(dataIn.m_el[i]);
690 }
691 
692 }
693 #endif //TF2_MATRIX3x3_H
694 
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:591
d
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
tf2Scalar tf2Scalar unsigned int solution_number
Definition: Matrix3x3.h:282
void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll) __attribute__((deprecated))
Set the matrix from euler angles using YPR around ZYX respectively.
Definition: Matrix3x3.h:166
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:215
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
Definition: Matrix3x3.h:364
TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
Definition: Matrix3x3.h:639
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
void getOpenGLSubMatrix(tf2Scalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:225
tf2Scalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:532
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:655
void setFromOpenGLSubMatrix(const tf2Scalar *m)
Set from a carray of tf2Scalars.
Definition: Matrix3x3.h:118
Matrix3x3(const tf2Scalar &xx, const tf2Scalar &xy, const tf2Scalar &xz, const tf2Scalar &yx, const tf2Scalar &yy, const tf2Scalar &yz, const tf2Scalar &zx, const tf2Scalar &zy, const tf2Scalar &zz)
Constructor with row major formatting.
Definition: Matrix3x3.h:52
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:556
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:576
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:674
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:686
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
Definition: Matrix3x3.h:43
TF2SIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:89
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x)
Definition: Scalar.h:178
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:390
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:539
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:401
#define TF2SIMD_PI
Definition: Scalar.h:193
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:393
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:548
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:38
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:397
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:203
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:160
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:31
void diagonalize(Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: Matrix3x3.h:416
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:680
TF2SIMD_FORCE_INLINE const Vector3 & operator[](int i) const
Get a const reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:105
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
Definition: Scalar.h:183
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:661
TF2SIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:97
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:185
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: Matrix3x3.h:34
for serialization
Definition: Matrix3x3.h:647
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:504
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:372
for serialization
Definition: Matrix3x3.h:653
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:146
unsigned int step
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:601
void setValue(const tf2Scalar &xx, const tf2Scalar &xy, const tf2Scalar &xz, const tf2Scalar &yx, const tf2Scalar &yy, const tf2Scalar &yz, const tf2Scalar &zx, const tf2Scalar &zy, const tf2Scalar &zz)
Set the values of the matrix explicitly (row major)
Definition: Matrix3x3.h:135
TF2SIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:61
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:243
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:180
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:564
TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:446
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
Definition: Matrix3x3.h:523
tf2Scalar tf2Scalar & roll
Definition: Matrix3x3.h:282
__attribute__((deprecated)) void getEulerZYX(tf2Scalar &yaw
Get the matrix represented as euler angles around ZYX.
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: Matrix3x3.h:81
#define Matrix3x3Data
Definition: Matrix3x3.h:26
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:667
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:649
TF2SIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
Definition: Matrix3x3.h:70
#define tf2FullAssert(x)
Definition: Scalar.h:148
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
void getEulerYPR(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const
Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR.
Definition: Matrix3x3.h:292
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:208
tf2Scalar & pitch
Definition: Matrix3x3.h:282


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jan 4 2018 03:35:01