Matrix4.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 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_MATH_MATRIX4_HH_
18 #define IGNITION_MATH_MATRIX4_HH_
19 
20 #include <algorithm>
21 #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: explicit 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: explicit 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 
382  public: Vector3<T> TransformAffine(const Vector3<T> &_v) const
383 #ifndef _WIN32
384  IGN_DEPRECATED(3.0)
385 #endif
386  {
387  if (this->IsAffine())
388  {
389  return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
390  this->data[0][2]*_v.Z() + this->data[0][3],
391  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
392  this->data[1][2]*_v.Z() + this->data[1][3],
393  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
394  this->data[2][2]*_v.Z() + this->data[2][3]);
395  }
396  else
397  {
398  return Vector3<T>();
399  }
400  }
401 
407  public: bool TransformAffine(const Vector3<T> &_v,
408  Vector3<T> &_result) const
409  {
410  if (!this->IsAffine())
411  return false;
412 
413  _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
414  this->data[0][2]*_v.Z() + this->data[0][3],
415  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
416  this->data[1][2]*_v.Z() + this->data[1][3],
417  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
418  this->data[2][2]*_v.Z() + this->data[2][3]);
419  return true;
420  }
421 
424  public: T Determinant() const
425  {
426  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
427 
428  v0 = this->data[2][0]*this->data[3][1]
429  - this->data[2][1]*this->data[3][0];
430  v1 = this->data[2][0]*this->data[3][2]
431  - this->data[2][2]*this->data[3][0];
432  v2 = this->data[2][0]*this->data[3][3]
433  - this->data[2][3]*this->data[3][0];
434  v3 = this->data[2][1]*this->data[3][2]
435  - this->data[2][2]*this->data[3][1];
436  v4 = this->data[2][1]*this->data[3][3]
437  - this->data[2][3]*this->data[3][1];
438  v5 = this->data[2][2]*this->data[3][3]
439  - this->data[2][3]*this->data[3][2];
440 
441  t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
442  t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
443  t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
444  t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
445 
446  return t00 * this->data[0][0]
447  + t10 * this->data[0][1]
448  + t20 * this->data[0][2]
449  + t30 * this->data[0][3];
450  }
451 
455  public: Matrix4<T> Inverse() const
456  {
457  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
458  Matrix4<T> r;
459 
460  v0 = this->data[2][0]*this->data[3][1] -
461  this->data[2][1]*this->data[3][0];
462  v1 = this->data[2][0]*this->data[3][2] -
463  this->data[2][2]*this->data[3][0];
464  v2 = this->data[2][0]*this->data[3][3] -
465  this->data[2][3]*this->data[3][0];
466  v3 = this->data[2][1]*this->data[3][2] -
467  this->data[2][2]*this->data[3][1];
468  v4 = this->data[2][1]*this->data[3][3] -
469  this->data[2][3]*this->data[3][1];
470  v5 = this->data[2][2]*this->data[3][3] -
471  this->data[2][3]*this->data[3][2];
472 
473  t00 = +(v5*this->data[1][1] -
474  v4*this->data[1][2] + v3*this->data[1][3]);
475  t10 = -(v5*this->data[1][0] -
476  v2*this->data[1][2] + v1*this->data[1][3]);
477  t20 = +(v4*this->data[1][0] -
478  v2*this->data[1][1] + v0*this->data[1][3]);
479  t30 = -(v3*this->data[1][0] -
480  v1*this->data[1][1] + v0*this->data[1][2]);
481 
482  T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
483  t20 * this->data[0][2] + t30 * this->data[0][3]);
484 
485  r(0, 0) = t00 * invDet;
486  r(1, 0) = t10 * invDet;
487  r(2, 0) = t20 * invDet;
488  r(3, 0) = t30 * invDet;
489 
490  r(0, 1) = -(v5*this->data[0][1] -
491  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
492  r(1, 1) = +(v5*this->data[0][0] -
493  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
494  r(2, 1) = -(v4*this->data[0][0] -
495  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
496  r(3, 1) = +(v3*this->data[0][0] -
497  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
498 
499  v0 = this->data[1][0]*this->data[3][1] -
500  this->data[1][1]*this->data[3][0];
501  v1 = this->data[1][0]*this->data[3][2] -
502  this->data[1][2]*this->data[3][0];
503  v2 = this->data[1][0]*this->data[3][3] -
504  this->data[1][3]*this->data[3][0];
505  v3 = this->data[1][1]*this->data[3][2] -
506  this->data[1][2]*this->data[3][1];
507  v4 = this->data[1][1]*this->data[3][3] -
508  this->data[1][3]*this->data[3][1];
509  v5 = this->data[1][2]*this->data[3][3] -
510  this->data[1][3]*this->data[3][2];
511 
512  r(0, 2) = +(v5*this->data[0][1] -
513  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
514  r(1, 2) = -(v5*this->data[0][0] -
515  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
516  r(2, 2) = +(v4*this->data[0][0] -
517  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
518  r(3, 2) = -(v3*this->data[0][0] -
519  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
520 
521  v0 = this->data[2][1]*this->data[1][0] -
522  this->data[2][0]*this->data[1][1];
523  v1 = this->data[2][2]*this->data[1][0] -
524  this->data[2][0]*this->data[1][2];
525  v2 = this->data[2][3]*this->data[1][0] -
526  this->data[2][0]*this->data[1][3];
527  v3 = this->data[2][2]*this->data[1][1] -
528  this->data[2][1]*this->data[1][2];
529  v4 = this->data[2][3]*this->data[1][1] -
530  this->data[2][1]*this->data[1][3];
531  v5 = this->data[2][3]*this->data[1][2] -
532  this->data[2][2]*this->data[1][3];
533 
534  r(0, 3) = -(v5*this->data[0][1] -
535  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
536  r(1, 3) = +(v5*this->data[0][0] -
537  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
538  r(2, 3) = -(v4*this->data[0][0] -
539  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
540  r(3, 3) = +(v3*this->data[0][0] -
541  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
542 
543  return r;
544  }
545 
547  public: void Transpose()
548  {
549  std::swap(this->data[0][1], this->data[1][0]);
550  std::swap(this->data[0][2], this->data[2][0]);
551  std::swap(this->data[0][3], this->data[3][0]);
552  std::swap(this->data[1][2], this->data[2][1]);
553  std::swap(this->data[1][3], this->data[3][1]);
554  std::swap(this->data[2][3], this->data[3][2]);
555  }
556 
559  public: Matrix4<T> Transposed() const
560  {
561  return Matrix4<T>(
562  this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
563  this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
564  this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
565  this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
566  }
567 
571  public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
572  {
573  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
574  return *this;
575  }
576 
580  public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
581  {
582  this->data[0][0] = _mat(0, 0);
583  this->data[0][1] = _mat(0, 1);
584  this->data[0][2] = _mat(0, 2);
585 
586  this->data[1][0] = _mat(1, 0);
587  this->data[1][1] = _mat(1, 1);
588  this->data[1][2] = _mat(1, 2);
589 
590  this->data[2][0] = _mat(2, 0);
591  this->data[2][1] = _mat(2, 1);
592  this->data[2][2] = _mat(2, 2);
593 
594  return *this;
595  }
596 
600  public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
601  {
602  return Matrix4<T>(
603  this->data[0][0] * _m2(0, 0) +
604  this->data[0][1] * _m2(1, 0) +
605  this->data[0][2] * _m2(2, 0) +
606  this->data[0][3] * _m2(3, 0),
607 
608  this->data[0][0] * _m2(0, 1) +
609  this->data[0][1] * _m2(1, 1) +
610  this->data[0][2] * _m2(2, 1) +
611  this->data[0][3] * _m2(3, 1),
612 
613  this->data[0][0] * _m2(0, 2) +
614  this->data[0][1] * _m2(1, 2) +
615  this->data[0][2] * _m2(2, 2) +
616  this->data[0][3] * _m2(3, 2),
617 
618  this->data[0][0] * _m2(0, 3) +
619  this->data[0][1] * _m2(1, 3) +
620  this->data[0][2] * _m2(2, 3) +
621  this->data[0][3] * _m2(3, 3),
622 
623  this->data[1][0] * _m2(0, 0) +
624  this->data[1][1] * _m2(1, 0) +
625  this->data[1][2] * _m2(2, 0) +
626  this->data[1][3] * _m2(3, 0),
627 
628  this->data[1][0] * _m2(0, 1) +
629  this->data[1][1] * _m2(1, 1) +
630  this->data[1][2] * _m2(2, 1) +
631  this->data[1][3] * _m2(3, 1),
632 
633  this->data[1][0] * _m2(0, 2) +
634  this->data[1][1] * _m2(1, 2) +
635  this->data[1][2] * _m2(2, 2) +
636  this->data[1][3] * _m2(3, 2),
637 
638  this->data[1][0] * _m2(0, 3) +
639  this->data[1][1] * _m2(1, 3) +
640  this->data[1][2] * _m2(2, 3) +
641  this->data[1][3] * _m2(3, 3),
642 
643  this->data[2][0] * _m2(0, 0) +
644  this->data[2][1] * _m2(1, 0) +
645  this->data[2][2] * _m2(2, 0) +
646  this->data[2][3] * _m2(3, 0),
647 
648  this->data[2][0] * _m2(0, 1) +
649  this->data[2][1] * _m2(1, 1) +
650  this->data[2][2] * _m2(2, 1) +
651  this->data[2][3] * _m2(3, 1),
652 
653  this->data[2][0] * _m2(0, 2) +
654  this->data[2][1] * _m2(1, 2) +
655  this->data[2][2] * _m2(2, 2) +
656  this->data[2][3] * _m2(3, 2),
657 
658  this->data[2][0] * _m2(0, 3) +
659  this->data[2][1] * _m2(1, 3) +
660  this->data[2][2] * _m2(2, 3) +
661  this->data[2][3] * _m2(3, 3),
662 
663  this->data[3][0] * _m2(0, 0) +
664  this->data[3][1] * _m2(1, 0) +
665  this->data[3][2] * _m2(2, 0) +
666  this->data[3][3] * _m2(3, 0),
667 
668  this->data[3][0] * _m2(0, 1) +
669  this->data[3][1] * _m2(1, 1) +
670  this->data[3][2] * _m2(2, 1) +
671  this->data[3][3] * _m2(3, 1),
672 
673  this->data[3][0] * _m2(0, 2) +
674  this->data[3][1] * _m2(1, 2) +
675  this->data[3][2] * _m2(2, 2) +
676  this->data[3][3] * _m2(3, 2),
677 
678  this->data[3][0] * _m2(0, 3) +
679  this->data[3][1] * _m2(1, 3) +
680  this->data[3][2] * _m2(2, 3) +
681  this->data[3][3] * _m2(3, 3));
682  }
683 
687  public: Vector3<T> operator*(const Vector3<T> &_vec) const
688  {
689  return Vector3<T>(
690  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
691  this->data[0][2]*_vec.Z() + this->data[0][3],
692  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
693  this->data[1][2]*_vec.Z() + this->data[1][3],
694  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
695  this->data[2][2]*_vec.Z() + this->data[2][3]);
696  }
697 
704  public: inline const T &operator()(const size_t _row,
705  const size_t _col) const
706  {
707  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)][
709  }
710 
718  public: inline T &operator()(const size_t _row, const size_t _col)
719  {
720  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]
722  }
723 
729  public: bool Equal(const Matrix4 &_m, const T &_tol) const
730  {
731  return equal<T>(this->data[0][0], _m(0, 0), _tol)
732  && equal<T>(this->data[0][1], _m(0, 1), _tol)
733  && equal<T>(this->data[0][2], _m(0, 2), _tol)
734  && equal<T>(this->data[0][3], _m(0, 3), _tol)
735  && equal<T>(this->data[1][0], _m(1, 0), _tol)
736  && equal<T>(this->data[1][1], _m(1, 1), _tol)
737  && equal<T>(this->data[1][2], _m(1, 2), _tol)
738  && equal<T>(this->data[1][3], _m(1, 3), _tol)
739  && equal<T>(this->data[2][0], _m(2, 0), _tol)
740  && equal<T>(this->data[2][1], _m(2, 1), _tol)
741  && equal<T>(this->data[2][2], _m(2, 2), _tol)
742  && equal<T>(this->data[2][3], _m(2, 3), _tol)
743  && equal<T>(this->data[3][0], _m(3, 0), _tol)
744  && equal<T>(this->data[3][1], _m(3, 1), _tol)
745  && equal<T>(this->data[3][2], _m(3, 2), _tol)
746  && equal<T>(this->data[3][3], _m(3, 3), _tol);
747  }
748 
753  public: bool operator==(const Matrix4<T> &_m) const
754  {
755  return this->Equal(_m, static_cast<T>(1e-6));
756  }
757 
761  public: bool operator!=(const Matrix4<T> &_m) const
762  {
763  return !(*this == _m);
764  }
765 
770  public: friend std::ostream &operator<<(
771  std::ostream &_out, const ignition::math::Matrix4<T> &_m)
772  {
773  _out << precision(_m(0, 0), 6) << " "
774  << precision(_m(0, 1), 6) << " "
775  << precision(_m(0, 2), 6) << " "
776  << precision(_m(0, 3), 6) << " "
777  << precision(_m(1, 0), 6) << " "
778  << precision(_m(1, 1), 6) << " "
779  << precision(_m(1, 2), 6) << " "
780  << precision(_m(1, 3), 6) << " "
781  << precision(_m(2, 0), 6) << " "
782  << precision(_m(2, 1), 6) << " "
783  << precision(_m(2, 2), 6) << " "
784  << precision(_m(2, 3), 6) << " "
785  << precision(_m(3, 0), 6) << " "
786  << precision(_m(3, 1), 6) << " "
787  << precision(_m(3, 2), 6) << " "
788  << precision(_m(3, 3), 6);
789 
790  return _out;
791  }
792 
797  public: friend std::istream &operator>>(
798  std::istream &_in, ignition::math::Matrix4<T> &_m)
799  {
800  // Skip white spaces
801  _in.setf(std::ios_base::skipws);
802  T d[16];
803  _in >> d[0] >> d[1] >> d[2] >> d[3]
804  >> d[4] >> d[5] >> d[6] >> d[7]
805  >> d[8] >> d[9] >> d[10] >> d[11]
806  >> d[12] >> d[13] >> d[14] >> d[15];
807 
808  _m.Set(d[0], d[1], d[2], d[3],
809  d[4], d[5], d[6], d[7],
810  d[8], d[9], d[10], d[11],
811  d[12], d[13], d[14], d[15]);
812  return _in;
813  }
814 
824  public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
825  const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
826  {
827  // Most important constraint: direction to point X axis at
828  auto front = _target - _eye;
829 
830  // Case when _eye == _target
831  if (front == Vector3<T>::Zero)
832  front = Vector3<T>::UnitX;
833  front.Normalize();
834 
835  // Desired direction to point Z axis at
836  auto up = _up;
837 
838  // Case when _up == Zero
839  if (up == Vector3<T>::Zero)
840  up = Vector3<T>::UnitZ;
841  else
842  up.Normalize();
843 
844  // Case when _up is parallel to X
845  if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
846  up = Vector3<T>::UnitZ;
847 
848  // Find direction to point Y axis at
849  auto left = up.Cross(front);
850 
851  // Case when front is parallel to up
852  if (left == Vector3<T>::Zero)
853  left = Vector3<T>::UnitY;
854  else
855  left.Normalize();
856 
857  // Fix up direction so it's perpendicular to XY
858  up = (front.Cross(left)).Normalize();
859 
860  return Matrix4<T>(
861  front.X(), left.X(), up.X(), _eye.X(),
862  front.Y(), left.Y(), up.Y(), _eye.Y(),
863  front.Z(), left.Z(), up.Z(), _eye.Z(),
864  0, 0, 0, 1);
865  }
866 
868  private: T data[4][4];
869  };
870 
871  template<typename T>
872  const Matrix4<T> Matrix4<T>::Identity(
873  1, 0, 0, 0,
874  0, 1, 0, 0,
875  0, 0, 1, 0,
876  0, 0, 0, 1);
877 
878  template<typename T>
879  const Matrix4<T> Matrix4<T>::Zero(
880  0, 0, 0, 0,
881  0, 0, 0, 0,
882  0, 0, 0, 0,
883  0, 0, 0, 0);
884 
888  }
889 }
890 #endif
const T & W() const
Get the w component.
Definition: Quaternion.hh:935
T X() const
Get the x value.
Definition: Vector3.hh:635
Pose3< T > Pose() const
Get the transformation as math::Pose.
Definition: Matrix4.hh:339
static const Matrix4< T > Identity
Identity matrix.
Definition: Matrix4.hh:36
const T & Z() const
Get the z component.
Definition: Quaternion.hh:956
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator.
Definition: Matrix4.hh:687
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:345
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:175
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:576
const Matrix4< T > & operator=(const Matrix3< T > &_mat)
Equal operator for 3x3 matrix.
Definition: Matrix4.hh:580
Matrix4< int > Matrix4i
Definition: Matrix4.hh:885
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition: Helpers.hh:222
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
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
bool TransformAffine(const Vector3< T > &_v, Vector3< T > &_result) const
Perform an affine transformation.
Definition: Matrix4.hh:407
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix4< T > &_m)
Stream extraction operator.
Definition: Matrix4.hh:797
A 4x4 matrix class.
Definition: Matrix4.hh:33
static const Matrix4< T > Zero
Zero matrix.
Definition: Matrix4.hh:39
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: Helpers.hh:542
void Scale(const Vector3< T > &_s)
Set the scale.
Definition: Matrix4.hh:346
bool operator==(const Matrix4< T > &_m) const
Equality operator.
Definition: Matrix4.hh:753
Matrix4< T > operator*(const Matrix4< T > &_m2) const
Multiplication operator.
Definition: Matrix4.hh:600
Vector3 Normalize()
Normalize the vector length.
Definition: Vector3.hh:129
bool operator!=(const Matrix4< T > &_m) const
Inequality test operator.
Definition: Matrix4.hh:761
Quaternion< T > Rotation() const
Get the rotation as a quaternion.
Definition: Matrix4.hh:218
A 3x3 matrix class.
Definition: Matrix3.hh:35
Matrix4()
Constructor.
Definition: Matrix4.hh:42
const T & Y() const
Get the y component.
Definition: Quaternion.hh:949
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix4.hh:424
T Y() const
Get the y value.
Definition: Vector3.hh:642
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
T & operator()(const size_t _row, const size_t _col)
Get a mutable version the value at the specified row, column index.
Definition: Matrix4.hh:718
void Transpose()
Transpose this matrix.
Definition: Matrix4.hh:547
Matrix4< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix4.hh:559
void Axis(const Vector3< T > &_axis, T _angle)
Set the upper-left 3x3 matrix from an axis and angle.
Definition: Matrix4.hh:163
Vector3< T > EulerRotation(bool _firstSolution) const
Get the rotation as a Euler angles.
Definition: Matrix4.hh:286
void Scale(T _x, T _y, T _z)
Set the scale.
Definition: Matrix4.hh:358
static const double IGN_DEPRECATED(2) DPRCT_MAX_D
const T & X() const
Get the x component.
Definition: Quaternion.hh:942
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
Matrix4< float > Matrix4f
Definition: Matrix4.hh:887
T Z() const
Get the z value.
Definition: Vector3.hh:649
bool Equal(const Matrix4 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix4.hh:729
bool IsAffine() const
Return true if the matrix is affine.
Definition: Matrix4.hh:368
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:185
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition: Matrix4.hh:455
void Translate(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:184
Vector3< T > TransformAffine(const Vector3< T > &_v) const IGN_DEPRECATED(3.0)
Perform an affine transformation.
Definition: Matrix4.hh:382
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
Vector3< T > Scale() const
Get the scale values as a Vector3<T>
Definition: Matrix4.hh:211
static Matrix4< T > LookAt(const Vector3< T > &_eye, const Vector3< T > &_target, const Vector3< T > &_up=Vector3< T >::UnitZ)
Get transform which translates to _eye and rotates the X axis so it faces the _target.
Definition: Matrix4.hh:824
Definition: Angle.hh:38
const T & operator()(const size_t _row, const size_t _col) const
Get the value at the specified row, column index.
Definition: Matrix4.hh:704
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
Vector3< T > Translation() const
Get the translational values as a Vector3.
Definition: Matrix4.hh:204
Matrix4< double > Matrix4d
Definition: Matrix4.hh:886
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:220
A quaternion class.
Definition: Matrix3.hh:30
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:173
Matrix4< T > & operator=(const Matrix4< T > &_mat)
Equal operator.
Definition: Matrix4.hh:571
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix4< T > &_m)
Stream insertion operator.
Definition: Matrix4.hh:770
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392