Gazebo Math

API Reference

7.5.1
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 
25 namespace gz::math
26 {
27  // Inline bracket to help doxygen filtering.
28  inline namespace GZ_MATH_VERSION_NAMESPACE {
29  //
32  template<typename T>
33  class Matrix6
34  {
36  public: enum Matrix6Corner
37  {
40  TOP_LEFT = 0,
41 
44  TOP_RIGHT = 1,
45 
48  BOTTOM_LEFT = 2,
49 
52  BOTTOM_RIGHT = 3
53  };
54 
56  public: static constexpr std::size_t MatrixSize{6};
57 
59  public: static const Matrix6<T> &Identity;
60 
62  public: static const Matrix6<T> &Zero;
63 
65  public: Matrix6()
66  {
67  memset(this->data, 0, sizeof(this->data[0][0])*MatrixSize*MatrixSize);
68  }
69 
72  public: Matrix6(const Matrix6<T> &_m) = default;
73 
111  public: constexpr Matrix6(T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
112  T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
113  T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
114  T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
115  T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
116  T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
117  : data{{_v00, _v01, _v02, _v03, _v04, _v05},
118  {_v10, _v11, _v12, _v13, _v14, _v15},
119  {_v20, _v21, _v22, _v23, _v24, _v25},
120  {_v30, _v31, _v32, _v33, _v34, _v35},
121  {_v40, _v41, _v42, _v43, _v44, _v45},
122  {_v50, _v51, _v52, _v53, _v54, _v55}}
123  {
124  }
125 
131  public: bool SetValue(size_t _row, size_t _col, T _v)
132  {
133  if (_row < MatrixSize && _col < MatrixSize)
134  {
135  this->data[_row][_col] = _v;
136  return true;
137  }
138  return false;
139  }
140 
178  public: void Set(
179  T _v00, T _v01, T _v02, T _v03, T _v04, T _v05,
180  T _v10, T _v11, T _v12, T _v13, T _v14, T _v15,
181  T _v20, T _v21, T _v22, T _v23, T _v24, T _v25,
182  T _v30, T _v31, T _v32, T _v33, T _v34, T _v35,
183  T _v40, T _v41, T _v42, T _v43, T _v44, T _v45,
184  T _v50, T _v51, T _v52, T _v53, T _v54, T _v55)
185  {
186  this->data[0][0] = _v00;
187  this->data[0][1] = _v01;
188  this->data[0][2] = _v02;
189  this->data[0][3] = _v03;
190  this->data[0][4] = _v04;
191  this->data[0][5] = _v05;
192 
193  this->data[1][0] = _v10;
194  this->data[1][1] = _v11;
195  this->data[1][2] = _v12;
196  this->data[1][3] = _v13;
197  this->data[1][4] = _v14;
198  this->data[1][5] = _v15;
199 
200  this->data[2][0] = _v20;
201  this->data[2][1] = _v21;
202  this->data[2][2] = _v22;
203  this->data[2][3] = _v23;
204  this->data[2][4] = _v24;
205  this->data[2][5] = _v25;
206 
207  this->data[3][0] = _v30;
208  this->data[3][1] = _v31;
209  this->data[3][2] = _v32;
210  this->data[3][3] = _v33;
211  this->data[3][4] = _v34;
212  this->data[3][5] = _v35;
213 
214  this->data[4][0] = _v40;
215  this->data[4][1] = _v41;
216  this->data[4][2] = _v42;
217  this->data[4][3] = _v43;
218  this->data[4][4] = _v44;
219  this->data[4][5] = _v45;
220 
221  this->data[5][0] = _v50;
222  this->data[5][1] = _v51;
223  this->data[5][2] = _v52;
224  this->data[5][3] = _v53;
225  this->data[5][4] = _v54;
226  this->data[5][5] = _v55;
227  }
228 
230  public: void Transpose()
231  {
232  std::swap(this->data[0][1], this->data[1][0]);
233  std::swap(this->data[0][2], this->data[2][0]);
234  std::swap(this->data[0][3], this->data[3][0]);
235  std::swap(this->data[0][4], this->data[4][0]);
236  std::swap(this->data[0][5], this->data[5][0]);
237  std::swap(this->data[1][2], this->data[2][1]);
238  std::swap(this->data[1][3], this->data[3][1]);
239  std::swap(this->data[1][4], this->data[4][1]);
240  std::swap(this->data[1][5], this->data[5][1]);
241  std::swap(this->data[2][3], this->data[3][2]);
242  std::swap(this->data[2][4], this->data[4][2]);
243  std::swap(this->data[2][5], this->data[5][2]);
244  std::swap(this->data[3][4], this->data[4][3]);
245  std::swap(this->data[3][5], this->data[5][3]);
246  std::swap(this->data[4][5], this->data[5][4]);
247  }
248 
251  public: Matrix6<T> Transposed() const
252  {
253  return Matrix6<T>(
254  this->data[0][0],
255  this->data[1][0],
256  this->data[2][0],
257  this->data[3][0],
258  this->data[4][0],
259  this->data[5][0],
260 
261  this->data[0][1],
262  this->data[1][1],
263  this->data[2][1],
264  this->data[3][1],
265  this->data[4][1],
266  this->data[5][1],
267 
268  this->data[0][2],
269  this->data[1][2],
270  this->data[2][2],
271  this->data[3][2],
272  this->data[4][2],
273  this->data[5][2],
274 
275  this->data[0][3],
276  this->data[1][3],
277  this->data[2][3],
278  this->data[3][3],
279  this->data[4][3],
280  this->data[5][3],
281 
282  this->data[0][4],
283  this->data[1][4],
284  this->data[2][4],
285  this->data[3][4],
286  this->data[4][4],
287  this->data[5][4],
288 
289  this->data[0][5],
290  this->data[1][5],
291  this->data[2][5],
292  this->data[3][5],
293  this->data[4][5],
294  this->data[5][5]);
295  }
296 
300  public: Matrix6<T> &operator=(const Matrix6<T> &_mat) = default;
301 
306  public: Matrix6<T> operator*=(const Matrix6<T> &_m2)
307  {
308  (*this) = (*this) * _m2;
309  return *this;
310  }
311 
315  public: Matrix6<T> operator*(const Matrix6<T> &_m2) const
316  {
317  auto el = [&](size_t _row, size_t _col) -> T
318  {
319  T result = static_cast<T>(0);
320  for (size_t i = 0; i < MatrixSize; ++i)
321  result += this->data[_row][i] * _m2(i, _col);
322  return result;
323  };
324  return Matrix6<T>(
325  el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
326  el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
327  el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
328  el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
329  el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
330  el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
331  }
332 
337  public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
338  {
339  (*this) = (*this) + _m2;
340  return *this;
341  }
342 
346  public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
347  {
348  auto el = [&](size_t _row, size_t _col) -> T
349  {
350  return this->data[_row][_col] + _m2(_row, _col);
351  };
352  return Matrix6<T>(
353  el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
354  el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
355  el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
356  el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
357  el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
358  el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
359  }
360 
367  public: inline const T &operator()(const size_t _row,
368  const size_t _col) const
369  {
370  return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)][
372  }
373 
381  public: inline T &operator()(const size_t _row, const size_t _col)
382  {
383  return this->data[clamp(_row, GZ_ZERO_SIZE_T, GZ_FIVE_SIZE_T)]
385  }
386 
392  public: Matrix3<T> Submatrix(Matrix6Corner _corner) const
393  {
394  size_t row = 0;
395  size_t col = 0;
396  if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
397  {
398  row = 3;
399  }
400  if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
401  {
402  col = 3;
403  }
404  return {this->data[row + 0][col + 0],
405  this->data[row + 0][col + 1],
406  this->data[row + 0][col + 2],
407  this->data[row + 1][col + 0],
408  this->data[row + 1][col + 1],
409  this->data[row + 1][col + 2],
410  this->data[row + 2][col + 0],
411  this->data[row + 2][col + 1],
412  this->data[row + 2][col + 2]};
413  }
414 
420  public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3<T> &_mat)
421  {
422  size_t row = 0;
423  size_t col = 0;
424  if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
425  {
426  row = 3;
427  }
428  if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
429  {
430  col = 3;
431  }
432  for (size_t r = 0; r < 3; ++r)
433  {
434  for (size_t c = 0; c < 3; ++c)
435  {
436  this->data[row + r][col + c] = _mat(r, c);
437  }
438  }
439  }
440 
446  public: bool Equal(const Matrix6 &_m, const T &_tol) const
447  {
448  return equal<T>(this->data[0][0], _m(0, 0), _tol)
449  && equal<T>(this->data[0][1], _m(0, 1), _tol)
450  && equal<T>(this->data[0][2], _m(0, 2), _tol)
451  && equal<T>(this->data[0][3], _m(0, 3), _tol)
452  && equal<T>(this->data[0][4], _m(0, 4), _tol)
453  && equal<T>(this->data[0][5], _m(0, 5), _tol)
454  && equal<T>(this->data[1][0], _m(1, 0), _tol)
455  && equal<T>(this->data[1][1], _m(1, 1), _tol)
456  && equal<T>(this->data[1][2], _m(1, 2), _tol)
457  && equal<T>(this->data[1][3], _m(1, 3), _tol)
458  && equal<T>(this->data[1][4], _m(1, 4), _tol)
459  && equal<T>(this->data[1][5], _m(1, 5), _tol)
460  && equal<T>(this->data[2][0], _m(2, 0), _tol)
461  && equal<T>(this->data[2][1], _m(2, 1), _tol)
462  && equal<T>(this->data[2][2], _m(2, 2), _tol)
463  && equal<T>(this->data[2][3], _m(2, 3), _tol)
464  && equal<T>(this->data[2][4], _m(2, 4), _tol)
465  && equal<T>(this->data[2][5], _m(2, 5), _tol)
466  && equal<T>(this->data[3][0], _m(3, 0), _tol)
467  && equal<T>(this->data[3][1], _m(3, 1), _tol)
468  && equal<T>(this->data[3][2], _m(3, 2), _tol)
469  && equal<T>(this->data[3][3], _m(3, 3), _tol)
470  && equal<T>(this->data[3][4], _m(3, 4), _tol)
471  && equal<T>(this->data[3][5], _m(3, 5), _tol)
472  && equal<T>(this->data[4][0], _m(4, 0), _tol)
473  && equal<T>(this->data[4][1], _m(4, 1), _tol)
474  && equal<T>(this->data[4][2], _m(4, 2), _tol)
475  && equal<T>(this->data[4][3], _m(4, 3), _tol)
476  && equal<T>(this->data[4][4], _m(4, 4), _tol)
477  && equal<T>(this->data[4][5], _m(4, 5), _tol)
478  && equal<T>(this->data[5][0], _m(5, 0), _tol)
479  && equal<T>(this->data[5][1], _m(5, 1), _tol)
480  && equal<T>(this->data[5][2], _m(5, 2), _tol)
481  && equal<T>(this->data[5][3], _m(5, 3), _tol)
482  && equal<T>(this->data[5][4], _m(5, 4), _tol)
483  && equal<T>(this->data[5][5], _m(5, 5), _tol);
484  }
485 
490  public: bool operator==(const Matrix6<T> &_m) const
491  {
492  return this->Equal(_m, static_cast<T>(1e-6));
493  }
494 
498  public: bool operator!=(const Matrix6<T> &_m) const
499  {
500  return !(*this == _m);
501  }
502 
507  public: friend std::ostream &operator<<(
508  std::ostream &_out, const gz::math::Matrix6<T> &_m)
509  {
510  for (auto i : {0, 1, 2, 3, 4, 5})
511  {
512  for (auto j : {0, 1, 2, 3, 4, 5})
513  {
514  if (!(i == 0 && j == 0))
515  _out << " ";
516 
517  appendToStream(_out, _m(i, j));
518  }
519  }
520 
521  return _out;
522  }
523 
528  public: friend std::istream &operator>>(
530  {
531  // Skip white spaces
532  _in.setf(std::ios_base::skipws);
533  T d[36];
534  _in >> d[0] >> d[1] >> d[2] >> d[3] >> d[4] >> d[5]
535  >> d[6] >> d[7] >> d[8] >> d[9] >> d[10] >> d[11]
536  >> d[12] >> d[13] >> d[14] >> d[15] >> d[16] >> d[17]
537  >> d[18] >> d[19] >> d[20] >> d[21] >> d[22] >> d[23]
538  >> d[24] >> d[25] >> d[26] >> d[27] >> d[28] >> d[29]
539  >> d[30] >> d[31] >> d[32] >> d[33] >> d[34] >> d[35];
540 
541  if (!_in.fail())
542  {
543  _m.Set(d[0], d[1], d[2], d[3], d[4], d[5],
544  d[6], d[7], d[8], d[9], d[10], d[11],
545  d[12], d[13], d[14], d[15], d[16], d[17],
546  d[18], d[19], d[20], d[21], d[22], d[23],
547  d[24], d[25], d[26], d[27], d[28], d[29],
548  d[30], d[31], d[32], d[33], d[34], d[35]);
549  }
550  return _in;
551  }
552 
554  private: T data[MatrixSize][MatrixSize];
555  };
556 
557  namespace detail {
558 
559  template<typename T>
560  constexpr Matrix6<T> gMatrix6Identity(
561  1, 0, 0, 0, 0, 0,
562  0, 1, 0, 0, 0, 0,
563  0, 0, 1, 0, 0, 0,
564  0, 0, 0, 1, 0, 0,
565  0, 0, 0, 0, 1, 0,
566  0, 0, 0, 0, 0, 1);
567 
568  template<typename T>
569  constexpr Matrix6<T> gMatrix6Zero(
570  0, 0, 0, 0, 0, 0,
571  0, 0, 0, 0, 0, 0,
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 
577  } // namespace detail
578 
579  template<typename T>
580  const Matrix6<T> &Matrix6<T>::Identity = detail::gMatrix6Identity<T>;
581 
582  template<typename T>
583  const Matrix6<T> &Matrix6<T>::Zero = detail::gMatrix6Zero<T>;
584 
588  } // namespace GZ_MATH_VERSION_NAMESPACE
589 } // namespace gz::math
590 #endif // GZ_MATH_MATRIX6_HH_