-
Notifications
You must be signed in to change notification settings - Fork 13
/
lgr_matrix4x4.hpp
149 lines (139 loc) · 6.74 KB
/
lgr_matrix4x4.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#pragma once
#include <hpc_macros.hpp>
namespace lgr {
template <typename Scalar>
class matrix4x4
{
public:
using scalar_type = Scalar;
private:
scalar_type raw[4][4];
public:
HPC_ALWAYS_INLINE HPC_HOST_DEVICE constexpr matrix4x4(
Scalar const a,
Scalar const b,
Scalar const c,
Scalar const d,
Scalar const e,
Scalar const f,
Scalar const g,
Scalar const h,
Scalar const i,
Scalar const j,
Scalar const k,
Scalar const l,
Scalar const m,
Scalar const n,
Scalar const o,
Scalar const p) noexcept
: raw{{a, b, c, d}, {e, f, g, h}, {i, j, k, l}, {m, n, o, p}}
{
}
HPC_ALWAYS_INLINE
matrix4x4() noexcept = default;
HPC_ALWAYS_INLINE HPC_HOST_DEVICE static constexpr matrix4x4
identity() noexcept
{
return matrix4x4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
}
HPC_ALWAYS_INLINE HPC_HOST_DEVICE static constexpr matrix4x4
zero() noexcept
{
return matrix4x4(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
HPC_ALWAYS_INLINE HPC_HOST_DEVICE constexpr Scalar
operator()(int const i, int const j) const noexcept
{
return raw[i][j];
}
HPC_ALWAYS_INLINE HPC_HOST_DEVICE Scalar&
operator()(int const i, int const j) noexcept
{
return raw[i][j];
}
};
template <typename Scalar>
HPC_HOST_DEVICE constexpr inline matrix4x4<Scalar>
operator*(matrix4x4<Scalar> const left, Scalar const right) noexcept
{
return matrix4x4<Scalar>(
left(0, 0) * right,
left(0, 1) * right,
left(0, 2) * right,
left(0, 3) * right,
left(1, 0) * right,
left(1, 1) * right,
left(1, 2) * right,
left(1, 3) * right,
left(2, 0) * right,
left(2, 1) * right,
left(2, 2) * right,
left(2, 3) * right,
left(3, 0) * right,
left(3, 1) * right,
left(3, 2) * right,
left(3, 3) * right);
}
template <typename Scalar>
HPC_HOST_DEVICE constexpr inline matrix4x4<Scalar>
operator/(matrix4x4<Scalar> const left, Scalar const right) noexcept
{
return left * (1.0 / right);
}
template <class Scalar>
HPC_HOST_DEVICE constexpr inline Scalar
determinant(matrix4x4<Scalar> const a)
{
return a(0, 3) * a(1, 2) * a(2, 1) * a(3, 0) - a(0, 2) * a(1, 3) * a(2, 1) * a(3, 0) -
a(0, 3) * a(1, 1) * a(2, 2) * a(3, 0) + a(0, 1) * a(1, 3) * a(2, 2) * a(3, 0) +
a(0, 2) * a(1, 1) * a(2, 3) * a(3, 0) - a(0, 1) * a(1, 2) * a(2, 3) * a(3, 0) -
a(0, 3) * a(1, 2) * a(2, 0) * a(3, 1) + a(0, 2) * a(1, 3) * a(2, 0) * a(3, 1) +
a(0, 3) * a(1, 0) * a(2, 2) * a(3, 1) - a(0, 0) * a(1, 3) * a(2, 2) * a(3, 1) -
a(0, 2) * a(1, 0) * a(2, 3) * a(3, 1) + a(0, 0) * a(1, 2) * a(2, 3) * a(3, 1) +
a(0, 3) * a(1, 1) * a(2, 0) * a(3, 2) - a(0, 1) * a(1, 3) * a(2, 0) * a(3, 2) -
a(0, 3) * a(1, 0) * a(2, 1) * a(3, 2) + a(0, 0) * a(1, 3) * a(2, 1) * a(3, 2) +
a(0, 1) * a(1, 0) * a(2, 3) * a(3, 2) - a(0, 0) * a(1, 1) * a(2, 3) * a(3, 2) -
a(0, 2) * a(1, 1) * a(2, 0) * a(3, 3) + a(0, 1) * a(1, 2) * a(2, 0) * a(3, 3) +
a(0, 2) * a(1, 0) * a(2, 1) * a(3, 3) - a(0, 0) * a(1, 2) * a(2, 1) * a(3, 3) -
a(0, 1) * a(1, 0) * a(2, 2) * a(3, 3) + a(0, 0) * a(1, 1) * a(2, 2) * a(3, 3);
}
template <class Scalar>
HPC_HOST_DEVICE constexpr inline matrix4x4<Scalar>
inverse(matrix4x4<Scalar> const a)
{
return matrix4x4<Scalar>(
(-a(1, 3) * a(2, 2) * a(3, 1) + a(1, 2) * a(2, 3) * a(3, 1) + a(1, 3) * a(2, 1) * a(3, 2) -
a(1, 1) * a(2, 3) * a(3, 2) - a(1, 2) * a(2, 1) * a(3, 3) + a(1, 1) * a(2, 2) * a(3, 3)),
(a(0, 3) * a(2, 2) * a(3, 1) - a(0, 2) * a(2, 3) * a(3, 1) - a(0, 3) * a(2, 1) * a(3, 2) +
a(0, 1) * a(2, 3) * a(3, 2) + a(0, 2) * a(2, 1) * a(3, 3) - a(0, 1) * a(2, 2) * a(3, 3)),
(-a(0, 3) * a(1, 2) * a(3, 1) + a(0, 2) * a(1, 3) * a(3, 1) + a(0, 3) * a(1, 1) * a(3, 2) -
a(0, 1) * a(1, 3) * a(3, 2) - a(0, 2) * a(1, 1) * a(3, 3) + a(0, 1) * a(1, 2) * a(3, 3)),
(a(0, 3) * a(1, 2) * a(2, 1) - a(0, 2) * a(1, 3) * a(2, 1) - a(0, 3) * a(1, 1) * a(2, 2) +
a(0, 1) * a(1, 3) * a(2, 2) + a(0, 2) * a(1, 1) * a(2, 3) - a(0, 1) * a(1, 2) * a(2, 3)),
(a(1, 3) * a(2, 2) * a(3, 0) - a(1, 2) * a(2, 3) * a(3, 0) - a(1, 3) * a(2, 0) * a(3, 2) +
a(1, 0) * a(2, 3) * a(3, 2) + a(1, 2) * a(2, 0) * a(3, 3) - a(1, 0) * a(2, 2) * a(3, 3)),
(-a(0, 3) * a(2, 2) * a(3, 0) + a(0, 2) * a(2, 3) * a(3, 0) + a(0, 3) * a(2, 0) * a(3, 2) -
a(0, 0) * a(2, 3) * a(3, 2) - a(0, 2) * a(2, 0) * a(3, 3) + a(0, 0) * a(2, 2) * a(3, 3)),
(a(0, 3) * a(1, 2) * a(3, 0) - a(0, 2) * a(1, 3) * a(3, 0) - a(0, 3) * a(1, 0) * a(3, 2) +
a(0, 0) * a(1, 3) * a(3, 2) + a(0, 2) * a(1, 0) * a(3, 3) - a(0, 0) * a(1, 2) * a(3, 3)),
(-a(0, 3) * a(1, 2) * a(2, 0) + a(0, 2) * a(1, 3) * a(2, 0) + a(0, 3) * a(1, 0) * a(2, 2) -
a(0, 0) * a(1, 3) * a(2, 2) - a(0, 2) * a(1, 0) * a(2, 3) + a(0, 0) * a(1, 2) * a(2, 3)),
(-a(1, 3) * a(2, 1) * a(3, 0) + a(1, 1) * a(2, 3) * a(3, 0) + a(1, 3) * a(2, 0) * a(3, 1) -
a(1, 0) * a(2, 3) * a(3, 1) - a(1, 1) * a(2, 0) * a(3, 3) + a(1, 0) * a(2, 1) * a(3, 3)),
(a(0, 3) * a(2, 1) * a(3, 0) - a(0, 1) * a(2, 3) * a(3, 0) - a(0, 3) * a(2, 0) * a(3, 1) +
a(0, 0) * a(2, 3) * a(3, 1) + a(0, 1) * a(2, 0) * a(3, 3) - a(0, 0) * a(2, 1) * a(3, 3)),
(-a(0, 3) * a(1, 1) * a(3, 0) + a(0, 1) * a(1, 3) * a(3, 0) + a(0, 3) * a(1, 0) * a(3, 1) -
a(0, 0) * a(1, 3) * a(3, 1) - a(0, 1) * a(1, 0) * a(3, 3) + a(0, 0) * a(1, 1) * a(3, 3)),
(a(0, 3) * a(1, 1) * a(2, 0) - a(0, 1) * a(1, 3) * a(2, 0) - a(0, 3) * a(1, 0) * a(2, 1) +
a(0, 0) * a(1, 3) * a(2, 1) + a(0, 1) * a(1, 0) * a(2, 3) - a(0, 0) * a(1, 1) * a(2, 3)),
(a(1, 2) * a(2, 1) * a(3, 0) - a(1, 1) * a(2, 2) * a(3, 0) - a(1, 2) * a(2, 0) * a(3, 1) +
a(1, 0) * a(2, 2) * a(3, 1) + a(1, 1) * a(2, 0) * a(3, 2) - a(1, 0) * a(2, 1) * a(3, 2)),
(-a(0, 2) * a(2, 1) * a(3, 0) + a(0, 1) * a(2, 2) * a(3, 0) + a(0, 2) * a(2, 0) * a(3, 1) -
a(0, 0) * a(2, 2) * a(3, 1) - a(0, 1) * a(2, 0) * a(3, 2) + a(0, 0) * a(2, 1) * a(3, 2)),
(a(0, 2) * a(1, 1) * a(3, 0) - a(0, 1) * a(1, 2) * a(3, 0) - a(0, 2) * a(1, 0) * a(3, 1) +
a(0, 0) * a(1, 2) * a(3, 1) + a(0, 1) * a(1, 0) * a(3, 2) - a(0, 0) * a(1, 1) * a(3, 2)),
(-a(0, 2) * a(1, 1) * a(2, 0) + a(0, 1) * a(1, 2) * a(2, 0) + a(0, 2) * a(1, 0) * a(2, 1) -
a(0, 0) * a(1, 2) * a(2, 1) - a(0, 1) * a(1, 0) * a(2, 2) + a(0, 0) * a(1, 1) * a(2, 2))) /
determinant(a);
}
} // namespace lgr