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 <algorithm>
21 #include <ignition/math/Helpers.hh>
23 #include <ignition/math/Matrix3.hh>
24 #include <ignition/math/Vector3.hh>
25 #include <ignition/math/Pose3.hh>
26 
27 namespace ignition
28 {
29  namespace math
30  {
33  template<typename T>
34  class Matrix4
35  {
37  public: static const Matrix4<T> Identity;
38 
40  public: static const Matrix4<T> Zero;
41 
43  public: Matrix4()
44  {
45  memset(this->data, 0, sizeof(this->data[0][0])*16);
46  }
47 
50  public: Matrix4(const Matrix4<T> &_m)
51  {
52  memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
53  }
54 
72  public: Matrix4(T _v00, T _v01, T _v02, T _v03,
73  T _v10, T _v11, T _v12, T _v13,
74  T _v20, T _v21, T _v22, T _v23,
75  T _v30, T _v31, T _v32, T _v33)
76  {
77  this->Set(_v00, _v01, _v02, _v03,
78  _v10, _v11, _v12, _v13,
79  _v20, _v21, _v22, _v23,
80  _v30, _v31, _v32, _v33);
81  }
82 
85  public: Matrix4(const Quaternion<T> &_q)
86  {
87  Quaternion<T> qt = _q;
88  qt.Normalize();
89  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
90  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
91  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
92  0,
93 
94  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
95  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
96  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
97  0,
98 
99  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
100  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
101  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
102  0,
103 
104  0, 0, 0, 1);
105  }
106 
109  public: Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
110  {
111  this->Translate(_pose.Pos());
112  }
113 
115  public: virtual ~Matrix4() {}
116 
134  public: void Set(
135  T _v00, T _v01, T _v02, T _v03,
136  T _v10, T _v11, T _v12, T _v13,
137  T _v20, T _v21, T _v22, T _v23,
138  T _v30, T _v31, T _v32, T _v33)
139  {
140  this->data[0][0] = _v00;
141  this->data[0][1] = _v01;
142  this->data[0][2] = _v02;
143  this->data[0][3] = _v03;
144 
145  this->data[1][0] = _v10;
146  this->data[1][1] = _v11;
147  this->data[1][2] = _v12;
148  this->data[1][3] = _v13;
149 
150  this->data[2][0] = _v20;
151  this->data[2][1] = _v21;
152  this->data[2][2] = _v22;
153  this->data[2][3] = _v23;
154 
155  this->data[3][0] = _v30;
156  this->data[3][1] = _v31;
157  this->data[3][2] = _v32;
158  this->data[3][3] = _v33;
159  }
160 
164  public: void Axis(const Vector3<T> &_axis, T _angle)
165  {
166  T c = cos(_angle);
167  T s = sin(_angle);
168  T C = 1-c;
169 
170  this->data[0][0] = _axis.X()*_axis.X()*C + c;
171  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
172  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
173 
174  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
175  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
176  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
177 
178  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
179  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
180  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
181  }
182 
185  public: void Translate(const Vector3<T> &_t)
186  {
187  this->data[0][3] = _t.X();
188  this->data[1][3] = _t.Y();
189  this->data[2][3] = _t.Z();
190  }
191 
196  public: void Translate(T _x, T _y, T _z)
197  {
198  this->data[0][3] = _x;
199  this->data[1][3] = _y;
200  this->data[2][3] = _z;
201  }
202 
205  public: Vector3<T> Translation() const
206  {
207  return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
208  }
209 
212  public: Vector3<T> Scale() const
213  {
214  return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
215  }
216 
219  public: Quaternion<T> Rotation() const
220  {
221  Quaternion<T> q;
224  T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
225  T root;
226  if (trace > 0)
227  {
228  root = sqrt(trace + 1.0);
229  q.W(root / 2.0);
230  root = 1.0 / (2.0 * root);
231  q.X((this->data[2][1] - this->data[1][2]) * root);
232  q.Y((this->data[0][2] - this->data[2][0]) * root);
233  q.Z((this->data[1][0] - this->data[0][1]) * root);
234  }
235  else
236  {
237  static unsigned int s_iNext[3] = {1, 2, 0};
238  unsigned int i = 0;
239  if (this->data[1][1] > this->data[0][0])
240  i = 1;
241  if (this->data[2][2] > this->data[i][i])
242  i = 2;
243  unsigned int j = s_iNext[i];
244  unsigned int k = s_iNext[j];
245 
246  root = sqrt(this->data[i][i] - this->data[j][j] -
247  this->data[k][k] + 1.0);
248 
249  T a, b, c;
250  a = root / 2.0;
251  root = 1.0 / (2.0 * root);
252  b = (this->data[j][i] + this->data[i][j]) * root;
253  c = (this->data[k][i] + this->data[i][k]) * root;
254 
255  switch (i)
256  {
257  default:
258  case 0: q.X(a); break;
259  case 1: q.Y(a); break;
260  case 2: q.Z(a); break;
261  };
262  switch (j)
263  {
264  default:
265  case 0: q.X(b); break;
266  case 1: q.Y(b); break;
267  case 2: q.Z(b); break;
268  };
269  switch (k)
270  {
271  default:
272  case 0: q.X(c); break;
273  case 1: q.Y(c); break;
274  case 2: q.Z(c); break;
275  };
276 
277  q.W((this->data[k][j] - this->data[j][k]) * root);
278  }
279 
280  return q;
281  }
282 
287  public: Vector3<T> EulerRotation(bool _firstSolution) const
288  {
289  Vector3<T> euler;
290  Vector3<T> euler2;
291 
292  T m31 = this->data[2][0];
293  T m11 = this->data[0][0];
294  T m12 = this->data[0][1];
295  T m13 = this->data[0][2];
296  T m32 = this->data[2][1];
297  T m33 = this->data[2][2];
298  T m21 = this->data[1][0];
299 
300  if (std::abs(m31) >= 1.0)
301  {
302  euler.Z(0.0);
303  euler2.Z(0.0);
304 
305  if (m31 < 0.0)
306  {
307  euler.Y(IGN_PI / 2.0);
308  euler2.Y(IGN_PI / 2.0);
309  euler.X(atan2(m12, m13));
310  euler2.X(atan2(m12, m13));
311  }
312  else
313  {
314  euler.Y(-IGN_PI / 2.0);
315  euler2.Y(-IGN_PI / 2.0);
316  euler.X(atan2(-m12, -m13));
317  euler2.X(atan2(-m12, -m13));
318  }
319  }
320  else
321  {
322  euler.Y(-asin(m31));
323  euler2.Y(IGN_PI - euler.Y());
324 
325  euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
326  euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
327 
328  euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
329  euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
330  }
331 
332  if (_firstSolution)
333  return euler;
334  else
335  return euler2;
336  }
337 
340  public: Pose3<T> Pose() const
341  {
342  return Pose3<T>(this->Translation(), this->Rotation());
343  }
344 
347  public: void Scale(const Vector3<T> &_s)
348  {
349  this->data[0][0] = _s.X();
350  this->data[1][1] = _s.Y();
351  this->data[2][2] = _s.Z();
352  this->data[3][3] = 1.0;
353  }
354 
359  public: void Scale(T _x, T _y, T _z)
360  {
361  this->data[0][0] = _x;
362  this->data[1][1] = _y;
363  this->data[2][2] = _z;
364  this->data[3][3] = 1.0;
365  }
366 
369  public: bool IsAffine() const
370  {
371  return equal(this->data[3][0], static_cast<T>(0)) &&
372  equal(this->data[3][1], static_cast<T>(0)) &&
373  equal(this->data[3][2], static_cast<T>(0)) &&
374  equal(this->data[3][3], static_cast<T>(1));
375  }
376 
381  public: Vector3<T> TransformAffine(const Vector3<T> &_v) const
382  {
383  if (!this->IsAffine())
384  throw AffineException();
385 
386  return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
387  this->data[0][2]*_v.Z() + this->data[0][3],
388  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
389  this->data[1][2]*_v.Z() + this->data[1][3],
390  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
391  this->data[2][2]*_v.Z() + this->data[2][3]);
392  }
393 
396  public: T Determinant() const
397  {
398  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
399 
400  v0 = this->data[2][0]*this->data[3][1]
401  - this->data[2][1]*this->data[3][0];
402  v1 = this->data[2][0]*this->data[3][2]
403  - this->data[2][2]*this->data[3][0];
404  v2 = this->data[2][0]*this->data[3][3]
405  - this->data[2][3]*this->data[3][0];
406  v3 = this->data[2][1]*this->data[3][2]
407  - this->data[2][2]*this->data[3][1];
408  v4 = this->data[2][1]*this->data[3][3]
409  - this->data[2][3]*this->data[3][1];
410  v5 = this->data[2][2]*this->data[3][3]
411  - this->data[2][3]*this->data[3][2];
412 
413  t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
414  t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
415  t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
416  t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
417 
418  return t00 * this->data[0][0]
419  + t10 * this->data[0][1]
420  + t20 * this->data[0][2]
421  + t30 * this->data[0][3];
422  }
423 
427  public: Matrix4<T> Inverse() const
428  {
429  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
430  Matrix4<T> r;
431 
432  v0 = this->data[2][0]*this->data[3][1] -
433  this->data[2][1]*this->data[3][0];
434  v1 = this->data[2][0]*this->data[3][2] -
435  this->data[2][2]*this->data[3][0];
436  v2 = this->data[2][0]*this->data[3][3] -
437  this->data[2][3]*this->data[3][0];
438  v3 = this->data[2][1]*this->data[3][2] -
439  this->data[2][2]*this->data[3][1];
440  v4 = this->data[2][1]*this->data[3][3] -
441  this->data[2][3]*this->data[3][1];
442  v5 = this->data[2][2]*this->data[3][3] -
443  this->data[2][3]*this->data[3][2];
444 
445  t00 = +(v5*this->data[1][1] -
446  v4*this->data[1][2] + v3*this->data[1][3]);
447  t10 = -(v5*this->data[1][0] -
448  v2*this->data[1][2] + v1*this->data[1][3]);
449  t20 = +(v4*this->data[1][0] -
450  v2*this->data[1][1] + v0*this->data[1][3]);
451  t30 = -(v3*this->data[1][0] -
452  v1*this->data[1][1] + v0*this->data[1][2]);
453 
454  T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
455  t20 * this->data[0][2] + t30 * this->data[0][3]);
456 
457  r(0, 0) = t00 * invDet;
458  r(1, 0) = t10 * invDet;
459  r(2, 0) = t20 * invDet;
460  r(3, 0) = t30 * invDet;
461 
462  r(0, 1) = -(v5*this->data[0][1] -
463  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
464  r(1, 1) = +(v5*this->data[0][0] -
465  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
466  r(2, 1) = -(v4*this->data[0][0] -
467  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
468  r(3, 1) = +(v3*this->data[0][0] -
469  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
470 
471  v0 = this->data[1][0]*this->data[3][1] -
472  this->data[1][1]*this->data[3][0];
473  v1 = this->data[1][0]*this->data[3][2] -
474  this->data[1][2]*this->data[3][0];
475  v2 = this->data[1][0]*this->data[3][3] -
476  this->data[1][3]*this->data[3][0];
477  v3 = this->data[1][1]*this->data[3][2] -
478  this->data[1][2]*this->data[3][1];
479  v4 = this->data[1][1]*this->data[3][3] -
480  this->data[1][3]*this->data[3][1];
481  v5 = this->data[1][2]*this->data[3][3] -
482  this->data[1][3]*this->data[3][2];
483 
484  r(0, 2) = +(v5*this->data[0][1] -
485  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
486  r(1, 2) = -(v5*this->data[0][0] -
487  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
488  r(2, 2) = +(v4*this->data[0][0] -
489  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
490  r(3, 2) = -(v3*this->data[0][0] -
491  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
492 
493  v0 = this->data[2][1]*this->data[1][0] -
494  this->data[2][0]*this->data[1][1];
495  v1 = this->data[2][2]*this->data[1][0] -
496  this->data[2][0]*this->data[1][2];
497  v2 = this->data[2][3]*this->data[1][0] -
498  this->data[2][0]*this->data[1][3];
499  v3 = this->data[2][2]*this->data[1][1] -
500  this->data[2][1]*this->data[1][2];
501  v4 = this->data[2][3]*this->data[1][1] -
502  this->data[2][1]*this->data[1][3];
503  v5 = this->data[2][3]*this->data[1][2] -
504  this->data[2][2]*this->data[1][3];
505 
506  r(0, 3) = -(v5*this->data[0][1] -
507  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
508  r(1, 3) = +(v5*this->data[0][0] -
509  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
510  r(2, 3) = -(v4*this->data[0][0] -
511  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
512  r(3, 3) = +(v3*this->data[0][0] -
513  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
514 
515  return r;
516  }
517 
519  public: void Transpose()
520  {
521  std::swap(this->data[0][1], this->data[1][0]);
522  std::swap(this->data[0][2], this->data[2][0]);
523  std::swap(this->data[0][3], this->data[3][0]);
524  std::swap(this->data[1][2], this->data[2][1]);
525  std::swap(this->data[1][3], this->data[3][1]);
526  std::swap(this->data[2][3], this->data[3][2]);
527  }
528 
531  public: Matrix4<T> Transposed() const
532  {
533  return Matrix4<T>(
534  this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
535  this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
536  this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
537  this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
538  }
539 
543  public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
544  {
545  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
546  return *this;
547  }
548 
552  public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
553  {
554  this->data[0][0] = _mat(0, 0);
555  this->data[0][1] = _mat(0, 1);
556  this->data[0][2] = _mat(0, 2);
557 
558  this->data[1][0] = _mat(1, 0);
559  this->data[1][1] = _mat(1, 1);
560  this->data[1][2] = _mat(1, 2);
561 
562  this->data[2][0] = _mat(2, 0);
563  this->data[2][1] = _mat(2, 1);
564  this->data[2][2] = _mat(2, 2);
565 
566  return *this;
567  }
568 
572  public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
573  {
574  return Matrix4<T>(
575  this->data[0][0] * _m2(0, 0) +
576  this->data[0][1] * _m2(1, 0) +
577  this->data[0][2] * _m2(2, 0) +
578  this->data[0][3] * _m2(3, 0),
579 
580  this->data[0][0] * _m2(0, 1) +
581  this->data[0][1] * _m2(1, 1) +
582  this->data[0][2] * _m2(2, 1) +
583  this->data[0][3] * _m2(3, 1),
584 
585  this->data[0][0] * _m2(0, 2) +
586  this->data[0][1] * _m2(1, 2) +
587  this->data[0][2] * _m2(2, 2) +
588  this->data[0][3] * _m2(3, 2),
589 
590  this->data[0][0] * _m2(0, 3) +
591  this->data[0][1] * _m2(1, 3) +
592  this->data[0][2] * _m2(2, 3) +
593  this->data[0][3] * _m2(3, 3),
594 
595  this->data[1][0] * _m2(0, 0) +
596  this->data[1][1] * _m2(1, 0) +
597  this->data[1][2] * _m2(2, 0) +
598  this->data[1][3] * _m2(3, 0),
599 
600  this->data[1][0] * _m2(0, 1) +
601  this->data[1][1] * _m2(1, 1) +
602  this->data[1][2] * _m2(2, 1) +
603  this->data[1][3] * _m2(3, 1),
604 
605  this->data[1][0] * _m2(0, 2) +
606  this->data[1][1] * _m2(1, 2) +
607  this->data[1][2] * _m2(2, 2) +
608  this->data[1][3] * _m2(3, 2),
609 
610  this->data[1][0] * _m2(0, 3) +
611  this->data[1][1] * _m2(1, 3) +
612  this->data[1][2] * _m2(2, 3) +
613  this->data[1][3] * _m2(3, 3),
614 
615  this->data[2][0] * _m2(0, 0) +
616  this->data[2][1] * _m2(1, 0) +
617  this->data[2][2] * _m2(2, 0) +
618  this->data[2][3] * _m2(3, 0),
619 
620  this->data[2][0] * _m2(0, 1) +
621  this->data[2][1] * _m2(1, 1) +
622  this->data[2][2] * _m2(2, 1) +
623  this->data[2][3] * _m2(3, 1),
624 
625  this->data[2][0] * _m2(0, 2) +
626  this->data[2][1] * _m2(1, 2) +
627  this->data[2][2] * _m2(2, 2) +
628  this->data[2][3] * _m2(3, 2),
629 
630  this->data[2][0] * _m2(0, 3) +
631  this->data[2][1] * _m2(1, 3) +
632  this->data[2][2] * _m2(2, 3) +
633  this->data[2][3] * _m2(3, 3),
634 
635  this->data[3][0] * _m2(0, 0) +
636  this->data[3][1] * _m2(1, 0) +
637  this->data[3][2] * _m2(2, 0) +
638  this->data[3][3] * _m2(3, 0),
639 
640  this->data[3][0] * _m2(0, 1) +
641  this->data[3][1] * _m2(1, 1) +
642  this->data[3][2] * _m2(2, 1) +
643  this->data[3][3] * _m2(3, 1),
644 
645  this->data[3][0] * _m2(0, 2) +
646  this->data[3][1] * _m2(1, 2) +
647  this->data[3][2] * _m2(2, 2) +
648  this->data[3][3] * _m2(3, 2),
649 
650  this->data[3][0] * _m2(0, 3) +
651  this->data[3][1] * _m2(1, 3) +
652  this->data[3][2] * _m2(2, 3) +
653  this->data[3][3] * _m2(3, 3));
654  }
655 
659  public: Vector3<T> operator*(const Vector3<T> &_vec) const
660  {
661  return Vector3<T>(
662  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
663  this->data[0][2]*_vec.Z() + this->data[0][3],
664  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
665  this->data[1][2]*_vec.Z() + this->data[1][3],
666  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
667  this->data[2][2]*_vec.Z() + this->data[2][3]);
668  }
669 
674  public: inline const T &operator()(size_t _row, size_t _col) const
675  {
676  if (_row >= 4 || _col >= 4)
677  throw IndexException();
678  return this->data[_row][_col];
679  }
680 
686  public: inline T &operator()(size_t _row, size_t _col)
687  {
688  if (_row >= 4 || _col >= 4)
689  throw IndexException();
690  return this->data[_row][_col];
691  }
692 
698  public: bool Equal(const Matrix4 &_m, const T &_tol) const
699  {
700  return equal<T>(this->data[0][0], _m(0, 0), _tol)
701  && equal<T>(this->data[0][1], _m(0, 1), _tol)
702  && equal<T>(this->data[0][2], _m(0, 2), _tol)
703  && equal<T>(this->data[0][3], _m(0, 3), _tol)
704  && equal<T>(this->data[1][0], _m(1, 0), _tol)
705  && equal<T>(this->data[1][1], _m(1, 1), _tol)
706  && equal<T>(this->data[1][2], _m(1, 2), _tol)
707  && equal<T>(this->data[1][3], _m(1, 3), _tol)
708  && equal<T>(this->data[2][0], _m(2, 0), _tol)
709  && equal<T>(this->data[2][1], _m(2, 1), _tol)
710  && equal<T>(this->data[2][2], _m(2, 2), _tol)
711  && equal<T>(this->data[2][3], _m(2, 3), _tol)
712  && equal<T>(this->data[3][0], _m(3, 0), _tol)
713  && equal<T>(this->data[3][1], _m(3, 1), _tol)
714  && equal<T>(this->data[3][2], _m(3, 2), _tol)
715  && equal<T>(this->data[3][3], _m(3, 3), _tol);
716  }
717 
722  public: bool operator==(const Matrix4<T> &_m) const
723  {
724  return this->Equal(_m, static_cast<T>(1e-6));
725  }
726 
730  public: bool operator!=(const Matrix4<T> &_m) const
731  {
732  return !(*this == _m);
733  }
734 
739  public: friend std::ostream &operator<<(
740  std::ostream &_out, const ignition::math::Matrix4<T> &_m)
741  {
742  _out << precision(_m(0, 0), 6) << " "
743  << precision(_m(0, 1), 6) << " "
744  << precision(_m(0, 2), 6) << " "
745  << precision(_m(0, 3), 6) << " "
746  << precision(_m(1, 0), 6) << " "
747  << precision(_m(1, 1), 6) << " "
748  << precision(_m(1, 2), 6) << " "
749  << precision(_m(1, 3), 6) << " "
750  << precision(_m(2, 0), 6) << " "
751  << precision(_m(2, 1), 6) << " "
752  << precision(_m(2, 2), 6) << " "
753  << precision(_m(2, 3), 6) << " "
754  << precision(_m(3, 0), 6) << " "
755  << precision(_m(3, 1), 6) << " "
756  << precision(_m(3, 2), 6) << " "
757  << precision(_m(3, 3), 6);
758 
759  return _out;
760  }
761 
766  public: friend std::istream &operator>>(
767  std::istream &_in, ignition::math::Matrix4<T> &_m)
768  {
769  // Skip white spaces
770  _in.setf(std::ios_base::skipws);
771  T d[16];
772  _in >> d[0] >> d[1] >> d[2] >> d[3]
773  >> d[4] >> d[5] >> d[6] >> d[7]
774  >> d[8] >> d[9] >> d[10] >> d[11]
775  >> d[12] >> d[13] >> d[14] >> d[15];
776 
777  _m.Set(d[0], d[1], d[2], d[3],
778  d[4], d[5], d[6], d[7],
779  d[8], d[9], d[10], d[11],
780  d[12], d[13], d[14], d[15]);
781  return _in;
782  }
783 
785  private: T data[4][4];
786  };
787 
788  template<typename T>
789  const Matrix4<T> Matrix4<T>::Identity(
790  1, 0, 0, 0,
791  0, 1, 0, 0,
792  0, 0, 1, 0,
793  0, 0, 0, 1);
794 
795  template<typename T>
796  const Matrix4<T> Matrix4<T>::Zero(
797  0, 0, 0, 0,
798  0, 0, 0, 0,
799  0, 0, 0, 0,
800  0, 0, 0, 0);
801 
805  }
806 }
807 #endif
ignition/math/AffineException.hh
Definition: AffineException.hh:37
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition: Matrix4.hh:427
static const Matrix4< T > Identity
Identity matrix.
Definition: Matrix4.hh:37
T & operator()(size_t _row, size_t _col)
Get a mutable version the value at the specified row, column index.
Definition: Matrix4.hh:686
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:269
const T & operator()(size_t _row, size_t _col) const
Get the value at the specified row, column index.
Definition: Matrix4.hh:674
const Matrix4< T > & operator=(const Matrix3< T > &_mat)
Equal operator for 3x3 matrix.
Definition: Matrix4.hh:552
Matrix4< int > Matrix4i
Definition: Matrix4.hh:802
Matrix4(const Matrix4< T > &_m)
Copy constructor.
Definition: Matrix4.hh:50
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix4.hh:396
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:109
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix4< T > &_m)
Stream extraction operator.
Definition: Matrix4.hh:766
A 4x4 matrix class.
Definition: Matrix4.hh:34
const T & Y() const
Get the y component.
Definition: Quaternion.hh:789
static const Matrix4< T > Zero
Zero matrix.
Definition: Matrix4.hh:40
void Scale(const Vector3< T > &_s)
Set the scale.
Definition: Matrix4.hh:347
T X() const
Get the x value.
Definition: Vector3.hh:630
const T & Z() const
Get the z component.
Definition: Quaternion.hh:796
Vector3< T > Scale() const
Get the scale values as a Vector3
Definition: Matrix4.hh:212
T Y() const
Get the y value.
Definition: Vector3.hh:637
A 3x3 matrix class.
Definition: Matrix3.hh:33
Matrix4()
Constructor.
Definition: Matrix4.hh:43
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator.
Definition: Matrix4.hh:659
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:72
bool IsAffine() const
Return true if the matrix is affine.
Definition: Matrix4.hh:369
Matrix4< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix4.hh:531
bool operator==(const Matrix4< T > &_m) const
Equality operator.
Definition: Matrix4.hh:722
T Z() const
Get the z value.
Definition: Vector3.hh:644
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
void Transpose()
Transpose this matrix.
Definition: Matrix4.hh:519
void Axis(const Vector3< T > &_axis, T _angle)
Set the upper-left 3x3 matrix from an axis and angle.
Definition: Matrix4.hh:164
Quaternion< T > Rotation() const
Get the rotation as a quaternion.
Definition: Matrix4.hh:219
Vector3< T > EulerRotation(bool _firstSolution) const
Get the rotation as a Euler angles.
Definition: Matrix4.hh:287
bool operator!=(const Matrix4< T > &_m) const
Inequality test operator.
Definition: Matrix4.hh:730
void Scale(T _x, T _y, T _z)
Set the scale.
Definition: Matrix4.hh:359
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:381
Matrix4< float > Matrix4f
Definition: Matrix4.hh:804
void Translate(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:185
virtual ~Matrix4()
Destructor.
Definition: Matrix4.hh:115
void Translate(T _x, T _y, T _z)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:196
Matrix4< T > operator*(const Matrix4< T > &_m2) const
Multiplication operator.
Definition: Matrix4.hh:572
Pose3< T > Pose() const
Get the transformation as math::Pose.
Definition: Matrix4.hh:340
const T & W() const
Get the w component.
Definition: Quaternion.hh:775
Definition: AffineException.hh:30
Vector3< T > Translation() const
Get the translational values as a Vector3.
Definition: Matrix4.hh:205
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:134
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:257
Matrix4< double > Matrix4d
Definition: Matrix4.hh:803
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:85
#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:543
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix4< T > &_m)
Stream insertion operator.
Definition: Matrix4.hh:739
bool Equal(const Matrix4 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix4.hh:698
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:345
const T & X() const
Get the x component.
Definition: Quaternion.hh:782