cgv
Loading...
Searching...
No Matches
inv.h
1#pragma once
2
3#include <cgv/math/lin_solve.h>
4#include <cgv/math/fmat.h>
5
6namespace cgv {
7 namespace math {
8
10template <typename T>
11diag_mat<T> inv(const diag_mat<T>& m)
12{
13 diag_mat<T> im(m.size());
14 for(unsigned i = 0; i < m.nrows();i++)
15 im(i) = (T)(1.0/m(i));
16 return im;
17}
18
20template <typename T>
21low_tri_mat<T> inv(const low_tri_mat<T>& m)
22{
23 unsigned N = m.nrows();
24 low_tri_mat<T> im(N,(T)0.0);
25 T sum;
26 for(unsigned k = 0; k < N;k++)
27 {
28 for(unsigned i = k; i < N;i++)
29 {
30 sum =0;
31 for(unsigned j = k;j < i; j++)
32 sum += m(i,j)*im(j,k);
33 assert(m(i,i) != 0);// not invertible
34 if(i == k)
35 im(i,k) = (1 - sum)/m(i,i);
36 else
37 im(i,k) = ( -sum)/m(i,i);
38 }
39 }
40
41
42 return im;
43}
44
46template <typename T>
47up_tri_mat<T> inv(const up_tri_mat<T>& m)
48{
49 up_tri_mat<T> im(m.nrows(),(T)0.0);
50 unsigned N = m.nrows();
51 vec<double> x;
52 vec<double> b(N);
53 x.resize(N);
54 T sum;
55 for(unsigned k = 0; k < N;k++)
56 {
57 for(int i = k; i >= 0;i--)
58 {
59 sum =0;
60 for(unsigned j = i+1;j <= k; j++)
61 sum += m(i,j)*im(j,k);
62 assert(m(i,i) != 0);
63
64
65 if(k == i)
66 im(i,k) = ((T)1.0 - sum)/m(i,i);
67 else
68 im(i,k) = ( - sum)/m(i,i);
69 }
70 }
71 return im;
72}
73
75template <typename T>
76mat<T> inv(const mat<T>& m)
77{
78 assert(m.nrows() == m.ncols());
79 mat<T> im(m.nrows(), m.nrows());
80 solve(m, identity<T>(m.ncols()), im);
81 return im;
82}
83
85template <typename T>
86mat<T> inv_22(const mat<T>& m)
87{
88 mat<T> im(2, 2);
89 T t4 = 1.0 / (-m(0, 0) * m(1, 1) + m(0, 1) * m(1, 0));
90 im(0, 0) = -m(1, 1) * t4;
91 im(1, 0) = m(1, 0) * t4;
92 im(0, 1) = m(0, 1) * t4;
93 im(1, 1) = -m(0, 0) * t4;
94
95 return im;
96}
97
99template <typename T>
100mat<T> inv_33(const mat<T>& m)
101{
102 mat<T> im(3, 3);
103 T t4 = m(2, 0) * m(0, 1);
104 T t6 = m(2, 0) * m(0, 2);
105 T t8 = m(1, 0) * m(0, 1);
106 T t10 = m(1, 0) * m(0, 2);
107 T t12 = m(0, 0) * m(1, 1);
108 T t14 = m(0, 0) * m(1, 2);
109 T t17 = (T)1.0 / (t4 * m(1, 2) - t6 * m(1, 1) - t8 * m(2, 2) + t10 * m(2, 1) + t12 * m(2, 2) - t14 * m(2, 1));
110 im(0, 0) = (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * t17;
111 im(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) * t17;
112 im(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * t17;
113 im(1, 0) = -(-m(2, 0) * m(1, 2) + m(1, 0) * m(2, 2)) * t17;
114 im(1, 1) = (-t6 + m(0, 0) * m(2, 2)) * t17;
115 im(1, 2) = -(-t10 + t14) * t17;
116 im(2, 0) = (-m(2, 0) * m(1, 1) + m(1, 0) * m(2, 1)) * t17;
117 im(2, 1) = -(-t4 + m(0, 0) * m(2, 1)) * t17;
118 im(2, 2) = (-t8 + t12) * t17;
119 return im;
120}
121
123template <typename T>
124mat<T> inv_44(const mat<T>& m)
125{
126 mat<T> im(4,4);
127 T t1 = m(3,3) * m(1,1);
128 T t3 = m(3,2) * m(1,1);
129 T t7 = m(3,1) * m(1,2);
130 T t9 = m(3,1) * m(1,3);
131 T t11 = m(3,2) * m(2,1);
132 T t14 = m(0,0) * m(1,1);
133 T t19 = m(0,0) * m(3,3);
134 T t20 = m(1,2) * m(2,1);
135 T t22 = m(3,1) * m(0,0);
136 T t23 = m(1,2) * m(2,3);
137 T t25 = m(1,3) * m(2,2);
138 T t27 = m(3,2) * m(0,0);
139 T t28 = m(2,1) * m(1,3);
140 T t30 = m(1,1) * m(3,0);
141 T t31 = m(0,3) * m(2,2);
142 T t33 = m(2,0) * m(0,3);
143 T t35 = m(0,2) * m(2,3);
144 T t37 = m(2,0) * m(0,2);
145 T t39 = m(3,0) * m(0,2);
146 T t41 = m(3,1) * m(1,0);
147 T t43 = t14 * m(3,3) * m(2,2) - t14 * m(3,2) * m(2,3) - t19 * t20 +
148 t22 * t23 - t22 * t25 + t27 * t28 - t30 * t31 + t3 * t33 + t30 * t35
149 - t1 * t37 - t39 * t28 - t41 * t35;
150 T t45 = m(3,0) * m(0,1);
151 T t47 = m(1,0) * m(3,3);
152 T t50 = m(2,0) * m(3,3);
153 T t51 = m(0,1) * m(1,2);
154 T t53 = m(3,2) * m(1,0);
155 T t56 = m(0,2) * m(2,1);
156 T t58 = m(3,0) * m(0,3);
157 T t63 = m(3,2) * m(2,0);
158 T t64 = m(0,1) * m(1,3);
159 T t66 = m(1,0) * m(0,3);
160 T t68 = -t7 * t33 - t45 * t23 - t47 * m(0,1) * m(2,2) + t50 * t51 + t53 *
161 m(0,1) * m(2,3) + t47 * t56 + t58 * t20 + t9 * t37 + t41 * t31 + t45 *
162 t25 - t63 * t64 - t11 * t66;
163 T t70 = (T)1.0 / (t43 + t68);
164 T t72 = m(3,3) * m(0,1);
165 T t74 = m(3,2) * m(0,1);
166 T t78 = m(0,3) * m(3,1);
167 T t108 = m(2,0) * m(1,2);
168 T t111 = m(1,3) * m(3,0);
169 T t131 = m(0,0) * m(1,2);
170 T t135 = m(1,0) * m(0,2);
171 T t148 = m(3,1) * m(2,0);
172 T t150 = m(1,0) * m(2,1);
173 T t156 = m(0,0) * m(2,1);
174 T t158 = m(0,0) * m(2,3);
175 T t161 = m(2,0) * m(0,1);
176 im(0,0) = (t1 * m(2,2) - t3 * m(2,3) - m(3,3) * m(1,2) * m(2,1) +
177 t7 * m(2,3) - t9 * m(2,2) + t11 * m(1,3)) * t70;
178 im(0,1) = -(t72 * m(2,2) - t74 * m(2,3) - t56 * m(3,3) + t35 * m(3,1) -
179 t78 * m(2,2) + m(0,3) * m(3,2) * m(2,1)) * t70;
180 im(0,2) = (t72 * m(1,2) - t74 * m(1,3) - t1 * m(0,2) + m(0,2) * m(3,1) *
181 m(1,3) + t3 * m(0,3) - t78 * m(1,2)) * t70;
182 im(0,3) = -(t51 * m(2,3) - t64 * m(2,2) - m(1,1) * m(0,2) * m(2,3) + t56 *
183 m(1,3) + m(1,1) * m(0,3) * m(2,2) - m(0,3) * m(1,2) * m(2,1)) * t70;
184 im(1,0) = -(t47 * m(2,2) - t53 * m(2,3) + m(1,3) * m(3,2) * m(2,0) - t108 *
185 m(3,3) + t23 * m(3,0) - t111 * m(2,2)) * t70;
186 im(1,1) = (t19 * m(2,2) - t27 * m(2,3) - t58 * m(2,2) + t63 * m(0,3) + t39 *
187 m(2,3) - t50 * m(0,2)) * t70;
188 im(1,2) = -(t19 * m(1,2) - t27 * m(1,3) - t47 * m(0,2) - t58 * m(1,2) + t111 *
189 m(0,2) + t66 * m(3,2)) * t70;
190 im(1,3) = (t131 * m(2,3) - m(0,0) * m(1,3) * m(2,2) - t135 * m(2,3) - t108 *
191 m(0,3) + m(1,3) * m(2,0) * m(0,2) + t66 * m(2,2)) * t70;
192 im(2,0) = (-m(1,1) * m(2,0) * m(3,3) + m(1,1) * m(2,3) * m(3,0) - t28 *
193 m(3,0) + t148 * m(1,3) + t150 * m(3,3) - m(2,3) * m(3,1) * m(1,0)) * t70;
194 im(2,1) = -(t156 * m(3,3) - t158 * m(3,1) + t33 * m(3,1) - t161 * m(3,3) - m(2,1) *
195 m(3,0) * m(0,3) + m(2,3) * m(3,0) * m(0,1)) * t70;
196 im(2,2) = (t19 * m(1,1) - t22 * m(1,3) - t58 * m(1,1) - t47 * m(0,1) + t41 *
197 m(0,3) + t45 * m(1,3)) * t70;
198 im(2,3) = -(-m(2,3) * m(1,0) * m(0,1) + t158 * m(1,1) - t33 * m(1,1) + t161 *
199 m(1,3) - t156 * m(1,3) + t150 * m(0,3)) * t70;
200 im(3,0) = -(-t3 * m(2,0) + t30 * m(2,2) + t11 * m(1,0) - m(3,0) * m(1,2) *
201 m(2,1) - t41 * m(2,2) + t7 * m(2,0)) * t70;
202 im(3,1) = (-t22 * m(2,2) + t27 * m(2,1) - t39 * m(2,1) + t148 * m(0,2) + t45 *
203 m(2,2) - t63 * m(0,1)) * t70;
204 im(3,2) = -(-t53 * m(0,1) + t27 * m(1,1) - t39 * m(1,1) + t41 * m(0,2) - t22 *
205 m(1,2) + t45 * m(1,2)) * t70;
206 im(3,3) = t70 * (t161 * m(1,2) - t37 * m(1,1) - m(1,0) * m(0,1) * m(2,2) + t135 *
207 m(2,1) + t14 * m(2,2) - t131 * m(2,1));
208
209 return im;
210}
211
213template <typename T, cgv::type::uint32_type N>
214fmat<T, N, N> inv(const fmat<T, N, N>& m)
215{
216 mat<T> M(N, N, &m(0, 0));
217 return fmat<T, N, N>(N, N, &(inv(M)(0, 0)));
218}
219
221template <typename T>
222fmat<T, 2, 2> inv(const fmat<T, 2, 2>& m)
223{
224 fmat<T, 2, 2> im;
225 T t4 = T(1) / (-m(0, 0) * m(1, 1) + m(0, 1) * m(1, 0));
226 im(0, 0) = -m(1, 1) * t4;
227 im(1, 0) = +m(1, 0) * t4;
228 im(0, 1) = +m(0, 1) * t4;
229 im(1, 1) = -m(0, 0) * t4;
230
231 return im;
232}
233
235template <typename T>
236fmat<T, 3, 3> inv(const fmat<T, 3, 3>& m)
237{
238 T inv_det = (T)1 / (
239 +m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))
240 - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0))
241 + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)));
242
243 fmat<T, 3, 3> im;
244 im(0, 0) = +(m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * inv_det;
245 im(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) * inv_det;
246 im(0, 2) = +(m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * inv_det;
247 im(1, 0) = -(m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) * inv_det;
248 im(1, 1) = +(m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * inv_det;
249 im(1, 2) = -(m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) * inv_det;
250 im(2, 0) = +(m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)) * inv_det;
251 im(2, 1) = -(m(0, 0) * m(2, 1) - m(0, 1) * m(2, 0)) * inv_det;
252 im(2, 2) = +(m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) * inv_det;
253
254 return im;
255}
256
258template <typename T>
259fmat<T, 4, 4> inv(const fmat<T, 4, 4>& m)
260{
261 T coef00 = m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2);
262 T coef02 = m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1);
263 T coef03 = m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1);
264
265 T coef04 = m(1, 2) * m(3, 3) - m(1, 3) * m(3, 2);
266 T coef06 = m(1, 1) * m(3, 3) - m(1, 3) * m(3, 1);
267 T coef07 = m(1, 1) * m(3, 2) - m(1, 2) * m(3, 1);
268
269 T coef08 = m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2);
270 T coef10 = m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1);
271 T coef11 = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1);
272
273 T coef12 = m(0, 2) * m(3, 3) - m(0, 3) * m(3, 2);
274 T coef14 = m(0, 1) * m(3, 3) - m(0, 3) * m(3, 1);
275 T coef15 = m(0, 1) * m(3, 2) - m(0, 2) * m(3, 1);
276
277 T coef16 = m(0, 2) * m(2, 3) - m(0, 3) * m(2, 2);
278 T coef18 = m(0, 1) * m(2, 3) - m(0, 3) * m(2, 1);
279 T coef19 = m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1);
280
281 T coef20 = m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2);
282 T coef22 = m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1);
283 T coef23 = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
284
285 fvec<T, 4> fac0(coef00, coef00, coef02, coef03);
286 fvec<T, 4> fac1(coef04, coef04, coef06, coef07);
287 fvec<T, 4> fac2(coef08, coef08, coef10, coef11);
288 fvec<T, 4> fac3(coef12, coef12, coef14, coef15);
289 fvec<T, 4> fac4(coef16, coef16, coef18, coef19);
290 fvec<T, 4> fac5(coef20, coef20, coef22, coef23);
291
292 fvec<T, 4> vec0(m(0, 1), m(0, 0), m(0, 0), m(0, 0));
293 fvec<T, 4> vec1(m(1, 1), m(1, 0), m(1, 0), m(1, 0));
294 fvec<T, 4> vec2(m(2, 1), m(2, 0), m(2, 0), m(2, 0));
295 fvec<T, 4> vec3(m(3, 1), m(3, 0), m(3, 0), m(3, 0));
296
297 fvec<T, 4> inv0(vec1 * fac0 - vec2 * fac1 + vec3 * fac2);
298 fvec<T, 4> inv1(vec0 * fac0 - vec2 * fac3 + vec3 * fac4);
299 fvec<T, 4> inv2(vec0 * fac1 - vec1 * fac3 + vec3 * fac5);
300 fvec<T, 4> inv3(vec0 * fac2 - vec1 * fac4 + vec2 * fac5);
301
302 fvec<T, 4> sign_a(+(T)1, -(T)1, +(T)1, -(T)1);
303 fvec<T, 4> sign_b(-(T)1, +(T)1, -(T)1, +(T)1);
304 fmat<T, 4, 4> im;
305 im.set_col(0, inv0 * sign_a);
306 im.set_col(1, inv1 * sign_b);
307 im.set_col(2, inv2 * sign_a);
308 im.set_col(3, inv3 * sign_b);
309
310 fvec<T, 4> row0(im(0, 0), im(1, 0), im(2, 0), im(3, 0));
311
312 fvec<T, 4> dot0(m.row(0) * row0);
313 T dot1 = (dot0[0] + dot0[1]) + (dot0[2] + dot0[3]);
314
315 T inv_det = (T)1 / dot1;
316
317 return im * inv_det;
318}
319
320inline const perm_mat inv(const perm_mat &p)
321{
322 perm_mat r=p;
323 r.transpose();
324 return r;
325}
326 }
327}
cgv
the cgv namespace
Definition print.h:11
cgv::vec2
cgv::math::fvec< float, 2 > vec2
declare type of 2d single precision floating point vectors
Definition fvec.h:667
cgv::vec3
cgv::math::fvec< float, 3 > vec3
declare type of 3d single precision floating point vectors
Definition fvec.h:669