gfx.math.inverse source code

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