Gazebo Math

API Reference

7.4.0
gz/math/Matrix6.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 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_MATRIX6_HH_
18#define GZ_MATH_MATRIX6_HH_
19
20#include <utility>
21#include <gz/math/config.hh>
22#include <gz/math/Helpers.hh>
23#include <gz/math/Matrix3.hh>
24
25namespace gz
26{
27 namespace math
28 {
29 // Inline bracket to help doxygen filtering.
30 inline namespace GZ_MATH_VERSION_NAMESPACE {
31 //
34 template<typename T>
35 class Matrix6
36 {
38 public: enum Matrix6Corner
39 {
42 TOP_LEFT = 0,
43
46 TOP_RIGHT = 1,
47
50 BOTTOM_LEFT = 2,
51
54 BOTTOM_RIGHT = 3
55 };
56
58 public: static constexpr std::size_t MatrixSize{6};
59
61 public: static const Matrix6<T> &Identity;
62
64 public: static const Matrix6<T> &Zero;
65
67 public: Matrix6()
68 {
69 memset(this->data, 0, sizeof(this->data[0][0])*MatrixSize*MatrixSize);
70 }
71
74 public: Matrix6(const Matrix6<T> &_m) = default;
75
113 public: constexpr Matrix6(T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
114 T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
115 T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
116 T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
117 T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
118 T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
119 : data{{_v00, _v01, _v02, _v03, _v04, _v05},
120 {_v10, _v11, _v12, _v13, _v14, _v15},
121 {_v20, _v21, _v22, _v23, _v24, _v25},
122 {_v30, _v31, _v32, _v33, _v34, _v35},
123 {_v40, _v41, _v42, _v43, _v44, _v45},
124 {_v50, _v51, _v52, _v53, _v54, _v55}}
125 {
126 }
127
133 public: bool SetValue(size_t _row, size_t _col, T _v)
134 {
135 if (_row < MatrixSize && _col < MatrixSize)
136 {
137 this->data[_row][_col] = _v;
138 return true;
139 }
140 return false;
141 }
142
180 public: void Set(
181 T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
182 T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
183 T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
184 T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
185 T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
186 T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
187 {
188 this->data[0][0] = _v00;
189 this->data[0][1] = _v01;
190 this->data[0][2] = _v02;
191 this->data[0][3] = _v03;
192 this->data[0][4] = _v04;
193 this->data[0][5] = _v05;
194
195 this->data[1][0] = _v10;
196 this->data[1][1] = _v11;
197 this->data[1][2] = _v12;
198 this->data[1][3] = _v13;
199 this->data[1][4] = _v14;
200 this->data[1][5] = _v15;
201
202 this->data[2][0] = _v20;
203 this->data[2][1] = _v21;
204 this->data[2][2] = _v22;
205 this->data[2][3] = _v23;
206 this->data[2][4] = _v24;
207 this->data[2][5] = _v25;
208
209 this->data[3][0] = _v30;
210 this->data[3][1] = _v31;
211 this->data[3][2] = _v32;
212 this->data[3][3] = _v33;
213 this->data[3][4] = _v34;
214 this->data[3][5] = _v35;
215
216 this->data[4][0] = _v40;
217 this->data[4][1] = _v41;
218 this->data[4][2] = _v42;
219 this->data[4][3] = _v43;
220 this->data[4][4] = _v44;
221 this->data[4][5] = _v45;
222
223 this->data[5][0] = _v50;
224 this->data[5][1] = _v51;
225 this->data[5][2] = _v52;
226 this->data[5][3] = _v53;
227 this->data[5][4] = _v54;
228 this->data[5][5] = _v55;
229 }
230
232 public: void Transpose()
233 {
234 std::swap(this->data[0][1], this->data[1][0]);
235 std::swap(this->data[0][2], this->data[2][0]);
236 std::swap(this->data[0][3], this->data[3][0]);
237 std::swap(this->data[0][4], this->data[4][0]);
238 std::swap(this->data[0][5], this->data[5][0]);
239 std::swap(this->data[1][2], this->data[2][1]);
240 std::swap(this->data[1][3], this->data[3][1]);
241 std::swap(this->data[1][4], this->data[4][1]);
242 std::swap(this->data[1][5], this->data[5][1]);
243 std::swap(this->data[2][3], this->data[3][2]);
244 std::swap(this->data[2][4], this->data[4][2]);
245 std::swap(this->data[2][5], this->data[5][2]);
246 std::swap(this->data[3][4], this->data[4][3]);
247 std::swap(this->data[3][5], this->data[5][3]);
248 std::swap(this->data[4][5], this->data[5][4]);
249 }
250
253 public: Matrix6<T> Transposed() const
254 {
255 return Matrix6<T>(
256 this->data[0][0],
257 this->data[1][0],
258 this->data[2][0],
259 this->data[3][0],
260 this->data[4][0],
261 this->data[5][0],
262
263 this->data[0][1],
264 this->data[1][1],
265 this->data[2][1],
266 this->data[3][1],
267 this->data[4][1],
268 this->data[5][1],
269
270 this->data[0][2],
271 this->data[1][2],
272 this->data[2][2],
273 this->data[3][2],
274 this->data[4][2],
275 this->data[5][2],
276
277 this->data[0][3],
278 this->data[1][3],
279 this->data[2][3],
280 this->data[3][3],
281 this->data[4][3],
282 this->data[5][3],
283
284 this->data[0][4],
285 this->data[1][4],
286 this->data[2][4],
287 this->data[3][4],
288 this->data[4][4],
289 this->data[5][4],
290
291 this->data[0][5],
292 this->data[1][5],
293 this->data[2][5],
294 this->data[3][5],
295 this->data[4][5],
296 this->data[5][5]);
297 }
298
302 public: Matrix6<T> &operator=(const Matrix6<T> &_mat) = default;
303
308 public: Matrix6<T> operator*=(const Matrix6<T> &_m2)
309 {
310 (*this) = (*this) * _m2;
311 return *this;
312 }
313
317 public: Matrix6<T> operator*(const Matrix6<T> &_m2) const
318 {
319 auto el = [&](size_t _row, size_t _col) -> T
320 {
321 T result = static_cast<T>(0);
322 for (size_t i = 0; i < MatrixSize; ++i)
323 result += this->data[_row][i] * _m2(i, _col);
324 return result;
325 };
326 return Matrix6<T>(
327 el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
328 el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
329 el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
330 el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
331 el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
332 el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
333 }
334
339 public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
340 {
341 (*this) = (*this) + _m2;
342 return *this;
343 }
344
348 public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
349 {
350 auto el = [&](size_t _row, size_t _col) -> T
351 {
352 return this->data[_row][_col] + _m2(_row, _col);
353 };
354 return Matrix6<T>(
355 el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
356 el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
357 el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
358 el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
359 el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
360 el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
361 }
362
369 public: inline const T &operator()(const size_t _row,
370 const size_t _col) const
371 {
372 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)][
373 clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
374 }
375
383 public: inline T &operator()(const size_t _row, const size_t _col)
384 {
385 return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)]
386 [clamp(_col, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)];
387 }
388
394 public: Matrix3<T> Submatrix(Matrix6Corner _corner) const
395 {
396 size_t row = 0;
397 size_t col = 0;
398 if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
399 {
400 row = 3;
401 }
402 if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
403 {
404 col = 3;
405 }
406 return {this->data[row + 0][col + 0],
407 this->data[row + 0][col + 1],
408 this->data[row + 0][col + 2],
409 this->data[row + 1][col + 0],
410 this->data[row + 1][col + 1],
411 this->data[row + 1][col + 2],
412 this->data[row + 2][col + 0],
413 this->data[row + 2][col + 1],
414 this->data[row + 2][col + 2]};
415 }
416
422 public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3<T> &_mat)
423 {
424 size_t row = 0;
425 size_t col = 0;
426 if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
427 {
428 row = 3;
429 }
430 if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
431 {
432 col = 3;
433 }
434 for (size_t r = 0; r < 3; ++r)
435 {
436 for (size_t c = 0; c < 3; ++c)
437 {
438 this->data[row + r][col + c] = _mat(r, c);
439 }
440 }
441 }
442
448 public: bool Equal(const Matrix6 &_m, const T &_tol) const
449 {
450 return equal<T>(this->data[0][0], _m(0, 0), _tol)
451 && equal<T>(this->data[0][1], _m(0, 1), _tol)
452 && equal<T>(this->data[0][2], _m(0, 2), _tol)
453 && equal<T>(this->data[0][3], _m(0, 3), _tol)
454 && equal<T>(this->data[0][4], _m(0, 4), _tol)
455 && equal<T>(this->data[0][5], _m(0, 5), _tol)
456 && equal<T>(this->data[1][0], _m(1, 0), _tol)
457 && equal<T>(this->data[1][1], _m(1, 1), _tol)
458 && equal<T>(this->data[1][2], _m(1, 2), _tol)
459 && equal<T>(this->data[1][3], _m(1, 3), _tol)
460 && equal<T>(this->data[1][4], _m(1, 4), _tol)
461 && equal<T>(this->data[1][5], _m(1, 5), _tol)
462 && equal<T>(this->data[2][0], _m(2, 0), _tol)
463 && equal<T>(this->data[2][1], _m(2, 1), _tol)
464 && equal<T>(this->data[2][2], _m(2, 2), _tol)
465 && equal<T>(this->data[2][3], _m(2, 3), _tol)
466 && equal<T>(this->data[2][4], _m(2, 4), _tol)
467 && equal<T>(this->data[2][5], _m(2, 5), _tol)
468 && equal<T>(this->data[3][0], _m(3, 0), _tol)
469 && equal<T>(this->data[3][1], _m(3, 1), _tol)
470 && equal<T>(this->data[3][2], _m(3, 2), _tol)
471 && equal<T>(this->data[3][3], _m(3, 3), _tol)
472 && equal<T>(this->data[3][4], _m(3, 4), _tol)
473 && equal<T>(this->data[3][5], _m(3, 5), _tol)
474 && equal<T>(this->data[4][0], _m(4, 0), _tol)
475 && equal<T>(this->data[4][1], _m(4, 1), _tol)
476 && equal<T>(this->data[4][2], _m(4, 2), _tol)
477 && equal<T>(this->data[4][3], _m(4, 3), _tol)
478 && equal<T>(this->data[4][4], _m(4, 4), _tol)
479 && equal<T>(this->data[4][5], _m(4, 5), _tol)
480 && equal<T>(this->data[5][0], _m(5, 0), _tol)
481 && equal<T>(this->data[5][1], _m(5, 1), _tol)
482 && equal<T>(this->data[5][2], _m(5, 2), _tol)
483 && equal<T>(this->data[5][3], _m(5, 3), _tol)
484 && equal<T>(this->data[5][4], _m(5, 4), _tol)
485 && equal<T>(this->data[5][5], _m(5, 5), _tol);
486 }
487
492 public: bool operator==(const Matrix6<T> &_m) const
493 {
494 return this->Equal(_m, static_cast<T>(1e-6));
495 }
496
500 public: bool operator!=(const Matrix6<T> &_m) const
501 {
502 return !(*this == _m);
503 }
504
509 public: friend std::ostream &operator<<(
510 std::ostream &_out, const gz::math::Matrix6<T> &_m)
511 {
512 for (auto i : {0, 1, 2, 3, 4, 5})
513 {
514 for (auto j : {0, 1, 2, 3, 4, 5})
515 {
516 if (!(i == 0 && j == 0))
517 _out << " ";
518
519 appendToStream(_out, _m(i, j));
520 }
521 }
522
523 return _out;
524 }
525
530 public: friend std::istream &operator>>(
532 {
533 // Skip white spaces
534 _in.setf(std::ios_base::skipws);
535 T d[36];
536 _in >> d[0] >> d[1] >> d[2] >> d[3] >> d[4] >> d[5]
537 >> d[6] >> d[7] >> d[8] >> d[9] >> d[10] >> d[11]
538 >> d[12] >> d[13] >> d[14] >> d[15] >> d[16] >> d[17]
539 >> d[18] >> d[19] >> d[20] >> d[21] >> d[22] >> d[23]
540 >> d[24] >> d[25] >> d[26] >> d[27] >> d[28] >> d[29]
541 >> d[30] >> d[31] >> d[32] >> d[33] >> d[34] >> d[35];
542
543 if (!_in.fail())
544 {
545 _m.Set(d[0], d[1], d[2], d[3], d[4], d[5],
546 d[6], d[7], d[8], d[9], d[10], d[11],
547 d[12], d[13], d[14], d[15], d[16], d[17],
548 d[18], d[19], d[20], d[21], d[22], d[23],
549 d[24], d[25], d[26], d[27], d[28], d[29],
550 d[30], d[31], d[32], d[33], d[34], d[35]);
551 }
552 return _in;
553 }
554
556 private: T data[MatrixSize][MatrixSize];
557 };
558
559 namespace detail {
560
561 template<typename T>
562 constexpr Matrix6<T> gMatrix6Identity(
563 1, 0, 0, 0, 0, 0,
564 0, 1, 0, 0, 0, 0,
565 0, 0, 1, 0, 0, 0,
566 0, 0, 0, 1, 0, 0,
567 0, 0, 0, 0, 1, 0,
568 0, 0, 0, 0, 0, 1);
569
570 template<typename T>
571 constexpr Matrix6<T> gMatrix6Zero(
572 0, 0, 0, 0, 0, 0,
573 0, 0, 0, 0, 0, 0,
574 0, 0, 0, 0, 0, 0,
575 0, 0, 0, 0, 0, 0,
576 0, 0, 0, 0, 0, 0,
577 0, 0, 0, 0, 0, 0);
578
579 } // namespace detail
580
581 template<typename T>
582 const Matrix6<T> &Matrix6<T>::Identity = detail::gMatrix6Identity<T>;
583
584 template<typename T>
585 const Matrix6<T> &Matrix6<T>::Zero = detail::gMatrix6Zero<T>;
586
590 }
591 }
592}
593#endif