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

tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sun Feb 4 2024 03:18:11