Gazebo Math

API Reference

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