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 #include <ignition/math/config.hh>
26 
27 namespace ignition
28 {
29  namespace math
30  {
31  inline namespace IGNITION_MATH_VERSION_NAMESPACE
32  {
35  template<typename T>
36  class Matrix4
37  {
39  public: static const Matrix4<T> Identity;
40 
42  public: static const Matrix4<T> Zero;
43 
45  public: Matrix4()
46  {
47  memset(this->data, 0, sizeof(this->data[0][0])*16);
48  }
49 
52  public: Matrix4(const Matrix4<T> &_m)
53  {
54  memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
55  }
56 
74  public: Matrix4(T _v00, T _v01, T _v02, T _v03,
75  T _v10, T _v11, T _v12, T _v13,
76  T _v20, T _v21, T _v22, T _v23,
77  T _v30, T _v31, T _v32, T _v33)
78  {
79  this->Set(_v00, _v01, _v02, _v03,
80  _v10, _v11, _v12, _v13,
81  _v20, _v21, _v22, _v23,
82  _v30, _v31, _v32, _v33);
83  }
84 
87  public: explicit Matrix4(const Quaternion<T> &_q)
88  {
89  Quaternion<T> qt = _q;
90  qt.Normalize();
91  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
92  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
93  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
94  0,
95 
96  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
97  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
98  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
99  0,
100 
101  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
102  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
103  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
104  0,
105 
106  0, 0, 0, 1);
107  }
108 
111  public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
112  {
113  this->SetTranslation(_pose.Pos());
114  }
115 
117  public: virtual ~Matrix4() {}
118 
136  public: void Set(
137  T _v00, T _v01, T _v02, T _v03,
138  T _v10, T _v11, T _v12, T _v13,
139  T _v20, T _v21, T _v22, T _v23,
140  T _v30, T _v31, T _v32, T _v33)
141  {
142  this->data[0][0] = _v00;
143  this->data[0][1] = _v01;
144  this->data[0][2] = _v02;
145  this->data[0][3] = _v03;
146 
147  this->data[1][0] = _v10;
148  this->data[1][1] = _v11;
149  this->data[1][2] = _v12;
150  this->data[1][3] = _v13;
151 
152  this->data[2][0] = _v20;
153  this->data[2][1] = _v21;
154  this->data[2][2] = _v22;
155  this->data[2][3] = _v23;
156 
157  this->data[3][0] = _v30;
158  this->data[3][1] = _v31;
159  this->data[3][2] = _v32;
160  this->data[3][3] = _v33;
161  }
162 
166  public: void Axis(const Vector3<T> &_axis, T _angle)
167  {
168  T c = cos(_angle);
169  T s = sin(_angle);
170  T C = 1-c;
171 
172  this->data[0][0] = _axis.X()*_axis.X()*C + c;
173  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
174  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
175 
176  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
177  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
178  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
179 
180  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
181  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
182  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
183  }
184 
188  public: void
190  Translate(const Vector3<T> &_t)
191  {
192  this->SetTranslation(_t);
193  }
194 
197  public: void SetTranslation(const Vector3<T> &_t)
198  {
199  this->data[0][3] = _t.X();
200  this->data[1][3] = _t.Y();
201  this->data[2][3] = _t.Z();
202  }
203 
209  public: void
210  IGN_DEPRECATED(4)
211  Translate(T _x, T _y, T _z)
212  {
213  this->SetTranslation(_x, _y, _z);
214  }
215 
220  public: void SetTranslation(T _x, T _y, T _z)
221  {
222  this->data[0][3] = _x;
223  this->data[1][3] = _y;
224  this->data[2][3] = _z;
225  }
226 
229  public: Vector3<T> Translation() const
230  {
231  return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
232  }
233 
236  public: Vector3<T> Scale() const
237  {
238  return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
239  }
240 
243  public: Quaternion<T> Rotation() const
244  {
245  Quaternion<T> q;
248  T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
249  T root;
250  if (trace > 0)
251  {
252  root = sqrt(trace + 1.0);
253  q.W(root / 2.0);
254  root = 1.0 / (2.0 * root);
255  q.X((this->data[2][1] - this->data[1][2]) * root);
256  q.Y((this->data[0][2] - this->data[2][0]) * root);
257  q.Z((this->data[1][0] - this->data[0][1]) * root);
258  }
259  else
260  {
261  static unsigned int s_iNext[3] = {1, 2, 0};
262  unsigned int i = 0;
263  if (this->data[1][1] > this->data[0][0])
264  i = 1;
265  if (this->data[2][2] > this->data[i][i])
266  i = 2;
267  unsigned int j = s_iNext[i];
268  unsigned int k = s_iNext[j];
269 
270  root = sqrt(this->data[i][i] - this->data[j][j] -
271  this->data[k][k] + 1.0);
272 
273  T a, b, c;
274  a = root / 2.0;
275  root = 1.0 / (2.0 * root);
276  b = (this->data[j][i] + this->data[i][j]) * root;
277  c = (this->data[k][i] + this->data[i][k]) * root;
278 
279  switch (i)
280  {
281  default:
282  case 0: q.X(a); break;
283  case 1: q.Y(a); break;
284  case 2: q.Z(a); break;
285  };
286  switch (j)
287  {
288  default:
289  case 0: q.X(b); break;
290  case 1: q.Y(b); break;
291  case 2: q.Z(b); break;
292  };
293  switch (k)
294  {
295  default:
296  case 0: q.X(c); break;
297  case 1: q.Y(c); break;
298  case 2: q.Z(c); break;
299  };
300 
301  q.W((this->data[k][j] - this->data[j][k]) * root);
302  }
303 
304  return q;
305  }
306 
311  public: Vector3<T> EulerRotation(bool _firstSolution) const
312  {
313  Vector3<T> euler;
314  Vector3<T> euler2;
315 
316  T m31 = this->data[2][0];
317  T m11 = this->data[0][0];
318  T m12 = this->data[0][1];
319  T m13 = this->data[0][2];
320  T m32 = this->data[2][1];
321  T m33 = this->data[2][2];
322  T m21 = this->data[1][0];
323 
324  if (std::abs(m31) >= 1.0)
325  {
326  euler.Z(0.0);
327  euler2.Z(0.0);
328 
329  if (m31 < 0.0)
330  {
331  euler.Y(IGN_PI / 2.0);
332  euler2.Y(IGN_PI / 2.0);
333  euler.X(atan2(m12, m13));
334  euler2.X(atan2(m12, m13));
335  }
336  else
337  {
338  euler.Y(-IGN_PI / 2.0);
339  euler2.Y(-IGN_PI / 2.0);
340  euler.X(atan2(-m12, -m13));
341  euler2.X(atan2(-m12, -m13));
342  }
343  }
344  else
345  {
346  euler.Y(-asin(m31));
347  euler2.Y(IGN_PI - euler.Y());
348 
349  euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
350  euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
351 
352  euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
353  euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
354  }
355 
356  if (_firstSolution)
357  return euler;
358  else
359  return euler2;
360  }
361 
364  public: Pose3<T> Pose() const
365  {
366  return Pose3<T>(this->Translation(), this->Rotation());
367  }
368 
371  public: void Scale(const Vector3<T> &_s)
372  {
373  this->data[0][0] = _s.X();
374  this->data[1][1] = _s.Y();
375  this->data[2][2] = _s.Z();
376  this->data[3][3] = 1.0;
377  }
378 
383  public: void Scale(T _x, T _y, T _z)
384  {
385  this->data[0][0] = _x;
386  this->data[1][1] = _y;
387  this->data[2][2] = _z;
388  this->data[3][3] = 1.0;
389  }
390 
393  public: bool IsAffine() const
394  {
395  return equal(this->data[3][0], static_cast<T>(0)) &&
396  equal(this->data[3][1], static_cast<T>(0)) &&
397  equal(this->data[3][2], static_cast<T>(0)) &&
398  equal(this->data[3][3], static_cast<T>(1));
399  }
400 
407  public: Vector3<T>
409  TransformAffine(const Vector3<T> &_v) const
410  {
411  if (this->IsAffine())
412  {
413  return Vector3<T>(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  }
420  else
421  {
422  return Vector3<T>();
423  }
424  }
425 
431  public: bool TransformAffine(const Vector3<T> &_v,
432  Vector3<T> &_result) const
433  {
434  if (!this->IsAffine())
435  return false;
436 
437  _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
438  this->data[0][2]*_v.Z() + this->data[0][3],
439  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
440  this->data[1][2]*_v.Z() + this->data[1][3],
441  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
442  this->data[2][2]*_v.Z() + this->data[2][3]);
443  return true;
444  }
445 
448  public: T Determinant() const
449  {
450  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
451 
452  v0 = this->data[2][0]*this->data[3][1]
453  - this->data[2][1]*this->data[3][0];
454  v1 = this->data[2][0]*this->data[3][2]
455  - this->data[2][2]*this->data[3][0];
456  v2 = this->data[2][0]*this->data[3][3]
457  - this->data[2][3]*this->data[3][0];
458  v3 = this->data[2][1]*this->data[3][2]
459  - this->data[2][2]*this->data[3][1];
460  v4 = this->data[2][1]*this->data[3][3]
461  - this->data[2][3]*this->data[3][1];
462  v5 = this->data[2][2]*this->data[3][3]
463  - this->data[2][3]*this->data[3][2];
464 
465  t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
466  t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
467  t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
468  t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
469 
470  return t00 * this->data[0][0]
471  + t10 * this->data[0][1]
472  + t20 * this->data[0][2]
473  + t30 * this->data[0][3];
474  }
475 
479  public: Matrix4<T> Inverse() const
480  {
481  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
482  Matrix4<T> r;
483 
484  v0 = this->data[2][0]*this->data[3][1] -
485  this->data[2][1]*this->data[3][0];
486  v1 = this->data[2][0]*this->data[3][2] -
487  this->data[2][2]*this->data[3][0];
488  v2 = this->data[2][0]*this->data[3][3] -
489  this->data[2][3]*this->data[3][0];
490  v3 = this->data[2][1]*this->data[3][2] -
491  this->data[2][2]*this->data[3][1];
492  v4 = this->data[2][1]*this->data[3][3] -
493  this->data[2][3]*this->data[3][1];
494  v5 = this->data[2][2]*this->data[3][3] -
495  this->data[2][3]*this->data[3][2];
496 
497  t00 = +(v5*this->data[1][1] -
498  v4*this->data[1][2] + v3*this->data[1][3]);
499  t10 = -(v5*this->data[1][0] -
500  v2*this->data[1][2] + v1*this->data[1][3]);
501  t20 = +(v4*this->data[1][0] -
502  v2*this->data[1][1] + v0*this->data[1][3]);
503  t30 = -(v3*this->data[1][0] -
504  v1*this->data[1][1] + v0*this->data[1][2]);
505 
506  T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
507  t20 * this->data[0][2] + t30 * this->data[0][3]);
508 
509  r(0, 0) = t00 * invDet;
510  r(1, 0) = t10 * invDet;
511  r(2, 0) = t20 * invDet;
512  r(3, 0) = t30 * invDet;
513 
514  r(0, 1) = -(v5*this->data[0][1] -
515  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
516  r(1, 1) = +(v5*this->data[0][0] -
517  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
518  r(2, 1) = -(v4*this->data[0][0] -
519  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
520  r(3, 1) = +(v3*this->data[0][0] -
521  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
522 
523  v0 = this->data[1][0]*this->data[3][1] -
524  this->data[1][1]*this->data[3][0];
525  v1 = this->data[1][0]*this->data[3][2] -
526  this->data[1][2]*this->data[3][0];
527  v2 = this->data[1][0]*this->data[3][3] -
528  this->data[1][3]*this->data[3][0];
529  v3 = this->data[1][1]*this->data[3][2] -
530  this->data[1][2]*this->data[3][1];
531  v4 = this->data[1][1]*this->data[3][3] -
532  this->data[1][3]*this->data[3][1];
533  v5 = this->data[1][2]*this->data[3][3] -
534  this->data[1][3]*this->data[3][2];
535 
536  r(0, 2) = +(v5*this->data[0][1] -
537  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
538  r(1, 2) = -(v5*this->data[0][0] -
539  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
540  r(2, 2) = +(v4*this->data[0][0] -
541  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
542  r(3, 2) = -(v3*this->data[0][0] -
543  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
544 
545  v0 = this->data[2][1]*this->data[1][0] -
546  this->data[2][0]*this->data[1][1];
547  v1 = this->data[2][2]*this->data[1][0] -
548  this->data[2][0]*this->data[1][2];
549  v2 = this->data[2][3]*this->data[1][0] -
550  this->data[2][0]*this->data[1][3];
551  v3 = this->data[2][2]*this->data[1][1] -
552  this->data[2][1]*this->data[1][2];
553  v4 = this->data[2][3]*this->data[1][1] -
554  this->data[2][1]*this->data[1][3];
555  v5 = this->data[2][3]*this->data[1][2] -
556  this->data[2][2]*this->data[1][3];
557 
558  r(0, 3) = -(v5*this->data[0][1] -
559  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
560  r(1, 3) = +(v5*this->data[0][0] -
561  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
562  r(2, 3) = -(v4*this->data[0][0] -
563  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
564  r(3, 3) = +(v3*this->data[0][0] -
565  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
566 
567  return r;
568  }
569 
571  public: void Transpose()
572  {
573  std::swap(this->data[0][1], this->data[1][0]);
574  std::swap(this->data[0][2], this->data[2][0]);
575  std::swap(this->data[0][3], this->data[3][0]);
576  std::swap(this->data[1][2], this->data[2][1]);
577  std::swap(this->data[1][3], this->data[3][1]);
578  std::swap(this->data[2][3], this->data[3][2]);
579  }
580 
583  public: Matrix4<T> Transposed() const
584  {
585  return Matrix4<T>(
586  this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
587  this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
588  this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
589  this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
590  }
591 
595  public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
596  {
597  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
598  return *this;
599  }
600 
604  public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
605  {
606  this->data[0][0] = _mat(0, 0);
607  this->data[0][1] = _mat(0, 1);
608  this->data[0][2] = _mat(0, 2);
609 
610  this->data[1][0] = _mat(1, 0);
611  this->data[1][1] = _mat(1, 1);
612  this->data[1][2] = _mat(1, 2);
613 
614  this->data[2][0] = _mat(2, 0);
615  this->data[2][1] = _mat(2, 1);
616  this->data[2][2] = _mat(2, 2);
617 
618  return *this;
619  }
620 
624  public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
625  {
626  return Matrix4<T>(
627  this->data[0][0] * _m2(0, 0) +
628  this->data[0][1] * _m2(1, 0) +
629  this->data[0][2] * _m2(2, 0) +
630  this->data[0][3] * _m2(3, 0),
631 
632  this->data[0][0] * _m2(0, 1) +
633  this->data[0][1] * _m2(1, 1) +
634  this->data[0][2] * _m2(2, 1) +
635  this->data[0][3] * _m2(3, 1),
636 
637  this->data[0][0] * _m2(0, 2) +
638  this->data[0][1] * _m2(1, 2) +
639  this->data[0][2] * _m2(2, 2) +
640  this->data[0][3] * _m2(3, 2),
641 
642  this->data[0][0] * _m2(0, 3) +
643  this->data[0][1] * _m2(1, 3) +
644  this->data[0][2] * _m2(2, 3) +
645  this->data[0][3] * _m2(3, 3),
646 
647  this->data[1][0] * _m2(0, 0) +
648  this->data[1][1] * _m2(1, 0) +
649  this->data[1][2] * _m2(2, 0) +
650  this->data[1][3] * _m2(3, 0),
651 
652  this->data[1][0] * _m2(0, 1) +
653  this->data[1][1] * _m2(1, 1) +
654  this->data[1][2] * _m2(2, 1) +
655  this->data[1][3] * _m2(3, 1),
656 
657  this->data[1][0] * _m2(0, 2) +
658  this->data[1][1] * _m2(1, 2) +
659  this->data[1][2] * _m2(2, 2) +
660  this->data[1][3] * _m2(3, 2),
661 
662  this->data[1][0] * _m2(0, 3) +
663  this->data[1][1] * _m2(1, 3) +
664  this->data[1][2] * _m2(2, 3) +
665  this->data[1][3] * _m2(3, 3),
666 
667  this->data[2][0] * _m2(0, 0) +
668  this->data[2][1] * _m2(1, 0) +
669  this->data[2][2] * _m2(2, 0) +
670  this->data[2][3] * _m2(3, 0),
671 
672  this->data[2][0] * _m2(0, 1) +
673  this->data[2][1] * _m2(1, 1) +
674  this->data[2][2] * _m2(2, 1) +
675  this->data[2][3] * _m2(3, 1),
676 
677  this->data[2][0] * _m2(0, 2) +
678  this->data[2][1] * _m2(1, 2) +
679  this->data[2][2] * _m2(2, 2) +
680  this->data[2][3] * _m2(3, 2),
681 
682  this->data[2][0] * _m2(0, 3) +
683  this->data[2][1] * _m2(1, 3) +
684  this->data[2][2] * _m2(2, 3) +
685  this->data[2][3] * _m2(3, 3),
686 
687  this->data[3][0] * _m2(0, 0) +
688  this->data[3][1] * _m2(1, 0) +
689  this->data[3][2] * _m2(2, 0) +
690  this->data[3][3] * _m2(3, 0),
691 
692  this->data[3][0] * _m2(0, 1) +
693  this->data[3][1] * _m2(1, 1) +
694  this->data[3][2] * _m2(2, 1) +
695  this->data[3][3] * _m2(3, 1),
696 
697  this->data[3][0] * _m2(0, 2) +
698  this->data[3][1] * _m2(1, 2) +
699  this->data[3][2] * _m2(2, 2) +
700  this->data[3][3] * _m2(3, 2),
701 
702  this->data[3][0] * _m2(0, 3) +
703  this->data[3][1] * _m2(1, 3) +
704  this->data[3][2] * _m2(2, 3) +
705  this->data[3][3] * _m2(3, 3));
706  }
707 
711  public: Vector3<T> operator*(const Vector3<T> &_vec) const
712  {
713  return Vector3<T>(
714  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
715  this->data[0][2]*_vec.Z() + this->data[0][3],
716  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
717  this->data[1][2]*_vec.Z() + this->data[1][3],
718  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
719  this->data[2][2]*_vec.Z() + this->data[2][3]);
720  }
721 
728  public: inline const T &operator()(const size_t _row,
729  const size_t _col) const
730  {
731  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)][
733  }
734 
742  public: inline T &operator()(const size_t _row, const size_t _col)
743  {
744  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]
746  }
747 
753  public: bool Equal(const Matrix4 &_m, const T &_tol) const
754  {
755  return equal<T>(this->data[0][0], _m(0, 0), _tol)
756  && equal<T>(this->data[0][1], _m(0, 1), _tol)
757  && equal<T>(this->data[0][2], _m(0, 2), _tol)
758  && equal<T>(this->data[0][3], _m(0, 3), _tol)
759  && equal<T>(this->data[1][0], _m(1, 0), _tol)
760  && equal<T>(this->data[1][1], _m(1, 1), _tol)
761  && equal<T>(this->data[1][2], _m(1, 2), _tol)
762  && equal<T>(this->data[1][3], _m(1, 3), _tol)
763  && equal<T>(this->data[2][0], _m(2, 0), _tol)
764  && equal<T>(this->data[2][1], _m(2, 1), _tol)
765  && equal<T>(this->data[2][2], _m(2, 2), _tol)
766  && equal<T>(this->data[2][3], _m(2, 3), _tol)
767  && equal<T>(this->data[3][0], _m(3, 0), _tol)
768  && equal<T>(this->data[3][1], _m(3, 1), _tol)
769  && equal<T>(this->data[3][2], _m(3, 2), _tol)
770  && equal<T>(this->data[3][3], _m(3, 3), _tol);
771  }
772 
777  public: bool operator==(const Matrix4<T> &_m) const
778  {
779  return this->Equal(_m, static_cast<T>(1e-6));
780  }
781 
785  public: bool operator!=(const Matrix4<T> &_m) const
786  {
787  return !(*this == _m);
788  }
789 
794  public: friend std::ostream &operator<<(
795  std::ostream &_out, const ignition::math::Matrix4<T> &_m)
796  {
797  _out << precision(_m(0, 0), 6) << " "
798  << precision(_m(0, 1), 6) << " "
799  << precision(_m(0, 2), 6) << " "
800  << precision(_m(0, 3), 6) << " "
801  << precision(_m(1, 0), 6) << " "
802  << precision(_m(1, 1), 6) << " "
803  << precision(_m(1, 2), 6) << " "
804  << precision(_m(1, 3), 6) << " "
805  << precision(_m(2, 0), 6) << " "
806  << precision(_m(2, 1), 6) << " "
807  << precision(_m(2, 2), 6) << " "
808  << precision(_m(2, 3), 6) << " "
809  << precision(_m(3, 0), 6) << " "
810  << precision(_m(3, 1), 6) << " "
811  << precision(_m(3, 2), 6) << " "
812  << precision(_m(3, 3), 6);
813 
814  return _out;
815  }
816 
821  public: friend std::istream &operator>>(
822  std::istream &_in, ignition::math::Matrix4<T> &_m)
823  {
824  // Skip white spaces
825  _in.setf(std::ios_base::skipws);
826  T d[16];
827  _in >> d[0] >> d[1] >> d[2] >> d[3]
828  >> d[4] >> d[5] >> d[6] >> d[7]
829  >> d[8] >> d[9] >> d[10] >> d[11]
830  >> d[12] >> d[13] >> d[14] >> d[15];
831 
832  _m.Set(d[0], d[1], d[2], d[3],
833  d[4], d[5], d[6], d[7],
834  d[8], d[9], d[10], d[11],
835  d[12], d[13], d[14], d[15]);
836  return _in;
837  }
838 
848  public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
849  const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
850  {
851  // Most important constraint: direction to point X axis at
852  auto front = _target - _eye;
853 
854  // Case when _eye == _target
855  if (front == Vector3<T>::Zero)
856  front = Vector3<T>::UnitX;
857  front.Normalize();
858 
859  // Desired direction to point Z axis at
860  auto up = _up;
861 
862  // Case when _up == Zero
863  if (up == Vector3<T>::Zero)
864  up = Vector3<T>::UnitZ;
865  else
866  up.Normalize();
867 
868  // Case when _up is parallel to X
869  if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
870  up = Vector3<T>::UnitZ;
871 
872  // Find direction to point Y axis at
873  auto left = up.Cross(front);
874 
875  // Case when front is parallel to up
876  if (left == Vector3<T>::Zero)
877  left = Vector3<T>::UnitY;
878  else
879  left.Normalize();
880 
881  // Fix up direction so it's perpendicular to XY
882  up = (front.Cross(left)).Normalize();
883 
884  return Matrix4<T>(
885  front.X(), left.X(), up.X(), _eye.X(),
886  front.Y(), left.Y(), up.Y(), _eye.Y(),
887  front.Z(), left.Z(), up.Z(), _eye.Z(),
888  0, 0, 0, 1);
889  }
890 
892  private: T data[4][4];
893  };
894 
895  template<typename T>
896  const Matrix4<T> Matrix4<T>::Identity(
897  1, 0, 0, 0,
898  0, 1, 0, 0,
899  0, 0, 1, 0,
900  0, 0, 0, 1);
901 
902  template<typename T>
903  const Matrix4<T> Matrix4<T>::Zero(
904  0, 0, 0, 0,
905  0, 0, 0, 0,
906  0, 0, 0, 0,
907  0, 0, 0, 0);
908 
912  }
913  }
914 }
915 #endif
Vector3< T > EulerRotation(bool _firstSolution) const
Get the rotation as a Euler angles.
Definition: Matrix4.hh:311
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:579
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:848
Matrix4< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix4.hh:583
Quaternion< T > Rotation() const
Get the rotation as a quaternion.
Definition: Matrix4.hh:243
Vector3< T > Scale() const
Get the scale values as a Vector3<T>
Definition: Matrix4.hh:236
Matrix4< T > & operator=(const Matrix4< T > &_mat)
Equal operator.
Definition: Matrix4.hh:595
Matrix4< int > Matrix4i
Definition: Matrix4.hh:909
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix4< T > &_m)
Stream extraction operator.
Definition: Matrix4.hh:821
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:348
bool TransformAffine(const Vector3< T > &_v, Vector3< T > &_result) const
Perform an affine transformation.
Definition: Matrix4.hh:431
bool IsAffine() const
Return true if the matrix is affine.
Definition: Matrix4.hh:393
void Axis(const Vector3< T > &_axis, T _angle)
Set the upper-left 3x3 matrix from an axis and angle.
Definition: Matrix4.hh:166
A quaternion class.
Definition: Matrix3.hh:33
Matrix4(const Matrix4< T > &_m)
Copy constructor.
Definition: Matrix4.hh:52
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition: Matrix4.hh:479
T Y() const
Get the y value.
Definition: Vector3.hh:654
bool operator==(const Matrix4< T > &_m) const
Equality operator.
Definition: Matrix4.hh:777
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:545
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:136
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:188
Vector3 Normalize()
Normalize the vector length.
Definition: Vector3.hh:132
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:74
static const Matrix4< T > Zero
Zero matrix.
Definition: Matrix4.hh:42
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:33
const T & Z() const
Get the z component.
Definition: Quaternion.hh:969
T Z() const
Get the z value.
Definition: Vector3.hh:661
Vector3< T > Translation() const
Get the translational values as a Vector3.
Definition: Matrix4.hh:229
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:216
const T & W() const
Get the w component.
Definition: Quaternion.hh:948
const T & Y() const
Get the y component.
Definition: Quaternion.hh:962
bool Equal(const Matrix4 &_m, const T &_tol) const
Equality test with tolerance.
Definition: Matrix4.hh:753
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Matrix4< float > Matrix4f
Definition: Matrix4.hh:911
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:742
const T & X() const
Get the x component.
Definition: Quaternion.hh:955
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:178
T X() const
Get the x value.
Definition: Vector3.hh:647
Pose3< T > Pose() const
Get the transformation as math::Pose.
Definition: Matrix4.hh:364
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:223
Matrix4(const Quaternion< T > &_q)
Construct Matrix4 from a quaternion.
Definition: Matrix4.hh:87
A 4x4 matrix class.
Definition: Matrix4.hh:36
void SetTranslation(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:197
void Scale(T _x, T _y, T _z)
Set the scale.
Definition: Matrix4.hh:383
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition: Helpers.hh:225
Vector3< T > IGN_DEPRECATED(3.0) TransformAffine(const Vector3< T > &_v) const
Perform an affine transformation.
Definition: Matrix4.hh:408
Matrix4< double > Matrix4d
Definition: Matrix4.hh:910
const Matrix4< T > & operator=(const Matrix3< T > &_mat)
Equal operator for 3x3 matrix.
Definition: Matrix4.hh:604
void Transpose()
Transpose this matrix.
Definition: Matrix4.hh:571
void SetTranslation(T _x, T _y, T _z)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:220
Matrix4()
Constructor.
Definition: Matrix4.hh:45
Matrix4< T > operator *(const Matrix4< T > &_m2) const
Multiplication operator.
Definition: Matrix4.hh:624
virtual ~Matrix4()
Destructor.
Definition: Matrix4.hh:117
Definition: Angle.hh:39
const T & operator()(const size_t _row, const size_t _col) const
Get the value at the specified row, column index.
Definition: Matrix4.hh:728
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:174
T Determinant() const
Return the determinant of the matrix.
Definition: Matrix4.hh:448
void Scale(const Vector3< T > &_s)
Set the scale.
Definition: Matrix4.hh:371
A 3x3 matrix class.
Definition: Matrix3.hh:38
void IGN_DEPRECATED(4) Translate(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: Matrix4.hh:189
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:395
static const Matrix4< T > Identity
Identity matrix.
Definition: Matrix4.hh:39
Matrix4(const Pose3< T > &_pose)
Construct Matrix4 from a math::Pose3.
Definition: Matrix4.hh:111
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix4< T > &_m)
Stream insertion operator.
Definition: Matrix4.hh:794
bool operator!=(const Matrix4< T > &_m) const
Inequality test operator.
Definition: Matrix4.hh:785