Matrix3x3.h
Go to the documentation of this file.
1 #include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */
2 /*
3 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 
17 #ifndef TF2_MATRIX3x3_H
18 #define TF2_MATRIX3x3_H
19 
20 #include "Vector3.h"
21 #include "Quaternion.h"
22 
23 namespace tf2
24 {
25 
26 
27 #define Matrix3x3Data Matrix3x3DoubleData
28 
29 
32 class Matrix3x3 {
33 
35  Vector3 m_el[3];
36 
37 public:
39  Matrix3x3 () {}
40 
41  // explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); }
42 
44  explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
45  /*
46  template <typename tf2Scalar>
47  Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
48  {
49  setEulerYPR(yaw, pitch, roll);
50  }
51  */
53  Matrix3x3(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
54  const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
55  const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
56  {
57  setValue(xx, xy, xz,
58  yx, yy, yz,
59  zx, zy, zz);
60  }
62  TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
63  {
64  m_el[0] = other.m_el[0];
65  m_el[1] = other.m_el[1];
66  m_el[2] = other.m_el[2];
67  }
68 
69 
71  TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
72  {
73  m_el[0] = other.m_el[0];
74  m_el[1] = other.m_el[1];
75  m_el[2] = other.m_el[2];
76  return *this;
77  }
78 
79 
82  TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
83  {
84  return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
85  }
86 
87 
90  TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const
91  {
92  tf2FullAssert(0 <= i && i < 3);
93  return m_el[i];
94  }
95 
98  TF2SIMD_FORCE_INLINE Vector3& operator[](int i)
99  {
100  tf2FullAssert(0 <= i && i < 3);
101  return m_el[i];
102  }
103 
106  TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const
107  {
108  tf2FullAssert(0 <= i && i < 3);
109  return m_el[i];
110  }
111 
115  Matrix3x3& operator*=(const Matrix3x3& m);
116 
119  void setFromOpenGLSubMatrix(const tf2Scalar *m)
120  {
121  m_el[0].setValue(m[0],m[4],m[8]);
122  m_el[1].setValue(m[1],m[5],m[9]);
123  m_el[2].setValue(m[2],m[6],m[10]);
124 
125  }
136  void setValue(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
137  const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
138  const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
139  {
140  m_el[0].setValue(xx,xy,xz);
141  m_el[1].setValue(yx,yy,yz);
142  m_el[2].setValue(zx,zy,zz);
143  }
144 
147  void setRotation(const Quaternion& q)
148  {
149  tf2Scalar d = q.length2();
150  tf2FullAssert(d != tf2Scalar(0.0));
151  tf2Scalar s = tf2Scalar(2.0) / d;
152  tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
153  tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
154  tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
155  tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
156  setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
157  xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx,
158  xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy));
159  }
160 
161 
167  void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated))
168  {
169  setEulerYPR(yaw, pitch, roll);
170  }
171 
181  void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) {
182  tf2Scalar ci ( tf2Cos(eulerX));
183  tf2Scalar cj ( tf2Cos(eulerY));
184  tf2Scalar ch ( tf2Cos(eulerZ));
185  tf2Scalar si ( tf2Sin(eulerX));
186  tf2Scalar sj ( tf2Sin(eulerY));
187  tf2Scalar sh ( tf2Sin(eulerZ));
188  tf2Scalar cc = ci * ch;
189  tf2Scalar cs = ci * sh;
190  tf2Scalar sc = si * ch;
191  tf2Scalar ss = si * sh;
192 
193  setValue(cj * ch, sj * sc - cs, sj * cc + ss,
194  cj * sh, sj * ss + cc, sj * cs - sc,
195  -sj, cj * si, cj * ci);
196  }
197 
204  void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) {
205  setEulerYPR(yaw, pitch, roll);
206  }
207 
209  void setIdentity()
210  {
211  setValue(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0),
212  tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0),
213  tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0));
214  }
215 
216  static const Matrix3x3& getIdentity()
217  {
218  static const Matrix3x3 identityMatrix(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0),
219  tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0),
220  tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0));
221  return identityMatrix;
222  }
223 
226  void getOpenGLSubMatrix(tf2Scalar *m) const
227  {
228  m[0] = tf2Scalar(m_el[0].x());
229  m[1] = tf2Scalar(m_el[1].x());
230  m[2] = tf2Scalar(m_el[2].x());
231  m[3] = tf2Scalar(0.0);
232  m[4] = tf2Scalar(m_el[0].y());
233  m[5] = tf2Scalar(m_el[1].y());
234  m[6] = tf2Scalar(m_el[2].y());
235  m[7] = tf2Scalar(0.0);
236  m[8] = tf2Scalar(m_el[0].z());
237  m[9] = tf2Scalar(m_el[1].z());
238  m[10] = tf2Scalar(m_el[2].z());
239  m[11] = tf2Scalar(0.0);
240  }
241 
244  void getRotation(Quaternion& q) const
245  {
246  tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
247  tf2Scalar temp[4];
248 
249  if (trace > tf2Scalar(0.0))
250  {
251  tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0));
252  temp[3]=(s * tf2Scalar(0.5));
253  s = tf2Scalar(0.5) / s;
254 
255  temp[0]=((m_el[2].y() - m_el[1].z()) * s);
256  temp[1]=((m_el[0].z() - m_el[2].x()) * s);
257  temp[2]=((m_el[1].x() - m_el[0].y()) * s);
258  }
259  else
260  {
261  int i = m_el[0].x() < m_el[1].y() ?
262  (m_el[1].y() < m_el[2].z() ? 2 : 1) :
263  (m_el[0].x() < m_el[2].z() ? 2 : 0);
264  int j = (i + 1) % 3;
265  int k = (i + 2) % 3;
266 
267  tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0));
268  temp[i] = s * tf2Scalar(0.5);
269  s = tf2Scalar(0.5) / s;
270 
271  temp[3] = (m_el[k][j] - m_el[j][k]) * s;
272  temp[j] = (m_el[j][i] + m_el[i][j]) * s;
273  temp[k] = (m_el[k][i] + m_el[i][k]) * s;
274  }
275  q.setValue(temp[0],temp[1],temp[2],temp[3]);
276  }
277 
283  __attribute__((deprecated)) void getEulerZYX(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
284  {
285  getEulerYPR(yaw, pitch, roll, solution_number);
286  };
287 
288 
293  void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
294  {
295  struct Euler
296  {
297  tf2Scalar yaw;
298  tf2Scalar pitch;
299  tf2Scalar roll;
300  };
301 
302  Euler euler_out;
303  Euler euler_out2; //second solution
304  //get the pointer to the raw data
305 
306  // Check that pitch is not at a singularity
307  // Check that pitch is not at a singularity
308  if (tf2Fabs(m_el[2].x()) >= 1)
309  {
310  euler_out.yaw = 0;
311  euler_out2.yaw = 0;
312 
313  // From difference of angles formula
314  tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z());
315  if (m_el[2].x() < 0) //gimbal locked down
316  {
317  euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0);
318  euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0);
319  euler_out.roll = delta;
320  euler_out2.roll = delta;
321  }
322  else // gimbal locked up
323  {
324  euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
325  euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
326  euler_out.roll = delta;
327  euler_out2.roll = delta;
328  }
329  }
330  else
331  {
332  euler_out.pitch = - tf2Asin(m_el[2].x());
333  euler_out2.pitch = TF2SIMD_PI - euler_out.pitch;
334 
335  euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch),
336  m_el[2].z()/tf2Cos(euler_out.pitch));
337  euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch),
338  m_el[2].z()/tf2Cos(euler_out2.pitch));
339 
340  euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch),
341  m_el[0].x()/tf2Cos(euler_out.pitch));
342  euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch),
343  m_el[0].x()/tf2Cos(euler_out2.pitch));
344  }
345 
346  if (solution_number == 1)
347  {
348  yaw = euler_out.yaw;
349  pitch = euler_out.pitch;
350  roll = euler_out.roll;
351  }
352  else
353  {
354  yaw = euler_out2.yaw;
355  pitch = euler_out2.pitch;
356  roll = euler_out2.roll;
357  }
358  }
359 
365  void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const
366  {
367  getEulerYPR(yaw, pitch, roll, solution_number);
368  }
369 
373  Matrix3x3 scaled(const Vector3& s) const
374  {
375  return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
376  m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
377  m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
378  }
379 
381  tf2Scalar determinant() const;
383  Matrix3x3 adjoint() const;
385  Matrix3x3 absolute() const;
387  Matrix3x3 transpose() const;
389  Matrix3x3 inverse() const;
390 
391  Matrix3x3 transposeTimes(const Matrix3x3& m) const;
392  Matrix3x3 timesTranspose(const Matrix3x3& m) const;
393 
394  TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3& v) const
395  {
396  return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
397  }
398  TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3& v) const
399  {
400  return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
401  }
402  TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3& v) const
403  {
404  return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
405  }
406 
407 
417  void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps)
418  {
419  rot.setIdentity();
420  for (int step = maxSteps; step > 0; step--)
421  {
422  // find off-diagonal element [p][q] with largest magnitude
423  int p = 0;
424  int q = 1;
425  int r = 2;
426  tf2Scalar max = tf2Fabs(m_el[0][1]);
427  tf2Scalar v = tf2Fabs(m_el[0][2]);
428  if (v > max)
429  {
430  q = 2;
431  r = 1;
432  max = v;
433  }
434  v = tf2Fabs(m_el[1][2]);
435  if (v > max)
436  {
437  p = 1;
438  q = 2;
439  r = 0;
440  max = v;
441  }
442 
443  tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2]));
444  if (max <= t)
445  {
446  if (max <= TF2SIMD_EPSILON * t)
447  {
448  return;
449  }
450  step = 1;
451  }
452 
453  // compute Jacobi rotation J which leads to a zero for element [p][q]
454  tf2Scalar mpq = m_el[p][q];
455  tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
456  tf2Scalar theta2 = theta * theta;
457  tf2Scalar cos;
458  tf2Scalar sin;
459  if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON))
460  {
461  t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2))
462  : 1 / (theta - tf2Sqrt(1 + theta2));
463  cos = 1 / tf2Sqrt(1 + t * t);
464  sin = cos * t;
465  }
466  else
467  {
468  // approximation for large theta-value, i.e., a nearly diagonal matrix
469  t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2));
470  cos = 1 - tf2Scalar(0.5) * t * t;
471  sin = cos * t;
472  }
473 
474  // apply rotation to matrix (this = J^T * this * J)
475  m_el[p][q] = m_el[q][p] = 0;
476  m_el[p][p] -= t * mpq;
477  m_el[q][q] += t * mpq;
478  tf2Scalar mrp = m_el[r][p];
479  tf2Scalar mrq = m_el[r][q];
480  m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
481  m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
482 
483  // apply rotation to rot (rot = rot * J)
484  for (int i = 0; i < 3; i++)
485  {
486  Vector3& row = rot[i];
487  mrp = row[p];
488  mrq = row[q];
489  row[p] = cos * mrp - sin * mrq;
490  row[q] = cos * mrq + sin * mrp;
491  }
492  }
493  }
494 
495 
496 
497 
505  tf2Scalar cofac(int r1, int c1, int r2, int c2) const
506  {
507  return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
508  }
509 
510  void serialize(struct Matrix3x3Data& dataOut) const;
511 
512  void serializeFloat(struct Matrix3x3FloatData& dataOut) const;
513 
514  void deSerialize(const struct Matrix3x3Data& dataIn);
515 
516  void deSerializeFloat(const struct Matrix3x3FloatData& dataIn);
517 
518  void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn);
519 
520 };
521 
522 
523 TF2SIMD_FORCE_INLINE Matrix3x3&
524 Matrix3x3::operator*=(const Matrix3x3& m)
525 {
526  setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
527  m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
528  m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
529  return *this;
530 }
531 
532 TF2SIMD_FORCE_INLINE tf2Scalar
533 Matrix3x3::determinant() const
534 {
535  return tf2Triple((*this)[0], (*this)[1], (*this)[2]);
536 }
537 
538 
539 TF2SIMD_FORCE_INLINE Matrix3x3
540 Matrix3x3::absolute() const
541 {
542  return Matrix3x3(
543  tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()),
544  tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()),
545  tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z()));
546 }
547 
548 TF2SIMD_FORCE_INLINE Matrix3x3
549 Matrix3x3::transpose() const
550 {
551  return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
552  m_el[0].y(), m_el[1].y(), m_el[2].y(),
553  m_el[0].z(), m_el[1].z(), m_el[2].z());
554 }
555 
556 TF2SIMD_FORCE_INLINE Matrix3x3
557 Matrix3x3::adjoint() const
558 {
559  return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
560  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
561  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
562 }
563 
564 TF2SIMD_FORCE_INLINE Matrix3x3
565 Matrix3x3::inverse() const
566 {
567  Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
568  tf2Scalar det = (*this)[0].dot(co);
569  tf2FullAssert(det != tf2Scalar(0.0));
570  tf2Scalar s = tf2Scalar(1.0) / det;
571  return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
572  co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
573  co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
574 }
575 
576 TF2SIMD_FORCE_INLINE Matrix3x3
577 Matrix3x3::transposeTimes(const Matrix3x3& m) const
578 {
579  return Matrix3x3(
580  m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
581  m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
582  m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
583  m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
584  m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
585  m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
586  m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
587  m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
588  m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
589 }
590 
591 TF2SIMD_FORCE_INLINE Matrix3x3
592 Matrix3x3::timesTranspose(const Matrix3x3& m) const
593 {
594  return Matrix3x3(
595  m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
596  m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
597  m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
598 
599 }
600 
601 TF2SIMD_FORCE_INLINE Vector3
602 operator*(const Matrix3x3& m, const Vector3& v)
603 {
604  return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
605 }
606 
607 
608 TF2SIMD_FORCE_INLINE Vector3
609 operator*(const Vector3& v, const Matrix3x3& m)
610 {
611  return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
612 }
613 
614 TF2SIMD_FORCE_INLINE Matrix3x3
615 operator*(const Matrix3x3& m1, const Matrix3x3& m2)
616 {
617  return Matrix3x3(
618  m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
619  m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
620  m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
621 }
622 
623 /*
624 TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) {
625 return Matrix3x3(
626 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
627 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
628 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
629 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
630 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
631 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
632 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
633 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
634 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
635 }
636 */
637 
640 TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
641 {
642  return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
643  m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
644  m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
645 }
646 
648 struct Matrix3x3FloatData
649 {
650  Vector3FloatData m_el[3];
651 };
652 
654 struct Matrix3x3DoubleData
655 {
656  Vector3DoubleData m_el[3];
657 };
658 
659 
660 
661 
662 TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
663 {
664  for (int i=0;i<3;i++)
665  m_el[i].serialize(dataOut.m_el[i]);
666 }
667 
668 TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
669 {
670  for (int i=0;i<3;i++)
671  m_el[i].serializeFloat(dataOut.m_el[i]);
672 }
673 
674 
675 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
676 {
677  for (int i=0;i<3;i++)
678  m_el[i].deSerialize(dataIn.m_el[i]);
679 }
680 
681 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
682 {
683  for (int i=0;i<3;i++)
684  m_el[i].deSerializeFloat(dataIn.m_el[i]);
685 }
686 
687 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn)
688 {
689  for (int i=0;i<3;i++)
690  m_el[i].deSerializeDouble(dataIn.m_el[i]);
691 }
692 
693 }
694 #endif //TF2_MATRIX3x3_H
695 
tf2::Matrix3x3::getEulerYPR
void getEulerYPR(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const
tf2::dot
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:391
geometry_msgs::Vector3
::geometry_msgs::Vector3_< std::allocator< void > > Vector3
Definition: kinetic/include/geometry_msgs/Vector3.h:58
tf2::Matrix3x3::diagonalize
void diagonalize(Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
tf2::Matrix3x3::setRPY
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
tf2Asin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
Definition: Scalar.h:184
tf2::Matrix3x3::operator*=
Matrix3x3 & operator*=(const Matrix3x3 &m)
Definition: Matrix3x3.h:524
tf2Atan2
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:186
tf2::Matrix3x3::getColumn
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
tf2::Matrix3x3::Matrix3x3
Matrix3x3()
tf2::Matrix3x3::deSerializeFloat
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:681
tf2::Matrix3x3::inverse
Matrix3x3 inverse() const
Definition: Matrix3x3.h:565
tf2::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
s
XmlRpcServer s
tf2::Matrix3x3::tdotz
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
TF2SIMD_EPSILON
#define TF2SIMD_EPSILON
Definition: Scalar.h:203
tf2Sin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:181
tf2::Matrix3x3::operator=
TF2SIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
tf2Sqrt
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:178
step
unsigned int step
tf2::Matrix3x3::getIdentity
static const Matrix3x3 & getIdentity()
tf2::Matrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(tf2Scalar *m) const
tf2::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
tf2::Matrix3x3::transposeTimes
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:577
tf2::Matrix3x3::tdotx
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
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)
tf2::Matrix3x3::getRPY
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
tf2FullAssert
#define tf2FullAssert(x)
Definition: Scalar.h:149
tf2::Matrix3x3::setEulerZYX
ROS_DEPRECATED void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
tf2::Matrix3x3::deSerialize
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:675
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:640
tf2::Matrix3x3::deSerializeDouble
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:687
tf2::Matrix3x3::setEulerYPR
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
tf2::operator*
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:602
Vector3
tf2::Matrix3x3::absolute
Matrix3x3 absolute() const
Definition: Matrix3x3.h:540
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:130
d
d
tf2::Matrix3x3::getRow
const TF2SIMD_FORCE_INLINE Vector3 & getRow(int i) const
tf2Cos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:180
tf2::Matrix3x3::setIdentity
void setIdentity()
tf2::Matrix3x3::scaled
Matrix3x3 scaled(const Vector3 &s) const
tf2::Matrix3x3::determinant
tf2Scalar determinant() const
Definition: Matrix3x3.h:533
Matrix3x3Data
#define Matrix3x3Data
Definition: Matrix3x3.h:27
tf2::Matrix3x3DoubleData::m_el
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:656
tf2::tf2Triple
TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: melodic/include/tf2/LinearMath/Vector3.h:447
tf2
geometry_msgs::Quaternion
::geometry_msgs::Quaternion_< std::allocator< void > > Quaternion
Definition: kinetic/include/geometry_msgs/Quaternion.h:63
tf2Fabs
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x)
Definition: Scalar.h:179
tf2::Matrix3x3::setFromOpenGLSubMatrix
void setFromOpenGLSubMatrix(const tf2Scalar *m)
tf2::Matrix3x3::serializeFloat
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:668
tf2::Matrix3x3::getEulerZYX
ROS_DEPRECATED void getEulerZYX(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const
sick_scan_base.h
tf2::Matrix3x3::m_el
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: Matrix3x3.h:35
tf2::Matrix3x3::timesTranspose
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:592
tf2::Matrix3x3::serialize
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:662
tf2::Matrix3x3::transpose
Matrix3x3 transpose() const
Definition: Matrix3x3.h:549
tf2Scalar
double tf2Scalar
TF2SIMD_PI
#define TF2SIMD_PI
Definition: Scalar.h:194
tf2::Matrix3x3FloatData::m_el
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:650
Quaternion.h
tf2::Matrix3x3::operator[]
TF2SIMD_FORCE_INLINE Vector3 & operator[](int i)
tf2::Matrix3x3::adjoint
Matrix3x3 adjoint() const
Definition: Matrix3x3.h:557
tf2::Matrix3x3::tdoty
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
tf2::Matrix3x3::cofac
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
t
geometry_msgs::TransformStamped t
Vector3.h

sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:09