1 | #include <gfx/std.h>
|
---|
2 | #include <gfx/math/Mat3.h>
|
---|
3 | #include <gfx/math/Mat4.h>
|
---|
4 |
|
---|
5 | // Adapted from VTK source code (see vtkMath.cxx)
|
---|
6 | // which seems to have been adapted directly from Numerical Recipes in C.
|
---|
7 |
|
---|
8 |
|
---|
9 | #define ROT(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);a[k][l]=h+s*(g-h*tau);
|
---|
10 |
|
---|
11 | #define MAX_ROTATIONS 60
|
---|
12 |
|
---|
13 |
|
---|
14 |
|
---|
15 | // Description:
|
---|
16 | // Jacobi iteration for the solution of eigenvectors/eigenvalues of a 3x3
|
---|
17 | // real symmetric matrix. Square 3x3 matrix a; output eigenvalues in w;
|
---|
18 | // and output eigenvectors in v. Resulting eigenvalues/vectors are sorted
|
---|
19 | // in decreasing order; eigenvectors are normalized.
|
---|
20 | //
|
---|
21 |
|
---|
22 | using namespace simplif;
|
---|
23 |
|
---|
24 | static
|
---|
25 | bool internal_jacobi(real a[3][3], real w[3], real v[3][3])
|
---|
26 | {
|
---|
27 | int i, j, k, iq, ip;
|
---|
28 | real tresh, theta, tau, t, sm, s, h, g, c;
|
---|
29 | real b[3], z[3], tmp;
|
---|
30 |
|
---|
31 | // initialize
|
---|
32 | for (ip=0; ip<3; ip++)
|
---|
33 | {
|
---|
34 | for (iq=0; iq<3; iq++) v[ip][iq] = 0.0;
|
---|
35 | v[ip][ip] = 1.0;
|
---|
36 | }
|
---|
37 | for (ip=0; ip<3; ip++)
|
---|
38 | {
|
---|
39 | b[ip] = w[ip] = a[ip][ip];
|
---|
40 | z[ip] = 0.0;
|
---|
41 | }
|
---|
42 |
|
---|
43 | // begin rotation sequence
|
---|
44 | for (i=0; i<MAX_ROTATIONS; i++)
|
---|
45 | {
|
---|
46 | sm = 0.0;
|
---|
47 | for (ip=0; ip<2; ip++)
|
---|
48 | {
|
---|
49 | for (iq=ip+1; iq<3; iq++) sm += fabs(a[ip][iq]);
|
---|
50 | }
|
---|
51 | if (sm == 0.0) break;
|
---|
52 |
|
---|
53 | if (i < 4) tresh = 0.2*sm/(9);
|
---|
54 | else tresh = 0.0;
|
---|
55 |
|
---|
56 | for (ip=0; ip<2; ip++)
|
---|
57 | {
|
---|
58 | for (iq=ip+1; iq<3; iq++)
|
---|
59 | {
|
---|
60 | g = 100.0*fabs(a[ip][iq]);
|
---|
61 | if (i > 4 && (fabs(w[ip])+g) == fabs(w[ip])
|
---|
62 | && (fabs(w[iq])+g) == fabs(w[iq]))
|
---|
63 | {
|
---|
64 | a[ip][iq] = 0.0;
|
---|
65 | }
|
---|
66 | else if (fabs(a[ip][iq]) > tresh)
|
---|
67 | {
|
---|
68 | h = w[iq] - w[ip];
|
---|
69 | if ( (fabs(h)+g) == fabs(h)) t = (a[ip][iq]) / h;
|
---|
70 | else
|
---|
71 | {
|
---|
72 | theta = 0.5*h / (a[ip][iq]);
|
---|
73 | t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta));
|
---|
74 | if (theta < 0.0) t = -t;
|
---|
75 | }
|
---|
76 | c = 1.0 / sqrt(1+t*t);
|
---|
77 | s = t*c;
|
---|
78 | tau = s/(1.0+c);
|
---|
79 | h = t*a[ip][iq];
|
---|
80 | z[ip] -= h;
|
---|
81 | z[iq] += h;
|
---|
82 | w[ip] -= h;
|
---|
83 | w[iq] += h;
|
---|
84 | a[ip][iq]=0.0;
|
---|
85 | for (j=0;j<ip-1;j++)
|
---|
86 | {
|
---|
87 | ROT(a,j,ip,j,iq)
|
---|
88 | }
|
---|
89 | for (j=ip+1;j<iq-1;j++)
|
---|
90 | {
|
---|
91 | ROT(a,ip,j,j,iq)
|
---|
92 | }
|
---|
93 | for (j=iq+1; j<3; j++)
|
---|
94 | {
|
---|
95 | ROT(a,ip,j,iq,j)
|
---|
96 | }
|
---|
97 | for (j=0; j<3; j++)
|
---|
98 | {
|
---|
99 | ROT(v,j,ip,j,iq)
|
---|
100 | }
|
---|
101 | }
|
---|
102 | }
|
---|
103 | }
|
---|
104 |
|
---|
105 | for (ip=0; ip<3; ip++)
|
---|
106 | {
|
---|
107 | b[ip] += z[ip];
|
---|
108 | w[ip] = b[ip];
|
---|
109 | z[ip] = 0.0;
|
---|
110 | }
|
---|
111 | }
|
---|
112 |
|
---|
113 | if ( i >= MAX_ROTATIONS )
|
---|
114 | {
|
---|
115 | std::cerr << "WARNING -- jacobi() -- Error computing eigenvalues." << std::endl;
|
---|
116 | return false;
|
---|
117 | }
|
---|
118 |
|
---|
119 | // sort eigenfunctions
|
---|
120 | for (j=0; j<3; j++)
|
---|
121 | {
|
---|
122 | k = j;
|
---|
123 | tmp = w[k];
|
---|
124 | for (i=j; i<3; i++)
|
---|
125 | {
|
---|
126 | if (w[i] >= tmp)
|
---|
127 | {
|
---|
128 | k = i;
|
---|
129 | tmp = w[k];
|
---|
130 | }
|
---|
131 | }
|
---|
132 | if (k != j)
|
---|
133 | {
|
---|
134 | w[k] = w[j];
|
---|
135 | w[j] = tmp;
|
---|
136 | for (i=0; i<3; i++)
|
---|
137 | {
|
---|
138 | tmp = v[i][j];
|
---|
139 | v[i][j] = v[i][k];
|
---|
140 | v[i][k] = tmp;
|
---|
141 | }
|
---|
142 | }
|
---|
143 | }
|
---|
144 | // insure eigenvector consistency (i.e., Jacobi can compute vectors that
|
---|
145 | // are negative of one another (.707,.707,0) and (-.707,-.707,0). This can
|
---|
146 | // reek havoc in hyperstreamline/other stuff. We will select the most
|
---|
147 | // positive eigenvector.
|
---|
148 | int numPos;
|
---|
149 | for (j=0; j<3; j++)
|
---|
150 | {
|
---|
151 | for (numPos=0, i=0; i<3; i++) if ( v[i][j] >= 0.0 ) numPos++;
|
---|
152 | if ( numPos < 2 ) for(i=0; i<3; i++) v[i][j] *= -1.0;
|
---|
153 | }
|
---|
154 |
|
---|
155 | return true;
|
---|
156 | }
|
---|
157 |
|
---|
158 | static
|
---|
159 | bool internal_jacobi4(real a[4][4], real w[4], real v[4][4])
|
---|
160 | {
|
---|
161 | int i, j, k, iq, ip;
|
---|
162 | real tresh, theta, tau, t, sm, s, h, g, c;
|
---|
163 | real b[4], z[4], tmp;
|
---|
164 |
|
---|
165 | // initialize
|
---|
166 | for (ip=0; ip<4; ip++)
|
---|
167 | {
|
---|
168 | for (iq=0; iq<4; iq++) v[ip][iq] = 0.0;
|
---|
169 | v[ip][ip] = 1.0;
|
---|
170 | }
|
---|
171 | for (ip=0; ip<4; ip++)
|
---|
172 | {
|
---|
173 | b[ip] = w[ip] = a[ip][ip];
|
---|
174 | z[ip] = 0.0;
|
---|
175 | }
|
---|
176 |
|
---|
177 | // begin rotation sequence
|
---|
178 | for (i=0; i<MAX_ROTATIONS; i++)
|
---|
179 | {
|
---|
180 | sm = 0.0;
|
---|
181 | for (ip=0; ip<3; ip++)
|
---|
182 | {
|
---|
183 | for (iq=ip+1; iq<4; iq++) sm += fabs(a[ip][iq]);
|
---|
184 | }
|
---|
185 | if (sm == 0.0) break;
|
---|
186 |
|
---|
187 | if (i < 4) tresh = 0.2*sm/(16);
|
---|
188 | else tresh = 0.0;
|
---|
189 |
|
---|
190 | for (ip=0; ip<3; ip++)
|
---|
191 | {
|
---|
192 | for (iq=ip+1; iq<4; iq++)
|
---|
193 | {
|
---|
194 | g = 100.0*fabs(a[ip][iq]);
|
---|
195 | if (i > 4 && (fabs(w[ip])+g) == fabs(w[ip])
|
---|
196 | && (fabs(w[iq])+g) == fabs(w[iq]))
|
---|
197 | {
|
---|
198 | a[ip][iq] = 0.0;
|
---|
199 | }
|
---|
200 | else if (fabs(a[ip][iq]) > tresh)
|
---|
201 | {
|
---|
202 | h = w[iq] - w[ip];
|
---|
203 | if ( (fabs(h)+g) == fabs(h)) t = (a[ip][iq]) / h;
|
---|
204 | else
|
---|
205 | {
|
---|
206 | theta = 0.5*h / (a[ip][iq]);
|
---|
207 | t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta));
|
---|
208 | if (theta < 0.0) t = -t;
|
---|
209 | }
|
---|
210 | c = 1.0 / sqrt(1+t*t);
|
---|
211 | s = t*c;
|
---|
212 | tau = s/(1.0+c);
|
---|
213 | h = t*a[ip][iq];
|
---|
214 | z[ip] -= h;
|
---|
215 | z[iq] += h;
|
---|
216 | w[ip] -= h;
|
---|
217 | w[iq] += h;
|
---|
218 | a[ip][iq]=0.0;
|
---|
219 | for (j=0;j<ip-1;j++)
|
---|
220 | {
|
---|
221 | ROT(a,j,ip,j,iq)
|
---|
222 | }
|
---|
223 | for (j=ip+1;j<iq-1;j++)
|
---|
224 | {
|
---|
225 | ROT(a,ip,j,j,iq)
|
---|
226 | }
|
---|
227 | for (j=iq+1; j<4; j++)
|
---|
228 | {
|
---|
229 | ROT(a,ip,j,iq,j)
|
---|
230 | }
|
---|
231 | for (j=0; j<4; j++)
|
---|
232 | {
|
---|
233 | ROT(v,j,ip,j,iq)
|
---|
234 | }
|
---|
235 | }
|
---|
236 | }
|
---|
237 | }
|
---|
238 |
|
---|
239 | for (ip=0; ip<4; ip++)
|
---|
240 | {
|
---|
241 | b[ip] += z[ip];
|
---|
242 | w[ip] = b[ip];
|
---|
243 | z[ip] = 0.0;
|
---|
244 | }
|
---|
245 | }
|
---|
246 |
|
---|
247 | if ( i >= MAX_ROTATIONS )
|
---|
248 | {
|
---|
249 | std::cerr << "WARNING -- jacobi() -- Error computing eigenvalues." << std::endl;
|
---|
250 | return false;
|
---|
251 | }
|
---|
252 |
|
---|
253 | // sort eigenfunctions
|
---|
254 | for (j=0; j<4; j++)
|
---|
255 | {
|
---|
256 | k = j;
|
---|
257 | tmp = w[k];
|
---|
258 | for (i=j; i<4; i++)
|
---|
259 | {
|
---|
260 | if (w[i] >= tmp)
|
---|
261 | {
|
---|
262 | k = i;
|
---|
263 | tmp = w[k];
|
---|
264 | }
|
---|
265 | }
|
---|
266 | if (k != j)
|
---|
267 | {
|
---|
268 | w[k] = w[j];
|
---|
269 | w[j] = tmp;
|
---|
270 | for (i=0; i<4; i++)
|
---|
271 | {
|
---|
272 | tmp = v[i][j];
|
---|
273 | v[i][j] = v[i][k];
|
---|
274 | v[i][k] = tmp;
|
---|
275 | }
|
---|
276 | }
|
---|
277 | }
|
---|
278 | // insure eigenvector consistency (i.e., Jacobi can compute vectors that
|
---|
279 | // are negative of one another (.707,.707,0) and (-.707,-.707,0). This can
|
---|
280 | // reek havoc in hyperstreamline/other stuff. We will select the most
|
---|
281 | // positive eigenvector.
|
---|
282 | int numPos;
|
---|
283 | for (j=0; j<4; j++)
|
---|
284 | {
|
---|
285 | for (numPos=0, i=0; i<4; i++) if ( v[i][j] >= 0.0 ) numPos++;
|
---|
286 | if ( numPos < 3 ) for(i=0; i<4; i++) v[i][j] *= -1.0;
|
---|
287 | }
|
---|
288 |
|
---|
289 | return true;
|
---|
290 | }
|
---|
291 |
|
---|
292 |
|
---|
293 | #undef ROT
|
---|
294 | #undef MAX_ROTATIONS
|
---|
295 |
|
---|
296 |
|
---|
297 | bool jacobi(const Mat3& m, Vec3& eig_vals, Vec3 eig_vecs[3])
|
---|
298 | {
|
---|
299 | real a[3][3], w[3], v[3][3];
|
---|
300 | int i,j;
|
---|
301 |
|
---|
302 | for(i=0;i<3;i++) for(j=0;j<3;j++) a[i][j] = m(i,j);
|
---|
303 |
|
---|
304 | bool result = internal_jacobi(a, w, v);
|
---|
305 | if( result )
|
---|
306 | {
|
---|
307 | for(i=0;i<3;i++) eig_vals[i] = w[i];
|
---|
308 |
|
---|
309 | for(i=0;i<3;i++) for(j=0;j<3;j++) eig_vecs[i][j] = v[j][i];
|
---|
310 | }
|
---|
311 |
|
---|
312 | return result;
|
---|
313 | }
|
---|
314 |
|
---|
315 | bool jacobi(const Mat4& m, Vec4& eig_vals, Vec4 eig_vecs[4])
|
---|
316 | {
|
---|
317 | real a[4][4], w[4], v[4][4];
|
---|
318 | int i,j;
|
---|
319 |
|
---|
320 | for(i=0;i<4;i++) for(j=0;j<4;j++) a[i][j] = m(i,j);
|
---|
321 |
|
---|
322 | bool result = internal_jacobi4(a, w, v);
|
---|
323 | if( result )
|
---|
324 | {
|
---|
325 | for(i=0;i<4;i++) eig_vals[i] = w[i];
|
---|
326 |
|
---|
327 | for(i=0;i<4;i++) for(j=0;j<4;j++) eig_vecs[i][j] = v[j][i];
|
---|
328 | }
|
---|
329 |
|
---|
330 | return result;
|
---|
331 | }
|
---|