MUDA
Loading...
Searching...
No Matches
analytic_inverse.h
1#pragma once
2#include <muda/muda_def.h>
3#include <muda/ext/eigen/eigen_core_cxx20.h>
4
5namespace muda::eigen
6{
7struct AnalyticalInverse
8{
9 template <typename T>
10 MUDA_INLINE MUDA_GENERIC Eigen::Matrix<T, 2, 2> operator()(const Eigen::Matrix<T, 2, 2>& m)
11 {
12 Eigen::Matrix<T, 2, 2> result;
13 invert2x2(m.data(), result.data());
14 return result;
15 }
16
17 template <typename T>
18 MUDA_INLINE MUDA_GENERIC Eigen::Matrix<T, 3, 3> operator()(const Eigen::Matrix<T, 3, 3>& m)
19 {
20 Eigen::Matrix<T, 3, 3> result;
21 invert3x3(m.data(), result.data());
22 return result;
23 }
24
25 template <typename T>
26 MUDA_INLINE MUDA_GENERIC Eigen::Matrix<T, 4, 4> operator()(const Eigen::Matrix<T, 4, 4>& m)
27 {
28 Eigen::Matrix<T, 4, 4> result;
29 invert4x4(m.data(), result.data());
30 return result;
31 }
32
33 private:
34 template <typename T>
35 MUDA_INLINE MUDA_GENERIC void invert2x2(const T* src, T* dst)
36 {
37 T det;
38
39 /* Compute adjoint: */
40
41 dst[0] = +src[3];
42 dst[1] = -src[1];
43 dst[2] = -src[2];
44 dst[3] = +src[0];
45
46 /* Compute determinant: */
47
48 det = src[0] * dst[0] + src[1] * dst[2];
49
50 /* Multiply adjoint with reciprocal of determinant: */
51
52 det = 1.0f / det;
53
54 dst[0] *= det;
55 dst[1] *= det;
56 dst[2] *= det;
57 dst[3] *= det;
58 }
59
60 template <typename T>
61 MUDA_INLINE MUDA_GENERIC void invert3x3(const T* src, T* dst)
62 {
63 T det;
64
65 /* Compute adjoint: */
66
67 dst[0] = +src[4] * src[8] - src[5] * src[7];
68 dst[1] = -src[1] * src[8] + src[2] * src[7];
69 dst[2] = +src[1] * src[5] - src[2] * src[4];
70 dst[3] = -src[3] * src[8] + src[5] * src[6];
71 dst[4] = +src[0] * src[8] - src[2] * src[6];
72 dst[5] = -src[0] * src[5] + src[2] * src[3];
73 dst[6] = +src[3] * src[7] - src[4] * src[6];
74 dst[7] = -src[0] * src[7] + src[1] * src[6];
75 dst[8] = +src[0] * src[4] - src[1] * src[3];
76
77 /* Compute determinant: */
78
79 det = src[0] * dst[0] + src[1] * dst[3] + src[2] * dst[6];
80
81 /* Multiply adjoint with reciprocal of determinant: */
82
83 det = 1.0f / det;
84
85 dst[0] *= det;
86 dst[1] *= det;
87 dst[2] *= det;
88 dst[3] *= det;
89 dst[4] *= det;
90 dst[5] *= det;
91 dst[6] *= det;
92 dst[7] *= det;
93 dst[8] *= det;
94 }
95
96 template <typename T>
97 MUDA_INLINE MUDA_GENERIC void invert4x4(const T* src, T* dst)
98 {
99 T det;
100
101 /* Compute adjoint: */
102
103 dst[0] = +src[5] * src[10] * src[15] - src[5] * src[11] * src[14]
104 - src[9] * src[6] * src[15] + src[9] * src[7] * src[14]
105 + src[13] * src[6] * src[11] - src[13] * src[7] * src[10];
106
107 dst[1] = -src[1] * src[10] * src[15] + src[1] * src[11] * src[14]
108 + src[9] * src[2] * src[15] - src[9] * src[3] * src[14]
109 - src[13] * src[2] * src[11] + src[13] * src[3] * src[10];
110
111 dst[2] = +src[1] * src[6] * src[15] - src[1] * src[7] * src[14]
112 - src[5] * src[2] * src[15] + src[5] * src[3] * src[14]
113 + src[13] * src[2] * src[7] - src[13] * src[3] * src[6];
114
115 dst[3] = -src[1] * src[6] * src[11] + src[1] * src[7] * src[10]
116 + src[5] * src[2] * src[11] - src[5] * src[3] * src[10]
117 - src[9] * src[2] * src[7] + src[9] * src[3] * src[6];
118
119 dst[4] = -src[4] * src[10] * src[15] + src[4] * src[11] * src[14]
120 + src[8] * src[6] * src[15] - src[8] * src[7] * src[14]
121 - src[12] * src[6] * src[11] + src[12] * src[7] * src[10];
122
123 dst[5] = +src[0] * src[10] * src[15] - src[0] * src[11] * src[14]
124 - src[8] * src[2] * src[15] + src[8] * src[3] * src[14]
125 + src[12] * src[2] * src[11] - src[12] * src[3] * src[10];
126
127 dst[6] = -src[0] * src[6] * src[15] + src[0] * src[7] * src[14]
128 + src[4] * src[2] * src[15] - src[4] * src[3] * src[14]
129 - src[12] * src[2] * src[7] + src[12] * src[3] * src[6];
130
131 dst[7] = +src[0] * src[6] * src[11] - src[0] * src[7] * src[10]
132 - src[4] * src[2] * src[11] + src[4] * src[3] * src[10]
133 + src[8] * src[2] * src[7] - src[8] * src[3] * src[6];
134
135 dst[8] = +src[4] * src[9] * src[15] - src[4] * src[11] * src[13]
136 - src[8] * src[5] * src[15] + src[8] * src[7] * src[13]
137 + src[12] * src[5] * src[11] - src[12] * src[7] * src[9];
138
139 dst[9] = -src[0] * src[9] * src[15] + src[0] * src[11] * src[13]
140 + src[8] * src[1] * src[15] - src[8] * src[3] * src[13]
141 - src[12] * src[1] * src[11] + src[12] * src[3] * src[9];
142
143 dst[10] = +src[0] * src[5] * src[15] - src[0] * src[7] * src[13]
144 - src[4] * src[1] * src[15] + src[4] * src[3] * src[13]
145 + src[12] * src[1] * src[7] - src[12] * src[3] * src[5];
146
147 dst[11] = -src[0] * src[5] * src[11] + src[0] * src[7] * src[9]
148 + src[4] * src[1] * src[11] - src[4] * src[3] * src[9]
149 - src[8] * src[1] * src[7] + src[8] * src[3] * src[5];
150
151 dst[12] = -src[4] * src[9] * src[14] + src[4] * src[10] * src[13]
152 + src[8] * src[5] * src[14] - src[8] * src[6] * src[13]
153 - src[12] * src[5] * src[10] + src[12] * src[6] * src[9];
154
155 dst[13] = +src[0] * src[9] * src[14] - src[0] * src[10] * src[13]
156 - src[8] * src[1] * src[14] + src[8] * src[2] * src[13]
157 + src[12] * src[1] * src[10] - src[12] * src[2] * src[9];
158
159 dst[14] = -src[0] * src[5] * src[14] + src[0] * src[6] * src[13]
160 + src[4] * src[1] * src[14] - src[4] * src[2] * src[13]
161 - src[12] * src[1] * src[6] + src[12] * src[2] * src[5];
162
163 dst[15] = +src[0] * src[5] * src[10] - src[0] * src[6] * src[9]
164 - src[4] * src[1] * src[10] + src[4] * src[2] * src[9]
165 + src[8] * src[1] * src[6] - src[8] * src[2] * src[5];
166
167 /* Compute determinant: */
168
169 det = +src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
170
171 /* Multiply adjoint with reciprocal of determinant: */
172
173 det = 1.0f / det;
174
175 dst[0] *= det;
176 dst[1] *= det;
177 dst[2] *= det;
178 dst[3] *= det;
179 dst[4] *= det;
180 dst[5] *= det;
181 dst[6] *= det;
182 dst[7] *= det;
183 dst[8] *= det;
184 dst[9] *= det;
185 dst[10] *= det;
186 dst[11] *= det;
187 dst[12] *= det;
188 dst[13] *= det;
189 dst[14] *= det;
190 dst[15] *= det;
191 }
192};
193} // namespace muda::eigen::inverse
muda::eigen::AnalyticalInverse
Definition analytic_inverse.h:8