Gazebo Math

API Reference

6.15.1
gz/math/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 GZ_MATH_MATRIX4_HH_
18 #define GZ_MATH_MATRIX4_HH_
19 
20 #include <algorithm>
21 #include <utility>
22 #include <gz/math/Helpers.hh>
23 #include <gz/math/Matrix3.hh>
24 #include <gz/math/Vector3.hh>
25 #include <gz/math/Pose3.hh>
26 #include <gz/math/config.hh>
27 
28 namespace ignition
29 {
30  namespace math
31  {
32  // Inline bracket to help doxygen filtering.
33  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
34  //
37  template<typename T>
38  class Matrix4
39  {
41  public: static const Matrix4<T> Identity;
42 
44  public: static const Matrix4<T> Zero;
45 
47  public: Matrix4()
48  {
49  memset(this->data, 0, sizeof(this->data[0][0])*16);
50  }
51 
54  public: Matrix4(const Matrix4<T> &_m)
55  {
56  memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
57  }
58 
76  public: Matrix4(T _v00, T _v01, T _v02, T _v03,
77  T _v10, T _v11, T _v12, T _v13,
78  T _v20, T _v21, T _v22, T _v23,
79  T _v30, T _v31, T _v32, T _v33)
80  {
81  this->Set(_v00, _v01, _v02, _v03,
82  _v10, _v11, _v12, _v13,
83  _v20, _v21, _v22, _v23,
84  _v30, _v31, _v32, _v33);
85  }
86 
89  public: explicit Matrix4(const Quaternion<T> &_q)
90  {
91  Quaternion<T> qt = _q;
92  qt.Normalize();
93  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
94  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
95  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
96  0,
97 
98  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
99  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
100  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
101  0,
102 
103  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
104  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
105  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
106  0,
107 
108  0, 0, 0, 1);
109  }
110 
113  public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
114  {
115  this->SetTranslation(_pose.Pos());
116  }
117 
119  public: virtual ~Matrix4() {}
120 
138  public: void Set(
139  T _v00, T _v01, T _v02, T _v03,
140  T _v10, T _v11, T _v12, T _v13,
141  T _v20, T _v21, T _v22, T _v23,
142  T _v30, T _v31, T _v32, T _v33)
143  {
144  this->data[0][0] = _v00;
145  this->data[0][1] = _v01;
146  this->data[0][2] = _v02;
147  this->data[0][3] = _v03;
148 
149  this->data[1][0] = _v10;
150  this->data[1][1] = _v11;
151  this->data[1][2] = _v12;
152  this->data[1][3] = _v13;
153 
154  this->data[2][0] = _v20;
155  this->data[2][1] = _v21;
156  this->data[2][2] = _v22;
157  this->data[2][3] = _v23;
158 
159  this->data[3][0] = _v30;
160  this->data[3][1] = _v31;
161  this->data[3][2] = _v32;
162  this->data[3][3] = _v33;
163  }
164 
168  public: void Axis(const Vector3<T> &_axis, T _angle)
169  {
170  T c = cos(_angle);
171  T s = sin(_angle);
172  T C = 1-c;
173 
174  this->data[0][0] = _axis.X()*_axis.X()*C + c;
175  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
176  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
177 
178  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
179  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
180  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
181 
182  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
183  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
184  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
185  }
186 
190  public: void
191  IGN_DEPRECATED(4)
192  Translate(const Vector3<T> &_t)
193  {
194  this->SetTranslation(_t);
195  }
196 
199  public: void SetTranslation(const Vector3<T> &_t)
200  {
201  this->data[0][3] = _t.X();
202  this->data[1][3] = _t.Y();
203  this->data[2][3] = _t.Z();
204  }
205 
211  public: void
212  IGN_DEPRECATED(4)
213  Translate(T _x, T _y, T _z)
214  {
215  this->SetTranslation(_x, _y, _z);
216  }
217 
222  public: void SetTranslation(T _x, T _y, T _z)
223  {
224  this->data[0][3] = _x;
225  this->data[1][3] = _y;
226  this->data[2][3] = _z;
227  }
228 
231  public: Vector3<T> Translation() const
232  {
233  return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
234  }
235 
238  public: Vector3<T> Scale() const
239  {
240  return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
241  }
242 
245  public: Quaternion<T> Rotation() const
246  {
247  Quaternion<T> q;
250  T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
251  T root;
252  if (trace > 0)
253  {
254  root = sqrt(trace + 1.0);
255  q.W(root / 2.0);
256  root = 1.0 / (2.0 * root);
257  q.X((this->data[2][1] - this->data[1][2]) * root);
258  q.Y((this->data[0][2] - this->data[2][0]) * root);
259  q.Z((this->data[1][0] - this->data[0][1]) * root);
260  }
261  else
262  {
263  static unsigned int s_iNext[3] = {1, 2, 0};
264  unsigned int i = 0;
265  if (this->data[1][1] > this->data[0][0])
266  i = 1;
267  if (this->data[2][2] > this->data[i][i])
268  i = 2;
269  unsigned int j = s_iNext[i];
270  unsigned int k = s_iNext[j];
271 
272  root = sqrt(this->data[i][i] - this->data[j][j] -
273  this->data[k][k] + 1.0);
274 
275  T a, b, c;
276  a = root / 2.0;
277  root = 1.0 / (2.0 * root);
278  b = (this->data[j][i] + this->data[i][j]) * root;
279  c = (this->data[k][i] + this->data[i][k]) * root;
280 
281  switch (i)
282  {
283  default:
284  case 0: q.X(a); break;
285  case 1: q.Y(a); break;
286  case 2: q.Z(a); break;
287  };
288  switch (j)
289  {
290  default:
291  case 0: q.X(b); break;
292  case 1: q.Y(b); break;
293  case 2: q.Z(b); break;
294  };
295  switch (k)
296  {
297  default:
298  case 0: q.X(c); break;
299  case 1: q.Y(c); break;
300  case 2: q.Z(c); break;
301  };
302 
303  q.W((this->data[k][j] - this->data[j][k]) * root);
304  }
305 
306  return q;
307  }
308 
313  public: Vector3<T> EulerRotation(bool _firstSolution) const
314  {
315  Vector3<T> euler;
316  Vector3<T> euler2;
317 
318  T m31 = this->data[2][0];
319  T m11 = this->data[0][0];
320  T m12 = this->data[0][1];
321  T m13 = this->data[0][2];
322  T m32 = this->data[2][1];
323  T m33 = this->data[2][2];
324  T m21 = this->data[1][0];
325 
326  if (std::abs(m31) >= 1.0)
327  {
328  euler.Z(0.0);
329  euler2.Z(0.0);
330 
331  if (m31 < 0.0)
332  {
333  euler.Y(IGN_PI / 2.0);
334  euler2.Y(IGN_PI / 2.0);
335  euler.X(atan2(m12, m13));
336  euler2.X(atan2(m12, m13));
337  }
338  else
339  {
340  euler.Y(-IGN_PI / 2.0);
341  euler2.Y(-IGN_PI / 2.0);
342  euler.X(atan2(-m12, -m13));
343  euler2.X(atan2(-m12, -m13));
344  }
345  }
346  else
347  {
348  euler.Y(-asin(m31));
349  euler2.Y(IGN_PI - euler.Y());
350 
351  euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
352  euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
353 
354  euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
355  euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
356  }
357 
358  if (_firstSolution)
359  return euler;
360  else
361  return euler2;
362  }
363 
366  public: Pose3<T> Pose() const
367  {
368  return Pose3<T>(this->Translation(), this->Rotation());
369  }
370 
373  public: void Scale(const Vector3<T> &_s)
374  {
375  this->data[0][0] = _s.X();
376  this->data[1][1] = _s.Y();
377  this->data[2][2] = _s.Z();
378  this->data[3][3] = 1.0;
379  }
380 
385  public: void Scale(T _x, T _y, T _z)
386  {
387  this->data[0][0] = _x;
388  this->data[1][1] = _y;
389  this->data[2][2] = _z;
390  this->data[3][3] = 1.0;
391  }
392 
395  public: bool IsAffine() const
396  {
397  return equal(this->data[3][0], static_cast<T>(0)) &&
398  equal(this->data[3][1], static_cast<T>(0)) &&
399  equal(this->data[3][2], static_cast<T>(0)) &&
400  equal(this->data[3][3], static_cast<T>(1));
401  }
402 
409  public: Vector3<T>
410  IGN_DEPRECATED(3.0)
411  TransformAffine(const Vector3<T> &_v) const
412  {
413  if (this->IsAffine())
414  {
415  return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
416  this->data[0][2]*_v.Z() + this->data[0][3],
417  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
418  this->data[1][2]*_v.Z() + this->data[1][3],
419  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
420  this->data[2][2]*_v.Z() + this->data[2][3]);
421  }
422  else
423  {
424  return Vector3<T>();
425  }
426  }
427 
433  public: bool TransformAffine(const Vector3<T> &_v,
434  Vector3<T> &_result) const
435  {
436  if (!this->IsAffine())
437  return false;
438 
439  _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
440  this->data[0][2]*_v.Z() + this->data[0][3],
441  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
442  this->data[1][2]*_v.Z() + this->data[1][3],
443  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
444  this->data[2][2]*_v.Z() + this->data[2][3]);
445  return true;
446  }
447 
450  public: T Determinant() const
451  {
452  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
453 
454  v0 = this->data[2][0]*this->data[3][1]
455  - this->data[2][1]*this->data[3][0];
456  v1 = this->data[2][0]*this->data[3][2]
457  - this->data[2][2]*this->data[3][0];
458  v2 = this->data[2][0]*this->data[3][3]
459  - this->data[2][3]*this->data[3][0];
460  v3 = this->data[2][1]*this->data[3][2]
461  - this->data[2][2]*this->data[3][1];
462  v4 = this->data[2][1]*this->data[3][3]
463  - this->data[2][3]*this->data[3][1];
464  v5 = this->data[2][2]*this->data[3][3]
465  - this->data[2][3]*this->data[3][2];
466 
467  t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
468  t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
469  t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
470  t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
471 
472  return t00 * this->data[0][0]
473  + t10 * this->data[0][1]
474  + t20 * this->data[0][2]
475  + t30 * this->data[0][3];
476  }
477 
481  public: Matrix4<T> Inverse() const
482  {
483  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
484  Matrix4<T> r;
485 
486  v0 = this->data[2][0]*this->data[3][1] -
487  this->data[2][1]*this->data[3][0];
488  v1 = this->data[2][0]*this->data[3][2] -
489  this->data[2][2]*this->data[3][0];
490  v2 = this->data[2][0]*this->data[3][3] -
491  this->data[2][3]*this->data[3][0];
492  v3 = this->data[2][1]*this->data[3][2] -
493  this->data[2][2]*this->data[3][1];
494  v4 = this->data[2][1]*this->data[3][3] -
495  this->data[2][3]*this->data[3][1];
496  v5 = this->data[2][2]*this->data[3][3] -
497  this->data[2][3]*this->data[3][2];
498 
499  t00 = +(v5*this->data[1][1] -
500  v4*this->data[1][2] + v3*this->data[1][3]);
501  t10 = -(v5*this->data[1][0] -
502  v2*this->data[1][2] + v1*this->data[1][3]);
503  t20 = +(v4*this->data[1][0] -
504  v2*this->data[1][1] + v0*this->data[1][3]);
505  t30 = -(v3*this->data[1][0] -
506  v1*this->data[1][1] + v0*this->data[1][2]);
507 
508  T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
509  t20 * this->data[0][2] + t30 * this->data[0][3]);
510 
511  r(0, 0) = t00 * invDet;
512  r(1, 0) = t10 * invDet;
513  r(2, 0) = t20 * invDet;
514  r(3, 0) = t30 * invDet;
515 
516  r(0, 1) = -(v5*this->data[0][1] -
517  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
518  r(1, 1) = +(v5*this->data[0][0] -
519  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
520  r(2, 1) = -(v4*this->data[0][0] -
521  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
522  r(3, 1) = +(v3*this->data[0][0] -
523  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
524 
525  v0 = this->data[1][0]*this->data[3][1] -
526  this->data[1][1]*this->data[3][0];
527  v1 = this->data[1][0]*this->data[3][2] -
528  this->data[1][2]*this->data[3][0];
529  v2 = this->data[1][0]*this->data[3][3] -
530  this->data[1][3]*this->data[3][0];
531  v3 = this->data[1][1]*this->data[3][2] -
532  this->data[1][2]*this->data[3][1];
533  v4 = this->data[1][1]*this->data[3][3] -
534  this->data[1][3]*this->data[3][1];
535  v5 = this->data[1][2]*this->data[3][3] -
536  this->data[1][3]*this->data[3][2];
537 
538  r(0, 2) = +(v5*this->data[0][1] -
539  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
540  r(1, 2) = -(v5*this->data[0][0] -
541  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
542  r(2, 2) = +(v4*this->data[0][0] -
543  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
544  r(3, 2) = -(v3*this->data[0][0] -
545  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
546 
547  v0 = this->data[2][1]*this->data[1][0] -
548  this->data[2][0]*this->data[1][1];
549  v1 = this->data[2][2]*this->data[1][0] -
550  this->data[2][0]*this->data[1][2];
551  v2 = this->data[2][3]*this->data[1][0] -
552  this->data[2][0]*this->data[1][3];
553  v3 = this->data[2][2]*this->data[1][1] -
554  this->data[2][1]*this->data[1][2];
555  v4 = this->data[2][3]*this->data[1][1] -
556  this->data[2][1]*this->data[1][3];
557  v5 = this->data[2][3]*this->data[1][2] -
558  this->data[2][2]*this->data[1][3];
559 
560  r(0, 3) = -(v5*this->data[0][1] -
561  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
562  r(1, 3) = +(v5*this->data[0][0] -
563  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
564  r(2, 3) = -(v4*this->data[0][0] -
565  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
566  r(3, 3) = +(v3*this->data[0][0] -
567  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
568 
569  return r;
570  }
571 
573  public: void Transpose()
574  {
575  std::swap(this->data[0][1], this->data[1][0]);
576  std::swap(this->data[0][2], this->data[2][0]);
577  std::swap(this->data[0][3], this->data[3][0]);
578  std::swap(this->data[1][2], this->data[2][1]);
579  std::swap(this->data[1][3], this->data[3][1]);
580  std::swap(this->data[2][3], this->data[3][2]);
581  }
582 
585  public: Matrix4<T> Transposed() const
586  {
587  return Matrix4<T>(
588  this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
589  this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
590  this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
591  this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
592  }
593 
597  public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
598  {
599  memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
600  return *this;
601  }
602 
606  public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
607  {
608  this->data[0][0] = _mat(0, 0);
609  this->data[0][1] = _mat(0, 1);
610  this->data[0][2] = _mat(0, 2);
611 
612  this->data[1][0] = _mat(1, 0);
613  this->data[1][1] = _mat(1, 1);
614  this->data[1][2] = _mat(1, 2);
615 
616  this->data[2][0] = _mat(2, 0);
617  this->data[2][1] = _mat(2, 1);
618  this->data[2][2] = _mat(2, 2);
619 
620  return *this;
621  }
622 
627  public: Matrix4<T> operator*=(const Matrix4<T> &_m2)
628  {
629  (*this) = (*this) * _m2;
630  return *this;
631  }
632 
636  public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
637  {
638  return Matrix4<T>(
639  this->data[0][0] * _m2(0, 0) +
640  this->data[0][1] * _m2(1, 0) +
641  this->data[0][2] * _m2(2, 0) +
642  this->data[0][3] * _m2(3, 0),
643 
644  this->data[0][0] * _m2(0, 1) +
645  this->data[0][1] * _m2(1, 1) +
646  this->data[0][2] * _m2(2, 1) +
647  this->data[0][3] * _m2(3, 1),
648 
649  this->data[0][0] * _m2(0, 2) +
650  this->data[0][1] * _m2(1, 2) +
651  this->data[0][2] * _m2(2, 2) +
652  this->data[0][3] * _m2(3, 2),
653 
654  this->data[0][0] * _m2(0, 3) +
655  this->data[0][1] * _m2(1, 3) +
656  this->data[0][2] * _m2(2, 3) +
657  this->data[0][3] * _m2(3, 3),
658 
659  this->data[1][0] * _m2(0, 0) +
660  this->data[1][1] * _m2(1, 0) +
661  this->data[1][2] * _m2(2, 0) +
662  this->data[1][3] * _m2(3, 0),
663 
664  this->data[1][0] * _m2(0, 1) +
665  this->data[1][1] * _m2(1, 1) +
666  this->data[1][2] * _m2(2, 1) +
667  this->data[1][3] * _m2(3, 1),
668 
669  this->data[1][0] * _m2(0, 2) +
670  this->data[1][1] * _m2(1, 2) +
671  this->data[1][2] * _m2(2, 2) +
672  this->data[1][3] * _m2(3, 2),
673 
674  this->data[1][0] * _m2(0, 3) +
675  this->data[1][1] * _m2(1, 3) +
676  this->data[1][2] * _m2(2, 3) +
677  this->data[1][3] * _m2(3, 3),
678 
679  this->data[2][0] * _m2(0, 0) +
680  this->data[2][1] * _m2(1, 0) +
681  this->data[2][2] * _m2(2, 0) +
682  this->data[2][3] * _m2(3, 0),
683 
684  this->data[2][0] * _m2(0, 1) +
685  this->data[2][1] * _m2(1, 1) +
686  this->data[2][2] * _m2(2, 1) +
687  this->data[2][3] * _m2(3, 1),
688 
689  this->data[2][0] * _m2(0, 2) +
690  this->data[2][1] * _m2(1, 2) +
691  this->data[2][2] * _m2(2, 2) +
692  this->data[2][3] * _m2(3, 2),
693 
694  this->data[2][0] * _m2(0, 3) +
695  this->data[2][1] * _m2(1, 3) +
696  this->data[2][2] * _m2(2, 3) +
697  this->data[2][3] * _m2(3, 3),
698 
699  this->data[3][0] * _m2(0, 0) +
700  this->data[3][1] * _m2(1, 0) +
701  this->data[3][2] * _m2(2, 0) +
702  this->data[3][3] * _m2(3, 0),
703 
704  this->data[3][0] * _m2(0, 1) +
705  this->data[3][1] * _m2(1, 1) +
706  this->data[3][2] * _m2(2, 1) +
707  this->data[3][3] * _m2(3, 1),
708 
709  this->data[3][0] * _m2(0, 2) +
710  this->data[3][1] * _m2(1, 2) +
711  this->data[3][2] * _m2(2, 2) +
712  this->data[3][3] * _m2(3, 2),
713 
714  this->data[3][0] * _m2(0, 3) +
715  this->data[3][1] * _m2(1, 3) +
716  this->data[3][2] * _m2(2, 3) +
717  this->data[3][3] * _m2(3, 3));
718  }
719 
723  public: Vector3<T> operator*(const Vector3<T> &_vec) const
724  {
725  return Vector3<T>(
726  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
727  this->data[0][2]*_vec.Z() + this->data[0][3],
728  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
729  this->data[1][2]*_vec.Z() + this->data[1][3],
730  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
731  this->data[2][2]*_vec.Z() + this->data[2][3]);
732  }
733 
740  public: inline const T &operator()(const size_t _row,
741  const size_t _col) const
742  {
743  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)][
745  }
746 
754  public: inline T &operator()(const size_t _row, const size_t _col)
755  {
756  return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]
758  }
759 
765  public: bool Equal(const Matrix4 &_m, const T &_tol) const
766  {
767  return equal<T>(this->data[0][0], _m(0, 0), _tol)
768  && equal<T>(this->data[0][1], _m(0, 1), _tol)
769  && equal<T>(this->data[0][2], _m(0, 2), _tol)
770  && equal<T>(this->data[0][3], _m(0, 3), _tol)
771  && equal<T>(this->data[1][0], _m(1, 0), _tol)
772  && equal<T>(this->data[1][1], _m(1, 1), _tol)
773  && equal<T>(this->data[1][2], _m(1, 2), _tol)
774  && equal<T>(this->data[1][3], _m(1, 3), _tol)
775  && equal<T>(this->data[2][0], _m(2, 0), _tol)
776  && equal<T>(this->data[2][1], _m(2, 1), _tol)
777  && equal<T>(this->data[2][2], _m(2, 2), _tol)
778  && equal<T>(this->data[2][3], _m(2, 3), _tol)
779  && equal<T>(this->data[3][0], _m(3, 0), _tol)
780  && equal<T>(this->data[3][1], _m(3, 1), _tol)
781  && equal<T>(this->data[3][2], _m(3, 2), _tol)
782  && equal<T>(this->data[3][3], _m(3, 3), _tol);
783  }
784 
789  public: bool operator==(const Matrix4<T> &_m) const
790  {
791  return this->Equal(_m, static_cast<T>(1e-6));
792  }
793 
797  public: bool operator!=(const Matrix4<T> &_m) const
798  {
799  return !(*this == _m);
800  }
801 
806  public: friend std::ostream &operator<<(
807  std::ostream &_out, const gz::math::Matrix4<T> &_m)
808  {
809  _out << precision(_m(0, 0), 6) << " "
810  << precision(_m(0, 1), 6) << " "
811  << precision(_m(0, 2), 6) << " "
812  << precision(_m(0, 3), 6) << " "
813  << precision(_m(1, 0), 6) << " "
814  << precision(_m(1, 1), 6) << " "
815  << precision(_m(1, 2), 6) << " "
816  << precision(_m(1, 3), 6) << " "
817  << precision(_m(2, 0), 6) << " "
818  << precision(_m(2, 1), 6) << " "
819  << precision(_m(2, 2), 6) << " "
820  << precision(_m(2, 3), 6) << " "
821  << precision(_m(3, 0), 6) << " "
822  << precision(_m(3, 1), 6) << " "
823  << precision(_m(3, 2), 6) << " "
824  << precision(_m(3, 3), 6);
825 
826  return _out;
827  }
828 
833  public: friend std::istream &operator>>(
834  std::istream &_in, gz::math::Matrix4<T> &_m)
835  {
836  // Skip white spaces
837  _in.setf(std::ios_base::skipws);
838  T d[16];
839  _in >> d[0] >> d[1] >> d[2] >> d[3]
840  >> d[4] >> d[5] >> d[6] >> d[7]
841  >> d[8] >> d[9] >> d[10] >> d[11]
842  >> d[12] >> d[13] >> d[14] >> d[15];
843 
844  if (!_in.fail())
845  {
846  _m.Set(d[0], d[1], d[2], d[3],
847  d[4], d[5], d[6], d[7],
848  d[8], d[9], d[10], d[11],
849  d[12], d[13], d[14], d[15]);
850  }
851  return _in;
852  }
853 
863  public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
864  const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
865  {
866  // Most important constraint: direction to point X axis at
867  auto front = _target - _eye;
868 
869  // Case when _eye == _target
870  if (front == Vector3<T>::Zero)
871  front = Vector3<T>::UnitX;
872  front.Normalize();
873 
874  // Desired direction to point Z axis at
875  auto up = _up;
876 
877  // Case when _up == Zero
878  if (up == Vector3<T>::Zero)
879  up = Vector3<T>::UnitZ;
880  else
881  up.Normalize();
882 
883  // Case when _up is parallel to X
884  if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
885  up = Vector3<T>::UnitZ;
886 
887  // Find direction to point Y axis at
888  auto left = up.Cross(front);
889 
890  // Case when front is parallel to up
891  if (left == Vector3<T>::Zero)
892  left = Vector3<T>::UnitY;
893  else
894  left.Normalize();
895 
896  // Fix up direction so it's perpendicular to XY
897  up = (front.Cross(left)).Normalize();
898 
899  return Matrix4<T>(
900  front.X(), left.X(), up.X(), _eye.X(),
901  front.Y(), left.Y(), up.Y(), _eye.Y(),
902  front.Z(), left.Z(), up.Z(), _eye.Z(),
903  0, 0, 0, 1);
904  }
905 
907  private: T data[4][4];
908  };
909 
910  template<typename T>
911  const Matrix4<T> Matrix4<T>::Identity(
912  1, 0, 0, 0,
913  0, 1, 0, 0,
914  0, 0, 1, 0,
915  0, 0, 0, 1);
916 
917  template<typename T>
918  const Matrix4<T> Matrix4<T>::Zero(
919  0, 0, 0, 0,
920  0, 0, 0, 0,
921  0, 0, 0, 0,
922  0, 0, 0, 0);
923 
927  }
928  }
929 }
930 #endif
friend std::ostream & operator<<(std::ostream &_out, const Matrix4< T > &_m)
Stream insertion operator.
Definition: gz/math/Matrix4.hh:806
void SetTranslation(T _x, T _y, T _z)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: gz/math/Matrix4.hh:222
A 4x4 matrix class.
Definition: gz/math/Matrix4.hh:38
Matrix4< T > operator*(const Matrix4< T > &_m2) const
Multiplication operator.
Definition: gz/math/Matrix4.hh:636
Encapsulates a position and rotation in three space.
Definition: gz/math/Pose3.hh:34
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition: gz/math/Helpers.hh:236
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4. This was put here for Windows support.
Definition: gz/math/Helpers.hh:184
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: gz/math/Helpers.hh:556
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: gz/math/Matrix4.hh:863
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator.
Definition: gz/math/Matrix4.hh:723
void SetTranslation(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: gz/math/Matrix4.hh:199
T Determinant() const
Return the determinant of the matrix.
Definition: gz/math/Matrix4.hh:450
Matrix4< float > Matrix4f
Definition: gz/math/Matrix4.hh:926
Matrix4< T > Inverse() const
Return the inverse matrix. This is a non-destructive operation.
Definition: gz/math/Matrix4.hh:481
const T & operator()(const size_t _row, const size_t _col) const
Get the value at the specified row, column index.
Definition: gz/math/Matrix4.hh:740
Vector3< T > Scale() const
Get the scale values as a Vector3<T>
Definition: gz/math/Matrix4.hh:238
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
static const Matrix4< T > Identity
Identity matrix.
Definition: gz/math/Matrix4.hh:41
A 3x3 matrix class.
Definition: gz/math/Matrix3.hh:40
Vector3< T > TransformAffine(const Vector3< T > &_v) const
Perform an affine transformation.
Definition: gz/math/Matrix4.hh:411
void Axis(const Vector3< T > &_axis, T _angle)
Set the upper-left 3x3 matrix from an axis and angle.
Definition: gz/math/Matrix4.hh:168
Matrix4< int > Matrix4i
Definition: gz/math/Matrix4.hh:924
void Normalize()
Normalize the quaternion.
Definition: gz/math/Quaternion.hh:224
void Translate(T _x, T _y, T _z)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: gz/math/Matrix4.hh:213
const Vector3< T > & Pos() const
Get the position.
Definition: gz/math/Pose3.hh:361
Matrix4(const Quaternion< T > &_q)
Construct Matrix4 from a quaternion.
Definition: gz/math/Matrix4.hh:89
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: gz/math/Helpers.hh:590
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
const T & Y() const
Get the y component.
Definition: gz/math/Quaternion.hh:979
void Scale(T _x, T _y, T _z)
Set the scale.
Definition: gz/math/Matrix4.hh:385
STL class.
Matrix4()
Constructor.
Definition: gz/math/Matrix4.hh:47
friend std::istream & operator>>(std::istream &_in, Matrix4< T > &_m)
Stream extraction operator.
Definition: gz/math/Matrix4.hh:833
Matrix4(const Matrix4< T > &_m)
Copy constructor.
Definition: gz/math/Matrix4.hh:54
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: gz/math/Helpers.hh:406
T setf(T... args)
Matrix4< T > & operator=(const Matrix4< T > &_mat)
Equal operator. this = _mat.
Definition: gz/math/Matrix4.hh:597
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
virtual ~Matrix4()
Destructor.
Definition: gz/math/Matrix4.hh:119
void Scale(const Vector3< T > &_s)
Set the scale.
Definition: gz/math/Matrix4.hh:373
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: gz/math/Vector3.hh:185
const T & Z() const
Get the z component.
Definition: gz/math/Quaternion.hh:986
Vector3< T > EulerRotation(bool _firstSolution) const
Get the rotation as a Euler angles.
Definition: gz/math/Matrix4.hh:313
Matrix4< double > Matrix4d
Definition: gz/math/Matrix4.hh:925
T swap(T... args)
Matrix4< T > Transposed() const
Return the transpose of this matrix.
Definition: gz/math/Matrix4.hh:585
T fail(T... args)
Vector3 Normalize()
Normalize the vector length.
Definition: gz/math/Vector3.hh:139
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: gz/math/Helpers.hh:227
Pose3< T > Pose() const
Get the transformation as math::Pose.
Definition: gz/math/Matrix4.hh:366
Matrix4< T > operator*=(const Matrix4< T > &_m2)
Multiplication assignment operator. This matrix will become equal to this * _m2.
Definition: gz/math/Matrix4.hh:627
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: gz/math/Matrix4.hh:76
bool Equal(const Matrix4 &_m, const T &_tol) const
Equality test with tolerance.
Definition: gz/math/Matrix4.hh:765
const T & X() const
Get the x component.
Definition: gz/math/Quaternion.hh:972
const Matrix4< T > & operator=(const Matrix3< T > &_mat)
Equal operator for 3x3 matrix.
Definition: gz/math/Matrix4.hh:606
void Transpose()
Transpose this matrix.
Definition: gz/math/Matrix4.hh:573
Quaternion< T > Rotation() const
Get the rotation as a quaternion.
Definition: gz/math/Matrix4.hh:245
Matrix4(const Pose3< T > &_pose)
Construct Matrix4 from a math::Pose3.
Definition: gz/math/Matrix4.hh:113
bool TransformAffine(const Vector3< T > &_v, Vector3< T > &_result) const
Perform an affine transformation.
Definition: gz/math/Matrix4.hh:433
T & operator()(const size_t _row, const size_t _col)
Get a mutable version the value at the specified row, column index.
Definition: gz/math/Matrix4.hh:754
STL class.
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661
static const Matrix4< T > Zero
Zero matrix.
Definition: gz/math/Matrix4.hh:44
bool operator==(const Matrix4< T > &_m) const
Equality operator.
Definition: gz/math/Matrix4.hh:789
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: gz/math/Matrix4.hh:138
const T & W() const
Get the w component.
Definition: gz/math/Quaternion.hh:965
bool IsAffine() const
Return true if the matrix is affine.
Definition: gz/math/Matrix4.hh:395
Vector3< T > Translation() const
Get the translational values as a Vector3.
Definition: gz/math/Matrix4.hh:231
void Translate(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition: gz/math/Matrix4.hh:192
bool operator!=(const Matrix4< T > &_m) const
Inequality test operator.
Definition: gz/math/Matrix4.hh:797