Gazebo Math

API Reference

7.5.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 gz::math
29 {
30  // Inline bracket to help doxygen filtering.
31  inline namespace GZ_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) = default;
53 
71  public: constexpr 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  : data{{_v00, _v01, _v02, _v03},
76  {_v10, _v11, _v12, _v13},
77  {_v20, _v21, _v22, _v23},
78  {_v30, _v31, _v32, _v33}}
79  {
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->SetTranslation(_pose.Pos());
111  }
112 
114  public: ~Matrix4() = default;
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 SetTranslation(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 SetTranslation(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.SetW(root / 2.0);
229  root = 1.0 / (2.0 * root);
230  q.SetX((this->data[2][1] - this->data[1][2]) * root);
231  q.SetY((this->data[0][2] - this->data[2][0]) * root);
232  q.SetZ((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.SetX(a); break;
258  case 1: q.SetY(a); break;
259  case 2: q.SetZ(a); break;
260  };
261  switch (j)
262  {
263  default:
264  case 0: q.SetX(b); break;
265  case 1: q.SetY(b); break;
266  case 2: q.SetZ(b); break;
267  };
268  switch (k)
269  {
270  default:
271  case 0: q.SetX(c); break;
272  case 1: q.SetY(c); break;
273  case 2: q.SetZ(c); break;
274  };
275 
276  q.SetW((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(GZ_PI / 2.0);
307  euler2.Y(GZ_PI / 2.0);
308  euler.X(atan2(m12, m13));
309  euler2.X(atan2(m12, m13));
310  }
311  else
312  {
313  euler.Y(-GZ_PI / 2.0);
314  euler2.Y(-GZ_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(GZ_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 
381  public: bool TransformAffine(const Vector3<T> &_v,
382  Vector3<T> &_result) const
383  {
384  if (!this->IsAffine())
385  return false;
386 
387  _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
388  this->data[0][2]*_v.Z() + this->data[0][3],
389  this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
390  this->data[1][2]*_v.Z() + this->data[1][3],
391  this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
392  this->data[2][2]*_v.Z() + this->data[2][3]);
393  return true;
394  }
395 
398  public: T Determinant() const
399  {
400  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
401 
402  v0 = this->data[2][0]*this->data[3][1]
403  - this->data[2][1]*this->data[3][0];
404  v1 = this->data[2][0]*this->data[3][2]
405  - this->data[2][2]*this->data[3][0];
406  v2 = this->data[2][0]*this->data[3][3]
407  - this->data[2][3]*this->data[3][0];
408  v3 = this->data[2][1]*this->data[3][2]
409  - this->data[2][2]*this->data[3][1];
410  v4 = this->data[2][1]*this->data[3][3]
411  - this->data[2][3]*this->data[3][1];
412  v5 = this->data[2][2]*this->data[3][3]
413  - this->data[2][3]*this->data[3][2];
414 
415  t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
416  t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
417  t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
418  t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
419 
420  return t00 * this->data[0][0]
421  + t10 * this->data[0][1]
422  + t20 * this->data[0][2]
423  + t30 * this->data[0][3];
424  }
425 
429  public: Matrix4<T> Inverse() const
430  {
431  T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
432  Matrix4<T> r;
433 
434  v0 = this->data[2][0]*this->data[3][1] -
435  this->data[2][1]*this->data[3][0];
436  v1 = this->data[2][0]*this->data[3][2] -
437  this->data[2][2]*this->data[3][0];
438  v2 = this->data[2][0]*this->data[3][3] -
439  this->data[2][3]*this->data[3][0];
440  v3 = this->data[2][1]*this->data[3][2] -
441  this->data[2][2]*this->data[3][1];
442  v4 = this->data[2][1]*this->data[3][3] -
443  this->data[2][3]*this->data[3][1];
444  v5 = this->data[2][2]*this->data[3][3] -
445  this->data[2][3]*this->data[3][2];
446 
447  t00 = +(v5*this->data[1][1] -
448  v4*this->data[1][2] + v3*this->data[1][3]);
449  t10 = -(v5*this->data[1][0] -
450  v2*this->data[1][2] + v1*this->data[1][3]);
451  t20 = +(v4*this->data[1][0] -
452  v2*this->data[1][1] + v0*this->data[1][3]);
453  t30 = -(v3*this->data[1][0] -
454  v1*this->data[1][1] + v0*this->data[1][2]);
455 
456  T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
457  t20 * this->data[0][2] + t30 * this->data[0][3]);
458 
459  r(0, 0) = t00 * invDet;
460  r(1, 0) = t10 * invDet;
461  r(2, 0) = t20 * invDet;
462  r(3, 0) = t30 * invDet;
463 
464  r(0, 1) = -(v5*this->data[0][1] -
465  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
466  r(1, 1) = +(v5*this->data[0][0] -
467  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
468  r(2, 1) = -(v4*this->data[0][0] -
469  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
470  r(3, 1) = +(v3*this->data[0][0] -
471  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
472 
473  v0 = this->data[1][0]*this->data[3][1] -
474  this->data[1][1]*this->data[3][0];
475  v1 = this->data[1][0]*this->data[3][2] -
476  this->data[1][2]*this->data[3][0];
477  v2 = this->data[1][0]*this->data[3][3] -
478  this->data[1][3]*this->data[3][0];
479  v3 = this->data[1][1]*this->data[3][2] -
480  this->data[1][2]*this->data[3][1];
481  v4 = this->data[1][1]*this->data[3][3] -
482  this->data[1][3]*this->data[3][1];
483  v5 = this->data[1][2]*this->data[3][3] -
484  this->data[1][3]*this->data[3][2];
485 
486  r(0, 2) = +(v5*this->data[0][1] -
487  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
488  r(1, 2) = -(v5*this->data[0][0] -
489  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
490  r(2, 2) = +(v4*this->data[0][0] -
491  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
492  r(3, 2) = -(v3*this->data[0][0] -
493  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
494 
495  v0 = this->data[2][1]*this->data[1][0] -
496  this->data[2][0]*this->data[1][1];
497  v1 = this->data[2][2]*this->data[1][0] -
498  this->data[2][0]*this->data[1][2];
499  v2 = this->data[2][3]*this->data[1][0] -
500  this->data[2][0]*this->data[1][3];
501  v3 = this->data[2][2]*this->data[1][1] -
502  this->data[2][1]*this->data[1][2];
503  v4 = this->data[2][3]*this->data[1][1] -
504  this->data[2][1]*this->data[1][3];
505  v5 = this->data[2][3]*this->data[1][2] -
506  this->data[2][2]*this->data[1][3];
507 
508  r(0, 3) = -(v5*this->data[0][1] -
509  v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
510  r(1, 3) = +(v5*this->data[0][0] -
511  v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
512  r(2, 3) = -(v4*this->data[0][0] -
513  v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
514  r(3, 3) = +(v3*this->data[0][0] -
515  v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
516 
517  return r;
518  }
519 
521  public: void Transpose()
522  {
523  std::swap(this->data[0][1], this->data[1][0]);
524  std::swap(this->data[0][2], this->data[2][0]);
525  std::swap(this->data[0][3], this->data[3][0]);
526  std::swap(this->data[1][2], this->data[2][1]);
527  std::swap(this->data[1][3], this->data[3][1]);
528  std::swap(this->data[2][3], this->data[3][2]);
529  }
530 
533  public: Matrix4<T> Transposed() const
534  {
535  return Matrix4<T>(
536  this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
537  this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
538  this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
539  this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
540  }
541 
545  public: Matrix4<T> &operator=(const Matrix4<T> &_mat) = default;
546 
550  public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
551  {
552  this->data[0][0] = _mat(0, 0);
553  this->data[0][1] = _mat(0, 1);
554  this->data[0][2] = _mat(0, 2);
555 
556  this->data[1][0] = _mat(1, 0);
557  this->data[1][1] = _mat(1, 1);
558  this->data[1][2] = _mat(1, 2);
559 
560  this->data[2][0] = _mat(2, 0);
561  this->data[2][1] = _mat(2, 1);
562  this->data[2][2] = _mat(2, 2);
563 
564  return *this;
565  }
566 
571  public: Matrix4<T> operator*=(const Matrix4<T> &_m2)
572  {
573  (*this) = (*this) * _m2;
574  return *this;
575  }
576 
580  public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
581  {
582  return Matrix4<T>(
583  this->data[0][0] * _m2(0, 0) +
584  this->data[0][1] * _m2(1, 0) +
585  this->data[0][2] * _m2(2, 0) +
586  this->data[0][3] * _m2(3, 0),
587 
588  this->data[0][0] * _m2(0, 1) +
589  this->data[0][1] * _m2(1, 1) +
590  this->data[0][2] * _m2(2, 1) +
591  this->data[0][3] * _m2(3, 1),
592 
593  this->data[0][0] * _m2(0, 2) +
594  this->data[0][1] * _m2(1, 2) +
595  this->data[0][2] * _m2(2, 2) +
596  this->data[0][3] * _m2(3, 2),
597 
598  this->data[0][0] * _m2(0, 3) +
599  this->data[0][1] * _m2(1, 3) +
600  this->data[0][2] * _m2(2, 3) +
601  this->data[0][3] * _m2(3, 3),
602 
603  this->data[1][0] * _m2(0, 0) +
604  this->data[1][1] * _m2(1, 0) +
605  this->data[1][2] * _m2(2, 0) +
606  this->data[1][3] * _m2(3, 0),
607 
608  this->data[1][0] * _m2(0, 1) +
609  this->data[1][1] * _m2(1, 1) +
610  this->data[1][2] * _m2(2, 1) +
611  this->data[1][3] * _m2(3, 1),
612 
613  this->data[1][0] * _m2(0, 2) +
614  this->data[1][1] * _m2(1, 2) +
615  this->data[1][2] * _m2(2, 2) +
616  this->data[1][3] * _m2(3, 2),
617 
618  this->data[1][0] * _m2(0, 3) +
619  this->data[1][1] * _m2(1, 3) +
620  this->data[1][2] * _m2(2, 3) +
621  this->data[1][3] * _m2(3, 3),
622 
623  this->data[2][0] * _m2(0, 0) +
624  this->data[2][1] * _m2(1, 0) +
625  this->data[2][2] * _m2(2, 0) +
626  this->data[2][3] * _m2(3, 0),
627 
628  this->data[2][0] * _m2(0, 1) +
629  this->data[2][1] * _m2(1, 1) +
630  this->data[2][2] * _m2(2, 1) +
631  this->data[2][3] * _m2(3, 1),
632 
633  this->data[2][0] * _m2(0, 2) +
634  this->data[2][1] * _m2(1, 2) +
635  this->data[2][2] * _m2(2, 2) +
636  this->data[2][3] * _m2(3, 2),
637 
638  this->data[2][0] * _m2(0, 3) +
639  this->data[2][1] * _m2(1, 3) +
640  this->data[2][2] * _m2(2, 3) +
641  this->data[2][3] * _m2(3, 3),
642 
643  this->data[3][0] * _m2(0, 0) +
644  this->data[3][1] * _m2(1, 0) +
645  this->data[3][2] * _m2(2, 0) +
646  this->data[3][3] * _m2(3, 0),
647 
648  this->data[3][0] * _m2(0, 1) +
649  this->data[3][1] * _m2(1, 1) +
650  this->data[3][2] * _m2(2, 1) +
651  this->data[3][3] * _m2(3, 1),
652 
653  this->data[3][0] * _m2(0, 2) +
654  this->data[3][1] * _m2(1, 2) +
655  this->data[3][2] * _m2(2, 2) +
656  this->data[3][3] * _m2(3, 2),
657 
658  this->data[3][0] * _m2(0, 3) +
659  this->data[3][1] * _m2(1, 3) +
660  this->data[3][2] * _m2(2, 3) +
661  this->data[3][3] * _m2(3, 3));
662  }
663 
667  public: Vector3<T> operator*(const Vector3<T> &_vec) const
668  {
669  return Vector3<T>(
670  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
671  this->data[0][2]*_vec.Z() + this->data[0][3],
672  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
673  this->data[1][2]*_vec.Z() + this->data[1][3],
674  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
675  this->data[2][2]*_vec.Z() + this->data[2][3]);
676  }
677 
684  public: inline const T &operator()(const size_t _row,
685  const size_t _col) const
686  {
687  return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)][
688  clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
689  }
690 
698  public: inline T &operator()(const size_t _row, const size_t _col)
699  {
700  return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)]
701  [clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
702  }
703 
709  public: bool Equal(const Matrix4 &_m, const T &_tol) const
710  {
711  return equal<T>(this->data[0][0], _m(0, 0), _tol)
712  && equal<T>(this->data[0][1], _m(0, 1), _tol)
713  && equal<T>(this->data[0][2], _m(0, 2), _tol)
714  && equal<T>(this->data[0][3], _m(0, 3), _tol)
715  && equal<T>(this->data[1][0], _m(1, 0), _tol)
716  && equal<T>(this->data[1][1], _m(1, 1), _tol)
717  && equal<T>(this->data[1][2], _m(1, 2), _tol)
718  && equal<T>(this->data[1][3], _m(1, 3), _tol)
719  && equal<T>(this->data[2][0], _m(2, 0), _tol)
720  && equal<T>(this->data[2][1], _m(2, 1), _tol)
721  && equal<T>(this->data[2][2], _m(2, 2), _tol)
722  && equal<T>(this->data[2][3], _m(2, 3), _tol)
723  && equal<T>(this->data[3][0], _m(3, 0), _tol)
724  && equal<T>(this->data[3][1], _m(3, 1), _tol)
725  && equal<T>(this->data[3][2], _m(3, 2), _tol)
726  && equal<T>(this->data[3][3], _m(3, 3), _tol);
727  }
728 
733  public: bool operator==(const Matrix4<T> &_m) const
734  {
735  return this->Equal(_m, static_cast<T>(1e-6));
736  }
737 
741  public: bool operator!=(const Matrix4<T> &_m) const
742  {
743  return !(*this == _m);
744  }
745 
750  public: friend std::ostream &operator<<(
751  std::ostream &_out, const gz::math::Matrix4<T> &_m)
752  {
753  for (auto i : {0, 1, 2, 3})
754  {
755  for (auto j : {0, 1, 2, 3})
756  {
757  if (!(i == 0 && j == 0))
758  _out << " ";
759 
760  appendToStream(_out, _m(i, j));
761  }
762  }
763 
764  return _out;
765  }
766 
771  public: friend std::istream &operator>>(
772  std::istream &_in, gz::math::Matrix4<T> &_m)
773  {
774  // Skip white spaces
775  _in.setf(std::ios_base::skipws);
776  T d[16];
777  _in >> 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 
782  if (!_in.fail())
783  {
784  _m.Set(d[0], d[1], d[2], d[3],
785  d[4], d[5], d[6], d[7],
786  d[8], d[9], d[10], d[11],
787  d[12], d[13], d[14], d[15]);
788  }
789  return _in;
790  }
791 
801  public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
802  const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
803  {
804  // Most important constraint: direction to point X axis at
805  auto front = _target - _eye;
806 
807  // Case when _eye == _target
808  if (front == Vector3<T>::Zero)
809  front = Vector3<T>::UnitX;
810  front.Normalize();
811 
812  // Desired direction to point Z axis at
813  auto up = _up;
814 
815  // Case when _up == Zero
816  if (up == Vector3<T>::Zero)
817  up = Vector3<T>::UnitZ;
818  else
819  up.Normalize();
820 
821  // Case when _up is parallel to X
822  if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
823  up = Vector3<T>::UnitZ;
824 
825  // Find direction to point Y axis at
826  auto left = up.Cross(front);
827 
828  // Case when front is parallel to up
829  if (left == Vector3<T>::Zero)
830  left = Vector3<T>::UnitY;
831  else
832  left.Normalize();
833 
834  // Fix up direction so it's perpendicular to XY
835  up = (front.Cross(left)).Normalize();
836 
837  return Matrix4<T>(
838  front.X(), left.X(), up.X(), _eye.X(),
839  front.Y(), left.Y(), up.Y(), _eye.Y(),
840  front.Z(), left.Z(), up.Z(), _eye.Z(),
841  0, 0, 0, 1);
842  }
843 
845  private: T data[4][4];
846  };
847 
848  namespace detail {
849 
850  template<typename T>
851  constexpr Matrix4<T> gMatrix4Identity(
852  1, 0, 0, 0,
853  0, 1, 0, 0,
854  0, 0, 1, 0,
855  0, 0, 0, 1);
856 
857  template<typename T>
858  constexpr Matrix4<T> gMatrix4Zero(
859  0, 0, 0, 0,
860  0, 0, 0, 0,
861  0, 0, 0, 0,
862  0, 0, 0, 0);
863 
864  } // namespace detail
865 
866  template<typename T>
867  const Matrix4<T> &Matrix4<T>::Identity = detail::gMatrix4Identity<T>;
868 
869  template<typename T>
870  const Matrix4<T> &Matrix4<T>::Zero = detail::gMatrix4Zero<T>;
871 
872  typedef Matrix4<int> Matrix4i;
873  typedef Matrix4<double> Matrix4d;
874  typedef Matrix4<float> Matrix4f;
875  } // namespace GZ_MATH_VERSION_NAMESPACE
876 } // namespace gz::math
877 #endif // GZ_MATH_MATRIX4_HH_