Gazebo Math

API Reference

8.1.0
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/config.hh>
25#include <gz/math/Vector3.hh>
26
27namespace gz::math
28{
29 // Inline bracket to help doxygen filtering.
30 inline namespace GZ_MATH_VERSION_NAMESPACE {
31 //
32 template <typename T> class Quaternion;
33
90 template<typename T>
91 class Matrix3
92 {
95 public: static const Matrix3<T> &Identity;
96
99 public: static const Matrix3<T> &Zero;
100
102 public: Matrix3()
103 {
104 std::memset(this->data, 0, sizeof(this->data[0][0])*9);
105 }
106
117 public: constexpr Matrix3(T _v00, T _v01, T _v02,
118 T _v10, T _v11, T _v12,
119 T _v20, T _v21, T _v22)
120 : data{{_v00, _v01, _v02},
121 {_v10, _v11, _v12},
122 {_v20, _v21, _v22}}
123 {
124 }
125
128 public: explicit Matrix3(const Quaternion<T> &_q)
129 {
130 Quaternion<T> qt = _q;
131 qt.Normalize();
132 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
133 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
134 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
135 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
136 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
137 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
138 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
139 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
140 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
141 }
142
147 public: void Set(size_t _row, size_t _col, T _v)
148 {
149 this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
150 [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)] = _v;
151 }
152
163 public: void Set(T _v00, T _v01, T _v02,
164 T _v10, T _v11, T _v12,
165 T _v20, T _v21, T _v22)
166 {
167 this->data[0][0] = _v00;
168 this->data[0][1] = _v01;
169 this->data[0][2] = _v02;
170 this->data[1][0] = _v10;
171 this->data[1][1] = _v11;
172 this->data[1][2] = _v12;
173 this->data[2][0] = _v20;
174 this->data[2][1] = _v21;
175 this->data[2][2] = _v22;
176 }
177
182 public: void SetAxes(const Vector3<T> &_xAxis,
183 const Vector3<T> &_yAxis,
184 const Vector3<T> &_zAxis)
185 {
186 this->SetCol(0, _xAxis);
187 this->SetCol(1, _yAxis);
188 this->SetCol(2, _zAxis);
189 }
190
194 public: void SetFromAxisAngle(const Vector3<T> &_axis, T _angle)
195 {
196 T c = cos(_angle);
197 T s = sin(_angle);
198 T C = 1-c;
199
200 this->data[0][0] = _axis.X()*_axis.X()*C + c;
201 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
202 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
203
204 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
205 this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
206 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
207
208 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
209 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
210 this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
211 }
212
219 public: void SetFrom2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
220 {
221 const T _v1LengthSquared = _v1.SquaredLength();
222 if (_v1LengthSquared <= 0.0)
223 {
224 // zero vector - we can't handle this
225 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
226 return;
227 }
228
229 const T _v2LengthSquared = _v2.SquaredLength();
230 if (_v2LengthSquared <= 0.0)
231 {
232 // zero vector - we can't handle this
233 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
234 return;
235 }
236
237 const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
238 if (fabs(dot - 1.0) <= 1e-6)
239 {
240 // the vectors are parallel
241 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
242 return;
243 }
244 else if (fabs(dot + 1.0) <= 1e-6)
245 {
246 // the vectors are opposite
247 this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
248 return;
249 }
250
251 const Vector3<T> cross = _v1.Cross(_v2).Normalize();
252
253 this->SetFromAxisAngle(cross, acos(dot));
254 }
255
260 public: void SetCol(unsigned int _c, const Vector3<T> &_v)
261 {
262 unsigned int c = clamp(_c, 0u, 2u);
263
264 this->data[0][c] = _v.X();
265 this->data[1][c] = _v.Y();
266 this->data[2][c] = _v.Z();
267 }
268
272 public: Matrix3<T> operator-(const Matrix3<T> &_m) const
273 {
274 return Matrix3<T>(
275 this->data[0][0] - _m(0, 0),
276 this->data[0][1] - _m(0, 1),
277 this->data[0][2] - _m(0, 2),
278 this->data[1][0] - _m(1, 0),
279 this->data[1][1] - _m(1, 1),
280 this->data[1][2] - _m(1, 2),
281 this->data[2][0] - _m(2, 0),
282 this->data[2][1] - _m(2, 1),
283 this->data[2][2] - _m(2, 2));
284 }
285
289 public: Matrix3<T> operator+(const Matrix3<T> &_m) const
290 {
291 return Matrix3<T>(
292 this->data[0][0]+_m(0, 0),
293 this->data[0][1]+_m(0, 1),
294 this->data[0][2]+_m(0, 2),
295 this->data[1][0]+_m(1, 0),
296 this->data[1][1]+_m(1, 1),
297 this->data[1][2]+_m(1, 2),
298 this->data[2][0]+_m(2, 0),
299 this->data[2][1]+_m(2, 1),
300 this->data[2][2]+_m(2, 2));
301 }
302
306 public: Matrix3<T> operator*(const T &_s) const
307 {
308 return Matrix3<T>(
309 _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
310 _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
311 _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
312 }
313
317 public: Matrix3<T> operator*(const Matrix3<T> &_m) const
318 {
319 return Matrix3<T>(
320 // first row
321 this->data[0][0]*_m(0, 0)+
322 this->data[0][1]*_m(1, 0)+
323 this->data[0][2]*_m(2, 0),
324
325 this->data[0][0]*_m(0, 1)+
326 this->data[0][1]*_m(1, 1)+
327 this->data[0][2]*_m(2, 1),
328
329 this->data[0][0]*_m(0, 2)+
330 this->data[0][1]*_m(1, 2)+
331 this->data[0][2]*_m(2, 2),
332
333 // second row
334 this->data[1][0]*_m(0, 0)+
335 this->data[1][1]*_m(1, 0)+
336 this->data[1][2]*_m(2, 0),
337
338 this->data[1][0]*_m(0, 1)+
339 this->data[1][1]*_m(1, 1)+
340 this->data[1][2]*_m(2, 1),
341
342 this->data[1][0]*_m(0, 2)+
343 this->data[1][1]*_m(1, 2)+
344 this->data[1][2]*_m(2, 2),
345
346 // third row
347 this->data[2][0]*_m(0, 0)+
348 this->data[2][1]*_m(1, 0)+
349 this->data[2][2]*_m(2, 0),
350
351 this->data[2][0]*_m(0, 1)+
352 this->data[2][1]*_m(1, 1)+
353 this->data[2][2]*_m(2, 1),
354
355 this->data[2][0]*_m(0, 2)+
356 this->data[2][1]*_m(1, 2)+
357 this->data[2][2]*_m(2, 2));
358 }
359
364 public: Vector3<T> operator*(const Vector3<T> &_vec) const
365 {
366 return Vector3<T>(
367 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
368 this->data[0][2]*_vec.Z(),
369 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
370 this->data[1][2]*_vec.Z(),
371 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
372 this->data[2][2]*_vec.Z());
373 }
374
379 public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
380 {
381 return _m * _s;
382 }
383
390 public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
391 const Matrix3<T> &_m)
392 {
393 return Vector3<T>(
394 _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
395 _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
396 _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
397 }
398
404 public: bool Equal(const Matrix3 &_m, const T &_tol) const
405 {
406 return equal<T>(this->data[0][0], _m(0, 0), _tol)
407 && equal<T>(this->data[0][1], _m(0, 1), _tol)
408 && equal<T>(this->data[0][2], _m(0, 2), _tol)
409 && equal<T>(this->data[1][0], _m(1, 0), _tol)
410 && equal<T>(this->data[1][1], _m(1, 1), _tol)
411 && equal<T>(this->data[1][2], _m(1, 2), _tol)
412 && equal<T>(this->data[2][0], _m(2, 0), _tol)
413 && equal<T>(this->data[2][1], _m(2, 1), _tol)
414 && equal<T>(this->data[2][2], _m(2, 2), _tol);
415 }
416
420 public: bool operator==(const Matrix3<T> &_m) const
421 {
422 return this->Equal(_m, static_cast<T>(1e-6));
423 }
424
428 public: Matrix3<T> &operator=(const Quaternion<T> &_q)
429 {
430 return *this = Matrix3<T>(_q);
431 }
432
436 public: bool operator!=(const Matrix3<T> &_m) const
437 {
438 return !(*this == _m);
439 }
440
445 public: inline T operator()(size_t _row, size_t _col) const
446 {
447 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
448 [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
449 }
450
455 public: inline T &operator()(size_t _row, size_t _col)
456 {
457 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)]
458 [clamp(_col, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
459 }
460
463 public: T Determinant() const
464 {
465 T t0 = this->data[2][2]*this->data[1][1]
466 - this->data[2][1]*this->data[1][2];
467
468 T t1 = -(this->data[2][2]*this->data[1][0]
469 -this->data[2][0]*this->data[1][2]);
470
471 T t2 = this->data[2][1]*this->data[1][0]
472 - this->data[2][0]*this->data[1][1];
473
474 return t0 * this->data[0][0]
475 + t1 * this->data[0][1]
476 + t2 * this->data[0][2];
477 }
478
481 public: Matrix3<T> Inverse() const
482 {
483 T t0 = this->data[2][2]*this->data[1][1] -
484 this->data[2][1]*this->data[1][2];
485
486 T t1 = -(this->data[2][2]*this->data[1][0] -
487 this->data[2][0]*this->data[1][2]);
488
489 T t2 = this->data[2][1]*this->data[1][0] -
490 this->data[2][0]*this->data[1][1];
491
492 T invDet = 1.0 / (t0 * this->data[0][0] +
493 t1 * this->data[0][1] +
494 t2 * this->data[0][2]);
495
496 return invDet * Matrix3<T>(
497 t0,
498 - (this->data[2][2] * this->data[0][1] -
499 this->data[2][1] * this->data[0][2]),
500 + (this->data[1][2] * this->data[0][1] -
501 this->data[1][1] * this->data[0][2]),
502 t1,
503 + (this->data[2][2] * this->data[0][0] -
504 this->data[2][0] * this->data[0][2]),
505 - (this->data[1][2] * this->data[0][0] -
506 this->data[1][0] * this->data[0][2]),
507 t2,
508 - (this->data[2][1] * this->data[0][0] -
509 this->data[2][0] * this->data[0][1]),
510 + (this->data[1][1] * this->data[0][0] -
511 this->data[1][0] * this->data[0][1]));
512 }
513
515 public: void Transpose()
516 {
517 std::swap(this->data[0][1], this->data[1][0]);
518 std::swap(this->data[0][2], this->data[2][0]);
519 std::swap(this->data[1][2], this->data[2][1]);
520 }
521
524 public: Matrix3<T> Transposed() const
525 {
526 return Matrix3<T>(
527 this->data[0][0], this->data[1][0], this->data[2][0],
528 this->data[0][1], this->data[1][1], this->data[2][1],
529 this->data[0][2], this->data[1][2], this->data[2][2]);
530 }
531
537 public: friend std::ostream &operator<<(
538 std::ostream &_out, const gz::math::Matrix3<T> &_m)
539 {
540 for (auto i : {0, 1, 2})
541 {
542 for (auto j : {0, 1, 2})
543 {
544 if (!(i == 0 && j == 0))
545 _out << " ";
546
547 appendToStream(_out, _m(i, j));
548 }
549 }
550
551 return _out;
552 }
553
559 public: friend std::istream &operator>>(
560 std::istream &_in, gz::math::Matrix3<T> &_m)
561 {
562 // Skip white spaces
563 _in.setf(std::ios_base::skipws);
564 T d[9];
565 _in >> d[0] >> d[1] >> d[2]
566 >> d[3] >> d[4] >> d[5]
567 >> d[6] >> d[7] >> d[8];
568
569 if (!_in.fail())
570 {
571 _m.Set(d[0], d[1], d[2],
572 d[3], d[4], d[5],
573 d[6], d[7], d[8]);
574 }
575 return _in;
576 }
577
579 private: T data[3][3];
580 };
581
582 namespace detail {
583
584 template<typename T>
585 constexpr Matrix3<T> gMatrix3Identity(
586 1, 0, 0,
587 0, 1, 0,
588 0, 0, 1);
589
590 template<typename T>
591 constexpr Matrix3<T> gMatrix3Zero(
592 0, 0, 0,
593 0, 0, 0,
594 0, 0, 0);
595
596 } // namespace detail
597
598 template<typename T>
599 const Matrix3<T> &Matrix3<T>::Identity = detail::gMatrix3Identity<T>;
600
601 template<typename T>
602 const Matrix3<T> &Matrix3<T>::Zero = detail::gMatrix3Zero<T>;
603
605 typedef Matrix3<int> Matrix3i;
606
608 typedef Matrix3<double> Matrix3d;
609
611 typedef Matrix3<float> Matrix3f;
612 } // namespace GZ_MATH_VERSION_NAMESPACE
613} // namespace gz::math
614#endif // GZ_MATH_MATRIX3_HH_