Gazebo Math

API Reference

7.4.0
gz/math/Matrix3.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_MATRIX3_HH_
18#define GZ_MATH_MATRIX3_HH_
19
20#include <algorithm>
21#include <cstring>
22#include <utility>
23#include <gz/math/Helpers.hh>
24#include <gz/math/Vector3.hh>
25#include <gz/math/Quaternion.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 //
35 template <typename T> class Quaternion;
36
93 template<typename T>
94 class Matrix3
95 {
98 public: static const Matrix3<T> &Identity;
99
102 public: static const Matrix3<T> &Zero;
103
105 public: Matrix3()
106 {
107 std::memset(this->data, 0, sizeof(this->data[0][0])*9);
108 }
109
112 public: Matrix3(const Matrix3<T> &_m) = default;
113
124 public: constexpr Matrix3(T _v00, T _v01, T _v02,
125 T _v10, T _v11, T _v12,
126 T _v20, T _v21, T _v22)
127 : data{{_v00, _v01, _v02},
128 {_v10, _v11, _v12},
129 {_v20, _v21, _v22}}
130 {
131 }
132
135 public: explicit Matrix3(const Quaternion<T> &_q)
136 {
137 Quaternion<T> qt = _q;
138 qt.Normalize();
139 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
140 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
141 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
142 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
143 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
144 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
145 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
146 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
147 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
148 }
149
151 public: ~Matrix3() = default;
152
157 public: void Set(size_t _row, size_t _col, T _v)
158 {
159 this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
160 [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)] = _v;
161 }
162
173 public: void Set(T _v00, T _v01, T _v02,
174 T _v10, T _v11, T _v12,
175 T _v20, T _v21, T _v22)
176 {
177 this->data[0][0] = _v00;
178 this->data[0][1] = _v01;
179 this->data[0][2] = _v02;
180 this->data[1][0] = _v10;
181 this->data[1][1] = _v11;
182 this->data[1][2] = _v12;
183 this->data[2][0] = _v20;
184 this->data[2][1] = _v21;
185 this->data[2][2] = _v22;
186 }
187
194 public: void GZ_DEPRECATED(7) Axes(const Vector3<T> &_xAxis,
195 const Vector3<T> &_yAxis,
196 const Vector3<T> &_zAxis)
197 {
198 this->SetAxes(_xAxis, _yAxis, _zAxis);
199 }
200
205 public: void SetAxes(const Vector3<T> &_xAxis,
206 const Vector3<T> &_yAxis,
207 const Vector3<T> &_zAxis)
208 {
209 this->SetCol(0, _xAxis);
210 this->SetCol(1, _yAxis);
211 this->SetCol(2, _zAxis);
212 }
213
218 public: void GZ_DEPRECATED(7) Axis(const Vector3<T> &_axis, T _angle)
219 {
220 this->SetFromAxisAngle(_axis, _angle);
221 }
222
226 public: void SetFromAxisAngle(const Vector3<T> &_axis, T _angle)
227 {
228 T c = cos(_angle);
229 T s = sin(_angle);
230 T C = 1-c;
231
232 this->data[0][0] = _axis.X()*_axis.X()*C + c;
233 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
234 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
235
236 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
237 this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
238 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
239
240 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
241 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
242 this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
243 }
244
252 public: void GZ_DEPRECATED(7) From2Axes(
253 const Vector3<T> &_v1, const Vector3<T> &_v2)
254 {
255 this->SetFrom2Axes(_v1, _v2);
256 }
257
264 public: void SetFrom2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
265 {
266 const T _v1LengthSquared = _v1.SquaredLength();
267 if (_v1LengthSquared <= 0.0)
268 {
269 // zero vector - we can't handle this
270 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
271 return;
272 }
273
274 const T _v2LengthSquared = _v2.SquaredLength();
275 if (_v2LengthSquared <= 0.0)
276 {
277 // zero vector - we can't handle this
278 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
279 return;
280 }
281
282 const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
283 if (fabs(dot - 1.0) <= 1e-6)
284 {
285 // the vectors are parallel
286 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
287 return;
288 }
289 else if (fabs(dot + 1.0) <= 1e-6)
290 {
291 // the vectors are opposite
292 this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
293 return;
294 }
295
296 const Vector3<T> cross = _v1.Cross(_v2).Normalize();
297
298 this->SetFromAxisAngle(cross, acos(dot));
299 }
300
306 public: void GZ_DEPRECATED(7) Col(unsigned int _c, const Vector3<T> &_v)
307 {
308 this->SetCol(_c, _v);
309 }
310
315 public: void SetCol(unsigned int _c, const Vector3<T> &_v)
316 {
317 unsigned int c = clamp(_c, 0u, 2u);
318
319 this->data[0][c] = _v.X();
320 this->data[1][c] = _v.Y();
321 this->data[2][c] = _v.Z();
322 }
323
327 public: Matrix3<T> &operator=(const Matrix3<T> &_mat) = default;
328
332 public: Matrix3<T> operator-(const Matrix3<T> &_m) const
333 {
334 return Matrix3<T>(
335 this->data[0][0] - _m(0, 0),
336 this->data[0][1] - _m(0, 1),
337 this->data[0][2] - _m(0, 2),
338 this->data[1][0] - _m(1, 0),
339 this->data[1][1] - _m(1, 1),
340 this->data[1][2] - _m(1, 2),
341 this->data[2][0] - _m(2, 0),
342 this->data[2][1] - _m(2, 1),
343 this->data[2][2] - _m(2, 2));
344 }
345
349 public: Matrix3<T> operator+(const Matrix3<T> &_m) const
350 {
351 return Matrix3<T>(
352 this->data[0][0]+_m(0, 0),
353 this->data[0][1]+_m(0, 1),
354 this->data[0][2]+_m(0, 2),
355 this->data[1][0]+_m(1, 0),
356 this->data[1][1]+_m(1, 1),
357 this->data[1][2]+_m(1, 2),
358 this->data[2][0]+_m(2, 0),
359 this->data[2][1]+_m(2, 1),
360 this->data[2][2]+_m(2, 2));
361 }
362
366 public: Matrix3<T> operator*(const T &_s) const
367 {
368 return Matrix3<T>(
369 _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
370 _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
371 _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
372 }
373
377 public: Matrix3<T> operator*(const Matrix3<T> &_m) const
378 {
379 return Matrix3<T>(
380 // first row
381 this->data[0][0]*_m(0, 0)+
382 this->data[0][1]*_m(1, 0)+
383 this->data[0][2]*_m(2, 0),
384
385 this->data[0][0]*_m(0, 1)+
386 this->data[0][1]*_m(1, 1)+
387 this->data[0][2]*_m(2, 1),
388
389 this->data[0][0]*_m(0, 2)+
390 this->data[0][1]*_m(1, 2)+
391 this->data[0][2]*_m(2, 2),
392
393 // second row
394 this->data[1][0]*_m(0, 0)+
395 this->data[1][1]*_m(1, 0)+
396 this->data[1][2]*_m(2, 0),
397
398 this->data[1][0]*_m(0, 1)+
399 this->data[1][1]*_m(1, 1)+
400 this->data[1][2]*_m(2, 1),
401
402 this->data[1][0]*_m(0, 2)+
403 this->data[1][1]*_m(1, 2)+
404 this->data[1][2]*_m(2, 2),
405
406 // third row
407 this->data[2][0]*_m(0, 0)+
408 this->data[2][1]*_m(1, 0)+
409 this->data[2][2]*_m(2, 0),
410
411 this->data[2][0]*_m(0, 1)+
412 this->data[2][1]*_m(1, 1)+
413 this->data[2][2]*_m(2, 1),
414
415 this->data[2][0]*_m(0, 2)+
416 this->data[2][1]*_m(1, 2)+
417 this->data[2][2]*_m(2, 2));
418 }
419
424 public: Vector3<T> operator*(const Vector3<T> &_vec) const
425 {
426 return Vector3<T>(
427 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
428 this->data[0][2]*_vec.Z(),
429 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
430 this->data[1][2]*_vec.Z(),
431 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
432 this->data[2][2]*_vec.Z());
433 }
434
439 public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
440 {
441 return _m * _s;
442 }
443
450 public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
451 const Matrix3<T> &_m)
452 {
453 return Vector3<T>(
454 _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
455 _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
456 _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
457 }
458
464 public: bool Equal(const Matrix3 &_m, const T &_tol) const
465 {
466 return equal<T>(this->data[0][0], _m(0, 0), _tol)
467 && equal<T>(this->data[0][1], _m(0, 1), _tol)
468 && equal<T>(this->data[0][2], _m(0, 2), _tol)
469 && equal<T>(this->data[1][0], _m(1, 0), _tol)
470 && equal<T>(this->data[1][1], _m(1, 1), _tol)
471 && equal<T>(this->data[1][2], _m(1, 2), _tol)
472 && equal<T>(this->data[2][0], _m(2, 0), _tol)
473 && equal<T>(this->data[2][1], _m(2, 1), _tol)
474 && equal<T>(this->data[2][2], _m(2, 2), _tol);
475 }
476
480 public: bool operator==(const Matrix3<T> &_m) const
481 {
482 return this->Equal(_m, static_cast<T>(1e-6));
483 }
484
489 {
490 return *this = Matrix3<T>(_q);
491 }
492
496 public: bool operator!=(const Matrix3<T> &_m) const
497 {
498 return !(*this == _m);
499 }
500
505 public: inline T operator()(size_t _row, size_t _col) const
506 {
507 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
508 [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
509 }
510
515 public: inline T &operator()(size_t _row, size_t _col)
516 {
517 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
518 [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
519 }
520
523 public: T Determinant() const
524 {
525 T t0 = this->data[2][2]*this->data[1][1]
526 - this->data[2][1]*this->data[1][2];
527
528 T t1 = -(this->data[2][2]*this->data[1][0]
529 -this->data[2][0]*this->data[1][2]);
530
531 T t2 = this->data[2][1]*this->data[1][0]
532 - this->data[2][0]*this->data[1][1];
533
534 return t0 * this->data[0][0]
535 + t1 * this->data[0][1]
536 + t2 * this->data[0][2];
537 }
538
541 public: Matrix3<T> Inverse() const
542 {
543 T t0 = this->data[2][2]*this->data[1][1] -
544 this->data[2][1]*this->data[1][2];
545
546 T t1 = -(this->data[2][2]*this->data[1][0] -
547 this->data[2][0]*this->data[1][2]);
548
549 T t2 = this->data[2][1]*this->data[1][0] -
550 this->data[2][0]*this->data[1][1];
551
552 T invDet = 1.0 / (t0 * this->data[0][0] +
553 t1 * this->data[0][1] +
554 t2 * this->data[0][2]);
555
556 return invDet * Matrix3<T>(
557 t0,
558 - (this->data[2][2] * this->data[0][1] -
559 this->data[2][1] * this->data[0][2]),
560 + (this->data[1][2] * this->data[0][1] -
561 this->data[1][1] * this->data[0][2]),
562 t1,
563 + (this->data[2][2] * this->data[0][0] -
564 this->data[2][0] * this->data[0][2]),
565 - (this->data[1][2] * this->data[0][0] -
566 this->data[1][0] * this->data[0][2]),
567 t2,
568 - (this->data[2][1] * this->data[0][0] -
569 this->data[2][0] * this->data[0][1]),
570 + (this->data[1][1] * this->data[0][0] -
571 this->data[1][0] * this->data[0][1]));
572 }
573
575 public: void Transpose()
576 {
577 std::swap(this->data[0][1], this->data[1][0]);
578 std::swap(this->data[0][2], this->data[2][0]);
579 std::swap(this->data[1][2], this->data[2][1]);
580 }
581
584 public: Matrix3<T> Transposed() const
585 {
586 return Matrix3<T>(
587 this->data[0][0], this->data[1][0], this->data[2][0],
588 this->data[0][1], this->data[1][1], this->data[2][1],
589 this->data[0][2], this->data[1][2], this->data[2][2]);
590 }
591
597 public: friend std::ostream &operator<<(
598 std::ostream &_out, const gz::math::Matrix3<T> &_m)
599 {
600 for (auto i : {0, 1, 2})
601 {
602 for (auto j : {0, 1, 2})
603 {
604 if (!(i == 0 && j == 0))
605 _out << " ";
606
607 appendToStream(_out, _m(i, j));
608 }
609 }
610
611 return _out;
612 }
613
619 public: friend std::istream &operator>>(
621 {
622 // Skip white spaces
623 _in.setf(std::ios_base::skipws);
624 T d[9];
625 _in >> d[0] >> d[1] >> d[2]
626 >> d[3] >> d[4] >> d[5]
627 >> d[6] >> d[7] >> d[8];
628
629 if (!_in.fail())
630 {
631 _m.Set(d[0], d[1], d[2],
632 d[3], d[4], d[5],
633 d[6], d[7], d[8]);
634 }
635 return _in;
636 }
637
639 private: T data[3][3];
640 };
641
642 namespace detail {
643
644 template<typename T>
645 constexpr Matrix3<T> gMatrix3Identity(
646 1, 0, 0,
647 0, 1, 0,
648 0, 0, 1);
649
650 template<typename T>
651 constexpr Matrix3<T> gMatrix3Zero(
652 0, 0, 0,
653 0, 0, 0,
654 0, 0, 0);
655
656 } // namespace detail
657
658 template<typename T>
659 const Matrix3<T> &Matrix3<T>::Identity = detail::gMatrix3Identity<T>;
660
661 template<typename T>
662 const Matrix3<T> &Matrix3<T>::Zero = detail::gMatrix3Zero<T>;
663
666
669
672 }
673 }
674}
675#endif