17 #ifndef GZ_MATH_MATRIX6_HH_
18 #define GZ_MATH_MATRIX6_HH_
21 #include <gz/math/config.hh>
28 inline namespace GZ_MATH_VERSION_NAMESPACE {
67 memset(this->data, 0,
sizeof(this->data[0][0])*MatrixSize*MatrixSize);
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}}
131 public:
bool SetValue(
size_t _row,
size_t _col, T _v)
133 if (_row < MatrixSize && _col < MatrixSize)
135 this->data[_row][_col] = _v;
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)
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;
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;
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;
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;
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;
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;
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]);
308 (*this) = (*this) * _m2;
317 auto el = [&](
size_t _row,
size_t _col) -> T
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);
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));
339 (*this) = (*this) + _m2;
348 auto el = [&](
size_t _row,
size_t _col) -> T
350 return this->data[_row][_col] + _m2(_row, _col);
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));
368 const size_t _col)
const
381 public:
inline T &
operator()(
const size_t _row,
const size_t _col)
396 if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
400 if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
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]};
424 if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT)
428 if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT)
432 for (
size_t r = 0; r < 3; ++r)
434 for (
size_t c = 0; c < 3; ++c)
436 this->data[row + r][col + c] = _mat(r, c);
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);
492 return this->Equal(_m,
static_cast<T
>(1e-6));
500 return !(*
this == _m);
510 for (
auto i : {0, 1, 2, 3, 4, 5})
512 for (
auto j : {0, 1, 2, 3, 4, 5})
514 if (!(i == 0 && j == 0))
532 _in.
setf(std::ios_base::skipws);
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];
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]);
554 private: T data[MatrixSize][MatrixSize];
560 constexpr Matrix6<T> gMatrix6Identity(
569 constexpr Matrix6<T> gMatrix6Zero(