Gazebo Math

API Reference

8.0.0
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::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
67 public: constexpr Matrix4(T _v00, T _v01, T _v02, T _v03,
68 T _v10, T _v11, T _v12, T _v13,
69 T _v20, T _v21, T _v22, T _v23,
70 T _v30, T _v31, T _v32, T _v33)
71 : data{{_v00, _v01, _v02, _v03},
72 {_v10, _v11, _v12, _v13},
73 {_v20, _v21, _v22, _v23},
74 {_v30, _v31, _v32, _v33}}
75 {
76 }
77
80 public: explicit Matrix4(const Quaternion<T> &_q)
81 {
82 Quaternion<T> qt = _q;
83 qt.Normalize();
84 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
85 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
86 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
87 0,
88
89 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
90 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
91 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
92 0,
93
94 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
95 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
96 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
97 0,
98
99 0, 0, 0, 1);
100 }
101
104 public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
105 {
106 this->SetTranslation(_pose.Pos());
107 }
108
126 public: void Set(
127 T _v00, T _v01, T _v02, T _v03,
128 T _v10, T _v11, T _v12, T _v13,
129 T _v20, T _v21, T _v22, T _v23,
130 T _v30, T _v31, T _v32, T _v33)
131 {
132 this->data[0][0] = _v00;
133 this->data[0][1] = _v01;
134 this->data[0][2] = _v02;
135 this->data[0][3] = _v03;
136
137 this->data[1][0] = _v10;
138 this->data[1][1] = _v11;
139 this->data[1][2] = _v12;
140 this->data[1][3] = _v13;
141
142 this->data[2][0] = _v20;
143 this->data[2][1] = _v21;
144 this->data[2][2] = _v22;
145 this->data[2][3] = _v23;
146
147 this->data[3][0] = _v30;
148 this->data[3][1] = _v31;
149 this->data[3][2] = _v32;
150 this->data[3][3] = _v33;
151 }
152
156 public: void Axis(const Vector3<T> &_axis, T _angle)
157 {
158 T c = cos(_angle);
159 T s = sin(_angle);
160 T C = 1-c;
161
162 this->data[0][0] = _axis.X()*_axis.X()*C + c;
163 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
164 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
165
166 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
167 this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
168 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
169
170 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
171 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
172 this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
173 }
174
177 public: void SetTranslation(const Vector3<T> &_t)
178 {
179 this->data[0][3] = _t.X();
180 this->data[1][3] = _t.Y();
181 this->data[2][3] = _t.Z();
182 }
183
188 public: void SetTranslation(T _x, T _y, T _z)
189 {
190 this->data[0][3] = _x;
191 this->data[1][3] = _y;
192 this->data[2][3] = _z;
193 }
194
197 public: Vector3<T> Translation() const
198 {
199 return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
200 }
201
204 public: Vector3<T> Scale() const
205 {
206 return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
207 }
208
211 public: Quaternion<T> Rotation() const
212 {
213 Quaternion<T> q;
216 T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
217 T root;
218 if (trace > 0)
219 {
220 root = sqrt(trace + 1.0);
221 q.SetW(root / 2.0);
222 root = 1.0 / (2.0 * root);
223 q.SetX((this->data[2][1] - this->data[1][2]) * root);
224 q.SetY((this->data[0][2] - this->data[2][0]) * root);
225 q.SetZ((this->data[1][0] - this->data[0][1]) * root);
226 }
227 else
228 {
229 static unsigned int s_iNext[3] = {1, 2, 0};
230 unsigned int i = 0;
231 if (this->data[1][1] > this->data[0][0])
232 i = 1;
233 if (this->data[2][2] > this->data[i][i])
234 i = 2;
235 unsigned int j = s_iNext[i];
236 unsigned int k = s_iNext[j];
237
238 root = sqrt(this->data[i][i] - this->data[j][j] -
239 this->data[k][k] + 1.0);
240
241 T a, b, c;
242 a = root / 2.0;
243 root = 1.0 / (2.0 * root);
244 b = (this->data[j][i] + this->data[i][j]) * root;
245 c = (this->data[k][i] + this->data[i][k]) * root;
246
247 switch (i)
248 {
249 default:
250 case 0: q.SetX(a); break;
251 case 1: q.SetY(a); break;
252 case 2: q.SetZ(a); break;
253 };
254 switch (j)
255 {
256 default:
257 case 0: q.SetX(b); break;
258 case 1: q.SetY(b); break;
259 case 2: q.SetZ(b); break;
260 };
261 switch (k)
262 {
263 default:
264 case 0: q.SetX(c); break;
265 case 1: q.SetY(c); break;
266 case 2: q.SetZ(c); break;
267 };
268
269 q.SetW((this->data[k][j] - this->data[j][k]) * root);
270 }
271
272 return q;
273 }
274
279 public: Vector3<T> EulerRotation(bool _firstSolution) const
280 {
281 Vector3<T> euler;
282 Vector3<T> euler2;
283
284 T m31 = this->data[2][0];
285 T m11 = this->data[0][0];
286 T m12 = this->data[0][1];
287 T m13 = this->data[0][2];
288 T m32 = this->data[2][1];
289 T m33 = this->data[2][2];
290 T m21 = this->data[1][0];
291
292 if (std::abs(m31) >= 1.0)
293 {
294 euler.Z(0.0);
295 euler2.Z(0.0);
296
297 if (m31 < 0.0)
298 {
299 euler.Y(GZ_PI / 2.0);
300 euler2.Y(GZ_PI / 2.0);
301 euler.X(atan2(m12, m13));
302 euler2.X(atan2(m12, m13));
303 }
304 else
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 }
312 else
313 {
314 euler.Y(-asin(m31));
315 euler2.Y(GZ_PI - euler.Y());
316
317 euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
318 euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
319
320 euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
321 euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
322 }
323
324 if (_firstSolution)
325 return euler;
326 else
327 return euler2;
328 }
329
332 public: Pose3<T> Pose() const
333 {
334 return Pose3<T>(this->Translation(), this->Rotation());
335 }
336
339 public: void Scale(const Vector3<T> &_s)
340 {
341 this->data[0][0] = _s.X();
342 this->data[1][1] = _s.Y();
343 this->data[2][2] = _s.Z();
344 this->data[3][3] = 1.0;
345 }
346
351 public: void Scale(T _x, T _y, T _z)
352 {
353 this->data[0][0] = _x;
354 this->data[1][1] = _y;
355 this->data[2][2] = _z;
356 this->data[3][3] = 1.0;
357 }
358
361 public: bool IsAffine() const
362 {
363 return equal(this->data[3][0], static_cast<T>(0)) &&
364 equal(this->data[3][1], static_cast<T>(0)) &&
365 equal(this->data[3][2], static_cast<T>(0)) &&
366 equal(this->data[3][3], static_cast<T>(1));
367 }
368
374 public: bool TransformAffine(const Vector3<T> &_v,
375 Vector3<T> &_result) const
376 {
377 if (!this->IsAffine())
378 return false;
379
380 _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
381 this->data[0][2]*_v.Z() + this->data[0][3],
382 this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
383 this->data[1][2]*_v.Z() + this->data[1][3],
384 this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
385 this->data[2][2]*_v.Z() + this->data[2][3]);
386 return true;
387 }
388
391 public: T Determinant() const
392 {
393 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
394
395 v0 = this->data[2][0]*this->data[3][1]
396 - this->data[2][1]*this->data[3][0];
397 v1 = this->data[2][0]*this->data[3][2]
398 - this->data[2][2]*this->data[3][0];
399 v2 = this->data[2][0]*this->data[3][3]
400 - this->data[2][3]*this->data[3][0];
401 v3 = this->data[2][1]*this->data[3][2]
402 - this->data[2][2]*this->data[3][1];
403 v4 = this->data[2][1]*this->data[3][3]
404 - this->data[2][3]*this->data[3][1];
405 v5 = this->data[2][2]*this->data[3][3]
406 - this->data[2][3]*this->data[3][2];
407
408 t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
409 t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
410 t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
411 t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
412
413 return t00 * this->data[0][0]
414 + t10 * this->data[0][1]
415 + t20 * this->data[0][2]
416 + t30 * this->data[0][3];
417 }
418
422 public: Matrix4<T> Inverse() const
423 {
424 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
425 Matrix4<T> r;
426
427 v0 = this->data[2][0]*this->data[3][1] -
428 this->data[2][1]*this->data[3][0];
429 v1 = this->data[2][0]*this->data[3][2] -
430 this->data[2][2]*this->data[3][0];
431 v2 = this->data[2][0]*this->data[3][3] -
432 this->data[2][3]*this->data[3][0];
433 v3 = this->data[2][1]*this->data[3][2] -
434 this->data[2][2]*this->data[3][1];
435 v4 = this->data[2][1]*this->data[3][3] -
436 this->data[2][3]*this->data[3][1];
437 v5 = this->data[2][2]*this->data[3][3] -
438 this->data[2][3]*this->data[3][2];
439
440 t00 = +(v5*this->data[1][1] -
441 v4*this->data[1][2] + v3*this->data[1][3]);
442 t10 = -(v5*this->data[1][0] -
443 v2*this->data[1][2] + v1*this->data[1][3]);
444 t20 = +(v4*this->data[1][0] -
445 v2*this->data[1][1] + v0*this->data[1][3]);
446 t30 = -(v3*this->data[1][0] -
447 v1*this->data[1][1] + v0*this->data[1][2]);
448
449 T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
450 t20 * this->data[0][2] + t30 * this->data[0][3]);
451
452 r(0, 0) = t00 * invDet;
453 r(1, 0) = t10 * invDet;
454 r(2, 0) = t20 * invDet;
455 r(3, 0) = t30 * invDet;
456
457 r(0, 1) = -(v5*this->data[0][1] -
458 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
459 r(1, 1) = +(v5*this->data[0][0] -
460 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
461 r(2, 1) = -(v4*this->data[0][0] -
462 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
463 r(3, 1) = +(v3*this->data[0][0] -
464 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
465
466 v0 = this->data[1][0]*this->data[3][1] -
467 this->data[1][1]*this->data[3][0];
468 v1 = this->data[1][0]*this->data[3][2] -
469 this->data[1][2]*this->data[3][0];
470 v2 = this->data[1][0]*this->data[3][3] -
471 this->data[1][3]*this->data[3][0];
472 v3 = this->data[1][1]*this->data[3][2] -
473 this->data[1][2]*this->data[3][1];
474 v4 = this->data[1][1]*this->data[3][3] -
475 this->data[1][3]*this->data[3][1];
476 v5 = this->data[1][2]*this->data[3][3] -
477 this->data[1][3]*this->data[3][2];
478
479 r(0, 2) = +(v5*this->data[0][1] -
480 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
481 r(1, 2) = -(v5*this->data[0][0] -
482 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
483 r(2, 2) = +(v4*this->data[0][0] -
484 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
485 r(3, 2) = -(v3*this->data[0][0] -
486 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
487
488 v0 = this->data[2][1]*this->data[1][0] -
489 this->data[2][0]*this->data[1][1];
490 v1 = this->data[2][2]*this->data[1][0] -
491 this->data[2][0]*this->data[1][2];
492 v2 = this->data[2][3]*this->data[1][0] -
493 this->data[2][0]*this->data[1][3];
494 v3 = this->data[2][2]*this->data[1][1] -
495 this->data[2][1]*this->data[1][2];
496 v4 = this->data[2][3]*this->data[1][1] -
497 this->data[2][1]*this->data[1][3];
498 v5 = this->data[2][3]*this->data[1][2] -
499 this->data[2][2]*this->data[1][3];
500
501 r(0, 3) = -(v5*this->data[0][1] -
502 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
503 r(1, 3) = +(v5*this->data[0][0] -
504 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
505 r(2, 3) = -(v4*this->data[0][0] -
506 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
507 r(3, 3) = +(v3*this->data[0][0] -
508 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
509
510 return r;
511 }
512
514 public: void Transpose()
515 {
516 std::swap(this->data[0][1], this->data[1][0]);
517 std::swap(this->data[0][2], this->data[2][0]);
518 std::swap(this->data[0][3], this->data[3][0]);
519 std::swap(this->data[1][2], this->data[2][1]);
520 std::swap(this->data[1][3], this->data[3][1]);
521 std::swap(this->data[2][3], this->data[3][2]);
522 }
523
526 public: Matrix4<T> Transposed() const
527 {
528 return Matrix4<T>(
529 this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
530 this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
531 this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
532 this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
533 }
534
538 public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
539 {
540 this->data[0][0] = _mat(0, 0);
541 this->data[0][1] = _mat(0, 1);
542 this->data[0][2] = _mat(0, 2);
543
544 this->data[1][0] = _mat(1, 0);
545 this->data[1][1] = _mat(1, 1);
546 this->data[1][2] = _mat(1, 2);
547
548 this->data[2][0] = _mat(2, 0);
549 this->data[2][1] = _mat(2, 1);
550 this->data[2][2] = _mat(2, 2);
551
552 return *this;
553 }
554
559 public: Matrix4<T> operator*=(const Matrix4<T> &_m2)
560 {
561 (*this) = (*this) * _m2;
562 return *this;
563 }
564
568 public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
569 {
570 return Matrix4<T>(
571 this->data[0][0] * _m2(0, 0) +
572 this->data[0][1] * _m2(1, 0) +
573 this->data[0][2] * _m2(2, 0) +
574 this->data[0][3] * _m2(3, 0),
575
576 this->data[0][0] * _m2(0, 1) +
577 this->data[0][1] * _m2(1, 1) +
578 this->data[0][2] * _m2(2, 1) +
579 this->data[0][3] * _m2(3, 1),
580
581 this->data[0][0] * _m2(0, 2) +
582 this->data[0][1] * _m2(1, 2) +
583 this->data[0][2] * _m2(2, 2) +
584 this->data[0][3] * _m2(3, 2),
585
586 this->data[0][0] * _m2(0, 3) +
587 this->data[0][1] * _m2(1, 3) +
588 this->data[0][2] * _m2(2, 3) +
589 this->data[0][3] * _m2(3, 3),
590
591 this->data[1][0] * _m2(0, 0) +
592 this->data[1][1] * _m2(1, 0) +
593 this->data[1][2] * _m2(2, 0) +
594 this->data[1][3] * _m2(3, 0),
595
596 this->data[1][0] * _m2(0, 1) +
597 this->data[1][1] * _m2(1, 1) +
598 this->data[1][2] * _m2(2, 1) +
599 this->data[1][3] * _m2(3, 1),
600
601 this->data[1][0] * _m2(0, 2) +
602 this->data[1][1] * _m2(1, 2) +
603 this->data[1][2] * _m2(2, 2) +
604 this->data[1][3] * _m2(3, 2),
605
606 this->data[1][0] * _m2(0, 3) +
607 this->data[1][1] * _m2(1, 3) +
608 this->data[1][2] * _m2(2, 3) +
609 this->data[1][3] * _m2(3, 3),
610
611 this->data[2][0] * _m2(0, 0) +
612 this->data[2][1] * _m2(1, 0) +
613 this->data[2][2] * _m2(2, 0) +
614 this->data[2][3] * _m2(3, 0),
615
616 this->data[2][0] * _m2(0, 1) +
617 this->data[2][1] * _m2(1, 1) +
618 this->data[2][2] * _m2(2, 1) +
619 this->data[2][3] * _m2(3, 1),
620
621 this->data[2][0] * _m2(0, 2) +
622 this->data[2][1] * _m2(1, 2) +
623 this->data[2][2] * _m2(2, 2) +
624 this->data[2][3] * _m2(3, 2),
625
626 this->data[2][0] * _m2(0, 3) +
627 this->data[2][1] * _m2(1, 3) +
628 this->data[2][2] * _m2(2, 3) +
629 this->data[2][3] * _m2(3, 3),
630
631 this->data[3][0] * _m2(0, 0) +
632 this->data[3][1] * _m2(1, 0) +
633 this->data[3][2] * _m2(2, 0) +
634 this->data[3][3] * _m2(3, 0),
635
636 this->data[3][0] * _m2(0, 1) +
637 this->data[3][1] * _m2(1, 1) +
638 this->data[3][2] * _m2(2, 1) +
639 this->data[3][3] * _m2(3, 1),
640
641 this->data[3][0] * _m2(0, 2) +
642 this->data[3][1] * _m2(1, 2) +
643 this->data[3][2] * _m2(2, 2) +
644 this->data[3][3] * _m2(3, 2),
645
646 this->data[3][0] * _m2(0, 3) +
647 this->data[3][1] * _m2(1, 3) +
648 this->data[3][2] * _m2(2, 3) +
649 this->data[3][3] * _m2(3, 3));
650 }
651
655 public: Vector3<T> operator*(const Vector3<T> &_vec) const
656 {
657 return Vector3<T>(
658 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
659 this->data[0][2]*_vec.Z() + this->data[0][3],
660 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
661 this->data[1][2]*_vec.Z() + this->data[1][3],
662 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
663 this->data[2][2]*_vec.Z() + this->data[2][3]);
664 }
665
672 public: inline const T &operator()(const size_t _row,
673 const size_t _col) const
674 {
675 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)][
676 clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
677 }
678
686 public: inline T &operator()(const size_t _row, const size_t _col)
687 {
688 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)]
689 [clamp(_col, GZ_ZERO_SIZE_T, GZ_THREE_SIZE_T)];
690 }
691
697 public: bool Equal(const Matrix4 &_m, const T &_tol) const
698 {
699 return equal<T>(this->data[0][0], _m(0, 0), _tol)
700 && equal<T>(this->data[0][1], _m(0, 1), _tol)
701 && equal<T>(this->data[0][2], _m(0, 2), _tol)
702 && equal<T>(this->data[0][3], _m(0, 3), _tol)
703 && equal<T>(this->data[1][0], _m(1, 0), _tol)
704 && equal<T>(this->data[1][1], _m(1, 1), _tol)
705 && equal<T>(this->data[1][2], _m(1, 2), _tol)
706 && equal<T>(this->data[1][3], _m(1, 3), _tol)
707 && equal<T>(this->data[2][0], _m(2, 0), _tol)
708 && equal<T>(this->data[2][1], _m(2, 1), _tol)
709 && equal<T>(this->data[2][2], _m(2, 2), _tol)
710 && equal<T>(this->data[2][3], _m(2, 3), _tol)
711 && equal<T>(this->data[3][0], _m(3, 0), _tol)
712 && equal<T>(this->data[3][1], _m(3, 1), _tol)
713 && equal<T>(this->data[3][2], _m(3, 2), _tol)
714 && equal<T>(this->data[3][3], _m(3, 3), _tol);
715 }
716
721 public: bool operator==(const Matrix4<T> &_m) const
722 {
723 return this->Equal(_m, static_cast<T>(1e-6));
724 }
725
729 public: bool operator!=(const Matrix4<T> &_m) const
730 {
731 return !(*this == _m);
732 }
733
738 public: friend std::ostream &operator<<(
739 std::ostream &_out, const gz::math::Matrix4<T> &_m)
740 {
741 for (auto i : {0, 1, 2, 3})
742 {
743 for (auto j : {0, 1, 2, 3})
744 {
745 if (!(i == 0 && j == 0))
746 _out << " ";
747
748 appendToStream(_out, _m(i, j));
749 }
750 }
751
752 return _out;
753 }
754
759 public: friend std::istream &operator>>(
760 std::istream &_in, gz::math::Matrix4<T> &_m)
761 {
762 // Skip white spaces
763 _in.setf(std::ios_base::skipws);
764 T d[16];
765 _in >> d[0] >> d[1] >> d[2] >> d[3]
766 >> d[4] >> d[5] >> d[6] >> d[7]
767 >> d[8] >> d[9] >> d[10] >> d[11]
768 >> d[12] >> d[13] >> d[14] >> d[15];
769
770 if (!_in.fail())
771 {
772 _m.Set(d[0], d[1], d[2], d[3],
773 d[4], d[5], d[6], d[7],
774 d[8], d[9], d[10], d[11],
775 d[12], d[13], d[14], d[15]);
776 }
777 return _in;
778 }
779
789 public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
790 const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
791 {
792 // Most important constraint: direction to point X axis at
793 auto front = _target - _eye;
794
795 // Case when _eye == _target
796 if (front == Vector3<T>::Zero)
797 front = Vector3<T>::UnitX;
798 front.Normalize();
799
800 // Desired direction to point Z axis at
801 auto up = _up;
802
803 // Case when _up == Zero
804 if (up == Vector3<T>::Zero)
805 up = Vector3<T>::UnitZ;
806 else
807 up.Normalize();
808
809 // Case when _up is parallel to X
810 if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
811 up = Vector3<T>::UnitZ;
812
813 // Find direction to point Y axis at
814 auto left = up.Cross(front);
815
816 // Case when front is parallel to up
817 if (left == Vector3<T>::Zero)
818 left = Vector3<T>::UnitY;
819 else
820 left.Normalize();
821
822 // Fix up direction so it's perpendicular to XY
823 up = (front.Cross(left)).Normalize();
824
825 return Matrix4<T>(
826 front.X(), left.X(), up.X(), _eye.X(),
827 front.Y(), left.Y(), up.Y(), _eye.Y(),
828 front.Z(), left.Z(), up.Z(), _eye.Z(),
829 0, 0, 0, 1);
830 }
831
833 private: T data[4][4];
834 };
835
836 namespace detail {
837
838 template<typename T>
839 constexpr Matrix4<T> gMatrix4Identity(
840 1, 0, 0, 0,
841 0, 1, 0, 0,
842 0, 0, 1, 0,
843 0, 0, 0, 1);
844
845 template<typename T>
846 constexpr Matrix4<T> gMatrix4Zero(
847 0, 0, 0, 0,
848 0, 0, 0, 0,
849 0, 0, 0, 0,
850 0, 0, 0, 0);
851
852 } // namespace detail
853
854 template<typename T>
855 const Matrix4<T> &Matrix4<T>::Identity = detail::gMatrix4Identity<T>;
856
857 template<typename T>
858 const Matrix4<T> &Matrix4<T>::Zero = detail::gMatrix4Zero<T>;
859
860 typedef Matrix4<int> Matrix4i;
861 typedef Matrix4<double> Matrix4d;
862 typedef Matrix4<float> Matrix4f;
863 } // namespace GZ_MATH_VERSION_NAMESPACE
864} // namespace gz::math
865#endif // GZ_MATH_MATRIX4_HH_