cgv
Loading...
Searching...
No Matches
inv.h
1#pragma once
2
3#include <cgv/math/lin_solve.h>
4
5namespace cgv {
6 namespace math {
7
9template <typename T>
10diag_mat<T> inv(const diag_mat<T>& m)
11{
12 diag_mat<T> im(m.size());
13 for(unsigned i = 0; i < m.nrows();i++)
14 im(i) = (T)(1.0/m(i));
15 return im;
16}
17
19template <typename T>
20low_tri_mat<T> inv(const low_tri_mat<T>& m)
21{
22 unsigned N = m.nrows();
23 low_tri_mat<T> im(N,(T)0.0);
24 T sum;
25 for(unsigned k = 0; k < N;k++)
26 {
27 for(unsigned i = k; i < N;i++)
28 {
29 sum =0;
30 for(unsigned j = k;j < i; j++)
31 sum += m(i,j)*im(j,k);
32 assert(m(i,i) != 0);// not invertible
33 if(i == k)
34 im(i,k) = (1 - sum)/m(i,i);
35 else
36 im(i,k) = ( -sum)/m(i,i);
37 }
38 }
39
40
41 return im;
42}
43
45template <typename T>
46up_tri_mat<T> inv(const up_tri_mat<T>& m)
47{
48 up_tri_mat<T> im(m.nrows(),(T)0.0);
49 unsigned N = m.nrows();
50 vec<double> x;
51 vec<double> b(N);
52 x.resize(N);
53 T sum;
54 for(unsigned k = 0; k < N;k++)
55 {
56 for(int i = k; i >= 0;i--)
57 {
58 sum =0;
59 for(unsigned j = i+1;j <= k; j++)
60 sum += m(i,j)*im(j,k);
61 assert(m(i,i) != 0);
62
63
64 if(k == i)
65 im(i,k) = ((T)1.0 - sum)/m(i,i);
66 else
67 im(i,k) = ( - sum)/m(i,i);
68 }
69 }
70 return im;
71}
72
74template <typename T>
75mat<T> inv(const mat<T>& m)
76{
77 assert(m.nrows() == m.ncols());
78 mat<T> im(m.nrows(), m.nrows());
79 solve(m, identity<T>(m.ncols()), im);
80 return im;
81}
82
84template <typename T>
85mat<T> inv_22(const mat<T>& m)
86{
87 mat<T> im(2, 2);
88 T t4 = 1.0 / (-m(0, 0) * m(1, 1) + m(0, 1) * m(1, 0));
89 im(0, 0) = -m(1, 1) * t4;
90 im(1, 0) = m(1, 0) * t4;
91 im(0, 1) = m(0, 1) * t4;
92 im(1, 1) = -m(0, 0) * t4;
93
94 return im;
95}
96
98template <typename T>
99mat<T> inv_33(const mat<T>& m)
100{
101 mat<T> im(3, 3);
102 T t4 = m(2, 0) * m(0, 1);
103 T t6 = m(2, 0) * m(0, 2);
104 T t8 = m(1, 0) * m(0, 1);
105 T t10 = m(1, 0) * m(0, 2);
106 T t12 = m(0, 0) * m(1, 1);
107 T t14 = m(0, 0) * m(1, 2);
108 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));
109 im(0, 0) = (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * t17;
110 im(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) * t17;
111 im(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * t17;
112 im(1, 0) = -(-m(2, 0) * m(1, 2) + m(1, 0) * m(2, 2)) * t17;
113 im(1, 1) = (-t6 + m(0, 0) * m(2, 2)) * t17;
114 im(1, 2) = -(-t10 + t14) * t17;
115 im(2, 0) = (-m(2, 0) * m(1, 1) + m(1, 0) * m(2, 1)) * t17;
116 im(2, 1) = -(-t4 + m(0, 0) * m(2, 1)) * t17;
117 im(2, 2) = (-t8 + t12) * t17;
118 return im;
119}
120
122template <typename T>
123mat<T> inv_44(const mat<T>& m)
124{
125 mat<T> im(4,4);
126 T t1 = m(3,3) * m(1,1);
127 T t3 = m(3,2) * m(1,1);
128 T t7 = m(3,1) * m(1,2);
129 T t9 = m(3,1) * m(1,3);
130 T t11 = m(3,2) * m(2,1);
131 T t14 = m(0,0) * m(1,1);
132 T t19 = m(0,0) * m(3,3);
133 T t20 = m(1,2) * m(2,1);
134 T t22 = m(3,1) * m(0,0);
135 T t23 = m(1,2) * m(2,3);
136 T t25 = m(1,3) * m(2,2);
137 T t27 = m(3,2) * m(0,0);
138 T t28 = m(2,1) * m(1,3);
139 T t30 = m(1,1) * m(3,0);
140 T t31 = m(0,3) * m(2,2);
141 T t33 = m(2,0) * m(0,3);
142 T t35 = m(0,2) * m(2,3);
143 T t37 = m(2,0) * m(0,2);
144 T t39 = m(3,0) * m(0,2);
145 T t41 = m(3,1) * m(1,0);
146 T t43 = t14 * m(3,3) * m(2,2) - t14 * m(3,2) * m(2,3) - t19 * t20 +
147 t22 * t23 - t22 * t25 + t27 * t28 - t30 * t31 + t3 * t33 + t30 * t35
148 - t1 * t37 - t39 * t28 - t41 * t35;
149 T t45 = m(3,0) * m(0,1);
150 T t47 = m(1,0) * m(3,3);
151 T t50 = m(2,0) * m(3,3);
152 T t51 = m(0,1) * m(1,2);
153 T t53 = m(3,2) * m(1,0);
154 T t56 = m(0,2) * m(2,1);
155 T t58 = m(3,0) * m(0,3);
156 T t63 = m(3,2) * m(2,0);
157 T t64 = m(0,1) * m(1,3);
158 T t66 = m(1,0) * m(0,3);
159 T t68 = -t7 * t33 - t45 * t23 - t47 * m(0,1) * m(2,2) + t50 * t51 + t53 *
160 m(0,1) * m(2,3) + t47 * t56 + t58 * t20 + t9 * t37 + t41 * t31 + t45 *
161 t25 - t63 * t64 - t11 * t66;
162 T t70 = (T)1.0 / (t43 + t68);
163 T t72 = m(3,3) * m(0,1);
164 T t74 = m(3,2) * m(0,1);
165 T t78 = m(0,3) * m(3,1);
166 T t108 = m(2,0) * m(1,2);
167 T t111 = m(1,3) * m(3,0);
168 T t131 = m(0,0) * m(1,2);
169 T t135 = m(1,0) * m(0,2);
170 T t148 = m(3,1) * m(2,0);
171 T t150 = m(1,0) * m(2,1);
172 T t156 = m(0,0) * m(2,1);
173 T t158 = m(0,0) * m(2,3);
174 T t161 = m(2,0) * m(0,1);
175 im(0,0) = (t1 * m(2,2) - t3 * m(2,3) - m(3,3) * m(1,2) * m(2,1) +
176 t7 * m(2,3) - t9 * m(2,2) + t11 * m(1,3)) * t70;
177 im(0,1) = -(t72 * m(2,2) - t74 * m(2,3) - t56 * m(3,3) + t35 * m(3,1) -
178 t78 * m(2,2) + m(0,3) * m(3,2) * m(2,1)) * t70;
179 im(0,2) = (t72 * m(1,2) - t74 * m(1,3) - t1 * m(0,2) + m(0,2) * m(3,1) *
180 m(1,3) + t3 * m(0,3) - t78 * m(1,2)) * t70;
181 im(0,3) = -(t51 * m(2,3) - t64 * m(2,2) - m(1,1) * m(0,2) * m(2,3) + t56 *
182 m(1,3) + m(1,1) * m(0,3) * m(2,2) - m(0,3) * m(1,2) * m(2,1)) * t70;
183 im(1,0) = -(t47 * m(2,2) - t53 * m(2,3) + m(1,3) * m(3,2) * m(2,0) - t108 *
184 m(3,3) + t23 * m(3,0) - t111 * m(2,2)) * t70;
185 im(1,1) = (t19 * m(2,2) - t27 * m(2,3) - t58 * m(2,2) + t63 * m(0,3) + t39 *
186 m(2,3) - t50 * m(0,2)) * t70;
187 im(1,2) = -(t19 * m(1,2) - t27 * m(1,3) - t47 * m(0,2) - t58 * m(1,2) + t111 *
188 m(0,2) + t66 * m(3,2)) * t70;
189 im(1,3) = (t131 * m(2,3) - m(0,0) * m(1,3) * m(2,2) - t135 * m(2,3) - t108 *
190 m(0,3) + m(1,3) * m(2,0) * m(0,2) + t66 * m(2,2)) * t70;
191 im(2,0) = (-m(1,1) * m(2,0) * m(3,3) + m(1,1) * m(2,3) * m(3,0) - t28 *
192 m(3,0) + t148 * m(1,3) + t150 * m(3,3) - m(2,3) * m(3,1) * m(1,0)) * t70;
193 im(2,1) = -(t156 * m(3,3) - t158 * m(3,1) + t33 * m(3,1) - t161 * m(3,3) - m(2,1) *
194 m(3,0) * m(0,3) + m(2,3) * m(3,0) * m(0,1)) * t70;
195 im(2,2) = (t19 * m(1,1) - t22 * m(1,3) - t58 * m(1,1) - t47 * m(0,1) + t41 *
196 m(0,3) + t45 * m(1,3)) * t70;
197 im(2,3) = -(-m(2,3) * m(1,0) * m(0,1) + t158 * m(1,1) - t33 * m(1,1) + t161 *
198 m(1,3) - t156 * m(1,3) + t150 * m(0,3)) * t70;
199 im(3,0) = -(-t3 * m(2,0) + t30 * m(2,2) + t11 * m(1,0) - m(3,0) * m(1,2) *
200 m(2,1) - t41 * m(2,2) + t7 * m(2,0)) * t70;
201 im(3,1) = (-t22 * m(2,2) + t27 * m(2,1) - t39 * m(2,1) + t148 * m(0,2) + t45 *
202 m(2,2) - t63 * m(0,1)) * t70;
203 im(3,2) = -(-t53 * m(0,1) + t27 * m(1,1) - t39 * m(1,1) + t41 * m(0,2) - t22 *
204 m(1,2) + t45 * m(1,2)) * t70;
205 im(3,3) = t70 * (t161 * m(1,2) - t37 * m(1,1) - m(1,0) * m(0,1) * m(2,2) + t135 *
206 m(2,1) + t14 * m(2,2) - t131 * m(2,1));
207
208 return im;
209}
210
211inline const perm_mat inv(const perm_mat &p)
212{
213 perm_mat r=p;
214 r.transpose();
215 return r;
216}
217 }
218}
cgv
the cgv namespace
Definition print.h:11