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 
36  Vector3 m_el[3];
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  }
63  TFSIMD_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  TFSIMD_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  TFSIMD_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  TFSIMD_FORCE_INLINE const Vector3& getRow(int i) const
92  {
93  tfFullAssert(0 <= i && i < 3);
94  return m_el[i];
95  }
96 
99  TFSIMD_FORCE_INLINE Vector3& operator[](int i)
100  {
101  tfFullAssert(0 <= i && i < 3);
102  return m_el[i];
103  }
104 
107  TFSIMD_FORCE_INLINE const Vector3& operator[](int i) const
108  {
109  tfFullAssert(0 <= i && i < 3);
110  return m_el[i];
111  }
112 
116  Matrix3x3& operator*=(const Matrix3x3& m);
117 
120  void setFromOpenGLSubMatrix(const tfScalar *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 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 
396  TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3& v) const
397  {
398  return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
399  }
400  TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3& v) const
401  {
402  return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
403  }
404  TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3& v) const
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 
525 TFSIMD_FORCE_INLINE Matrix3x3&
526 Matrix3x3::operator*=(const Matrix3x3& m)
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 
534 TFSIMD_FORCE_INLINE tfScalar
535 Matrix3x3::determinant() const
536 {
537  return tfTriple((*this)[0], (*this)[1], (*this)[2]);
538 }
539 
540 
541 TFSIMD_FORCE_INLINE Matrix3x3
542 Matrix3x3::absolute() const
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 
550 TFSIMD_FORCE_INLINE Matrix3x3
551 Matrix3x3::transpose() const
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 
558 TFSIMD_FORCE_INLINE Matrix3x3
559 Matrix3x3::adjoint() const
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 
566 TFSIMD_FORCE_INLINE Matrix3x3
567 Matrix3x3::inverse() const
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 
578 TFSIMD_FORCE_INLINE Matrix3x3
579 Matrix3x3::transposeTimes(const Matrix3x3& m) const
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 
593 TFSIMD_FORCE_INLINE Matrix3x3
594 Matrix3x3::timesTranspose(const Matrix3x3& m) const
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 
603 TFSIMD_FORCE_INLINE Vector3
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 
610 TFSIMD_FORCE_INLINE Vector3
611 operator*(const Vector3& v, const Matrix3x3& m)
612 {
613  return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
614 }
615 
616 TFSIMD_FORCE_INLINE Matrix3x3
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 
642 TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
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 
650 struct Matrix3x3FloatData
651 {
652  Vector3FloatData m_el[3];
653 };
654 
656 struct Matrix3x3DoubleData
657 {
658  Vector3DoubleData m_el[3];
659 };
660 
661 
662 
663 
664 TFSIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
665 {
666  for (int i=0;i<3;i++)
667  m_el[i].serialize(dataOut.m_el[i]);
668 }
669 
670 TFSIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
671 {
672  for (int i=0;i<3;i++)
673  m_el[i].serializeFloat(dataOut.m_el[i]);
674 }
675 
676 
677 TFSIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
678 {
679  for (int i=0;i<3;i++)
680  m_el[i].deSerialize(dataIn.m_el[i]);
681 }
682 
683 TFSIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
684 {
685  for (int i=0;i<3;i++)
686  m_el[i].deSerializeFloat(dataIn.m_el[i]);
687 }
688 
689 TFSIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn)
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 
tf::Matrix3x3::getColumn
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: Matrix3x3.h:83
d
d
tf::Matrix3x3::transposeTimes
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:579
tf::operator*
TFSIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:604
tf::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:245
tf::Matrix3x3::Matrix3x3
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
tf::Matrix3x3::absolute
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:542
tf::Matrix3x3::setEulerZYX
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
tf::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
tf::Matrix3x3::scaled
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:375
tf::Matrix3x3::deSerializeDouble
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:689
tfScalar
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
tf::Matrix3x3::diagonalize
void diagonalize(Matrix3x3 &rot, tfScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: Matrix3x3.h:419
tf::operator==
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
tf::Matrix3x3::adjoint
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:559
tf::Matrix3x3::inverse
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:567
s
XmlRpcServer s
TFSIMD_EPSILON
#define TFSIMD_EPSILON
Definition: Scalar.h:204
tf::Matrix3x3::transpose
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:551
tf::Matrix3x3::setEulerYPR
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:182
tf::tfTriple
TFSIMD_FORCE_INLINE tfScalar tfTriple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:446
tf::Matrix3x3::serialize
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:664
tf::Matrix3x3::operator=
TFSIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
Definition: Matrix3x3.h:72
tf
Definition: exceptions.h:38
tf::Matrix3x3::setValue
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
tf::Matrix3x3::getEulerYPR
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
tf::Matrix3x3
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:33
tf::Matrix3x3::determinant
tfScalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:535
tf::Matrix3x3FloatData
for serialization
Definition: Matrix3x3.h:650
tf::Matrix3x3::tdotz
TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:404
tf::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:148
tf::Matrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:210
tf::Matrix3x3::cofac
tfScalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:507
tfCos
TFSIMD_FORCE_INLINE tfScalar tfCos(tfScalar x)
Definition: Scalar.h:181
tf::Matrix3x3::serializeFloat
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:670
tf::Matrix3x3DoubleData
for serialization
Definition: Matrix3x3.h:656
tf::Matrix3x3::operator[]
TFSIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:99
tf::Matrix3x3::Matrix3x3
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:40
tf::Matrix3x3::setRPY
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:205
tf::Matrix3x3::tdotx
TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:396
tf::Matrix3x3::operator*=
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
Definition: Matrix3x3.h:526
tf::Matrix3x3::timesTranspose
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:594
TFSIMD_FORCE_INLINE
#define TFSIMD_FORCE_INLINE
Definition: Scalar.h:130
tf::Quaternion::length2
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:163
tfAtan2
TFSIMD_FORCE_INLINE tfScalar tfAtan2(tfScalar x, tfScalar y)
Definition: Scalar.h:187
tf::Matrix3x3::deSerializeFloat
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:683
tf::Matrix3x3DoubleData::m_el
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:658
tf::dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:392
ROS_DEPRECATED
#define ROS_DEPRECATED
tf::Matrix3x3::getEulerZYX
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
tfSqrt
TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x)
Definition: Scalar.h:179
step
unsigned int step
tf::Matrix3x3::operator[]
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
TFSIMD_PI
#define TFSIMD_PI
Definition: Scalar.h:195
tf::Matrix3x3::tdoty
TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:400
tf::Matrix3x3FloatData::m_el
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:652
tf::Matrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(tfScalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:227
tf::Matrix3x3::getRow
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:91
tfAsin
TFSIMD_FORCE_INLINE tfScalar tfAsin(tfScalar x)
Definition: Scalar.h:185
tf::Matrix3x3::setFromOpenGLSubMatrix
void setFromOpenGLSubMatrix(const tfScalar *m)
Set from a carray of tfScalars.
Definition: Matrix3x3.h:120
tf::Matrix3x3::getRPY
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
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
tfSin
TFSIMD_FORCE_INLINE tfScalar tfSin(tfScalar x)
Definition: Scalar.h:182
tfFabs
TFSIMD_FORCE_INLINE tfScalar tfFabs(tfScalar x)
Definition: Scalar.h:180
Matrix3x3Data
#define Matrix3x3Data
Definition: Matrix3x3.h:28
tfFullAssert
#define tfFullAssert(x)
Definition: Scalar.h:149
tf::Matrix3x3::deSerialize
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:677
tf::Matrix3x3::m_el
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: Matrix3x3.h:36
tf::Matrix3x3::Matrix3x3
TFSIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:63
tf::Matrix3x3::getIdentity
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:217
tf::Matrix3x3::Matrix3x3
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