17 #ifndef GZ_MATH_MATRIX4_HH_
18 #define GZ_MATH_MATRIX4_HH_
26 #include <gz/math/config.hh>
33 inline namespace IGNITION_MATH_VERSION_NAMESPACE {
49 memset(this->data, 0,
sizeof(this->data[0][0])*16);
56 memcpy(this->data, _m.data,
sizeof(this->data[0][0])*16);
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)
81 this->Set(_v00, _v01, _v02, _v03,
82 _v10, _v11, _v12, _v13,
83 _v20, _v21, _v22, _v23,
84 _v30, _v31, _v32, _v33);
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(),
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(),
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(),
115 this->SetTranslation(_pose.
Pos());
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)
144 this->data[0][0] = _v00;
145 this->data[0][1] = _v01;
146 this->data[0][2] = _v02;
147 this->data[0][3] = _v03;
149 this->data[1][0] = _v10;
150 this->data[1][1] = _v11;
151 this->data[1][2] = _v12;
152 this->data[1][3] = _v13;
154 this->data[2][0] = _v20;
155 this->data[2][1] = _v21;
156 this->data[2][2] = _v22;
157 this->data[2][3] = _v23;
159 this->data[3][0] = _v30;
160 this->data[3][1] = _v31;
161 this->data[3][2] = _v32;
162 this->data[3][3] = _v33;
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;
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;
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;
194 this->SetTranslation(_t);
201 this->data[0][3] = _t.
X();
202 this->data[1][3] = _t.
Y();
203 this->data[2][3] = _t.
Z();
213 Translate(T _x, T _y, T _z)
215 this->SetTranslation(_x, _y, _z);
224 this->data[0][3] = _x;
225 this->data[1][3] = _y;
226 this->data[2][3] = _z;
233 return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
240 return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
250 T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
254 root = sqrt(trace + 1.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);
263 static unsigned int s_iNext[3] = {1, 2, 0};
265 if (this->data[1][1] > this->data[0][0])
267 if (this->data[2][2] > this->data[i][i])
269 unsigned int j = s_iNext[i];
270 unsigned int k = s_iNext[j];
272 root = sqrt(this->data[i][i] - this->data[j][j] -
273 this->data[k][k] + 1.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;
284 case 0: q.
X(a);
break;
285 case 1: q.
Y(a);
break;
286 case 2: q.
Z(a);
break;
291 case 0: q.
X(b);
break;
292 case 1: q.
Y(b);
break;
293 case 2: q.
Z(b);
break;
298 case 0: q.
X(c);
break;
299 case 1: q.
Y(c);
break;
300 case 2: q.
Z(c);
break;
303 q.
W((this->data[k][j] - this->data[j][k]) * root);
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];
326 if (std::abs(m31) >= 1.0)
335 euler.
X(atan2(m12, m13));
336 euler2.
X(atan2(m12, m13));
342 euler.
X(atan2(-m12, -m13));
343 euler2.
X(atan2(-m12, -m13));
351 euler.
X(atan2(m32 / cos(euler.
Y()), m33 / cos(euler.
Y())));
352 euler2.
X(atan2(m32 / cos(euler2.
Y()), m33 / cos(euler2.
Y())));
354 euler.
Z(atan2(m21 / cos(euler.
Y()), m11 / cos(euler.
Y())));
355 euler2.
Z(atan2(m21 / cos(euler2.
Y()), m11 / cos(euler2.
Y())));
368 return Pose3<T>(this->Translation(), this->Rotation());
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;
385 public:
void Scale(T _x, T _y, T _z)
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;
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));
413 if (this->IsAffine())
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]);
436 if (!this->IsAffine())
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]);
452 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
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];
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];
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];
483 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
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];
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]);
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]);
511 r(0, 0) = t00 * invDet;
512 r(1, 0) = t10 * invDet;
513 r(2, 0) = t20 * invDet;
514 r(3, 0) = t30 * invDet;
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;
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];
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;
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];
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;
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]);
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]);
599 memcpy(this->data, _mat.data,
sizeof(this->data[0][0])*16);
608 this->data[0][0] = _mat(0, 0);
609 this->data[0][1] = _mat(0, 1);
610 this->data[0][2] = _mat(0, 2);
612 this->data[1][0] = _mat(1, 0);
613 this->data[1][1] = _mat(1, 1);
614 this->data[1][2] = _mat(1, 2);
616 this->data[2][0] = _mat(2, 0);
617 this->data[2][1] = _mat(2, 1);
618 this->data[2][2] = _mat(2, 2);
629 (*this) = (*this) * _m2;
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),
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),
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),
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),
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),
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),
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),
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),
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),
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),
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),
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),
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),
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),
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),
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));
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]);
741 const size_t _col)
const
754 public:
inline T &
operator()(
const size_t _row,
const size_t _col)
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);
791 return this->Equal(_m,
static_cast<T
>(1e-6));
799 return !(*
this == _m);
837 _in.
setf(std::ios_base::skipws);
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];
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]);
867 auto front = _target - _eye;
888 auto left = up.Cross(front);
897 up = (front.Cross(left)).Normalize();
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(),
907 private: T data[4][4];