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 
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  }
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 
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 
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 
98  {
99  tfFullAssert(0 <= i && i < 3);
100  return m_el[i];
101  }
102 
106  {
107  tfFullAssert(0 <= i && i < 3);
108  return m_el[i];
109  }
110 
114  Matrix3x3& operator*=(const Matrix3x3& m);
115 
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 
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 
395  {
396  return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
397  }
399  {
400  return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
401  }
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 
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 
534 {
535  return tfTriple((*this)[0], (*this)[1], (*this)[2]);
536 }
537 
538 
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 
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 
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 
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 
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 
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 
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 
609 operator*(const Vector3& v, const Matrix3x3& m)
610 {
611  return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
612 }
613 
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 
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 
649 {
651 };
652 
655 {
657 };
658 
659 
660 
661 
663 {
664  for (int i=0;i<3;i++)
665  m_el[i].serialize(dataOut.m_el[i]);
666 }
667 
669 {
670  for (int i=0;i<3;i++)
671  m_el[i].serializeFloat(dataOut.m_el[i]);
672 }
673 
674 
676 {
677  for (int i=0;i<3;i++)
678  m_el[i].deSerialize(dataIn.m_el[i]);
679 }
680 
682 {
683  for (int i=0;i<3;i++)
684  m_el[i].deSerializeFloat(dataIn.m_el[i]);
685 }
686 
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
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
TFSIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:602
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
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:668
tfScalar tfScalar & roll
Definition: Matrix3x3.h:282
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
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
__attribute__((deprecated)) void getEulerZYX(tfScalar &yaw
Get the matrix represented as euler angles around ZYX.
TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:398
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:687
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
void diagonalize(Matrix3x3 &rot, tfScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: Matrix3x3.h:417
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:161
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
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
XmlRpcServer s
#define TFSIMD_EPSILON
Definition: Scalar.h:204
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:180
TFSIMD_FORCE_INLINE tfScalar tfTriple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:446
TFSIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
Definition: Matrix3x3.h:70
Definition: exceptions.h:38
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:565
tfScalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:505
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
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:31
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
Definition: Vector3.h:308
for serialization
Definition: Matrix3x3.h:648
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:243
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:146
TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:402
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:208
TFSIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:89
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
tfScalar tfScalar unsigned int solution_number
Definition: Matrix3x3.h:282
TFSIMD_FORCE_INLINE tfScalar tfCos(tfScalar x)
Definition: Scalar.h:181
for serialization
Definition: Matrix3x3.h:654
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:267
TFSIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:97
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
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:373
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:38
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:203
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
Definition: Matrix3x3.h:524
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
Definition: Vector3.h:293
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
#define TFSIMD_FORCE_INLINE
Definition: Scalar.h:130
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:577
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
tfScalar & pitch
Definition: Matrix3x3.h:282
TFSIMD_FORCE_INLINE tfScalar tfAtan2(tfScalar x, tfScalar y)
Definition: Scalar.h:187
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:681
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:656
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:390
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:540
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x)
Definition: Scalar.h:179
unsigned int step
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:592
#define TFSIMD_PI
Definition: Scalar.h:195
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:650
TFSIMD_FORCE_INLINE tfScalar tfAsin(tfScalar x)
Definition: Scalar.h:185
TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:394
void setFromOpenGLSubMatrix(const tfScalar *m)
Set from a carray of tfScalars.
Definition: Matrix3x3.h:118
tfScalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:533
TFSIMD_FORCE_INLINE tfScalar tfSin(tfScalar x)
Definition: Scalar.h:182
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:557
TFSIMD_FORCE_INLINE tfScalar tfFabs(tfScalar x)
Definition: Scalar.h:180
#define Matrix3x3Data
Definition: Matrix3x3.h:26
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:549
#define tfFullAssert(x)
Definition: Scalar.h:149
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:675
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
Definition: Matrix3x3.h:34
void getOpenGLSubMatrix(tfScalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:225
TFSIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:61
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:215
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
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:662
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: Matrix3x3.h:81
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