Loading...
Searching...
No Matches
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 IGNITION_MATH_MATRIX4_HH_
18#define IGNITION_MATH_MATRIX4_HH_
19
20#include <algorithm>
25#include <ignition/math/config.hh>
26
27namespace ignition
28{
29 namespace math
30 {
31 inline namespace IGNITION_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)
53 {
54 memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
55 }
56
74 public: Matrix4(T _v00, T _v01, T _v02, T _v03,
75 T _v10, T _v11, T _v12, T _v13,
76 T _v20, T _v21, T _v22, T _v23,
77 T _v30, T _v31, T _v32, T _v33)
78 {
79 this->Set(_v00, _v01, _v02, _v03,
80 _v10, _v11, _v12, _v13,
81 _v20, _v21, _v22, _v23,
82 _v30, _v31, _v32, _v33);
83 }
84
87 public: explicit Matrix4(const Quaternion<T> &_q)
88 {
89 Quaternion<T> qt = _q;
90 qt.Normalize();
91 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
92 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
93 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
94 0,
95
96 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
97 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
98 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
99 0,
100
101 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
102 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
103 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
104 0,
105
106 0, 0, 0, 1);
107 }
108
111 public: explicit Matrix4(const Pose3<T> &_pose) : Matrix4(_pose.Rot())
112 {
113 this->SetTranslation(_pose.Pos());
114 }
115
117 public: virtual ~Matrix4() {}
118
136 public: void Set(
137 T _v00, T _v01, T _v02, T _v03,
138 T _v10, T _v11, T _v12, T _v13,
139 T _v20, T _v21, T _v22, T _v23,
140 T _v30, T _v31, T _v32, T _v33)
141 {
142 this->data[0][0] = _v00;
143 this->data[0][1] = _v01;
144 this->data[0][2] = _v02;
145 this->data[0][3] = _v03;
146
147 this->data[1][0] = _v10;
148 this->data[1][1] = _v11;
149 this->data[1][2] = _v12;
150 this->data[1][3] = _v13;
151
152 this->data[2][0] = _v20;
153 this->data[2][1] = _v21;
154 this->data[2][2] = _v22;
155 this->data[2][3] = _v23;
156
157 this->data[3][0] = _v30;
158 this->data[3][1] = _v31;
159 this->data[3][2] = _v32;
160 this->data[3][3] = _v33;
161 }
162
166 public: void Axis(const Vector3<T> &_axis, T _angle)
167 {
168 T c = cos(_angle);
169 T s = sin(_angle);
170 T C = 1-c;
171
172 this->data[0][0] = _axis.X()*_axis.X()*C + c;
173 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
174 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
175
176 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
177 this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
178 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
179
180 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
181 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
182 this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
183 }
184
188 public: void
190 Translate(const Vector3<T> &_t)
191 {
192 this->SetTranslation(_t);
193 }
194
197 public: void SetTranslation(const Vector3<T> &_t)
198 {
199 this->data[0][3] = _t.X();
200 this->data[1][3] = _t.Y();
201 this->data[2][3] = _t.Z();
202 }
203
209 public: void
211 Translate(T _x, T _y, T _z)
212 {
213 this->SetTranslation(_x, _y, _z);
214 }
215
220 public: void SetTranslation(T _x, T _y, T _z)
221 {
222 this->data[0][3] = _x;
223 this->data[1][3] = _y;
224 this->data[2][3] = _z;
225 }
226
229 public: Vector3<T> Translation() const
230 {
231 return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
232 }
233
236 public: Vector3<T> Scale() const
237 {
238 return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
239 }
240
243 public: Quaternion<T> Rotation() const
244 {
248 T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
249 T root;
250 if (trace > 0)
251 {
252 root = sqrt(trace + 1.0);
253 q.W(root / 2.0);
254 root = 1.0 / (2.0 * root);
255 q.X((this->data[2][1] - this->data[1][2]) * root);
256 q.Y((this->data[0][2] - this->data[2][0]) * root);
257 q.Z((this->data[1][0] - this->data[0][1]) * root);
258 }
259 else
260 {
261 static unsigned int s_iNext[3] = {1, 2, 0};
262 unsigned int i = 0;
263 if (this->data[1][1] > this->data[0][0])
264 i = 1;
265 if (this->data[2][2] > this->data[i][i])
266 i = 2;
267 unsigned int j = s_iNext[i];
268 unsigned int k = s_iNext[j];
269
270 root = sqrt(this->data[i][i] - this->data[j][j] -
271 this->data[k][k] + 1.0);
272
273 T a, b, c;
274 a = root / 2.0;
275 root = 1.0 / (2.0 * root);
276 b = (this->data[j][i] + this->data[i][j]) * root;
277 c = (this->data[k][i] + this->data[i][k]) * root;
278
279 switch (i)
280 {
281 default:
282 case 0: q.X(a); break;
283 case 1: q.Y(a); break;
284 case 2: q.Z(a); break;
285 };
286 switch (j)
287 {
288 default:
289 case 0: q.X(b); break;
290 case 1: q.Y(b); break;
291 case 2: q.Z(b); break;
292 };
293 switch (k)
294 {
295 default:
296 case 0: q.X(c); break;
297 case 1: q.Y(c); break;
298 case 2: q.Z(c); break;
299 };
300
301 q.W((this->data[k][j] - this->data[j][k]) * root);
302 }
303
304 return q;
305 }
306
311 public: Vector3<T> EulerRotation(bool _firstSolution) const
312 {
313 Vector3<T> euler;
314 Vector3<T> euler2;
315
316 T m31 = this->data[2][0];
317 T m11 = this->data[0][0];
318 T m12 = this->data[0][1];
319 T m13 = this->data[0][2];
320 T m32 = this->data[2][1];
321 T m33 = this->data[2][2];
322 T m21 = this->data[1][0];
323
324 if (std::abs(m31) >= 1.0)
325 {
326 euler.Z(0.0);
327 euler2.Z(0.0);
328
329 if (m31 < 0.0)
330 {
331 euler.Y(IGN_PI / 2.0);
332 euler2.Y(IGN_PI / 2.0);
333 euler.X(atan2(m12, m13));
334 euler2.X(atan2(m12, m13));
335 }
336 else
337 {
338 euler.Y(-IGN_PI / 2.0);
339 euler2.Y(-IGN_PI / 2.0);
340 euler.X(atan2(-m12, -m13));
341 euler2.X(atan2(-m12, -m13));
342 }
343 }
344 else
345 {
346 euler.Y(-asin(m31));
347 euler2.Y(IGN_PI - euler.Y());
348
349 euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
350 euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));
351
352 euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
353 euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
354 }
355
356 if (_firstSolution)
357 return euler;
358 else
359 return euler2;
360 }
361
364 public: Pose3<T> Pose() const
365 {
366 return Pose3<T>(this->Translation(), this->Rotation());
367 }
368
371 public: void Scale(const Vector3<T> &_s)
372 {
373 this->data[0][0] = _s.X();
374 this->data[1][1] = _s.Y();
375 this->data[2][2] = _s.Z();
376 this->data[3][3] = 1.0;
377 }
378
383 public: void Scale(T _x, T _y, T _z)
384 {
385 this->data[0][0] = _x;
386 this->data[1][1] = _y;
387 this->data[2][2] = _z;
388 this->data[3][3] = 1.0;
389 }
390
393 public: bool IsAffine() const
394 {
395 return equal(this->data[3][0], static_cast<T>(0)) &&
396 equal(this->data[3][1], static_cast<T>(0)) &&
397 equal(this->data[3][2], static_cast<T>(0)) &&
398 equal(this->data[3][3], static_cast<T>(1));
399 }
400
407 public: Vector3<T>
409 TransformAffine(const Vector3<T> &_v) const
410 {
411 if (this->IsAffine())
412 {
413 return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
414 this->data[0][2]*_v.Z() + this->data[0][3],
415 this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
416 this->data[1][2]*_v.Z() + this->data[1][3],
417 this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
418 this->data[2][2]*_v.Z() + this->data[2][3]);
419 }
420 else
421 {
422 return Vector3<T>();
423 }
424 }
425
431 public: bool TransformAffine(const Vector3<T> &_v,
432 Vector3<T> &_result) const
433 {
434 if (!this->IsAffine())
435 return false;
436
437 _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
438 this->data[0][2]*_v.Z() + this->data[0][3],
439 this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
440 this->data[1][2]*_v.Z() + this->data[1][3],
441 this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
442 this->data[2][2]*_v.Z() + this->data[2][3]);
443 return true;
444 }
445
448 public: T Determinant() const
449 {
450 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
451
452 v0 = this->data[2][0]*this->data[3][1]
453 - this->data[2][1]*this->data[3][0];
454 v1 = this->data[2][0]*this->data[3][2]
455 - this->data[2][2]*this->data[3][0];
456 v2 = this->data[2][0]*this->data[3][3]
457 - this->data[2][3]*this->data[3][0];
458 v3 = this->data[2][1]*this->data[3][2]
459 - this->data[2][2]*this->data[3][1];
460 v4 = this->data[2][1]*this->data[3][3]
461 - this->data[2][3]*this->data[3][1];
462 v5 = this->data[2][2]*this->data[3][3]
463 - this->data[2][3]*this->data[3][2];
464
465 t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3];
466 t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3];
467 t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3];
468 t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2];
469
470 return t00 * this->data[0][0]
471 + t10 * this->data[0][1]
472 + t20 * this->data[0][2]
473 + t30 * this->data[0][3];
474 }
475
479 public: Matrix4<T> Inverse() const
480 {
481 T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
482 Matrix4<T> r;
483
484 v0 = this->data[2][0]*this->data[3][1] -
485 this->data[2][1]*this->data[3][0];
486 v1 = this->data[2][0]*this->data[3][2] -
487 this->data[2][2]*this->data[3][0];
488 v2 = this->data[2][0]*this->data[3][3] -
489 this->data[2][3]*this->data[3][0];
490 v3 = this->data[2][1]*this->data[3][2] -
491 this->data[2][2]*this->data[3][1];
492 v4 = this->data[2][1]*this->data[3][3] -
493 this->data[2][3]*this->data[3][1];
494 v5 = this->data[2][2]*this->data[3][3] -
495 this->data[2][3]*this->data[3][2];
496
497 t00 = +(v5*this->data[1][1] -
498 v4*this->data[1][2] + v3*this->data[1][3]);
499 t10 = -(v5*this->data[1][0] -
500 v2*this->data[1][2] + v1*this->data[1][3]);
501 t20 = +(v4*this->data[1][0] -
502 v2*this->data[1][1] + v0*this->data[1][3]);
503 t30 = -(v3*this->data[1][0] -
504 v1*this->data[1][1] + v0*this->data[1][2]);
505
506 T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
507 t20 * this->data[0][2] + t30 * this->data[0][3]);
508
509 r(0, 0) = t00 * invDet;
510 r(1, 0) = t10 * invDet;
511 r(2, 0) = t20 * invDet;
512 r(3, 0) = t30 * invDet;
513
514 r(0, 1) = -(v5*this->data[0][1] -
515 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
516 r(1, 1) = +(v5*this->data[0][0] -
517 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
518 r(2, 1) = -(v4*this->data[0][0] -
519 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
520 r(3, 1) = +(v3*this->data[0][0] -
521 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
522
523 v0 = this->data[1][0]*this->data[3][1] -
524 this->data[1][1]*this->data[3][0];
525 v1 = this->data[1][0]*this->data[3][2] -
526 this->data[1][2]*this->data[3][0];
527 v2 = this->data[1][0]*this->data[3][3] -
528 this->data[1][3]*this->data[3][0];
529 v3 = this->data[1][1]*this->data[3][2] -
530 this->data[1][2]*this->data[3][1];
531 v4 = this->data[1][1]*this->data[3][3] -
532 this->data[1][3]*this->data[3][1];
533 v5 = this->data[1][2]*this->data[3][3] -
534 this->data[1][3]*this->data[3][2];
535
536 r(0, 2) = +(v5*this->data[0][1] -
537 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
538 r(1, 2) = -(v5*this->data[0][0] -
539 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
540 r(2, 2) = +(v4*this->data[0][0] -
541 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
542 r(3, 2) = -(v3*this->data[0][0] -
543 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
544
545 v0 = this->data[2][1]*this->data[1][0] -
546 this->data[2][0]*this->data[1][1];
547 v1 = this->data[2][2]*this->data[1][0] -
548 this->data[2][0]*this->data[1][2];
549 v2 = this->data[2][3]*this->data[1][0] -
550 this->data[2][0]*this->data[1][3];
551 v3 = this->data[2][2]*this->data[1][1] -
552 this->data[2][1]*this->data[1][2];
553 v4 = this->data[2][3]*this->data[1][1] -
554 this->data[2][1]*this->data[1][3];
555 v5 = this->data[2][3]*this->data[1][2] -
556 this->data[2][2]*this->data[1][3];
557
558 r(0, 3) = -(v5*this->data[0][1] -
559 v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
560 r(1, 3) = +(v5*this->data[0][0] -
561 v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
562 r(2, 3) = -(v4*this->data[0][0] -
563 v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
564 r(3, 3) = +(v3*this->data[0][0] -
565 v1*this->data[0][1] + v0*this->data[0][2]) * invDet;
566
567 return r;
568 }
569
571 public: void Transpose()
572 {
573 std::swap(this->data[0][1], this->data[1][0]);
574 std::swap(this->data[0][2], this->data[2][0]);
575 std::swap(this->data[0][3], this->data[3][0]);
576 std::swap(this->data[1][2], this->data[2][1]);
577 std::swap(this->data[1][3], this->data[3][1]);
578 std::swap(this->data[2][3], this->data[3][2]);
579 }
580
583 public: Matrix4<T> Transposed() const
584 {
585 return Matrix4<T>(
586 this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0],
587 this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1],
588 this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2],
589 this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]);
590 }
591
595 public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
596 {
597 memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
598 return *this;
599 }
600
604 public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
605 {
606 this->data[0][0] = _mat(0, 0);
607 this->data[0][1] = _mat(0, 1);
608 this->data[0][2] = _mat(0, 2);
609
610 this->data[1][0] = _mat(1, 0);
611 this->data[1][1] = _mat(1, 1);
612 this->data[1][2] = _mat(1, 2);
613
614 this->data[2][0] = _mat(2, 0);
615 this->data[2][1] = _mat(2, 1);
616 this->data[2][2] = _mat(2, 2);
617
618 return *this;
619 }
620
624 public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
625 {
626 return Matrix4<T>(
627 this->data[0][0] * _m2(0, 0) +
628 this->data[0][1] * _m2(1, 0) +
629 this->data[0][2] * _m2(2, 0) +
630 this->data[0][3] * _m2(3, 0),
631
632 this->data[0][0] * _m2(0, 1) +
633 this->data[0][1] * _m2(1, 1) +
634 this->data[0][2] * _m2(2, 1) +
635 this->data[0][3] * _m2(3, 1),
636
637 this->data[0][0] * _m2(0, 2) +
638 this->data[0][1] * _m2(1, 2) +
639 this->data[0][2] * _m2(2, 2) +
640 this->data[0][3] * _m2(3, 2),
641
642 this->data[0][0] * _m2(0, 3) +
643 this->data[0][1] * _m2(1, 3) +
644 this->data[0][2] * _m2(2, 3) +
645 this->data[0][3] * _m2(3, 3),
646
647 this->data[1][0] * _m2(0, 0) +
648 this->data[1][1] * _m2(1, 0) +
649 this->data[1][2] * _m2(2, 0) +
650 this->data[1][3] * _m2(3, 0),
651
652 this->data[1][0] * _m2(0, 1) +
653 this->data[1][1] * _m2(1, 1) +
654 this->data[1][2] * _m2(2, 1) +
655 this->data[1][3] * _m2(3, 1),
656
657 this->data[1][0] * _m2(0, 2) +
658 this->data[1][1] * _m2(1, 2) +
659 this->data[1][2] * _m2(2, 2) +
660 this->data[1][3] * _m2(3, 2),
661
662 this->data[1][0] * _m2(0, 3) +
663 this->data[1][1] * _m2(1, 3) +
664 this->data[1][2] * _m2(2, 3) +
665 this->data[1][3] * _m2(3, 3),
666
667 this->data[2][0] * _m2(0, 0) +
668 this->data[2][1] * _m2(1, 0) +
669 this->data[2][2] * _m2(2, 0) +
670 this->data[2][3] * _m2(3, 0),
671
672 this->data[2][0] * _m2(0, 1) +
673 this->data[2][1] * _m2(1, 1) +
674 this->data[2][2] * _m2(2, 1) +
675 this->data[2][3] * _m2(3, 1),
676
677 this->data[2][0] * _m2(0, 2) +
678 this->data[2][1] * _m2(1, 2) +
679 this->data[2][2] * _m2(2, 2) +
680 this->data[2][3] * _m2(3, 2),
681
682 this->data[2][0] * _m2(0, 3) +
683 this->data[2][1] * _m2(1, 3) +
684 this->data[2][2] * _m2(2, 3) +
685 this->data[2][3] * _m2(3, 3),
686
687 this->data[3][0] * _m2(0, 0) +
688 this->data[3][1] * _m2(1, 0) +
689 this->data[3][2] * _m2(2, 0) +
690 this->data[3][3] * _m2(3, 0),
691
692 this->data[3][0] * _m2(0, 1) +
693 this->data[3][1] * _m2(1, 1) +
694 this->data[3][2] * _m2(2, 1) +
695 this->data[3][3] * _m2(3, 1),
696
697 this->data[3][0] * _m2(0, 2) +
698 this->data[3][1] * _m2(1, 2) +
699 this->data[3][2] * _m2(2, 2) +
700 this->data[3][3] * _m2(3, 2),
701
702 this->data[3][0] * _m2(0, 3) +
703 this->data[3][1] * _m2(1, 3) +
704 this->data[3][2] * _m2(2, 3) +
705 this->data[3][3] * _m2(3, 3));
706 }
707
711 public: Vector3<T> operator*(const Vector3<T> &_vec) const
712 {
713 return Vector3<T>(
714 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
715 this->data[0][2]*_vec.Z() + this->data[0][3],
716 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
717 this->data[1][2]*_vec.Z() + this->data[1][3],
718 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
719 this->data[2][2]*_vec.Z() + this->data[2][3]);
720 }
721
728 public: inline const T &operator()(const size_t _row,
729 const size_t _col) const
730 {
731 return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)][
733 }
734
742 public: inline T &operator()(const size_t _row, const size_t _col)
743 {
744 return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]
746 }
747
753 public: bool Equal(const Matrix4 &_m, const T &_tol) const
754 {
755 return equal<T>(this->data[0][0], _m(0, 0), _tol)
756 && equal<T>(this->data[0][1], _m(0, 1), _tol)
757 && equal<T>(this->data[0][2], _m(0, 2), _tol)
758 && equal<T>(this->data[0][3], _m(0, 3), _tol)
759 && equal<T>(this->data[1][0], _m(1, 0), _tol)
760 && equal<T>(this->data[1][1], _m(1, 1), _tol)
761 && equal<T>(this->data[1][2], _m(1, 2), _tol)
762 && equal<T>(this->data[1][3], _m(1, 3), _tol)
763 && equal<T>(this->data[2][0], _m(2, 0), _tol)
764 && equal<T>(this->data[2][1], _m(2, 1), _tol)
765 && equal<T>(this->data[2][2], _m(2, 2), _tol)
766 && equal<T>(this->data[2][3], _m(2, 3), _tol)
767 && equal<T>(this->data[3][0], _m(3, 0), _tol)
768 && equal<T>(this->data[3][1], _m(3, 1), _tol)
769 && equal<T>(this->data[3][2], _m(3, 2), _tol)
770 && equal<T>(this->data[3][3], _m(3, 3), _tol);
771 }
772
777 public: bool operator==(const Matrix4<T> &_m) const
778 {
779 return this->Equal(_m, static_cast<T>(1e-6));
780 }
781
785 public: bool operator!=(const Matrix4<T> &_m) const
786 {
787 return !(*this == _m);
788 }
789
794 public: friend std::ostream &operator<<(
795 std::ostream &_out, const ignition::math::Matrix4<T> &_m)
796 {
797 _out << precision(_m(0, 0), 6) << " "
798 << precision(_m(0, 1), 6) << " "
799 << precision(_m(0, 2), 6) << " "
800 << precision(_m(0, 3), 6) << " "
801 << precision(_m(1, 0), 6) << " "
802 << precision(_m(1, 1), 6) << " "
803 << precision(_m(1, 2), 6) << " "
804 << precision(_m(1, 3), 6) << " "
805 << precision(_m(2, 0), 6) << " "
806 << precision(_m(2, 1), 6) << " "
807 << precision(_m(2, 2), 6) << " "
808 << precision(_m(2, 3), 6) << " "
809 << precision(_m(3, 0), 6) << " "
810 << precision(_m(3, 1), 6) << " "
811 << precision(_m(3, 2), 6) << " "
812 << precision(_m(3, 3), 6);
813
814 return _out;
815 }
816
821 public: friend std::istream &operator>>(
822 std::istream &_in, ignition::math::Matrix4<T> &_m)
823 {
824 // Skip white spaces
825 _in.setf(std::ios_base::skipws);
826 T d[16];
827 _in >> d[0] >> d[1] >> d[2] >> d[3]
828 >> d[4] >> d[5] >> d[6] >> d[7]
829 >> d[8] >> d[9] >> d[10] >> d[11]
830 >> d[12] >> d[13] >> d[14] >> d[15];
831
832 _m.Set(d[0], d[1], d[2], d[3],
833 d[4], d[5], d[6], d[7],
834 d[8], d[9], d[10], d[11],
835 d[12], d[13], d[14], d[15]);
836 return _in;
837 }
838
848 public: static Matrix4<T> LookAt(const Vector3<T> &_eye,
849 const Vector3<T> &_target, const Vector3<T> &_up = Vector3<T>::UnitZ)
850 {
851 // Most important constraint: direction to point X axis at
852 auto front = _target - _eye;
853
854 // Case when _eye == _target
855 if (front == Vector3<T>::Zero)
856 front = Vector3<T>::UnitX;
857 front.Normalize();
858
859 // Desired direction to point Z axis at
860 auto up = _up;
861
862 // Case when _up == Zero
863 if (up == Vector3<T>::Zero)
865 else
866 up.Normalize();
867
868 // Case when _up is parallel to X
869 if (up.Cross(Vector3<T>::UnitX) == Vector3<T>::Zero)
871
872 // Find direction to point Y axis at
873 auto left = up.Cross(front);
874
875 // Case when front is parallel to up
876 if (left == Vector3<T>::Zero)
877 left = Vector3<T>::UnitY;
878 else
879 left.Normalize();
880
881 // Fix up direction so it's perpendicular to XY
882 up = (front.Cross(left)).Normalize();
883
884 return Matrix4<T>(
885 front.X(), left.X(), up.X(), _eye.X(),
886 front.Y(), left.Y(), up.Y(), _eye.Y(),
887 front.Z(), left.Z(), up.Z(), _eye.Z(),
888 0, 0, 0, 1);
889 }
890
892 private: T data[4][4];
893 };
894
895 template<typename T>
897 1, 0, 0, 0,
898 0, 1, 0, 0,
899 0, 0, 1, 0,
900 0, 0, 0, 1);
901
902 template<typename T>
904 0, 0, 0, 0,
905 0, 0, 0, 0,
906 0, 0, 0, 0,
907 0, 0, 0, 0);
908
912 }
913 }
914}
915#endif
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition Helpers.hh:174
A 3x3 matrix class.
Definition Quaternion.hh:32
A 4x4 matrix class.
Definition Matrix4.hh:37
void Axis(const Vector3< T > &_axis, T _angle)
Set the upper-left 3x3 matrix from an axis and angle.
Definition Matrix4.hh:166
Matrix4< T > operator*(const Matrix4< T > &_m2) const
Multiplication operator.
Definition Matrix4.hh:624
void Transpose()
Transpose this matrix.
Definition Matrix4.hh:571
Vector3< T > IGN_DEPRECATED(3.0) TransformAffine(const Vector3< T > &_v) const
Perform an affine transformation.
Definition Matrix4.hh:408
T & operator()(const size_t _row, const size_t _col)
Get a mutable version the value at the specified row, column index.
Definition Matrix4.hh:742
bool operator==(const Matrix4< T > &_m) const
Equality operator.
Definition Matrix4.hh:777
void Scale(T _x, T _y, T _z)
Set the scale.
Definition Matrix4.hh:383
void SetTranslation(T _x, T _y, T _z)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition Matrix4.hh:220
static const Matrix4< T > Zero
Zero matrix.
Definition Matrix4.hh:42
bool TransformAffine(const Vector3< T > &_v, Vector3< T > &_result) const
Perform an affine transformation.
Definition Matrix4.hh:431
Matrix4(const Quaternion< T > &_q)
Construct Matrix4 from a quaternion.
Definition Matrix4.hh:87
Vector3< T > EulerRotation(bool _firstSolution) const
Get the rotation as a Euler angles.
Definition Matrix4.hh:311
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 Matrix4.hh:74
const T & operator()(const size_t _row, const size_t _col) const
Get the value at the specified row, column index.
Definition Matrix4.hh:728
Vector3< T > Scale() const
Get the scale values as a Vector3<T>
Definition Matrix4.hh:236
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 Matrix4.hh:136
Vector3< T > Translation() const
Get the translational values as a Vector3.
Definition Matrix4.hh:229
Quaternion< T > Rotation() const
Get the rotation as a quaternion.
Definition Matrix4.hh:243
void IGN_DEPRECATED(4) Translate(T _x
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix4< T > &_m)
Stream extraction operator.
Definition Matrix4.hh:821
static const Matrix4< T > Identity
Identity matrix.
Definition Matrix4.hh:39
void IGN_DEPRECATED(4) Translate(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition Matrix4.hh:189
virtual ~Matrix4()
Destructor.
Definition Matrix4.hh:117
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix4< T > &_m)
Stream insertion operator.
Definition Matrix4.hh:794
Matrix4()
Constructor.
Definition Matrix4.hh:45
Matrix4(const Matrix4< T > &_m)
Copy constructor.
Definition Matrix4.hh:52
void Scale(const Vector3< T > &_s)
Set the scale.
Definition Matrix4.hh:371
bool operator!=(const Matrix4< T > &_m) const
Inequality test operator.
Definition Matrix4.hh:785
T Determinant() const
Return the determinant of the matrix.
Definition Matrix4.hh:448
Matrix4< T > & operator=(const Matrix4< T > &_mat)
Equal operator.
Definition Matrix4.hh:595
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition Matrix4.hh:479
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 Matrix4.hh:848
Matrix4(const Pose3< T > &_pose)
Construct Matrix4 from a math::Pose3.
Definition Matrix4.hh:111
Matrix4< T > Transposed() const
Return the transpose of this matrix.
Definition Matrix4.hh:583
bool IsAffine() const
Return true if the matrix is affine.
Definition Matrix4.hh:393
void SetTranslation(const Vector3< T > &_t)
Set the translational values [ (0, 3) (1, 3) (2, 3) ].
Definition Matrix4.hh:197
bool Equal(const Matrix4 &_m, const T &_tol) const
Equality test with tolerance.
Definition Matrix4.hh:753
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator.
Definition Matrix4.hh:711
Pose3< T > Pose() const
Get the transformation as math::Pose.
Definition Matrix4.hh:364
const Matrix4< T > & operator=(const Matrix3< T > &_mat)
Equal operator for 3x3 matrix.
Definition Matrix4.hh:604
Encapsulates a position and rotation in three space.
Definition Pose3.hh:34
const Vector3< T > & Pos() const
Get the position.
Definition Pose3.hh:348
A quaternion class.
Definition Quaternion.hh:38
void Normalize()
Normalize the quaternion.
Definition Quaternion.hh:223
const T & X() const
Get the x component.
Definition Quaternion.hh:955
const T & Y() const
Get the y component.
Definition Quaternion.hh:962
const T & Z() const
Get the z component.
Definition Quaternion.hh:969
const T & W() const
Get the w component.
Definition Quaternion.hh:948
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition Vector3.hh:178
T Z() const
Get the z value.
Definition Vector3.hh:661
T Y() const
Get the y value.
Definition Vector3.hh:654
Vector3 Normalize()
Normalize the vector length.
Definition Vector3.hh:132
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition Vector3.hh:188
T X() const
Get the x value.
Definition Vector3.hh:647
Matrix4< int > Matrix4i
Definition Matrix4.hh:909
Matrix4< double > Matrix4d
Definition Matrix4.hh:910
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition Helpers.hh:579
Matrix4< float > Matrix4f
Definition Matrix4.hh:911
static const size_t IGN_THREE_SIZE_T
size_t type with a value of 3
Definition Helpers.hh:225
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition Helpers.hh:545
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition Helpers.hh:216
Definition Angle.hh:40