gfx.math.inverse source code

1 /// Matrix determinant and inverse
2 module gfx.math.inverse;
3 
4 import gfx.math.mat;
5 
6 // algos in this module are from GLM
7 
8 /// Compute the determinant of a matrix.
9 @property auto determinant(M)(in M m) if (isMat2!M)
10 {
11     return m.ct!(0, 0) * m.ct!(1, 1) - m.ct!(0, 1) * m.ct!(1, 0);
12 }
13 
14 /// ditto
15 @property auto determinant(M)(in M m) if (isMat3!M)
16 {
17     return
18         + m.ct!(0, 0) * (m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 1))
19         - m.ct!(0, 1) * (m.ct!(1, 0) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 0))
20         + m.ct!(0, 2) * (m.ct!(1, 0) * m.ct!(2, 1) - m.ct!(1, 1) * m.ct!(2, 0));
21 }
22 
23 /// ditto
24 @property auto determinant(M)(in M m) if (isMat4!M)
25 {
26     import gfx.math.vec : vec4;
27 
28     const subFactor00 = m.ct!(2, 2) * m.ct!(3, 3) - m.ct!(2, 3) * m.ct!(3, 2);
29     const subFactor01 = m.ct!(1, 2) * m.ct!(3, 3) - m.ct!(1, 3) * m.ct!(3, 2);
30     const subFactor02 = m.ct!(1, 2) * m.ct!(2, 3) - m.ct!(1, 3) * m.ct!(2, 2);
31     const subFactor03 = m.ct!(0, 2) * m.ct!(3, 3) - m.ct!(0, 3) * m.ct!(3, 2);
32     const subFactor04 = m.ct!(0, 2) * m.ct!(2, 3) - m.ct!(0, 3) * m.ct!(2, 2);
33     const subFactor05 = m.ct!(0, 2) * m.ct!(1, 3) - m.ct!(0, 3) * m.ct!(1, 2);
34 
35     const detCof = vec4 (
36         + (m.ct!(1, 1) * subFactor00 - m.ct!(2, 1) * subFactor01 + m.ct!(3, 1) * subFactor02),
37         - (m.ct!(0, 1) * subFactor00 - m.ct!(2, 1) * subFactor03 + m.ct!(3, 1) * subFactor04),
38         + (m.ct!(0, 1) * subFactor01 - m.ct!(1, 1) * subFactor03 + m.ct!(3, 1) * subFactor05),
39         - (m.ct!(0, 1) * subFactor02 - m.ct!(1, 1) * subFactor04 + m.ct!(2, 1) * subFactor05)
40     );
41 
42     return
43         m.ct!(0, 0) * detCof[0] + m.ct!(1, 0) * detCof[1] +
44         m.ct!(2, 0) * detCof[2] + m.ct!(3, 0) * detCof[3];
45 }
46 
47 /// Compute the inverse of a matrix
48 M inverse(M)(in M m) if (isMat2!M)
49 {
50     alias T = M.Component;
51 
52     const oneOverD = T(1) / (
53         + m.ct!(0, 0) * m.ct!(1, 1)
54         - m.ct!(0, 1) * m.ct!(1, 0));
55 
56     return Mat2!T (
57         + m.ct!(1, 1) * oneOverD,
58         - m.ct!(1, 0) * oneOverD,
59         - m.ct!(0, 1) * oneOverD,
60         + m.ct!(0, 0) * oneOverD
61     );
62 }
63 
64 /// ditto
65 M inverse(M)(in M m) if (isMat3!M)
66 {
67     alias T = M.Component;
68 
69     const oneOverD = T(1) / (
70         + m.ct!(0, 0) * (m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 1))
71         - m.ct!(0, 1) * (m.ct!(1, 0) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 0))
72         + m.ct!(0, 2) * (m.ct!(1, 0) * m.ct!(2, 1) - m.ct!(1, 1) * m.ct!(2, 0))
73     );
74 
75     return M(
76         + (m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 1)) * oneOverD,
77         - (m.ct!(0, 1) * m.ct!(2, 2) - m.ct!(0, 2) * m.ct!(2, 1)) * oneOverD,
78         + (m.ct!(0, 1) * m.ct!(1, 2) - m.ct!(0, 2) * m.ct!(1, 1)) * oneOverD,
79         - (m.ct!(1, 0) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 0)) * oneOverD,
80         + (m.ct!(0, 0) * m.ct!(2, 2) - m.ct!(0, 2) * m.ct!(2, 0)) * oneOverD,
81         - (m.ct!(0, 0) * m.ct!(1, 2) - m.ct!(0, 2) * m.ct!(1, 0)) * oneOverD,
82         + (m.ct!(1, 0) * m.ct!(2, 1) - m.ct!(1, 1) * m.ct!(2, 0)) * oneOverD,
83         - (m.ct!(0, 0) * m.ct!(2, 1) - m.ct!(0, 1) * m.ct!(2, 0)) * oneOverD,
84         + (m.ct!(0, 0) * m.ct!(1, 1) - m.ct!(0, 1) * m.ct!(1, 0)) * oneOverD,
85     );
86 }
87 
88 ///
89 unittest
90 {
91     /// Example from https://en.wikipedia.org/wiki/Gaussian_elimination
92     const m = FMat3(
93         2, -1, 0,
94         -1, 2, -1,
95         0, -1, 2
96     );
97     const invM = inverse(m);
98 
99     import gfx.math.approx : approxUlp;
100     assert(approxUlp(invM, FMat3(
101         0.75f, 0.5f, 0.25f,
102         0.5f,  1f,   0.5f,
103         0.25f, 0.5f, 0.75f
104     )));
105     assert(approxUlp(inverse(invM), m));
106 }
107 
108 
109 /// ditto
110 M inverse(M)(in M m) if (isMat4!M)
111 {
112     import gfx.math.vec : vec;
113 
114     alias T = M.Component;
115 
116     const coef00 = m.ct!(2, 2) * m.ct!(3, 3) - m.ct!(2, 3) * m.ct!(3, 2);
117     const coef02 = m.ct!(2, 1) * m.ct!(3, 3) - m.ct!(2, 3) * m.ct!(3, 1);
118     const coef03 = m.ct!(2, 1) * m.ct!(3, 2) - m.ct!(2, 2) * m.ct!(3, 1);
119 
120     const coef04 = m.ct!(1, 2) * m.ct!(3, 3) - m.ct!(1, 3) * m.ct!(3, 2);
121     const coef06 = m.ct!(1, 1) * m.ct!(3, 3) - m.ct!(1, 3) * m.ct!(3, 1);
122     const coef07 = m.ct!(1, 1) * m.ct!(3, 2) - m.ct!(1, 2) * m.ct!(3, 1);
123 
124     const coef08 = m.ct!(1, 2) * m.ct!(2, 3) - m.ct!(1, 3) * m.ct!(2, 2);
125     const coef10 = m.ct!(1, 1) * m.ct!(2, 3) - m.ct!(1, 3) * m.ct!(2, 1);
126     const coef11 = m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 1);
127 
128     const coef12 = m.ct!(0, 2) * m.ct!(3, 3) - m.ct!(0, 3) * m.ct!(3, 2);
129     const coef14 = m.ct!(0, 1) * m.ct!(3, 3) - m.ct!(0, 3) * m.ct!(3, 1);
130     const coef15 = m.ct!(0, 1) * m.ct!(3, 2) - m.ct!(0, 2) * m.ct!(3, 1);
131 
132     const coef16 = m.ct!(0, 2) * m.ct!(2, 3) - m.ct!(0, 3) * m.ct!(2, 2);
133     const coef18 = m.ct!(0, 1) * m.ct!(2, 3) - m.ct!(0, 3) * m.ct!(2, 1);
134     const coef19 = m.ct!(0, 1) * m.ct!(2, 2) - m.ct!(0, 2) * m.ct!(2, 1);
135 
136     const coef20 = m.ct!(0, 2) * m.ct!(1, 3) - m.ct!(0, 3) * m.ct!(1, 2);
137     const coef22 = m.ct!(0, 1) * m.ct!(1, 3) - m.ct!(0, 3) * m.ct!(1, 1);
138     const coef23 = m.ct!(0, 1) * m.ct!(1, 2) - m.ct!(0, 2) * m.ct!(1, 1);
139 
140     const fac0 = vec(coef00, coef00, coef02, coef03);
141     const fac1 = vec(coef04, coef04, coef06, coef07);
142     const fac2 = vec(coef08, coef08, coef10, coef11);
143     const fac3 = vec(coef12, coef12, coef14, coef15);
144     const fac4 = vec(coef16, coef16, coef18, coef19);
145     const fac5 = vec(coef20, coef20, coef22, coef23);
146 
147     const v0 = vec(m.ct!(0, 1), m.ct!(0, 0), m.ct!(0, 0), m.ct!(0, 0));
148     const v1 = vec(m.ct!(1, 1), m.ct!(1, 0), m.ct!(1, 0), m.ct!(1, 0));
149     const v2 = vec(m.ct!(2, 1), m.ct!(2, 0), m.ct!(2, 0), m.ct!(2, 0));
150     const v3 = vec(m.ct!(3, 1), m.ct!(3, 0), m.ct!(3, 0), m.ct!(3, 0));
151 
152     const inv0 = v1 * fac0 - v2 * fac1 + v3 * fac2;
153     const inv1 = v0 * fac0 - v2 * fac3 + v3 * fac4;
154     const inv2 = v0 * fac1 - v1 * fac3 + v3 * fac5;
155     const inv3 = v0 * fac2 - v1 * fac4 + v2 * fac5;
156 
157     const signA = vec(+1, -1, +1, -1);
158     const signB = vec(-1, +1, -1, +1);
159 
160     // GLM is column major!
161     // We have to transpose or refactor the algorithm
162     // Note: without this transpose gfx:math beats gl3n on inversion benchmark
163     const inverse = transpose(mat(
164         inv0 * signA, inv1 * signB, inv2 * signA, inv3 * signB
165     ));
166 
167     const dot0 = m.column(0) * inverse.row(0);
168     const dot1 = (dot0.x + dot0.y) + (dot0.z + dot0.w);
169 
170     const oneOverD = T(1) / dot1;
171 
172     return inverse * oneOverD;
173 }
174 
175 ///
176 unittest {
177     import gfx.math.transform : translation;
178     import gfx.math.approx : approxUlpAndAbs;
179 
180     const trM = translation!float(3, 4, 5);
181     const expected = translation!float(-3, -4, -5);
182     const inv = inverse(trM);
183 
184     assert(approxUlpAndAbs( inv, expected ));
185     assert(approxUlpAndAbs( inverse(inv), trM ));
186     assert(approxUlpAndAbs( inv * trM, FMat4.identity ));
187 }
188 
189 /// Fast matrix inverse for affine matrices
190 M affineInverse(M)(in M m) if(isMat3!M)
191 {
192     import gfx.math.vec : vec;
193     alias T = M.Component;
194 
195     const inv = inverse(m.slice!(0, 2, 0, 2));
196     const col = -inv * vec(m.column(2), T(1));
197 
198     return M (
199         vec!T( inv[0], col[0] ),
200         vec!T( inv[1], col[1] ),
201         vec!T( 0, 0,    1 ),
202     );
203 }
204 
205 /// ditto
206 M affineInverse(M)(in M m) if(isMat4!M)
207 {
208     import gfx.math.vec : vec;
209     alias T = M.Component;
210 
211     const inv = inverse(m.slice!(0, 3, 0, 3));
212     const col = -(inv * m.column(3).xyz);
213 
214     return M (
215         vec!T( inv[0], col[0] ),
216         vec!T( inv[1], col[1] ),
217         vec!T( inv[2], col[2] ),
218         vec!T( 0, 0, 0,    1 ),
219     );
220 }
221 
222 /// Compute the invert transpose of a matrix
223 M inverseTranspose(M)(in M m) if(isMat2!M)
224 {
225     alias T = M.Component;
226 
227     const oneOverD = T(1) / (m.ct!(0, 0) * m.ct!(1, 1) - m.ct!(0, 1) * m.ct!(1, 0));
228 
229     return M (
230         + m.ct!(1, 1) * oneOverD,
231         - m.ct!(0, 1) * oneOverD,
232         - m.ct!(1, 0) * oneOverD,
233         + m.ct!(0, 0) * oneOverD,
234     );
235 }
236 
237 /// ditto
238 M inverseTranspose(M)(in M m) if(isMat3!M)
239 {
240     alias T = M.Component;
241 
242     const oneOverD = T(1) / (
243         + m.ct!(0, 0) * (m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(2, 1) * m.ct!(1, 2))
244         - m.ct!(1, 0) * (m.ct!(0, 1) * m.ct!(2, 2) - m.ct!(2, 1) * m.ct!(0, 2))
245         + m.ct!(2, 0) * (m.ct!(0, 1) * m.ct!(1, 2) - m.ct!(1, 1) * m.ct!(0, 2))
246     );
247 
248     return M (
249         + (m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 1)) * oneOverD,
250         - (m.ct!(0, 1) * m.ct!(2, 2) - m.ct!(0, 2) * m.ct!(2, 1)) * oneOverD,
251         + (m.ct!(0, 1) * m.ct!(1, 2) - m.ct!(0, 2) * m.ct!(1, 1)) * oneOverD,
252         - (m.ct!(1, 0) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 0)) * oneOverD,
253         + (m.ct!(0, 0) * m.ct!(2, 2) - m.ct!(0, 2) * m.ct!(2, 0)) * oneOverD,
254         - (m.ct!(0, 0) * m.ct!(1, 2) - m.ct!(0, 2) * m.ct!(1, 0)) * oneOverD,
255         + (m.ct!(1, 0) * m.ct!(2, 1) - m.ct!(1, 1) * m.ct!(2, 0)) * oneOverD,
256         - (m.ct!(0, 0) * m.ct!(2, 1) - m.ct!(0, 1) * m.ct!(2, 0)) * oneOverD,
257         + (m.ct!(0, 0) * m.ct!(1, 1) - m.ct!(0, 1) * m.ct!(1, 0)) * oneOverD,
258     );
259 }
260 
261 /// ditto
262 M inverseTranspose(M)(in M m) if(isMat4!M)
263 {
264     alias T = M.Component;
265 
266     const subFactor00 = m.ct!(2, 2) * m.ct!(3, 3) - m.ct!(2, 3) * m.ct!(3, 2);
267     const subFactor01 = m.ct!(1, 2) * m.ct!(3, 3) - m.ct!(1, 3) * m.ct!(3, 2);
268     const subFactor02 = m.ct!(1, 2) * m.ct!(2, 3) - m.ct!(1, 3) * m.ct!(2, 2);
269     const subFactor03 = m.ct!(0, 2) * m.ct!(3, 3) - m.ct!(0, 3) * m.ct!(3, 2);
270     const subFactor04 = m.ct!(0, 2) * m.ct!(2, 3) - m.ct!(0, 3) * m.ct!(2, 2);
271     const subFactor05 = m.ct!(0, 2) * m.ct!(1, 3) - m.ct!(0, 3) * m.ct!(1, 2);
272     const subFactor06 = m.ct!(2, 1) * m.ct!(3, 3) - m.ct!(2, 3) * m.ct!(3, 1);
273     const subFactor07 = m.ct!(1, 1) * m.ct!(3, 3) - m.ct!(1, 3) * m.ct!(3, 1);
274     const subFactor08 = m.ct!(1, 1) * m.ct!(2, 3) - m.ct!(1, 3) * m.ct!(2, 1);
275     const subFactor09 = m.ct!(0, 1) * m.ct!(3, 3) - m.ct!(0, 3) * m.ct!(3, 1);
276     const subFactor10 = m.ct!(0, 1) * m.ct!(2, 3) - m.ct!(0, 3) * m.ct!(2, 1);
277     const subFactor11 = m.ct!(1, 1) * m.ct!(3, 3) - m.ct!(1, 3) * m.ct!(3, 1);
278     const subFactor12 = m.ct!(0, 1) * m.ct!(1, 3) - m.ct!(0, 3) * m.ct!(1, 1);
279     const subFactor13 = m.ct!(2, 1) * m.ct!(3, 2) - m.ct!(2, 2) * m.ct!(3, 1);
280     const subFactor14 = m.ct!(1, 1) * m.ct!(3, 2) - m.ct!(1, 2) * m.ct!(3, 1);
281     const subFactor15 = m.ct!(1, 1) * m.ct!(2, 2) - m.ct!(1, 2) * m.ct!(2, 1);
282     const subFactor16 = m.ct!(0, 1) * m.ct!(3, 2) - m.ct!(0, 2) * m.ct!(3, 1);
283     const subFactor17 = m.ct!(0, 1) * m.ct!(2, 2) - m.ct!(0, 2) * m.ct!(2, 1);
284     const subFactor18 = m.ct!(0, 1) * m.ct!(1, 2) - m.ct!(0, 2) * m.ct!(1, 1);
285 
286     const inv = M (
287         + (m.ct!(1, 1) * subFactor00 - m.ct!(2, 1) * subFactor01 + m.ct!(3, 1) * subFactor02),
288         - (m.ct!(1, 0) * subFactor00 - m.ct!(2, 0) * subFactor01 + m.ct!(3, 0) * subFactor02),
289         + (m.ct!(1, 0) * subFactor06 - m.ct!(2, 0) * subFactor07 + m.ct!(3, 0) * subFactor08),
290         - (m.ct!(1, 0) * subFactor13 - m.ct!(2, 0) * subFactor14 + m.ct!(3, 0) * subFactor15),
291 
292         - (m.ct!(0, 1) * subFactor00 - m.ct!(2, 1) * subFactor03 + m.ct!(3, 1) * subFactor04),
293         + (m.ct!(0, 0) * subFactor00 - m.ct!(2, 0) * subFactor03 + m.ct!(3, 0) * subFactor04),
294         - (m.ct!(0, 0) * subFactor06 - m.ct!(2, 0) * subFactor09 + m.ct!(3, 0) * subFactor10),
295         + (m.ct!(0, 0) * subFactor13 - m.ct!(2, 0) * subFactor16 + m.ct!(3, 0) * subFactor17),
296 
297         + (m.ct!(0, 1) * subFactor01 - m.ct!(1, 1) * subFactor03 + m.ct!(3, 1) * subFactor05),
298         - (m.ct!(0, 0) * subFactor01 - m.ct!(1, 0) * subFactor03 + m.ct!(3, 0) * subFactor05),
299         + (m.ct!(0, 0) * subFactor11 - m.ct!(1, 0) * subFactor09 + m.ct!(3, 0) * subFactor12),
300         - (m.ct!(0, 0) * subFactor14 - m.ct!(1, 0) * subFactor16 + m.ct!(3, 0) * subFactor18),
301 
302         - (m.ct!(0, 1) * subFactor02 - m.ct!(1, 1) * subFactor04 + m.ct!(2, 1) * subFactor05),
303         + (m.ct!(0, 0) * subFactor02 - m.ct!(1, 0) * subFactor04 + m.ct!(2, 0) * subFactor05),
304         - (m.ct!(0, 0) * subFactor08 - m.ct!(1, 0) * subFactor10 + m.ct!(2, 0) * subFactor12),
305         + (m.ct!(0, 0) * subFactor15 - m.ct!(1, 0) * subFactor17 + m.ct!(2, 0) * subFactor18),
306     );
307 
308     const oneOverD = T(1) / (
309         + m.ct!(0, 0) * inv.ct!(0, 0)
310         + m.ct!(1, 0) * inv.ct!(1, 0)
311         + m.ct!(2, 0) * inv.ct!(2, 0)
312         + m.ct!(3, 0) * inv.ct!(3, 0)
313     );
314 
315     return inv * oneOverD;
316 }