All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Matrix4.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef _IGNITION_MATRIX4_HH_
18 #define _IGNITION_MATRIX4_HH_
19 
20 #include <ignition/math/Helpers.hh>
22 #include <ignition/math/Matrix3.hh>
23 #include <ignition/math/Vector3.hh>
24 #include <ignition/math/Pose3.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
32  template<typename T>
33  class Matrix4
34  {
36  public: static const Matrix4<T> Identity;
37 
39  public: static const Matrix4<T> Zero;
40 
42  public: Matrix4()
43  {
44  memset(this->data, 0, sizeof(this->data[0][0])*16);
45  }
46 
49  public: Matrix4(const Matrix4<T> &_m)
50  {
51  memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
52  }
53 
71  public: Matrix4(T _v00, T _v01, T _v02, T _v03,
72  T _v10, T _v11, T _v12, T _v13,
73  T _v20, T _v21, T _v22, T _v23,
74  T _v30, T _v31, T _v32, T _v33)
75  {
76  this->Set(_v00, _v01, _v02, _v03,
77  _v10, _v11, _v12, _v13,
78  _v20, _v21, _v22, _v23,
79  _v30, _v31, _v32, _v33);
80  }
81 
84  public: Matrix4(const Quaternion<T> &_q)
85  {
86  Quaternion<T> qt = _q;
87  qt.Normalize();
88  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
89  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
90  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
91  0,
92 
93  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
94  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
95  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
96  0,
97 
98  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
99  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
100  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
101  0,
102 
103  0, 0, 0, 1);
104  }
105 
108  public: Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
109  {
110  this->Translate(_pose.Pos());
111  }
112 
114  public: virtual ~Matrix4() {}
115 
133  public: void Set(
134  T _v00, T _v01, T _v02, T _v03,
135  T _v10, T _v11, T _v12, T _v13,
136  T _v20, T _v21, T _v22, T _v23,
137  T _v30, T _v31, T _v32, T _v33)
138  {
139  this->data[0][0] = _v00;
140  this->data[0][1] = _v01;
141  this->data[0][2] = _v02;
142  this->data[0][3] = _v03;
143 
144  this->data[1][0] = _v10;
145  this->data[1][1] = _v11;
146  this->data[1][2] = _v12;
147  this->data[1][3] = _v13;
148 
149  this->data[2][0] = _v20;
150  this->data[2][1] = _v21;
151  this->data[2][2] = _v22;
152  this->data[2][3] = _v23;
153 
154  this->data[3][0] = _v30;
155  this->data[3][1] = _v31;
156  this->data[3][2] = _v32;
157  this->data[3][3] = _v33;
158  }
159 
163  public: void Axis(const Vector3<T> &_axis, T _angle)
164  {
165  T c = cos(_angle);
166  T s = sin(_angle);
167  T C = 1-c;
168 
169  this->data[0][0] = _axis.X()*_axis.X()*C + c;
170  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
171  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
172 
173  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
174  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
175  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
176 
177  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
178  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
179  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
180  }
181 
184  public: void Translate(const Vector3<T> &_t)
185  {
186  this->data[0][3] = _t.X();
187  this->data[1][3] = _t.Y();
188  this->data[2][3] = _t.Z();
189  }
190 
195  public: void Translate(T _x, T _y, T _z)
196  {
197  this->data[0][3] = _x;
198  this->data[1][3] = _y;
199  this->data[2][3] = _z;
200  }
201 
204  public: Vector3<T> Translation() const
205  {
206  return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
207  }
208 
211  public: Vector3<T> Scale() const
212  {
213  return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
214  }
215 
218  public: Quaternion<T> Rotation() const
219  {
220  Quaternion<T> q;
223  T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
224  T root;
225  if (trace > 0)
226  {
227  root = sqrt(trace + 1.0);
228  q.W(root / 2.0);
229  root = 1.0 / (2.0 * root);
230  q.X((this->data[2][1] - this->data[1][2]) * root);
231  q.Y((this->data[0][2] - this->data[2][0]) * root);
232  q.Z((this->data[1][0] - this->data[0][1]) * root);
233  }
234  else
235  {
236  static unsigned int s_iNext[3] = {1, 2, 0};
237  unsigned int i = 0;
238  if (this->data[1][1] > this->data[0][0])
239  i = 1;
240  if (this->data[2][2] > this->data[i][i])
241  i = 2;
242  unsigned int j = s_iNext[i];
243  unsigned int k = s_iNext[j];
244 
245  root = sqrt(this->data[i][i] - this->data[j][j] -
246  this->data[k][k] + 1.0);
247 
248  T a, b, c;
249  a = root / 2.0;
250  root = 1.0 / (2.0 * root);
251  b = (this->data[j][i] + this->data[i][j]) * root;
252  c = (this->data[k][i] + this->data[i][k]) * root;
253 
254  switch (i)
255  {
256  default:
257  case 0: q.X(a); break;
258  case 1: q.Y(a); break;
259  case 2: q.Z(a); break;
260  };
261  switch (j)
262  {
263  default:
264  case 0: q.X(b); break;
265  case 1: q.Y(b); break;
266  case 2: q.Z(b); break;
267  };
268  switch (k)
269  {
270  default:
271  case 0: q.X(c); break;
272  case 1: q.Y(c); break;
273  case 2: q.Z(c); break;
274  };
275 
276  q.W((this->data[k][j] - this->data[j][k]) * root);
277  }
278 
279  return q;
280  }
281 
286  public: Vector3<T> EulerRotation(bool _firstSolution) const
287  {
288  Vector3<T> euler;
289  Vector3<T> euler2;
290 
291  T m31 = this->data[2][0];
292  T m11 = this->data[0][0];
293  T m12 = this->data[0][1];
294  T m13 = this->data[0][2];
295  T m32 = this->data[2][1];
296  T m33 = this->data[2][2];
297  T m21 = this->data[1][0];
298 
299  if (std::abs(m31) >= 1.0)
300  {
301  euler.Z(0.0);
302  euler2.Z(0.0);
303 
304  if (m31 < 0.0)
305  {
306  euler.Y(IGN_PI / 2.0);
307  euler2.Y(IGN_PI / 2.0);
308  euler.X(atan2(m12, m13));
309  euler2.X(atan2(m12, m13));
310  }
311  else
312  {
313  euler.Y(-IGN_PI / 2.0);
314  euler2.Y(-IGN_PI / 2.0);
315  euler.X(atan2(-m12, -m13));
316  euler2.X(atan2(-m12, -m13));
317  }
318  }
319  else
320  {
321  euler.Y(-asin(m31));
322  euler2.Y(IGN_PI - euler.Y());
323 
324  euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
325  euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
326 
327  euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
328  euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
329  }
330 
331  if (_firstSolution)
332  return euler;
333  else
334  return euler2;
335  }
336 
339  public: Pose3<T> Pose() const
340  {
341  return Pose3<T>(this->Translation(), this->Rotation());
342  }
343 
346  public: void Scale(const Vector3<T> &_s)
347  {
348  this->data[0][0] = _s.X();
349  this->data[1][1] = _s.Y();
350  this->data[2][2] = _s.Z();
351  this->data[3][3] = 1.0;
352  }
353 
358  public: void Scale(T _x, T _y, T _z)
359  {
360  this->data[0][0] = _x;
361  this->data[1][1] = _y;
362  this->data[2][2] = _z;
363  this->data[3][3] = 1.0;
364  }
365 
368  public: bool IsAffine() const
369  {
370  return equal(this->data[3][0], static_cast<T>(0)) &&
371  equal(this->data[3][1], static_cast<T>(0)) &&
372  equal(this->data[3][2], static_cast<T>(0)) &&
373  equal(this->data[3][3], static_cast<T>(1));
374  }
375 
380  public: Vector3<T> TransformAffine(const Vector3<T> &_v) const
381  {
382  if (!this->IsAffine())
383  throw AffineException();
384 
385  return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
386  this->data[0][2]*_v.Z() + this->data[0][3],
387  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
388  this->data[1][2]*_v.Z() + this->data[1][3],
389  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
390  this->data[2][2]*_v.Z() + this->data[2][3]);
391  }
392 
396  public: Matrix4<T> Inverse() const
397  {
398  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
399  Matrix4<T> r;
400 
401  v0 = this->data[2][0]*this->data[3][1] -
402  this->data[2][1]*this->data[3][0];
403  v1 = this->data[2][0]*this->data[3][2] -
404  this->data[2][2]*this->data[3][0];
405  v2 = this->data[2][0]*this->data[3][3] -
406  this->data[2][3]*this->data[3][0];
407  v3 = this->data[2][1]*this->data[3][2] -
408  this->data[2][2]*this->data[3][1];
409  v4 = this->data[2][1]*this->data[3][3] -
410  this->data[2][3]*this->data[3][1];
411  v5 = this->data[2][2]*this->data[3][3] -
412  this->data[2][3]*this->data[3][2];
413 
414  t00 = +(v5*this->data[1][1] -
415  v4*this->data[1][2] + v3*this->data[1][3]);
416  t10 = -(v5*this->data[1][0] -
417  v2*this->data[1][2] + v1*this->data[1][3]);
418  t20 = +(v4*this->data[1][0] -
419  v2*this->data[1][1] + v0*this->data[1][3]);
420  t30 = -(v3*this->data[1][0] -
421  v1*this->data[1][1] + v0*this->data[1][2]);
422 
423  T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
424  t20 * this->data[0][2] + t30 * this->data[0][3]);
425 
426  r(0, 0) = t00 * invDet;
427  r(1, 0) = t10 * invDet;
428  r(2, 0) = t20 * invDet;
429  r(3, 0) = t30 * invDet;
430 
431  r(0, 1) = -(v5*this->data[0][1] -
432  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
433  r(1, 1) = +(v5*this->data[0][0] -
434  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
435  r(2, 1) = -(v4*this->data[0][0] -
436  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
437  r(3, 1) = +(v3*this->data[0][0] -
438  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
439 
440  v0 = this->data[1][0]*this->data[3][1] -
441  this->data[1][1]*this->data[3][0];
442  v1 = this->data[1][0]*this->data[3][2] -
443  this->data[1][2]*this->data[3][0];
444  v2 = this->data[1][0]*this->data[3][3] -
445  this->data[1][3]*this->data[3][0];
446  v3 = this->data[1][1]*this->data[3][2] -
447  this->data[1][2]*this->data[3][1];
448  v4 = this->data[1][1]*this->data[3][3] -
449  this->data[1][3]*this->data[3][1];
450  v5 = this->data[1][2]*this->data[3][3] -
451  this->data[1][3]*this->data[3][2];
452 
453  r(0, 2) = +(v5*this->data[0][1] -
454  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
455  r(1, 2) = -(v5*this->data[0][0] -
456  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
457  r(2, 2) = +(v4*this->data[0][0] -
458  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
459  r(3, 2) = -(v3*this->data[0][0] -
460  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
461 
462  v0 = this->data[2][1]*this->data[1][0] -
463  this->data[2][0]*this->data[1][1];
464  v1 = this->data[2][2]*this->data[1][0] -
465  this->data[2][0]*this->data[1][2];
466  v2 = this->data[2][3]*this->data[1][0] -
467  this->data[2][0]*this->data[1][3];
468  v3 = this->data[2][2]*this->data[1][1] -
469  this->data[2][1]*this->data[1][2];
470  v4 = this->data[2][3]*this->data[1][1] -
471  this->data[2][1]*this->data[1][3];
472  v5 = this->data[2][3]*this->data[1][2] -
473  this->data[2][2]*this->data[1][3];
474 
475  r(0, 3) = -(v5*this->data[0][1] -
476  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
477  r(1, 3) = +(v5*this->data[0][0] -
478  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
479  r(2, 3) = -(v4*this->data[0][0] -
480  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
481  r(3, 3) = +(v3*this->data[0][0] -
482  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
483 
484  return r;
485  }
486 
490  public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
491  {
492  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
493  return *this;
494  }
495 
499  public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
500  {
501  this->data[0][0] = _mat(0, 0);
502  this->data[0][1] = _mat(0, 1);
503  this->data[0][2] = _mat(0, 2);
504 
505  this->data[1][0] = _mat(1, 0);
506  this->data[1][1] = _mat(1, 1);
507  this->data[1][2] = _mat(1, 2);
508 
509  this->data[2][0] = _mat(2, 0);
510  this->data[2][1] = _mat(2, 1);
511  this->data[2][2] = _mat(2, 2);
512 
513  return *this;
514  }
515 
519  public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
520  {
521  return Matrix4<T>(
522  this->data[0][0] * _m2(0, 0) +
523  this->data[0][1] * _m2(1, 0) +
524  this->data[0][2] * _m2(2, 0) +
525  this->data[0][3] * _m2(3, 0),
526 
527  this->data[0][0] * _m2(0, 1) +
528  this->data[0][1] * _m2(1, 1) +
529  this->data[0][2] * _m2(2, 1) +
530  this->data[0][3] * _m2(3, 1),
531 
532  this->data[0][0] * _m2(0, 2) +
533  this->data[0][1] * _m2(1, 2) +
534  this->data[0][2] * _m2(2, 2) +
535  this->data[0][3] * _m2(3, 2),
536 
537  this->data[0][0] * _m2(0, 3) +
538  this->data[0][1] * _m2(1, 3) +
539  this->data[0][2] * _m2(2, 3) +
540  this->data[0][3] * _m2(3, 3),
541 
542  this->data[1][0] * _m2(0, 0) +
543  this->data[1][1] * _m2(1, 0) +
544  this->data[1][2] * _m2(2, 0) +
545  this->data[1][3] * _m2(3, 0),
546 
547  this->data[1][0] * _m2(0, 1) +
548  this->data[1][1] * _m2(1, 1) +
549  this->data[1][2] * _m2(2, 1) +
550  this->data[1][3] * _m2(3, 1),
551 
552  this->data[1][0] * _m2(0, 2) +
553  this->data[1][1] * _m2(1, 2) +
554  this->data[1][2] * _m2(2, 2) +
555  this->data[1][3] * _m2(3, 2),
556 
557  this->data[1][0] * _m2(0, 3) +
558  this->data[1][1] * _m2(1, 3) +
559  this->data[1][2] * _m2(2, 3) +
560  this->data[1][3] * _m2(3, 3),
561 
562  this->data[2][0] * _m2(0, 0) +
563  this->data[2][1] * _m2(1, 0) +
564  this->data[2][2] * _m2(2, 0) +
565  this->data[2][3] * _m2(3, 0),
566 
567  this->data[2][0] * _m2(0, 1) +
568  this->data[2][1] * _m2(1, 1) +
569  this->data[2][2] * _m2(2, 1) +
570  this->data[2][3] * _m2(3, 1),
571 
572  this->data[2][0] * _m2(0, 2) +
573  this->data[2][1] * _m2(1, 2) +
574  this->data[2][2] * _m2(2, 2) +
575  this->data[2][3] * _m2(3, 2),
576 
577  this->data[2][0] * _m2(0, 3) +
578  this->data[2][1] * _m2(1, 3) +
579  this->data[2][2] * _m2(2, 3) +
580  this->data[2][3] * _m2(3, 3),
581 
582  this->data[3][0] * _m2(0, 0) +
583  this->data[3][1] * _m2(1, 0) +
584  this->data[3][2] * _m2(2, 0) +
585  this->data[3][3] * _m2(3, 0),
586 
587  this->data[3][0] * _m2(0, 1) +
588  this->data[3][1] * _m2(1, 1) +
589  this->data[3][2] * _m2(2, 1) +
590  this->data[3][3] * _m2(3, 1),
591 
592  this->data[3][0] * _m2(0, 2) +
593  this->data[3][1] * _m2(1, 2) +
594  this->data[3][2] * _m2(2, 2) +
595  this->data[3][3] * _m2(3, 2),
596 
597  this->data[3][0] * _m2(0, 3) +
598  this->data[3][1] * _m2(1, 3) +
599  this->data[3][2] * _m2(2, 3) +
600  this->data[3][3] * _m2(3, 3));
601  }
602 
606  public: Vector3<T> operator*(const Vector3<T> &_vec) const
607  {
608  return Vector3<T>(
609  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
610  this->data[0][2]*_vec.Z() + this->data[0][3],
611  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
612  this->data[1][2]*_vec.Z() + this->data[1][3],
613  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
614  this->data[2][2]*_vec.Z() + this->data[2][3]);
615  }
616 
621  public: inline const T &operator()(size_t _row, size_t _col) const
622  {
623  if (_row >= 4 || _col >= 4)
624  throw IndexException();
625  return this->data[_row][_col];
626  }
627 
633  public: inline T &operator()(size_t _row, size_t _col)
634  {
635  if (_row >= 4 || _col >= 4)
636  throw IndexException();
637  return this->data[_row][_col];
638  }
639 
644  public: bool operator==(const Matrix4<T> &_m) const
645  {
646  return math::equal(this->data[0][0], _m(0, 0)) &&
647  math::equal(this->data[0][1], _m(0, 1)) &&
648  math::equal(this->data[0][2], _m(0, 2)) &&
649  math::equal(this->data[0][3], _m(0, 3)) &&
650 
651  math::equal(this->data[1][0], _m(1, 0)) &&
652  math::equal(this->data[1][1], _m(1, 1)) &&
653  math::equal(this->data[1][2], _m(1, 2)) &&
654  math::equal(this->data[1][3], _m(1, 3)) &&
655 
656  math::equal(this->data[2][0], _m(2, 0)) &&
657  math::equal(this->data[2][1], _m(2, 1)) &&
658  math::equal(this->data[2][2], _m(2, 2)) &&
659  math::equal(this->data[2][3], _m(2, 3)) &&
660 
661  math::equal(this->data[3][0], _m(3, 0)) &&
662  math::equal(this->data[3][1], _m(3, 1)) &&
663  math::equal(this->data[3][2], _m(3, 2)) &&
664  math::equal(this->data[3][3], _m(3, 3));
665  }
666 
670  public: bool operator!=(const Matrix4<T> &_m) const
671  {
672  return !(*this == _m);
673  }
674 
679  public: friend std::ostream &operator<<(
680  std::ostream &_out, const ignition::math::Matrix4<T> &_m)
681  {
682  _out << precision(_m(0, 0), 6) << " "
683  << precision(_m(0, 1), 6) << " "
684  << precision(_m(0, 2), 6) << " "
685  << precision(_m(0, 3), 6) << " "
686  << precision(_m(1, 0), 6) << " "
687  << precision(_m(1, 1), 6) << " "
688  << precision(_m(1, 2), 6) << " "
689  << precision(_m(1, 3), 6) << " "
690  << precision(_m(2, 0), 6) << " "
691  << precision(_m(2, 1), 6) << " "
692  << precision(_m(2, 2), 6) << " "
693  << precision(_m(2, 3), 6) << " "
694  << precision(_m(3, 0), 6) << " "
695  << precision(_m(3, 1), 6) << " "
696  << precision(_m(3, 2), 6) << " "
697  << precision(_m(3, 3), 6);
698 
699  return _out;
700  }
701 
706  public: friend std::istream &operator>>(
707  std::istream &_in, ignition::math::Matrix4<T> &_m)
708  {
709  // Skip white spaces
710  _in.setf(std::ios_base::skipws);
711  T d[16];
712  _in >> d[0] >> d[1] >> d[2] >> d[3]
713  >> d[4] >> d[5] >> d[6] >> d[7]
714  >> d[8] >> d[9] >> d[10] >> d[11]
715  >> d[12] >> d[13] >> d[14] >> d[15];
716 
717  _m.Set(d[0], d[1], d[2], d[3],
718  d[4], d[5], d[6], d[7],
719  d[8], d[9], d[10], d[11],
720  d[12], d[13], d[14], d[15]);
721  return _in;
722  }
723 
725  private: T data[4][4];
726  };
727 
728  template<typename T>
729  const Matrix4<T> Matrix4<T>::Identity(
730  1, 0, 0, 0,
731  0, 1, 0, 0,
732  0, 0, 1, 0,
733  0, 0, 0, 1);
734 
735  template<typename T>
736  const Matrix4<T> Matrix4<T>::Zero(
737  0, 0, 0, 0,
738  0, 0, 0, 0,
739  0, 0, 0, 0,
740  0, 0, 0, 0);
741 
745  }
746 }
747 #endif
ignition/math/AffineException.hh
Definition: AffineException.hh:37
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition: Matrix4.hh:396
static const Matrix4< T > Identity
Identity matrix.
Definition: Matrix4.hh:36
T & operator()(size_t _row, size_t _col)
Get a mutable version the value at the specified row, column index.
Definition: Matrix4.hh:633
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:250
const T & operator()(size_t _row, size_t _col) const
Get the value at the specified row, column index.
Definition: Matrix4.hh:621
const Matrix4< T > & operator=(const Matrix3< T > &_mat)
Equal operator for 3x3 matrix.
Definition: Matrix4.hh:499
Matrix4< int > Matrix4i
Definition: Matrix4.hh:742
Matrix4(const Matrix4< T > &_m)
Copy constructor.
Definition: Matrix4.hh:49
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
Matrix4(const Pose3< T > &_pose)
Construct Matrix4 from a math::Pose3.
Definition: Matrix4.hh:108
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix4< T > &_m)
Stream extraction operator.
Definition: Matrix4.hh:706
A 4x4 matrix class.
Definition: Matrix4.hh:33
const T & Y() const
Get the y component.
Definition: Quaternion.hh:789
static const Matrix4< T > Zero
Zero matrix.
Definition: Matrix4.hh:39
void Scale(const Vector3< T > &_s)
Set the scale.
Definition: Matrix4.hh:346
T X() const
Get the x value.
Definition: Vector3.hh:560
const T & Z() const
Get the z component.
Definition: Quaternion.hh:796
Vector3< T > Scale() const
Get the scale values as a Vector3&lt;T&gt;
Definition: Matrix4.hh:211
T Y() const
Get the y value.
Definition: Vector3.hh:567
A 3x3 matrix class.
Definition: Matrix3.hh:32
Matrix4()
Constructor.
Definition: Matrix4.hh:42
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator.
Definition: Matrix4.hh:606
Matrix4(T _v00, T _v01, T _v02, T _v03, T _v10, T _v11, T _v12, T _v13, T _v20, T _v21, T _v22, T _v23, T _v30, T _v31, T _v32, T _v33)
Constructor.
Definition: Matrix4.hh:71
bool IsAffine() const
Return true if the matrix is affine.
Definition: Matrix4.hh:368
bool operator==(const Matrix4< T > &_m) const
Equality operator.
Definition: Matrix4.hh:644
T Z() const
Get the z value.
Definition: Vector3.hh:574
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
void Axis(const Vector3< T > &_axis, T _angle)
Set the upper-left 3x3 matrix from an axis and angle.
Definition: Matrix4.hh:163
Quaternion< T > Rotation() const
Get the rotation as a quaternion.
Definition: Matrix4.hh:218
Vector3< T > EulerRotation(bool _firstSolution) const
Get the rotation as a Euler angles.
Definition: Matrix4.hh:286
bool operator!=(const Matrix4< T > &_m) const
Inequality test operator.
Definition: Matrix4.hh:670
void Scale(T _x, T _y, T _z)
Set the scale.
Definition: Matrix4.hh:358
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
Vector3< T > TransformAffine(const Vector3< T > &_v) const
Perform an affine transformation.
Definition: Matrix4.hh:380
Matrix4< float > Matrix4f
Definition: Matrix4.hh:744
void Translate(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:184
virtual ~Matrix4()
Destructor.
Definition: Matrix4.hh:114
void Translate(T _x, T _y, T _z)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:195
Matrix4< T > operator*(const Matrix4< T > &_m2) const
Multiplication operator.
Definition: Matrix4.hh:519
Pose3< T > Pose() const
Get the transformation as math::Pose.
Definition: Matrix4.hh:339
const T & W() const
Get the w component.
Definition: Quaternion.hh:775
Vector3< T > Translation() const
Get the translational values as a Vector3.
Definition: Matrix4.hh:204
void Set(T _v00, T _v01, T _v02, T _v03, T _v10, T _v11, T _v12, T _v13, T _v20, T _v21, T _v22, T _v23, T _v30, T _v31, T _v32, T _v33)
Change the values.
Definition: Matrix4.hh:133
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:238
Matrix4< double > Matrix4d
Definition: Matrix4.hh:743
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:206
A quaternion class.
Definition: Quaternion.hh:31
Matrix4(const Quaternion< T > &_q)
Construct Matrix4 from a quaternion.
Definition: Matrix4.hh:84
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:79
Matrix4< T > & operator=(const Matrix4< T > &_mat)
Equal operator.
Definition: Matrix4.hh:490
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix4< T > &_m)
Stream insertion operator.
Definition: Matrix4.hh:679
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:345
const T & X() const
Get the x component.
Definition: Quaternion.hh:782