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


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Feb 28 2022 22:26:19