Gazebo Math

API Reference

7.4.0
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
28namespace gz
29{
30 namespace math
31 {
32 // Inline bracket to help doxygen filtering.
33 inline namespace GZ_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) = default;
55
73 public: constexpr Matrix4(T _v00, T _v01, T _v02, T _v03,
74 T _v10, T _v11, T _v12, T _v13,
75 T _v20, T _v21, T _v22, T _v23,
76 T _v30, T _v31, T _v32, T _v33)
77 : data{{_v00, _v01, _v02, _v03},
78 {_v10, _v11, _v12, _v13},
79 {_v20, _v21, _v22, _v23},
80 {_v30, _v31, _v32, _v33}}
81 {
82 }
83
86 public: explicit Matrix4(const Quaternion<T> &_q)
87 {
88 Quaternion<T> qt = _q;
89 qt.Normalize();
90 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
91 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
92 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
93 0,
94
95 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
96 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
97 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
98 0,
99
100 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
101 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
102 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
103 0,
104
105 0, 0, 0, 1);
106 }
107
110 public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
111 {
112 this->SetTranslation(_pose.Pos());
113 }
114
116 public: ~Matrix4() = default;
117
135 public: void Set(
136 T _v00, T _v01, T _v02, T _v03,
137 T _v10, T _v11, T _v12, T _v13,
138 T _v20, T _v21, T _v22, T _v23,
139 T _v30, T _v31, T _v32, T _v33)
140 {
141 this->data[0][0] = _v00;
142 this->data[0][1] = _v01;
143 this->data[0][2] = _v02;
144 this->data[0][3] = _v03;
145
146 this->data[1][0] = _v10;
147 this->data[1][1] = _v11;
148 this->data[1][2] = _v12;
149 this->data[1][3] = _v13;
150
151 this->data[2][0] = _v20;
152 this->data[2][1] = _v21;
153 this->data[2][2] = _v22;
154 this->data[2][3] = _v23;
155
156 this->data[3][0] = _v30;
157 this->data[3][1] = _v31;
158 this->data[3][2] = _v32;
159 this->data[3][3] = _v33;
160 }
161
165 public: void Axis(const Vector3<T> &_axis, T _angle)
166 {
167 T c = cos(_angle);
168 T s = sin(_angle);
169 T C = 1-c;
170
171 this->data[0][0] = _axis.X()*_axis.X()*C + c;
172 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
173 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
174
175 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
176 this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
177 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
178
179 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
180 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
181 this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
182 }
183
186 public: void SetTranslation(const Vector3<T> &_t)
187 {
188 this->data[0][3] = _t.X();
189 this->data[1][3] = _t.Y();
190 this->data[2][3] = _t.Z();
191 }
192
197 public: void SetTranslation(T _x, T _y, T _z)
198 {
199 this->data[0][3] = _x;
200 this->data[1][3] = _y;
201 this->data[2][3] = _z;
202 }
203
206 public: Vector3<T> Translation() const
207 {
208 return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
209 }
210
213 public: Vector3<T> Scale() const
214 {
215 return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
216 }
217
220 public: Quaternion<T> Rotation() const
221 {
225 T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
226 T root;
227 if (trace > 0)
228 {
229 root = sqrt(trace + 1.0);
230 q.SetW(root / 2.0);
231 root = 1.0 / (2.0 * root);
232 q.SetX((this->data[2][1] - this->data[1][2]) * root);
233 q.SetY((this->data[0][2] - this->data[2][0]) * root);
234 q.SetZ((this->data[1][0] - this->data[0][1]) * root);
235 }
236 else
237 {
238 static unsigned int s_iNext[3] = {1, 2, 0};
239 unsigned int i = 0;
240 if (this->data[1][1] > this->data[0][0])
241 i = 1;
242 if (this->data[2][2] > this->data[i][i])
243 i = 2;
244 unsigned int j = s_iNext[i];
245 unsigned int k = s_iNext[j];
246
247 root = sqrt(this->data[i][i] - this->data[j][j] -
248 this->data[k][k] + 1.0);
249
250 T a, b, c;
251 a = root / 2.0;
252 root = 1.0 / (2.0 * root);
253 b = (this->data[j][i] + this->data[i][j]) * root;
254 c = (this->data[k][i] + this->data[i][k]) * root;
255
256 switch (i)
257 {
258 default:
259 case 0: q.SetX(a); break;
260 case 1: q.SetY(a); break;
261 case 2: q.SetZ(a); break;
262 };
263 switch (j)
264 {
265 default:
266 case 0: q.SetX(b); break;
267 case 1: q.SetY(b); break;
268 case 2: q.SetZ(b); break;
269 };
270 switch (k)
271 {
272 default:
273 case 0: q.SetX(c); break;
274 case 1: q.SetY(c); break;
275 case 2: q.SetZ(c); break;
276 };
277
278 q.SetW((this->data[k][j] - this->data[j][k]) * root);
279 }
280
281 return q;
282 }
283
288 public: Vector3<T> EulerRotation(bool _firstSolution) const
289 {
290 Vector3<T> euler;
291 Vector3<T> euler2;
292
293 T m31 = this->data[2][0];
294 T m11 = this->data[0][0];
295 T m12 = this->data[0][1];
296 T m13 = this->data[0][2];
297 T m32 = this->data[2][1];
298 T m33 = this->data[2][2];
299 T m21 = this->data[1][0];
300
301 if (std::abs(m31) >= 1.0)
302 {
303 euler.Z(0.0);
304 euler2.Z(0.0);
305
306 if (m31 < 0.0)
307 {
308 euler.Y(GZ_PI / 2.0);
309 euler2.Y(GZ_PI / 2.0);
310 euler.X(atan2(m12, m13));
311 euler2.X(atan2(m12, m13));
312 }
313 else
314 {
315 euler.Y(-GZ_PI / 2.0);
316 euler2.Y(-GZ_PI / 2.0);
317 euler.X(atan2(-m12, -m13));
318 euler2.X(atan2(-m12, -m13));
319 }
320 }
321 else
322 {
323 euler.Y(-asin(m31));
324 euler2.Y(GZ_PI - euler.Y());
325
326 euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
327 euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
328
329 euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
330 euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
331 }
332
333 if (_firstSolution)
334 return euler;
335 else
336 return euler2;
337 }
338
341 public: Pose3<T> Pose() const
342 {
343 return Pose3<T>(this->Translation(), this->Rotation());
344 }
345
348 public: void Scale(const Vector3<T> &_s)
349 {
350 this->data[0][0] = _s.X();
351 this->data[1][1] = _s.Y();
352 this->data[2][2] = _s.Z();
353 this->data[3][3] = 1.0;
354 }
355
360 public: void Scale(T _x, T _y, T _z)
361 {
362 this->data[0][0] = _x;
363 this->data[1][1] = _y;
364 this->data[2][2] = _z;
365 this->data[3][3] = 1.0;
366 }
367
370 public: bool IsAffine() const
371 {
372 return equal(this->data[3][0], static_cast<T>(0)) &&
373 equal(this->data[3][1], static_cast<T>(0)) &&
374 equal(this->data[3][2], static_cast<T>(0)) &&
375 equal(this->data[3][3], static_cast<T>(1));
376 }
377
383 public: bool TransformAffine(const Vector3<T> &_v,
384 Vector3<T> &_result) const
385 {
386 if (!this->IsAffine())
387 return false;
388
389 _result.Set(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 return true;
396 }
397
400 public: T Determinant() const
401 {
402 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
403
404 v0 = this->data[2][0]*this->data[3][1]
405 - this->data[2][1]*this->data[3][0];
406 v1 = this->data[2][0]*this->data[3][2]
407 - this->data[2][2]*this->data[3][0];
408 v2 = this->data[2][0]*this->data[3][3]
409 - this->data[2][3]*this->data[3][0];
410 v3 = this->data[2][1]*this->data[3][2]
411 - this->data[2][2]*this->data[3][1];
412 v4 = this->data[2][1]*this->data[3][3]
413 - this->data[2][3]*this->data[3][1];
414 v5 = this->data[2][2]*this->data[3][3]
415 - this->data[2][3]*this->data[3][2];
416
417 t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
418 t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
419 t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
420 t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
421
422 return t00 * this->data[0][0]
423 + t10 * this->data[0][1]
424 + t20 * this->data[0][2]
425 + t30 * this->data[0][3];
426 }
427
431 public: Matrix4<T> Inverse() const
432 {
433 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
434 Matrix4<T> r;
435
436 v0 = this->data[2][0]*this->data[3][1] -
437 this->data[2][1]*this->data[3][0];
438 v1 = this->data[2][0]*this->data[3][2] -
439 this->data[2][2]*this->data[3][0];
440 v2 = this->data[2][0]*this->data[3][3] -
441 this->data[2][3]*this->data[3][0];
442 v3 = this->data[2][1]*this->data[3][2] -
443 this->data[2][2]*this->data[3][1];
444 v4 = this->data[2][1]*this->data[3][3] -
445 this->data[2][3]*this->data[3][1];
446 v5 = this->data[2][2]*this->data[3][3] -
447 this->data[2][3]*this->data[3][2];
448
449 t00 = +(v5*this->data[1][1] -
450 v4*this->data[1][2] + v3*this->data[1][3]);
451 t10 = -(v5*this->data[1][0] -
452 v2*this->data[1][2] + v1*this->data[1][3]);
453 t20 = +(v4*this->data[1][0] -
454 v2*this->data[1][1] + v0*this->data[1][3]);
455 t30 = -(v3*this->data[1][0] -
456 v1*this->data[1][1] + v0*this->data[1][2]);
457
458 T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
459 t20 * this->data[0][2] + t30 * this->data[0][3]);
460
461 r(0, 0) = t00 * invDet;
462 r(1, 0) = t10 * invDet;
463 r(2, 0) = t20 * invDet;
464 r(3, 0) = t30 * invDet;
465
466 r(0, 1) = -(v5*this->data[0][1] -
467 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
468 r(1, 1) = +(v5*this->data[0][0] -
469 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
470 r(2, 1) = -(v4*this->data[0][0] -
471 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
472 r(3, 1) = +(v3*this->data[0][0] -
473 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
474
475 v0 = this->data[1][0]*this->data[3][1] -
476 this->data[1][1]*this->data[3][0];
477 v1 = this->data[1][0]*this->data[3][2] -
478 this->data[1][2]*this->data[3][0];
479 v2 = this->data[1][0]*this->data[3][3] -
480 this->data[1][3]*this->data[3][0];
481 v3 = this->data[1][1]*this->data[3][2] -
482 this->data[1][2]*this->data[3][1];
483 v4 = this->data[1][1]*this->data[3][3] -
484 this->data[1][3]*this->data[3][1];
485 v5 = this->data[1][2]*this->data[3][3] -
486 this->data[1][3]*this->data[3][2];
487
488 r(0, 2) = +(v5*this->data[0][1] -
489 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
490 r(1, 2) = -(v5*this->data[0][0] -
491 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
492 r(2, 2) = +(v4*this->data[0][0] -
493 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
494 r(3, 2) = -(v3*this->data[0][0] -
495 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
496
497 v0 = this->data[2][1]*this->data[1][0] -
498 this->data[2][0]*this->data[1][1];
499 v1 = this->data[2][2]*this->data[1][0] -
500 this->data[2][0]*this->data[1][2];
501 v2 = this->data[2][3]*this->data[1][0] -
502 this->data[2][0]*this->data[1][3];
503 v3 = this->data[2][2]*this->data[1][1] -
504 this->data[2][1]*this->data[1][2];
505 v4 = this->data[2][3]*this->data[1][1] -
506 this->data[2][1]*this->data[1][3];
507 v5 = this->data[2][3]*this->data[1][2] -
508 this->data[2][2]*this->data[1][3];
509
510 r(0, 3) = -(v5*this->data[0][1] -
511 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
512 r(1, 3) = +(v5*this->data[0][0] -
513 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
514 r(2, 3) = -(v4*this->data[0][0] -
515 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
516 r(3, 3) = +(v3*this->data[0][0] -
517 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
518
519 return r;
520 }
521
523 public: void Transpose()
524 {
525 std::swap(this->data[0][1], this->data[1][0]);
526 std::swap(this->data[0][2], this->data[2][0]);
527 std::swap(this->data[0][3], this->data[3][0]);
528 std::swap(this->data[1][2], this->data[2][1]);
529 std::swap(this->data[1][3], this->data[3][1]);
530 std::swap(this->data[2][3], this->data[3][2]);
531 }
532
535 public: Matrix4<T> Transposed() const
536 {
537 return Matrix4<T>(
538 this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
539 this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
540 this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
541 this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
542 }
543
547 public: Matrix4<T> &operator=(const Matrix4<T> &_mat) = default;
548
552 public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
553 {
554 this->data[0][0] = _mat(0, 0);
555 this->data[0][1] = _mat(0, 1);
556 this->data[0][2] = _mat(0, 2);
557
558 this->data[1][0] = _mat(1, 0);
559 this->data[1][1] = _mat(1, 1);
560 this->data[1][2] = _mat(1, 2);
561
562 this->data[2][0] = _mat(2, 0);
563 this->data[2][1] = _mat(2, 1);
564 this->data[2][2] = _mat(2, 2);
565
566 return *this;
567 }
568
573 public: Matrix4<T> operator*=(const Matrix4<T> &_m2)
574 {
575 (*this) = (*this) * _m2;
576 return *this;
577 }
578
582 public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
583 {
584 return Matrix4<T>(
585 this->data[0][0] * _m2(0, 0) +
586 this->data[0][1] * _m2(1, 0) +
587 this->data[0][2] * _m2(2, 0) +
588 this->data[0][3] * _m2(3, 0),
589
590 this->data[0][0] * _m2(0, 1) +
591 this->data[0][1] * _m2(1, 1) +
592 this->data[0][2] * _m2(2, 1) +
593 this->data[0][3] * _m2(3, 1),
594
595 this->data[0][0] * _m2(0, 2) +
596 this->data[0][1] * _m2(1, 2) +
597 this->data[0][2] * _m2(2, 2) +
598 this->data[0][3] * _m2(3, 2),
599
600 this->data[0][0] * _m2(0, 3) +
601 this->data[0][1] * _m2(1, 3) +
602 this->data[0][2] * _m2(2, 3) +
603 this->data[0][3] * _m2(3, 3),
604
605 this->data[1][0] * _m2(0, 0) +
606 this->data[1][1] * _m2(1, 0) +
607 this->data[1][2] * _m2(2, 0) +
608 this->data[1][3] * _m2(3, 0),
609
610 this->data[1][0] * _m2(0, 1) +
611 this->data[1][1] * _m2(1, 1) +
612 this->data[1][2] * _m2(2, 1) +
613 this->data[1][3] * _m2(3, 1),
614
615 this->data[1][0] * _m2(0, 2) +
616 this->data[1][1] * _m2(1, 2) +
617 this->data[1][2] * _m2(2, 2) +
618 this->data[1][3] * _m2(3, 2),
619
620 this->data[1][0] * _m2(0, 3) +
621 this->data[1][1] * _m2(1, 3) +
622 this->data[1][2] * _m2(2, 3) +
623 this->data[1][3] * _m2(3, 3),
624
625 this->data[2][0] * _m2(0, 0) +
626 this->data[2][1] * _m2(1, 0) +
627 this->data[2][2] * _m2(2, 0) +
628 this->data[2][3] * _m2(3, 0),
629
630 this->data[2][0] * _m2(0, 1) +
631 this->data[2][1] * _m2(1, 1) +
632 this->data[2][2] * _m2(2, 1) +
633 this->data[2][3] * _m2(3, 1),
634
635 this->data[2][0] * _m2(0, 2) +
636 this->data[2][1] * _m2(1, 2) +
637 this->data[2][2] * _m2(2, 2) +
638 this->data[2][3] * _m2(3, 2),
639
640 this->data[2][0] * _m2(0, 3) +
641 this->data[2][1] * _m2(1, 3) +
642 this->data[2][2] * _m2(2, 3) +
643 this->data[2][3] * _m2(3, 3),
644
645 this->data[3][0] * _m2(0, 0) +
646 this->data[3][1] * _m2(1, 0) +
647 this->data[3][2] * _m2(2, 0) +
648 this->data[3][3] * _m2(3, 0),
649
650 this->data[3][0] * _m2(0, 1) +
651 this->data[3][1] * _m2(1, 1) +
652 this->data[3][2] * _m2(2, 1) +
653 this->data[3][3] * _m2(3, 1),
654
655 this->data[3][0] * _m2(0, 2) +
656 this->data[3][1] * _m2(1, 2) +
657 this->data[3][2] * _m2(2, 2) +
658 this->data[3][3] * _m2(3, 2),
659
660 this->data[3][0] * _m2(0, 3) +
661 this->data[3][1] * _m2(1, 3) +
662 this->data[3][2] * _m2(2, 3) +
663 this->data[3][3] * _m2(3, 3));
664 }
665
669 public: Vector3<T> operator*(const Vector3<T> &_vec) const
670 {
671 return Vector3<T>(
672 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
673 this->data[0][2]*_vec.Z() + this->data[0][3],
674 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
675 this->data[1][2]*_vec.Z() + this->data[1][3],
676 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
677 this->data[2][2]*_vec.Z() + this->data[2][3]);
678 }
679
686 public: inline const T &operator()(const size_t _row,
687 const size_t _col) const
688 {
689 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)][
690 clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
691 }
692
700 public: inline T &operator()(const size_t _row, const size_t _col)
701 {
702 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)]
703 [clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
704 }
705
711 public: bool Equal(const Matrix4 &_m, const T &_tol) const
712 {
713 return equal<T>(this->data[0][0], _m(0, 0), _tol)
714 && equal<T>(this->data[0][1], _m(0, 1), _tol)
715 && equal<T>(this->data[0][2], _m(0, 2), _tol)
716 && equal<T>(this->data[0][3], _m(0, 3), _tol)
717 && equal<T>(this->data[1][0], _m(1, 0), _tol)
718 && equal<T>(this->data[1][1], _m(1, 1), _tol)
719 && equal<T>(this->data[1][2], _m(1, 2), _tol)
720 && equal<T>(this->data[1][3], _m(1, 3), _tol)
721 && equal<T>(this->data[2][0], _m(2, 0), _tol)
722 && equal<T>(this->data[2][1], _m(2, 1), _tol)
723 && equal<T>(this->data[2][2], _m(2, 2), _tol)
724 && equal<T>(this->data[2][3], _m(2, 3), _tol)
725 && equal<T>(this->data[3][0], _m(3, 0), _tol)
726 && equal<T>(this->data[3][1], _m(3, 1), _tol)
727 && equal<T>(this->data[3][2], _m(3, 2), _tol)
728 && equal<T>(this->data[3][3], _m(3, 3), _tol);
729 }
730
735 public: bool operator==(const Matrix4<T> &_m) const
736 {
737 return this->Equal(_m, static_cast<T>(1e-6));
738 }
739
743 public: bool operator!=(const Matrix4<T> &_m) const
744 {
745 return !(*this == _m);
746 }
747
752 public: friend std::ostream &operator<<(
753 std::ostream &_out, const gz::math::Matrix4<T> &_m)
754 {
755 for (auto i : {0, 1, 2, 3})
756 {
757 for (auto j : {0, 1, 2, 3})
758 {
759 if (!(i == 0 && j == 0))
760 _out << " ";
761
762 appendToStream(_out, _m(i, j));
763 }
764 }
765
766 return _out;
767 }
768
773 public: friend std::istream &operator>>(
775 {
776 // Skip white spaces
777 _in.setf(std::ios_base::skipws);
778 T d[16];
779 _in >> d[0] >> d[1] >> d[2] >> d[3]
780 >> d[4] >> d[5] >> d[6] >> d[7]
781 >> d[8] >> d[9] >> d[10] >> d[11]
782 >> d[12] >> d[13] >> d[14] >> d[15];
783
784 if (!_in.fail())
785 {
786 _m.Set(d[0], d[1], d[2], d[3],
787 d[4], d[5], d[6], d[7],
788 d[8], d[9], d[10], d[11],
789 d[12], d[13], d[14], d[15]);
790 }
791 return _in;
792 }
793
803 public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
804 const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
805 {
806 // Most important constraint: direction to point X axis at
807 auto front = _target - _eye;
808
809 // Case when _eye == _target
810 if (front == Vector3<T>::Zero)
811 front = Vector3<T>::UnitX;
812 front.Normalize();
813
814 // Desired direction to point Z axis at
815 auto up = _up;
816
817 // Case when _up == Zero
818 if (up == Vector3<T>::Zero)
820 else
821 up.Normalize();
822
823 // Case when _up is parallel to X
824 if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
826
827 // Find direction to point Y axis at
828 auto left = up.Cross(front);
829
830 // Case when front is parallel to up
831 if (left == Vector3<T>::Zero)
832 left = Vector3<T>::UnitY;
833 else
834 left.Normalize();
835
836 // Fix up direction so it's perpendicular to XY
837 up = (front.Cross(left)).Normalize();
838
839 return Matrix4<T>(
840 front.X(), left.X(), up.X(), _eye.X(),
841 front.Y(), left.Y(), up.Y(), _eye.Y(),
842 front.Z(), left.Z(), up.Z(), _eye.Z(),
843 0, 0, 0, 1);
844 }
845
847 private: T data[4][4];
848 };
849
850 namespace detail {
851
852 template<typename T>
853 constexpr Matrix4<T> gMatrix4Identity(
854 1, 0, 0, 0,
855 0, 1, 0, 0,
856 0, 0, 1, 0,
857 0, 0, 0, 1);
858
859 template<typename T>
860 constexpr Matrix4<T> gMatrix4Zero(
861 0, 0, 0, 0,
862 0, 0, 0, 0,
863 0, 0, 0, 0,
864 0, 0, 0, 0);
865
866 } // namespace detail
867
868 template<typename T>
869 const Matrix4<T> &Matrix4<T>::Identity = detail::gMatrix4Identity<T>;
870
871 template<typename T>
872 const Matrix4<T> &Matrix4<T>::Zero = detail::gMatrix4Zero<T>;
873
877 }
878 }
879}
880#endif