tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
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 "tf2/visibility_control.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  TF2_PUBLIC
41  Matrix3x3 () {}
42 
43  // explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); }
44 
46  TF2_PUBLIC
47  explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
48  /*
49  template <typename tf2Scalar>
50  Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
51  {
52  setEulerYPR(yaw, pitch, roll);
53  }
54  */
56  TF2_PUBLIC
57  Matrix3x3(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
58  const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
59  const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
60  {
61  setValue(xx, xy, xz,
62  yx, yy, yz,
63  zx, zy, zz);
64  }
66  TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
67  {
68  m_el[0] = other.m_el[0];
69  m_el[1] = other.m_el[1];
70  m_el[2] = other.m_el[2];
71  }
72 
73 
75  TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
76  {
77  m_el[0] = other.m_el[0];
78  m_el[1] = other.m_el[1];
79  m_el[2] = other.m_el[2];
80  return *this;
81  }
82 
83 
86  TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
87  {
88  return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
89  }
90 
91 
94  TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const
95  {
96  tf2FullAssert(0 <= i && i < 3);
97  return m_el[i];
98  }
99 
102  TF2SIMD_FORCE_INLINE Vector3& operator[](int i)
103  {
104  tf2FullAssert(0 <= i && i < 3);
105  return m_el[i];
106  }
107 
110  TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const
111  {
112  tf2FullAssert(0 <= i && i < 3);
113  return m_el[i];
114  }
115 
119  TF2_PUBLIC
120  Matrix3x3& operator*=(const Matrix3x3& m);
121 
124  TF2_PUBLIC
125  void setFromOpenGLSubMatrix(const tf2Scalar *m)
126  {
127  m_el[0].setValue(m[0],m[4],m[8]);
128  m_el[1].setValue(m[1],m[5],m[9]);
129  m_el[2].setValue(m[2],m[6],m[10]);
130 
131  }
142  TF2_PUBLIC
143  void setValue(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
144  const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
145  const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
146  {
147  m_el[0].setValue(xx,xy,xz);
148  m_el[1].setValue(yx,yy,yz);
149  m_el[2].setValue(zx,zy,zz);
150  }
151 
154  TF2_PUBLIC
155  void setRotation(const Quaternion& q)
156  {
157  tf2Scalar d = q.length2();
158  tf2FullAssert(d != tf2Scalar(0.0));
159  tf2Scalar s = tf2Scalar(2.0) / d;
160  tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
161  tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
162  tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
163  tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
164  setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
165  xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx,
166  xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy));
167  }
168 
178  TF2_PUBLIC
179  void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) {
180  tf2Scalar ci ( tf2Cos(eulerX));
181  tf2Scalar cj ( tf2Cos(eulerY));
182  tf2Scalar ch ( tf2Cos(eulerZ));
183  tf2Scalar si ( tf2Sin(eulerX));
184  tf2Scalar sj ( tf2Sin(eulerY));
185  tf2Scalar sh ( tf2Sin(eulerZ));
186  tf2Scalar cc = ci * ch;
187  tf2Scalar cs = ci * sh;
188  tf2Scalar sc = si * ch;
189  tf2Scalar ss = si * sh;
190 
191  setValue(cj * ch, sj * sc - cs, sj * cc + ss,
192  cj * sh, sj * ss + cc, sj * cs - sc,
193  -sj, cj * si, cj * ci);
194  }
195 
202  TF2_PUBLIC
203  void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) {
204  setEulerYPR(yaw, pitch, roll);
205  }
206 
208  TF2_PUBLIC
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  TF2_PUBLIC
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  TF2_PUBLIC
228  void getOpenGLSubMatrix(tf2Scalar *m) const
229  {
230  m[0] = tf2Scalar(m_el[0].x());
231  m[1] = tf2Scalar(m_el[1].x());
232  m[2] = tf2Scalar(m_el[2].x());
233  m[3] = tf2Scalar(0.0);
234  m[4] = tf2Scalar(m_el[0].y());
235  m[5] = tf2Scalar(m_el[1].y());
236  m[6] = tf2Scalar(m_el[2].y());
237  m[7] = tf2Scalar(0.0);
238  m[8] = tf2Scalar(m_el[0].z());
239  m[9] = tf2Scalar(m_el[1].z());
240  m[10] = tf2Scalar(m_el[2].z());
241  m[11] = tf2Scalar(0.0);
242  }
243 
246  TF2_PUBLIC
247  void getRotation(Quaternion& q) const
248  {
249  tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
250  tf2Scalar temp[4];
251 
252  if (trace > tf2Scalar(0.0))
253  {
254  tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0));
255  temp[3]=(s * tf2Scalar(0.5));
256  s = tf2Scalar(0.5) / s;
257 
258  temp[0]=((m_el[2].y() - m_el[1].z()) * s);
259  temp[1]=((m_el[0].z() - m_el[2].x()) * s);
260  temp[2]=((m_el[1].x() - m_el[0].y()) * s);
261  }
262  else
263  {
264  int i = m_el[0].x() < m_el[1].y() ?
265  (m_el[1].y() < m_el[2].z() ? 2 : 1) :
266  (m_el[0].x() < m_el[2].z() ? 2 : 0);
267  int j = (i + 1) % 3;
268  int k = (i + 2) % 3;
269 
270  tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0));
271  temp[i] = s * tf2Scalar(0.5);
272  s = tf2Scalar(0.5) / s;
273 
274  temp[3] = (m_el[k][j] - m_el[j][k]) * s;
275  temp[j] = (m_el[j][i] + m_el[i][j]) * s;
276  temp[k] = (m_el[k][i] + m_el[i][k]) * s;
277  }
278  q.setValue(temp[0],temp[1],temp[2],temp[3]);
279  }
280 
285  TF2_PUBLIC
286  void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
287  {
288  struct Euler
289  {
290  tf2Scalar yaw;
291  tf2Scalar pitch;
292  tf2Scalar roll;
293  };
294 
295  Euler euler_out;
296  Euler euler_out2; //second solution
297  //get the pointer to the raw data
298 
299  // Check that pitch is not at a singularity
300  // Check that pitch is not at a singularity
301  if (tf2Fabs(m_el[2].x()) >= 1)
302  {
303  euler_out.yaw = 0;
304  euler_out2.yaw = 0;
305 
306  // From difference of angles formula
307  tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z());
308  if (m_el[2].x() < 0) //gimbal locked down
309  {
310  euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0);
311  euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0);
312  euler_out.roll = delta;
313  euler_out2.roll = delta;
314  }
315  else // gimbal locked up
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  }
323  else
324  {
325  euler_out.pitch = - tf2Asin(m_el[2].x());
326  euler_out2.pitch = TF2SIMD_PI - euler_out.pitch;
327 
328  euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch),
329  m_el[2].z()/tf2Cos(euler_out.pitch));
330  euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch),
331  m_el[2].z()/tf2Cos(euler_out2.pitch));
332 
333  euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch),
334  m_el[0].x()/tf2Cos(euler_out.pitch));
335  euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch),
336  m_el[0].x()/tf2Cos(euler_out2.pitch));
337  }
338 
339  if (solution_number == 1)
340  {
341  yaw = euler_out.yaw;
342  pitch = euler_out.pitch;
343  roll = euler_out.roll;
344  }
345  else
346  {
347  yaw = euler_out2.yaw;
348  pitch = euler_out2.pitch;
349  roll = euler_out2.roll;
350  }
351  }
352 
358  TF2_PUBLIC
359  void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const
360  {
361  getEulerYPR(yaw, pitch, roll, solution_number);
362  }
363 
367  TF2_PUBLIC
368  Matrix3x3 scaled(const Vector3& s) const
369  {
370  return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
371  m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
372  m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
373  }
374 
376  TF2_PUBLIC
377  tf2Scalar determinant() const;
379  TF2_PUBLIC
380  Matrix3x3 adjoint() const;
382  TF2_PUBLIC
383  Matrix3x3 absolute() const;
385  TF2_PUBLIC
386  Matrix3x3 transpose() const;
388  TF2_PUBLIC
389  Matrix3x3 inverse() const;
390 
391  TF2_PUBLIC
392  Matrix3x3 transposeTimes(const Matrix3x3& m) const;
393  TF2_PUBLIC
394  Matrix3x3 timesTranspose(const Matrix3x3& m) const;
395 
396  TF2SIMD_FORCE_INLINE tf2Scalar 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  TF2SIMD_FORCE_INLINE tf2Scalar 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  TF2SIMD_FORCE_INLINE tf2Scalar 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  TF2_PUBLIC
420  void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps)
421  {
422  rot.setIdentity();
423  for (int step = maxSteps; step > 0; step--)
424  {
425  // find off-diagonal element [p][q] with largest magnitude
426  int p = 0;
427  int q = 1;
428  int r = 2;
429  tf2Scalar max = tf2Fabs(m_el[0][1]);
430  tf2Scalar v = tf2Fabs(m_el[0][2]);
431  if (v > max)
432  {
433  q = 2;
434  r = 1;
435  max = v;
436  }
437  v = tf2Fabs(m_el[1][2]);
438  if (v > max)
439  {
440  p = 1;
441  q = 2;
442  r = 0;
443  max = v;
444  }
445 
446  tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2]));
447  if (max <= t)
448  {
449  if (max <= TF2SIMD_EPSILON * t)
450  {
451  return;
452  }
453  step = 1;
454  }
455 
456  // compute Jacobi rotation J which leads to a zero for element [p][q]
457  tf2Scalar mpq = m_el[p][q];
458  tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
459  tf2Scalar theta2 = theta * theta;
460  tf2Scalar cos;
461  tf2Scalar sin;
462  if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON))
463  {
464  t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2))
465  : 1 / (theta - tf2Sqrt(1 + theta2));
466  cos = 1 / tf2Sqrt(1 + t * t);
467  sin = cos * t;
468  }
469  else
470  {
471  // approximation for large theta-value, i.e., a nearly diagonal matrix
472  t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2));
473  cos = 1 - tf2Scalar(0.5) * t * t;
474  sin = cos * t;
475  }
476 
477  // apply rotation to matrix (this = J^T * this * J)
478  m_el[p][q] = m_el[q][p] = 0;
479  m_el[p][p] -= t * mpq;
480  m_el[q][q] += t * mpq;
481  tf2Scalar mrp = m_el[r][p];
482  tf2Scalar mrq = m_el[r][q];
483  m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
484  m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
485 
486  // apply rotation to rot (rot = rot * J)
487  for (int i = 0; i < 3; i++)
488  {
489  Vector3& row = rot[i];
490  mrp = row[p];
491  mrq = row[q];
492  row[p] = cos * mrp - sin * mrq;
493  row[q] = cos * mrq + sin * mrp;
494  }
495  }
496  }
497 
498 
499 
500 
508  TF2_PUBLIC
509  tf2Scalar cofac(int r1, int c1, int r2, int c2) const
510  {
511  return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
512  }
513 
514  TF2_PUBLIC
515  void serialize(struct Matrix3x3Data& dataOut) const;
516 
517  TF2_PUBLIC
518  void serializeFloat(struct Matrix3x3FloatData& dataOut) const;
519 
520  TF2_PUBLIC
521  void deSerialize(const struct Matrix3x3Data& dataIn);
522 
523  TF2_PUBLIC
524  void deSerializeFloat(const struct Matrix3x3FloatData& dataIn);
525 
526  TF2_PUBLIC
527  void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn);
528 
529 };
530 
531 
532 TF2SIMD_FORCE_INLINE Matrix3x3&
533 Matrix3x3::operator*=(const Matrix3x3& m)
534 {
535  setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
536  m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
537  m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
538  return *this;
539 }
540 
541 TF2SIMD_FORCE_INLINE tf2Scalar
542 Matrix3x3::determinant() const
543 {
544  return tf2Triple((*this)[0], (*this)[1], (*this)[2]);
545 }
546 
547 
548 TF2SIMD_FORCE_INLINE Matrix3x3
549 Matrix3x3::absolute() const
550 {
551  return Matrix3x3(
552  tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()),
553  tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()),
554  tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z()));
555 }
556 
557 TF2SIMD_FORCE_INLINE Matrix3x3
558 Matrix3x3::transpose() const
559 {
560  return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
561  m_el[0].y(), m_el[1].y(), m_el[2].y(),
562  m_el[0].z(), m_el[1].z(), m_el[2].z());
563 }
564 
565 TF2SIMD_FORCE_INLINE Matrix3x3
566 Matrix3x3::adjoint() const
567 {
568  return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
569  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
570  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
571 }
572 
573 TF2SIMD_FORCE_INLINE Matrix3x3
574 Matrix3x3::inverse() const
575 {
576  Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
577  tf2Scalar det = (*this)[0].dot(co);
578  tf2FullAssert(det != tf2Scalar(0.0));
579  tf2Scalar s = tf2Scalar(1.0) / det;
580  return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
581  co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
582  co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
583 }
584 
585 TF2SIMD_FORCE_INLINE Matrix3x3
586 Matrix3x3::transposeTimes(const Matrix3x3& m) const
587 {
588  return Matrix3x3(
589  m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
590  m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
591  m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
592  m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
593  m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
594  m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
595  m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
596  m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
597  m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
598 }
599 
600 TF2SIMD_FORCE_INLINE Matrix3x3
601 Matrix3x3::timesTranspose(const Matrix3x3& m) const
602 {
603  return Matrix3x3(
604  m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
605  m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
606  m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
607 
608 }
609 
610 TF2SIMD_FORCE_INLINE Vector3
611 operator*(const Matrix3x3& m, const Vector3& v)
612 {
613  return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
614 }
615 
616 
617 TF2SIMD_FORCE_INLINE Vector3
618 operator*(const Vector3& v, const Matrix3x3& m)
619 {
620  return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
621 }
622 
623 TF2SIMD_FORCE_INLINE Matrix3x3
624 operator*(const Matrix3x3& m1, const Matrix3x3& m2)
625 {
626  return Matrix3x3(
627  m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
628  m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
629  m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
630 }
631 
632 /*
633 TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) {
634 return Matrix3x3(
635 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
636 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
637 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
638 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
639 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
640 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
641 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
642 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
643 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
644 }
645 */
646 
649 TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
650 {
651  return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
652  m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
653  m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
654 }
655 
657 struct Matrix3x3FloatData
658 {
659  Vector3FloatData m_el[3];
660 };
661 
663 struct Matrix3x3DoubleData
664 {
665  Vector3DoubleData m_el[3];
666 };
667 
668 
669 
670 
671 TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
672 {
673  for (int i=0;i<3;i++)
674  m_el[i].serialize(dataOut.m_el[i]);
675 }
676 
677 TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
678 {
679  for (int i=0;i<3;i++)
680  m_el[i].serializeFloat(dataOut.m_el[i]);
681 }
682 
683 
684 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
685 {
686  for (int i=0;i<3;i++)
687  m_el[i].deSerialize(dataIn.m_el[i]);
688 }
689 
690 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
691 {
692  for (int i=0;i<3;i++)
693  m_el[i].deSerializeFloat(dataIn.m_el[i]);
694 }
695 
696 TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn)
697 {
698  for (int i=0;i<3;i++)
699  m_el[i].deSerializeDouble(dataIn.m_el[i]);
700 }
701 
702 }
703 #endif //TF2_MATRIX3x3_H
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:286
tf2::dot
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:395
tf2::Matrix3x3::diagonalize
void diagonalize(Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: Matrix3x3.h:420
tf2::Matrix3x3::setRPY
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:203
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:533
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:86
tf2::Matrix3x3::Matrix3x3
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:41
tf2::Matrix3x3::deSerializeFloat
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:690
tf2::Matrix3x3::inverse
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:574
tf2::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:247
tf2::Matrix3x3::tdotz
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:404
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:75
tf2::Vector3DoubleData
Definition: Vector3.h:686
tf2Sqrt
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
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:228
tf2::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:155
tf2::Matrix3x3::transposeTimes
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:586
tf2::Matrix3x3::tdotx
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:396
tf2::Vector3FloatData
Definition: Vector3.h:681
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:143
tf2::Matrix3x3::Matrix3x3
TF2SIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:66
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:359
tf2FullAssert
#define tf2FullAssert(x)
Definition: Scalar.h:148
tf2::Matrix3x3FloatData
for serialization
Definition: Matrix3x3.h:657
tf2::Matrix3x3::deSerialize
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:684
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:649
tf2::Matrix3x3::deSerializeDouble
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:696
tf2::Matrix3x3::setEulerYPR
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:179
tf2::operator*
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:611
tf2::Matrix3x3::Matrix3x3
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
Definition: Matrix3x3.h:47
tf2::Matrix3x3::absolute
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:549
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
Quaternion.h
tf2::Matrix3x3DoubleData
for serialization
Definition: Matrix3x3.h:663
visibility_control.h
tf2::Matrix3x3::getRow
const TF2SIMD_FORCE_INLINE Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:94
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:209
tf2::Matrix3x3::scaled
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:368
tf2::Matrix3x3::determinant
tf2Scalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:542
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:110
tf2::Matrix3x3DoubleData::m_el
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:665
tf2::tf2Triple
TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:449
Vector3.h
tf2
Definition: buffer_core.h:53
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:125
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:57
tf2::Matrix3x3::serializeFloat
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:677
TF2_PUBLIC
#define TF2_PUBLIC
Definition: visibility_control.h:57
tf2::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:29
tf2::Matrix3x3::timesTranspose
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:601
tf2::Matrix3x3::serialize
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:671
tf2::Matrix3x3::transpose
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:558
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:659
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:102
tf2::Matrix3x3::adjoint
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:566
tf2::Matrix3x3::tdoty
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:400
tf2::Matrix3x3::cofac
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:509
tf2::Quaternion::length2
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:152
Generated by   doxygen 1.8.17