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  }
61  TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
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 
70  TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
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 
97  TF2SIMD_FORCE_INLINE Vector3& operator[](int i)
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 
118  void setFromOpenGLSubMatrix(const tf2Scalar *m)
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 
203  void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) {
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 
225  void getOpenGLSubMatrix(tf2Scalar *m) const
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;
297  tf2Scalar pitch;
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 
522 TF2SIMD_FORCE_INLINE Matrix3x3&
523 Matrix3x3::operator*=(const Matrix3x3& m)
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 
531 TF2SIMD_FORCE_INLINE tf2Scalar
532 Matrix3x3::determinant() const
533 {
534  return tf2Triple((*this)[0], (*this)[1], (*this)[2]);
535 }
536 
537 
538 TF2SIMD_FORCE_INLINE Matrix3x3
539 Matrix3x3::absolute() const
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 
547 TF2SIMD_FORCE_INLINE Matrix3x3
548 Matrix3x3::transpose() const
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 
555 TF2SIMD_FORCE_INLINE Matrix3x3
556 Matrix3x3::adjoint() const
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 
563 TF2SIMD_FORCE_INLINE Matrix3x3
564 Matrix3x3::inverse() const
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 
575 TF2SIMD_FORCE_INLINE Matrix3x3
576 Matrix3x3::transposeTimes(const Matrix3x3& m) const
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 
590 TF2SIMD_FORCE_INLINE Matrix3x3
591 Matrix3x3::timesTranspose(const Matrix3x3& m) const
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 
613 TF2SIMD_FORCE_INLINE Matrix3x3
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 
639 TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
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 
647 struct Matrix3x3FloatData
648 {
649  Vector3FloatData m_el[3];
650 };
651 
653 struct Matrix3x3DoubleData
654 {
655  Vector3DoubleData m_el[3];
656 };
657 
658 
659 
660 
661 TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
662 {
663  for (int i=0;i<3;i++)
664  m_el[i].serialize(dataOut.m_el[i]);
665 }
666 
667 TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
668 {
669  for (int i=0;i<3;i++)
670  m_el[i].serializeFloat(dataOut.m_el[i]);
671 }
672 
673 
674 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
675 {
676  for (int i=0;i<3;i++)
677  m_el[i].deSerialize(dataIn.m_el[i]);
678 }
679 
680 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
681 {
682  for (int i=0;i<3;i++)
683  m_el[i].deSerializeFloat(dataIn.m_el[i]);
684 }
685 
686 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn)
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 
tf2::Matrix3x3::timesTranspose
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:591
d
d
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
tf2::Matrix3x3::solution_number
tf2Scalar tf2Scalar unsigned int solution_number
Definition: Matrix3x3.h:282
tf2::Matrix3x3::setEulerZYX
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
tf2::Matrix3x3::getIdentity
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:215
tf2::Matrix3x3::getRPY
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
tf2::operator==
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
tf2Cos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
tf2::Matrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(tf2Scalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:225
tf2::Matrix3x3::determinant
tf2Scalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:532
tf2::Matrix3x3DoubleData::m_el
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:655
tf2::Matrix3x3::setFromOpenGLSubMatrix
void setFromOpenGLSubMatrix(const tf2Scalar *m)
Set from a carray of tf2Scalars.
Definition: Matrix3x3.h:118
tf2::Matrix3x3::Matrix3x3
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
tf2::Matrix3x3::adjoint
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:556
tf2::Matrix3x3::transposeTimes
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:576
tf2::Matrix3x3::deSerialize
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:674
tf2::Matrix3x3::deSerializeDouble
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:686
tf2::Matrix3x3::Matrix3x3
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
Definition: Matrix3x3.h:43
tf2::Matrix3x3::getRow
TF2SIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:89
tf2Scalar
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
tf2Fabs
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x)
Definition: Scalar.h:178
tf2::dot
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:390
tf2::Matrix3x3::absolute
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:539
tf2::Matrix3x3::tdotz
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:401
TF2SIMD_PI
#define TF2SIMD_PI
Definition: Scalar.h:193
tf2::Matrix3x3::tdotx
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:393
tf2::Matrix3x3::transpose
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:548
tf2::Matrix3x3::Matrix3x3
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:38
tf2::Matrix3x3::tdoty
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:397
tf2::Matrix3x3::setRPY
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:203
tf2::Quaternion::length2
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:160
tf2::Matrix3x3
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:31
tf2::Matrix3x3::diagonalize
void diagonalize(Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: Matrix3x3.h:416
tf2::Matrix3x3::deSerializeFloat
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:680
tf2::Matrix3x3::operator[]
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
tf2Asin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
Definition: Scalar.h:183
tf2::Matrix3x3::serialize
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:661
tf2::Matrix3x3::operator[]
TF2SIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:97
tf2Atan2
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:185
tf2::Matrix3x3::m_el
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: Matrix3x3.h:34
tf2::Matrix3x3FloatData
for serialization
Definition: Matrix3x3.h:647
tf2::Matrix3x3::cofac
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:504
tf2::Matrix3x3::scaled
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:372
tf2::Matrix3x3DoubleData
for serialization
Definition: Matrix3x3.h:653
tf2Sin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
tf2Sqrt
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
TF2SIMD_EPSILON
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
tf2::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:146
step
unsigned int step
Definition: cache_unittest.cpp:41
tf2::operator*
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:601
tf2::Matrix3x3::setValue
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
tf2::Matrix3x3::Matrix3x3
TF2SIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:61
tf2::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:243
tf2::Matrix3x3::setEulerYPR
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:180
tf2::Matrix3x3::inverse
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:564
tf2::tf2Triple
TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:446
tf2::Matrix3x3::operator*=
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
Definition: Matrix3x3.h:523
tf2::Matrix3x3::roll
tf2Scalar tf2Scalar & roll
Definition: Matrix3x3.h:282
tf2::Matrix3x3::__attribute__
__attribute__((deprecated)) void getEulerZYX(tf2Scalar &yaw
Get the matrix represented as euler angles around ZYX.
tf2::Matrix3x3::getColumn
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: Matrix3x3.h:81
Matrix3x3Data
#define Matrix3x3Data
Definition: Matrix3x3.h:26
tf2::Matrix3x3::serializeFloat
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:667
tf2::Matrix3x3FloatData::m_el
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:649
tf2::Matrix3x3::operator=
TF2SIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
Definition: Matrix3x3.h:70
tf2FullAssert
#define tf2FullAssert(x)
Definition: Scalar.h:148
tf2::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
tf2::Matrix3x3::getEulerYPR
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
tf2::Matrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:208
tf2::Matrix3x3::pitch
tf2Scalar & pitch
Definition: Matrix3x3.h:282

tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:50