32 #ifndef HERMES_GEOMETRY_MATRIX_H
33 #define HERMES_GEOMETRY_MATRIX_H
55 inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] +
56 m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
58 inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] -
59 m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
61 inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] +
62 m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
64 inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] -
65 m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
67 inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] -
68 m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
70 inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] +
71 m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
73 inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] -
74 m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
76 inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] +
77 m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
79 inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] +
80 m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
82 inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] -
83 m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
85 inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] +
86 m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
88 inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] -
89 m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
91 inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] -
92 m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
94 inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] +
95 m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
97 inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] -
98 m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
100 inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] +
101 m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
103 det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
110 for (i = 0; i < 16; i++)
111 invOut[i] = inv[i] * det;
140 std::memset(m_, 0,
sizeof(m_));
142 for (
int i = 0; i < 4; i++)
149 for (
auto v : values) {
167 for (
int c = 0; c < 4; c++)
172 for (
int c = 0; c < 4; c++)
177 for (
int i = 0; i < 4; i++)
178 for (
int j = 0; j < 4; j++)
179 m_[i][j] = mat[i][j];
198 T m21, T m22, T m23, T m30, T m31, T m32, T m33) {
222 for (
int i = 0; i < 4; ++i)
223 for (
int j = 0; j < 4; ++j)
224 r[i][j] = m_[i][0] * B[0][j] + m_[i][1] * B[1][j] +
225 m_[i][2] * B[2][j] + m_[i][3] * B[3][j];
230 for (
int i = 0; i < 4; i++)
231 for (
int j = 0; j < 4; j++)
232 r[i] += m_[i][j] * v[j];
236 for (
int i = 0; i < 4; i++)
237 for (
int j = 0; j < 4; j++)
243 for (
int i = 0; i < 4; i++)
244 for (
int j = 0; j < 4; j++)
250 return !((*this) == B);
257 #if defined(__CUDA_ARCH__) && __CUDA_ARCH__ > 0
259 for (
int j = 0; j < 4; j++)
262 std::memset(m_, 0,
sizeof(m_));
264 for (
int i = 0; i < 4; i++)
272 for (
int j = 0; j < 4; j++)
279 for (
int i = 0; i < 4; i++)
286 for (
int i = 0; i < 4; i++)
287 for (
int j = 0; j < 4; j++)
347 for (
int i = 0; i < 3; i++)
348 for (
int j = 0; j < 3; j++)
349 r[i] += m_[i][j] * v[j];
355 for (
int i = 0; i < 3; ++i)
356 for (
int j = 0; j < 3; ++j)
358 m_[i][0] * B[0][j] + m_[i][1] * B[1][j] + m_[i][2] * B[2][j];
362 #define ARITHMETIC_OP(OP) \
363 HERMES_DEVICE_CALLABLE Matrix3x3<T> operator OP##=(const Matrix3x3<T> &B) const { \
364 for (int i = 0; i < 3; ++i) \
365 for (int j = 0; j < 3; ++j) \
366 m_[i][j] OP##= B[i][j]; \
368 HERMES_DEVICE_CALLABLE Matrix3x3<T> operator OP(const Matrix3x3<T> &B) const { \
370 for (int i = 0; i < 3; ++i) \
371 for (int j = 0; j < 3; ++j) \
372 r[i][j] = m_[i][j] OP B[i][j]; \
380 return Matrix3x3<T>(m_[0][0] * f, m_[0][1] * f, m_[0][2] * f, m_[1][0] * f,
381 m_[1][1] * f, m_[1][2] * f, m_[2][0] * f, m_[2][1] * f,
386 return Matrix3x3<T>(m_[0][0] / f, m_[0][1] / f, m_[0][2] / f, m_[1][0] / f,
387 m_[1][1] / f, m_[1][2] / f, m_[2][0] / f, m_[2][1] / f,
394 std::memset(m_, 0,
sizeof(m_));
395 for (
int i = 0; i < 3; i++)
399 return m_[0][0] * m_[1][1] * m_[2][2] + m_[0][1] * m_[1][2] * m_[2][0] +
400 m_[0][2] * m_[1][0] * m_[2][1] - m_[2][0] * m_[1][1] * m_[0][2] -
401 m_[2][1] * m_[1][2] * m_[0][0] - m_[2][2] * m_[1][0] * m_[0][1];
424 std::memset(m_, 0,
sizeof(m_));
425 for (
int i = 0; i < 2; i++)
440 for (
int i = 0; i < 2; i++)
441 for (
int j = 0; j < 2; j++)
442 r[i] += m_[i][j] * v[j];
447 for (
int i = 0; i < 2; ++i)
448 for (
int j = 0; j < 2; ++j)
449 r[i][j] = m_[i][0] * B[0][j] + m_[i][1] * B[1][j];
453 return Matrix2x2<T>(m_[0][0] * f, m_[0][1] * f, m_[1][0] * f,
460 std::memset(m_, 0,
sizeof(m_));
461 for (
int i = 0; i < 2; i++)
489 return Matrix4x4<T>(m[0][0], m[1][0], m[2][0], m[3][0], m[0][1],
490 m[1][1], m[2][1], m[3][1], m[0][2], m[1][2],
491 m[2][2], m[3][2], m[0][3], m[1][3], m[2][3],
501 for (
int i = 0; i < 4; i++)
502 for (
int j = 0; j < 4; j++)
507 T det = m[0][0] * m[1][1] * m[2][2] * m[3][3] +
508 m[1][2] * m[2][3] * m[3][1] * m[1][3] +
509 m[2][1] * m[3][2] * m[1][1] * m[2][3] +
510 m[3][2] * m[1][2] * m[2][1] * m[3][3] +
511 m[1][3] * m[2][2] * m[3][1] * m[0][1] +
512 m[0][1] * m[2][3] * m[3][2] * m[0][2] +
513 m[2][1] * m[3][3] * m[0][3] * m[2][2] +
514 m[3][1] * m[0][1] * m[2][2] * m[3][3] +
515 m[0][2] * m[2][3] * m[3][1] * m[0][3] +
516 m[2][1] * m[3][2] * m[0][2] * m[0][1] +
517 m[1][2] * m[3][3] * m[0][2] * m[1][3] +
518 m[3][1] * m[0][3] * m[1][1] * m[3][2] -
519 m[0][1] * m[1][3] * m[3][2] * m[0][2] -
520 m[1][1] * m[3][3] * m[0][3] * m[1][2] -
521 m[3][1] * m[0][3] * m[0][1] * m[1][3] -
522 m[2][2] * m[0][2] * m[1][1] * m[2][3] -
523 m[0][3] * m[1][2] * m[2][1] * m[0][1] -
524 m[1][2] * m[2][3] * m[0][2] * m[1][3] -
525 m[2][1] * m[0][3] * m[1][1] * m[2][2] -
526 m[1][0] * m[1][0] * m[2][3] * m[3][2] -
527 m[1][2] * m[2][0] * m[3][3] * m[1][3] -
528 m[2][2] * m[3][0] * m[1][0] * m[2][2] -
529 m[3][3] * m[1][2] * m[2][3] * m[3][0] -
530 m[1][3] * m[2][0] * m[3][2] * m[1][1];
531 if (fabs(det) < 1e-8)
535 (m[1][1] * m[2][2] * m[3][3] + m[1][2] * m[2][3] * m[3][1] +
536 m[1][3] * m[2][1] * m[3][2] - m[1][1] * m[2][3] * m[3][2] -
537 m[1][2] * m[2][1] * m[3][3] - m[1][3] * m[2][2] * m[3][1]) /
540 (m[0][1] * m[2][3] * m[3][2] + m[0][2] * m[2][1] * m[3][3] +
541 m[0][3] * m[2][2] * m[3][1] - m[0][1] * m[2][2] * m[3][3] -
542 m[0][2] * m[2][3] * m[3][1] - m[0][3] * m[2][1] * m[3][2]) /
545 (m[0][1] * m[1][2] * m[3][3] + m[0][2] * m[1][3] * m[3][1] +
546 m[0][3] * m[1][1] * m[3][2] - m[0][1] * m[1][3] * m[3][2] -
547 m[0][2] * m[1][1] * m[3][3] - m[0][3] * m[1][2] * m[3][1]) /
550 (m[0][1] * m[1][3] * m[2][2] + m[0][2] * m[1][1] * m[2][3] +
551 m[0][3] * m[1][2] * m[2][1] - m[0][1] * m[1][2] * m[2][3] -
552 m[0][2] * m[1][3] * m[2][1] - m[0][3] * m[1][1] * m[2][2]) /
555 (m[1][0] * m[2][3] * m[3][2] + m[1][2] * m[2][0] * m[3][3] +
556 m[1][3] * m[2][2] * m[3][0] - m[1][0] * m[2][2] * m[3][3] -
557 m[1][2] * m[2][3] * m[3][0] - m[1][3] * m[2][0] * m[3][2]) /
560 (m[0][0] * m[2][2] * m[3][3] + m[0][2] * m[2][3] * m[3][0] +
561 m[0][3] * m[2][0] * m[3][2] - m[0][0] * m[2][3] * m[3][2] -
562 m[0][2] * m[2][0] * m[3][3] - m[0][3] * m[2][2] * m[3][0]) /
565 (m[0][0] * m[1][3] * m[3][2] + m[0][2] * m[1][0] * m[3][3] +
566 m[0][3] * m[1][2] * m[3][0] - m[0][0] * m[1][2] * m[3][3] -
567 m[0][2] * m[1][3] * m[3][0] - m[0][3] * m[1][0] * m[3][2]) /
570 (m[0][0] * m[1][2] * m[2][3] + m[0][2] * m[1][3] * m[2][0] +
571 m[0][3] * m[1][0] * m[2][2] - m[0][0] * m[1][3] * m[2][2] -
572 m[0][2] * m[1][0] * m[2][3] - m[0][3] * m[1][2] * m[2][0]) /
575 (m[1][0] * m[2][1] * m[3][3] + m[1][1] * m[2][3] * m[3][0] +
576 m[1][3] * m[2][0] * m[3][1] - m[1][0] * m[2][3] * m[3][1] -
577 m[1][1] * m[2][0] * m[3][3] - m[1][3] * m[2][1] * m[3][0]) /
580 (m[0][0] * m[2][3] * m[3][1] + m[0][1] * m[2][0] * m[3][3] +
581 m[0][3] * m[2][1] * m[3][0] - m[0][0] * m[2][1] * m[3][3] -
582 m[0][1] * m[2][3] * m[3][0] - m[0][3] * m[2][0] * m[3][1]) /
585 (m[0][0] * m[1][1] * m[3][3] + m[0][1] * m[1][3] * m[3][0] +
586 m[0][3] * m[1][0] * m[3][1] - m[0][0] * m[1][3] * m[3][1] -
587 m[0][1] * m[1][0] * m[3][3] - m[0][3] * m[1][1] * m[3][0]) /
590 (m[0][0] * m[1][3] * m[2][1] + m[0][1] * m[1][0] * m[2][3] +
591 m[0][3] * m[1][1] * m[2][0] - m[0][0] * m[1][1] * m[2][3] -
592 m[0][1] * m[1][3] * m[2][0] - m[0][3] * m[1][0] * m[2][1]) /
595 (m[1][0] * m[2][2] * m[3][1] + m[1][1] * m[2][0] * m[3][2] +
596 m[1][2] * m[2][1] * m[3][0] - m[1][0] * m[2][1] * m[3][2] -
597 m[1][1] * m[2][2] * m[3][0] - m[1][2] * m[2][0] * m[3][1]) /
600 (m[0][0] * m[2][1] * m[3][2] + m[0][1] * m[2][2] * m[3][0] +
601 m[0][2] * m[2][0] * m[3][1] - m[0][0] * m[2][2] * m[3][1] -
602 m[0][1] * m[2][0] * m[3][2] - m[0][2] * m[2][1] * m[3][0]) /
605 (m[0][0] * m[1][2] * m[3][1] + m[0][1] * m[1][0] * m[3][2] +
606 m[0][2] * m[1][1] * m[3][0] - m[0][0] * m[1][1] * m[3][2] -
607 m[0][1] * m[1][2] * m[3][0] - m[0][2] * m[1][0] * m[3][1]) /
610 (m[0][0] * m[1][1] * m[2][2] + m[0][1] * m[1][2] * m[2][0] +
611 m[0][2] * m[1][0] * m[2][1] - m[0][0] * m[1][2] * m[2][1] -
612 m[0][1] * m[1][0] * m[2][2] - m[0][2] * m[1][1] * m[2][0]) /
626 Matrix4x4<T> Rit = inverse(transpose(r));
627 for (
int i = 0; i < 4; i++)
628 for (
int j = 0; j < 4; j++)
629 Rnext[i][j] = .5f * (r[i][j] + Rit[i][j]);
632 for (
int i = 0; i < 3; i++) {
633 T n = fabsf(r[i][0] - Rnext[i][0]) +
634 fabsf(r[i][1] - Rnext[i][1]) + fabsf(r[i][2] - Rnext[i][2]);
635 norm = std::max(norm, n);
637 }
while (++count < 100 && norm > .0001f);
650 T det = m[0][0] * m[1][1] - m[0][1] * m[1][0];
654 r[0][0] = m[1][1] * k;
655 r[0][1] = -m[0][1] * k;
656 r[1][0] = -m[1][0] * k;
657 r[1][1] = m[0][0] * k;
663 return Matrix2x2<T>(m[0][0], m[1][0], m[0][1], m[1][1]);
671 m[0][0] * m[1][1] * m[2][2] + m[1][0] * m[2][1] * m[0][2] +
672 m[2][0] * m[0][1] * m[1][2] - m[0][0] * m[2][1] * m[1][2] -
673 m[2][0] * m[1][1] * m[0][2] - m[1][0] * m[0][1] * m[2][2];
674 if (std::fabs(det) < 1e-8)
676 r[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) / det;
677 r[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) / det;
678 r[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) / det;
679 r[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) / det;
680 r[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) / det;
681 r[1][2] = (m[0][2] * m[1][0] - m[0][0] * m[1][2]) / det;
682 r[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) / det;
683 r[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) / det;
684 r[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) / det;
689 return Matrix3x3<T>(m[0][0], m[1][0], m[2][0], m[0][1], m[1][1],
690 m[2][1], m[0][2], m[1][2], m[2][2]);
694 return Matrix3x3<T>(0, -a[2], a[1], a[2], 0, -a[0], -a[1], a[0], 0);
709 std::ostream &
operator<<(std::ostream &os,
const Matrix4x4<T> &m) {
710 for (
int i = 0; i < 4; i++) {
711 for (
int j = 0; j < 4; j++)
712 os << m[i][j] <<
" ";
718 std::ostream &
operator<<(std::ostream &os,
const Matrix3x3<T> &m) {
719 for (
int i = 0; i < 3; i++) {
720 for (
int j = 0; j < 3; j++)
721 os << m[i][j] <<
" ";
727 std::ostream &
operator<<(std::ostream &os,
const Matrix2x2<T> &m) {
728 for (
int i = 0; i < 2; i++) {
729 for (
int j = 0; j < 2; j++)
730 os << m[i][j] <<
" ";
739 typedef Matrix4x4<real_t> mat4;
740 typedef Matrix3x3<real_t> mat3;
741 typedef Matrix2x2<real_t> mat2;
Interface used by all basic geometric entities.
Definition: math_element.h:44
2x2 Matrix representation
Definition: matrix.h:418
3x3 Matrix representation
Definition: matrix.h:308
4x4 Matrix representation
Definition: matrix.h:121
HERMES_DEVICE_CALLABLE bool isIdentity() const
Definition: matrix.h:285
HERMES_DEVICE_CALLABLE Matrix4x4(std::initializer_list< T > values, bool columnMajor=false)
Definition: matrix.h:147
HERMES_DEVICE_CALLABLE void column_major(T *a) const
Definition: matrix.h:277
HERMES_DEVICE_CALLABLE Matrix4x4(T mat[4][4])
Definition: matrix.h:176
HERMES_DEVICE_CALLABLE Matrix4x4(const T mat[16], bool columnMajor=false)
Definition: matrix.h:164
HERMES_DEVICE_CALLABLE Matrix4x4(bool is_identity=false)
Definition: matrix.h:139
HERMES_DEVICE_CALLABLE Matrix4x4(T m00, T m01, T m02, T m03, T m10, T m11, T m12, T m13, T m20, T m21, T m22, T m23, T m30, T m31, T m32, T m33)
Definition: matrix.h:197
HERMES_DEVICE_CALLABLE void row_major(T *a) const
Definition: matrix.h:269
static HERMES_DEVICE_CALLABLE Matrix4x4 I()
Creates an identity matrix.
Definition: matrix.h:128
Geometric 2-dimensional vector (x, y)
Definition: vector.h:54
T x
0-th component
Definition: vector.h:415
T z
2-th component
Definition: vector.h:417
T y
1-th component
Definition: vector.h:416
std::ostream & operator<<(std::ostream &o, const LaunchInfo &info)
LaunchInfo support for std::ostream << operator.
Definition: cuda_utils.h:234
#define HERMES_DEVICE_CALLABLE
Specifies that the function can be called from both host and device sides.
Definition: defs.h:45
uint32_t u32
32 bit size unsigned integer type
Definition: defs.h:88
#define ARITHMETIC_OP(OP)
asd
Definition: index.h:408
Base class for all geometric objects.
HERMES_DEVICE_CALLABLE bool gluInvertMatrix(const T m[16], T invOut[16])
Inverts a 4x4 matrix.
Definition: matrix.h:51
static constexpr HERMES_DEVICE_CALLABLE bool is_equal(T a, T b)
Checks if two numbers are at most 1e-8 apart.
Definition: numeric.h:847
Geometric vector classes.