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 namespace tf
23 {
24 
25 
26 #define Matrix3x3Data Matrix3x3DoubleData
27 
28 
31 class Matrix3x3 {
32 
34  Vector3 m_el[3];
35 
36 public:
38  Matrix3x3 () {}
39 
40  // explicit Matrix3x3(const tfScalar *m) { setFromOpenGLSubMatrix(m); }
41 
43  explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
44  /*
45  template <typename tfScalar>
46  Matrix3x3(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
47  {
48  setEulerYPR(yaw, pitch, roll);
49  }
50  */
52  Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
53  const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
54  const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
55  {
56  setValue(xx, xy, xz,
57  yx, yy, yz,
58  zx, zy, zz);
59  }
61  TFSIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
62  {
63  m_el[0] = other.m_el[0];
64  m_el[1] = other.m_el[1];
65  m_el[2] = other.m_el[2];
66  }
67 
68 
70  TFSIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
71  {
72  m_el[0] = other.m_el[0];
73  m_el[1] = other.m_el[1];
74  m_el[2] = other.m_el[2];
75  return *this;
76  }
77 
78 
81  TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
82  {
83  return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
84  }
85 
86 
89  TFSIMD_FORCE_INLINE const Vector3& getRow(int i) const
90  {
91  tfFullAssert(0 <= i && i < 3);
92  return m_el[i];
93  }
94 
97  TFSIMD_FORCE_INLINE Vector3& operator[](int i)
98  {
99  tfFullAssert(0 <= i && i < 3);
100  return m_el[i];
101  }
102 
105  TFSIMD_FORCE_INLINE const Vector3& operator[](int i) const
106  {
107  tfFullAssert(0 <= i && i < 3);
108  return m_el[i];
109  }
110 
114  Matrix3x3& operator*=(const Matrix3x3& m);
115 
118  void setFromOpenGLSubMatrix(const tfScalar *m)
119  {
120  m_el[0].setValue(m[0],m[4],m[8]);
121  m_el[1].setValue(m[1],m[5],m[9]);
122  m_el[2].setValue(m[2],m[6],m[10]);
123 
124  }
135  void setValue(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
136  const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
137  const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
138  {
139  m_el[0].setValue(xx,xy,xz);
140  m_el[1].setValue(yx,yy,yz);
141  m_el[2].setValue(zx,zy,zz);
142  }
143 
146  void setRotation(const Quaternion& q)
147  {
148  tfScalar d = q.length2();
149  tfFullAssert(d != tfScalar(0.0));
150  tfScalar s = tfScalar(2.0) / d;
151  tfScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
152  tfScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
153  tfScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
154  tfScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
155  setValue(tfScalar(1.0) - (yy + zz), xy - wz, xz + wy,
156  xy + wz, tfScalar(1.0) - (xx + zz), yz - wx,
157  xz - wy, yz + wx, tfScalar(1.0) - (xx + yy));
158  }
159 
160 
166  void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
167  {
168  setEulerYPR(yaw, pitch, roll);
169  }
170 
180  void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) {
181  tfScalar ci ( tfCos(eulerX));
182  tfScalar cj ( tfCos(eulerY));
183  tfScalar ch ( tfCos(eulerZ));
184  tfScalar si ( tfSin(eulerX));
185  tfScalar sj ( tfSin(eulerY));
186  tfScalar sh ( tfSin(eulerZ));
187  tfScalar cc = ci * ch;
188  tfScalar cs = ci * sh;
189  tfScalar sc = si * ch;
190  tfScalar ss = si * sh;
191 
192  setValue(cj * ch, sj * sc - cs, sj * cc + ss,
193  cj * sh, sj * ss + cc, sj * cs - sc,
194  -sj, cj * si, cj * ci);
195  }
196 
203  void setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw) {
204  setEulerYPR(yaw, pitch, roll);
205  }
206 
208  void setIdentity()
209  {
210  setValue(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0),
211  tfScalar(0.0), tfScalar(1.0), tfScalar(0.0),
212  tfScalar(0.0), tfScalar(0.0), tfScalar(1.0));
213  }
214 
215  static const Matrix3x3& getIdentity()
216  {
217  static const Matrix3x3 identityMatrix(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0),
218  tfScalar(0.0), tfScalar(1.0), tfScalar(0.0),
219  tfScalar(0.0), tfScalar(0.0), tfScalar(1.0));
220  return identityMatrix;
221  }
222 
225  void getOpenGLSubMatrix(tfScalar *m) const
226  {
227  m[0] = tfScalar(m_el[0].x());
228  m[1] = tfScalar(m_el[1].x());
229  m[2] = tfScalar(m_el[2].x());
230  m[3] = tfScalar(0.0);
231  m[4] = tfScalar(m_el[0].y());
232  m[5] = tfScalar(m_el[1].y());
233  m[6] = tfScalar(m_el[2].y());
234  m[7] = tfScalar(0.0);
235  m[8] = tfScalar(m_el[0].z());
236  m[9] = tfScalar(m_el[1].z());
237  m[10] = tfScalar(m_el[2].z());
238  m[11] = tfScalar(0.0);
239  }
240 
243  void getRotation(Quaternion& q) const
244  {
245  tfScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
246  tfScalar temp[4];
247 
248  if (trace > tfScalar(0.0))
249  {
250  tfScalar s = tfSqrt(trace + tfScalar(1.0));
251  temp[3]=(s * tfScalar(0.5));
252  s = tfScalar(0.5) / s;
253 
254  temp[0]=((m_el[2].y() - m_el[1].z()) * s);
255  temp[1]=((m_el[0].z() - m_el[2].x()) * s);
256  temp[2]=((m_el[1].x() - m_el[0].y()) * s);
257  }
258  else
259  {
260  int i = m_el[0].x() < m_el[1].y() ?
261  (m_el[1].y() < m_el[2].z() ? 2 : 1) :
262  (m_el[0].x() < m_el[2].z() ? 2 : 0);
263  int j = (i + 1) % 3;
264  int k = (i + 2) % 3;
265 
266  tfScalar s = tfSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tfScalar(1.0));
267  temp[i] = s * tfScalar(0.5);
268  s = tfScalar(0.5) / s;
269 
270  temp[3] = (m_el[k][j] - m_el[j][k]) * s;
271  temp[j] = (m_el[j][i] + m_el[i][j]) * s;
272  temp[k] = (m_el[k][i] + m_el[i][k]) * s;
273  }
274  q.setValue(temp[0],temp[1],temp[2],temp[3]);
275  }
276 
282  __attribute__((deprecated)) void getEulerZYX(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
283  {
284  getEulerYPR(yaw, pitch, roll, solution_number);
285  };
286 
287 
292  void getEulerYPR(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
293  {
294  struct Euler
295  {
296  tfScalar yaw;
297  tfScalar pitch;
298  tfScalar roll;
299  };
300 
301  Euler euler_out;
302  Euler euler_out2; //second solution
303  //get the pointer to the raw data
304 
305  // Check that pitch is not at a singularity
306  // Check that pitch is not at a singularity
307  if (tfFabs(m_el[2].x()) >= 1)
308  {
309  euler_out.yaw = 0;
310  euler_out2.yaw = 0;
311 
312  // From difference of angles formula
313  if (m_el[2].x() < 0) //gimbal locked down
314  {
315  tfScalar delta = tfAtan2(m_el[0].y(),m_el[0].z());
316  euler_out.pitch = TFSIMD_PI / tfScalar(2.0);
317  euler_out2.pitch = TFSIMD_PI / tfScalar(2.0);
318  euler_out.roll = delta;
319  euler_out2.roll = delta;
320  }
321  else // gimbal locked up
322  {
323  tfScalar delta = tfAtan2(-m_el[0].y(),-m_el[0].z());
324  euler_out.pitch = -TFSIMD_PI / tfScalar(2.0);
325  euler_out2.pitch = -TFSIMD_PI / tfScalar(2.0);
326  euler_out.roll = delta;
327  euler_out2.roll = delta;
328  }
329  }
330  else
331  {
332  euler_out.pitch = - tfAsin(m_el[2].x());
333  euler_out2.pitch = TFSIMD_PI - euler_out.pitch;
334 
335  euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch),
336  m_el[2].z()/tfCos(euler_out.pitch));
337  euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch),
338  m_el[2].z()/tfCos(euler_out2.pitch));
339 
340  euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch),
341  m_el[0].x()/tfCos(euler_out.pitch));
342  euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch),
343  m_el[0].x()/tfCos(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(tfScalar& roll, tfScalar& pitch, tfScalar& 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  tfScalar 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  TFSIMD_FORCE_INLINE tfScalar 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  TFSIMD_FORCE_INLINE tfScalar 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  TFSIMD_FORCE_INLINE tfScalar 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, tfScalar 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  tfScalar max = tfFabs(m_el[0][1]);
427  tfScalar v = tfFabs(m_el[0][2]);
428  if (v > max)
429  {
430  q = 2;
431  r = 1;
432  max = v;
433  }
434  v = tfFabs(m_el[1][2]);
435  if (v > max)
436  {
437  p = 1;
438  q = 2;
439  r = 0;
440  max = v;
441  }
442 
443  tfScalar t = threshold * (tfFabs(m_el[0][0]) + tfFabs(m_el[1][1]) + tfFabs(m_el[2][2]));
444  if (max <= t)
445  {
446  if (max <= TFSIMD_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  tfScalar mpq = m_el[p][q];
455  tfScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
456  tfScalar theta2 = theta * theta;
457  tfScalar cos;
458  tfScalar sin;
459  if (theta2 * theta2 < tfScalar(10 / TFSIMD_EPSILON))
460  {
461  t = (theta >= 0) ? 1 / (theta + tfSqrt(1 + theta2))
462  : 1 / (theta - tfSqrt(1 + theta2));
463  cos = 1 / tfSqrt(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 + tfScalar(0.5) / theta2));
470  cos = 1 - tfScalar(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  tfScalar mrp = m_el[r][p];
479  tfScalar 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  tfScalar 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 TFSIMD_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 TFSIMD_FORCE_INLINE tfScalar
533 Matrix3x3::determinant() const
534 {
535  return tfTriple((*this)[0], (*this)[1], (*this)[2]);
536 }
537 
538 
539 TFSIMD_FORCE_INLINE Matrix3x3
540 Matrix3x3::absolute() const
541 {
542  return Matrix3x3(
543  tfFabs(m_el[0].x()), tfFabs(m_el[0].y()), tfFabs(m_el[0].z()),
544  tfFabs(m_el[1].x()), tfFabs(m_el[1].y()), tfFabs(m_el[1].z()),
545  tfFabs(m_el[2].x()), tfFabs(m_el[2].y()), tfFabs(m_el[2].z()));
546 }
547 
548 TFSIMD_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 TFSIMD_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 TFSIMD_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  tfScalar det = (*this)[0].dot(co);
569  tfFullAssert(det != tfScalar(0.0));
570  tfScalar s = tfScalar(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 TFSIMD_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 TFSIMD_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 TFSIMD_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 TFSIMD_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 TFSIMD_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 TFSIMD_FORCE_INLINE Matrix3x3 tfMultTransposeLeft(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 TFSIMD_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 TFSIMD_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 TFSIMD_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 TFSIMD_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 TFSIMD_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 TFSIMD_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 
695 #endif //TF_MATRIX3x3_H
696 
d
d
tf::Matrix3x3::setEulerZYX
void setEulerZYX(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll) __attribute__((deprecated))
Set the matrix from euler angles using YPR around ZYX respectively.
Definition: Matrix3x3.h:166
tf::operator*
TFSIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:602
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:105
tf::Matrix3x3::serializeFloat
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:668
tf::Matrix3x3::roll
tfScalar tfScalar & roll
Definition: Matrix3x3.h:282
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:52
tf::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
tf::Matrix3x3::__attribute__
__attribute__((deprecated)) void getEulerZYX(tfScalar &yaw
Get the matrix represented as euler angles around ZYX.
tf::Matrix3x3::tdoty
TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:398
tf::Matrix3x3::deSerializeDouble
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:687
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:417
tf::Quaternion::length2
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:161
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:292
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:640
s
XmlRpcServer s
TFSIMD_EPSILON
#define TFSIMD_EPSILON
Definition: Scalar.h:204
tf::Matrix3x3::setEulerYPR
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:180
tf::tfTriple
TFSIMD_FORCE_INLINE tfScalar tfTriple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:446
tf::Matrix3x3::operator=
TFSIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
Definition: Matrix3x3.h:70
tf
Definition: exceptions.h:38
tf::Matrix3x3::inverse
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:565
tf::Matrix3x3::cofac
tfScalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:505
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:135
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
tf::Matrix3x3
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:31
tf::Vector3::setValue
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
Definition: Vector3.h:308
tf::Matrix3x3FloatData
for serialization
Definition: Matrix3x3.h:648
tf::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:243
tf::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:146
tf::Matrix3x3::tdotz
TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:402
tf::Matrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:208
tf::Matrix3x3::getRow
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:89
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
tf::Matrix3x3::solution_number
tfScalar tfScalar unsigned int solution_number
Definition: Matrix3x3.h:282
tfCos
TFSIMD_FORCE_INLINE tfScalar tfCos(tfScalar x)
Definition: Scalar.h:181
tf::Matrix3x3DoubleData
for serialization
Definition: Matrix3x3.h:654
tf::Vector3::z
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:267
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:97
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:365
tf::Matrix3x3::scaled
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:373
tf::Matrix3x3::Matrix3x3
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:38
tf::Matrix3x3::setRPY
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:203
tf::Matrix3x3::operator*=
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
Definition: Matrix3x3.h:524
Vector3
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
Definition: Vector3.h:293
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
TFSIMD_FORCE_INLINE
#define TFSIMD_FORCE_INLINE
Definition: Scalar.h:130
tf::Matrix3x3::transposeTimes
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:577
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
tf::Matrix3x3::pitch
tfScalar & pitch
Definition: Matrix3x3.h:282
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:681
tf::Matrix3x3DoubleData::m_el
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:656
tf::dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:390
tf::Matrix3x3::absolute
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:540
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
tfSqrt
TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x)
Definition: Scalar.h:179
step
unsigned int step
tf::Matrix3x3::timesTranspose
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:592
TFSIMD_PI
#define TFSIMD_PI
Definition: Scalar.h:195
tf::Matrix3x3FloatData::m_el
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:650
tfAsin
TFSIMD_FORCE_INLINE tfScalar tfAsin(tfScalar x)
Definition: Scalar.h:185
tf::Matrix3x3::tdotx
TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:394
tf::Matrix3x3::setFromOpenGLSubMatrix
void setFromOpenGLSubMatrix(const tfScalar *m)
Set from a carray of tfScalars.
Definition: Matrix3x3.h:118
tf::Matrix3x3::determinant
tfScalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:533
tfSin
TFSIMD_FORCE_INLINE tfScalar tfSin(tfScalar x)
Definition: Scalar.h:182
tf::Matrix3x3::adjoint
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:557
tfFabs
TFSIMD_FORCE_INLINE tfScalar tfFabs(tfScalar x)
Definition: Scalar.h:180
Matrix3x3Data
#define Matrix3x3Data
Definition: Matrix3x3.h:26
tf::Matrix3x3::transpose
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:549
tfFullAssert
#define tfFullAssert(x)
Definition: Scalar.h:149
tf::Matrix3x3::deSerialize
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:675
tf::Matrix3x3::m_el
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: Matrix3x3.h:34
tf::Matrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(tfScalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:225
tf::Matrix3x3::Matrix3x3
TFSIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:61
tf::Matrix3x3::getIdentity
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:215
tf::Vector3
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38
tf::Matrix3x3::serialize
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:662
tf::Matrix3x3::getColumn
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: Matrix3x3.h:81
tf::Matrix3x3::Matrix3x3
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
Definition: Matrix3x3.h:43

tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:28