1 | /*********************************************************** |
---|
2 | * This eigen() routine works for eigenvalue/vector analysis |
---|
3 | * for real general square matrix A |
---|
4 | * A will be destroyed |
---|
5 | * rr,ri are vectors containing eigenvalues |
---|
6 | * vr,vi are matrices containing (right) eigenvectors |
---|
7 | * |
---|
8 | * A*[vr+vi*i] = [vr+vi*i] * diag{rr+ri*i} |
---|
9 | * |
---|
10 | * Algorithm: Handbook for Automatic Computation, vol 2 |
---|
11 | * by Wilkinson and Reinsch, 1971 |
---|
12 | * most of source codes were taken from a public domain |
---|
13 | * solftware called MATCALC. |
---|
14 | * Credits: to the authors of MATCALC |
---|
15 | * |
---|
16 | * return -1 not converged |
---|
17 | * 0 no complex eigenvalues/vectors |
---|
18 | * 1 complex eigenvalues/vectors |
---|
19 | * Tianlin Wang at University of Illinois |
---|
20 | * Thu May 6 15:22:31 CDT 1993 |
---|
21 | ***************************************************************/ |
---|
22 | |
---|
23 | #include "utilities.h" |
---|
24 | #include "eigen.h" |
---|
25 | |
---|
26 | #define BASE 2 /* base of floating point arithmetic */ |
---|
27 | #define DIGITS 40 /* no. of digits to the base BASE in the fraction */ |
---|
28 | /* |
---|
29 | #define DIGITS 53 |
---|
30 | */ |
---|
31 | #define MAXITER 30 /* max2. no. of iterations to converge */ |
---|
32 | |
---|
33 | #define pos(i,j,n) ((i)*(n)+(j)) |
---|
34 | |
---|
35 | |
---|
36 | /* rr/vr : real parts of eigen values/vectors */ |
---|
37 | /* ri/vi : imaginary part s of eigen values/vectors */ |
---|
38 | |
---|
39 | |
---|
40 | int eigen(int job, double *A, int n, double *rr, double *ri, |
---|
41 | double *vr, double *vi, double *work) |
---|
42 | { |
---|
43 | /* job=0: eigen values only |
---|
44 | 1: both eigen values and eigen vectors |
---|
45 | double w[n*2]: work space |
---|
46 | */ |
---|
47 | int low,hi,i,j,k, it, istate=0; |
---|
48 | double tiny=sqrt(pow((double)BASE,(double)(1-DIGITS))), t; |
---|
49 | |
---|
50 | /* printf("EIGEN\n"); */ |
---|
51 | |
---|
52 | balance(A,n,&low,&hi,work); |
---|
53 | elemhess(job,A,n,low,hi,vr,vi, (int*)(work+n)); |
---|
54 | if (-1 == realeig(job,A,n,low,hi,rr,ri,vr,vi)) return (-1); |
---|
55 | if (job) unbalance(n,vr,vi,low,hi,work); |
---|
56 | |
---|
57 | /* sort, added by Z. Yang */ |
---|
58 | for (i=0; i<n; i++) { |
---|
59 | for (j=i+1,it=i,t=rr[i]; j<n; j++) |
---|
60 | if (t<rr[j]) { t=rr[j]; it=j; } |
---|
61 | rr[it]=rr[i]; rr[i]=t; |
---|
62 | t=ri[it]; ri[it]=ri[i]; ri[i]=t; |
---|
63 | for (k=0; k<n; k++) { |
---|
64 | t=vr[k*n+it]; vr[k*n+it]=vr[k*n+i]; vr[k*n+i]=t; |
---|
65 | t=vi[k*n+it]; vi[k*n+it]=vi[k*n+i]; vi[k*n+i]=t; |
---|
66 | } |
---|
67 | if (fabs(ri[i])>tiny) istate=1; |
---|
68 | } |
---|
69 | |
---|
70 | return (istate) ; |
---|
71 | } |
---|
72 | |
---|
73 | /* complex funcctions |
---|
74 | */ |
---|
75 | |
---|
76 | complex compl (double re,double im) |
---|
77 | { |
---|
78 | complex r; |
---|
79 | |
---|
80 | r.re = re; |
---|
81 | r.im = im; |
---|
82 | return(r); |
---|
83 | } |
---|
84 | |
---|
85 | complex _conj (complex a) |
---|
86 | { |
---|
87 | a.im = -a.im; |
---|
88 | return(a); |
---|
89 | } |
---|
90 | |
---|
91 | #define csize(a) (fabs(a.re)+fabs(a.im)) |
---|
92 | |
---|
93 | complex cplus (complex a, complex b) |
---|
94 | { |
---|
95 | complex c; |
---|
96 | c.re = a.re+b.re; |
---|
97 | c.im = a.im+b.im; |
---|
98 | return (c); |
---|
99 | } |
---|
100 | |
---|
101 | complex cminus (complex a, complex b) |
---|
102 | { |
---|
103 | complex c; |
---|
104 | c.re = a.re-b.re; |
---|
105 | c.im = a.im-b.im; |
---|
106 | return (c); |
---|
107 | } |
---|
108 | |
---|
109 | complex cby (complex a, complex b) |
---|
110 | { |
---|
111 | complex c; |
---|
112 | c.re = a.re*b.re-a.im*b.im ; |
---|
113 | c.im = a.re*b.im+a.im*b.re ; |
---|
114 | return (c); |
---|
115 | } |
---|
116 | |
---|
117 | complex cdiv (complex a,complex b) |
---|
118 | { |
---|
119 | double ratio, den; |
---|
120 | complex c; |
---|
121 | |
---|
122 | if (fabs(b.re) <= fabs(b.im)) { |
---|
123 | ratio = b.re / b.im; |
---|
124 | den = b.im * (1 + ratio * ratio); |
---|
125 | c.re = (a.re * ratio + a.im) / den; |
---|
126 | c.im = (a.im * ratio - a.re) / den; |
---|
127 | } |
---|
128 | else { |
---|
129 | ratio = b.im / b.re; |
---|
130 | den = b.re * (1 + ratio * ratio); |
---|
131 | c.re = (a.re + a.im * ratio) / den; |
---|
132 | c.im = (a.im - a.re * ratio) / den; |
---|
133 | } |
---|
134 | return(c); |
---|
135 | } |
---|
136 | |
---|
137 | /* complex local_cexp (complex a) */ |
---|
138 | /* { */ |
---|
139 | /* complex c; */ |
---|
140 | /* c.re = exp(a.re); */ |
---|
141 | /* if (fabs(a.im)==0) c.im = 0; */ |
---|
142 | /* else { c.im = c.re*sin(a.im); c.re*=cos(a.im); } */ |
---|
143 | /* return (c); */ |
---|
144 | /* } */ |
---|
145 | |
---|
146 | complex cfactor (complex x, double a) |
---|
147 | { |
---|
148 | complex c; |
---|
149 | c.re = a*x.re; |
---|
150 | c.im = a*x.im; |
---|
151 | return (c); |
---|
152 | } |
---|
153 | |
---|
154 | int cxtoy (complex *x, complex *y, int n) |
---|
155 | { |
---|
156 | int i; |
---|
157 | For (i,n) y[i]=x[i]; |
---|
158 | return (0); |
---|
159 | } |
---|
160 | |
---|
161 | int cmatby (complex *a, complex *b, complex *c, int n,int m,int k) |
---|
162 | /* a[n*m], b[m*k], c[n*k] ...... c = a*b |
---|
163 | */ |
---|
164 | { |
---|
165 | int i,j,i1; |
---|
166 | complex t; |
---|
167 | |
---|
168 | For (i,n) For(j,k) { |
---|
169 | for (i1=0,t=compl(0,0); i1<m; i1++) |
---|
170 | t = cplus (t, cby(a[i*m+i1],b[i1*k+j])); |
---|
171 | c[i*k+j] = t; |
---|
172 | } |
---|
173 | return (0); |
---|
174 | } |
---|
175 | |
---|
176 | int cmatinv( complex *x, int n, int m, double *space) |
---|
177 | { |
---|
178 | /* x[n*m] ... m>=n |
---|
179 | */ |
---|
180 | int i,j,k, *irow=(int*) space; |
---|
181 | double xmaxsize, ee=1e-20; |
---|
182 | complex t,t1; |
---|
183 | |
---|
184 | For(i,n) { |
---|
185 | xmaxsize = 0.; |
---|
186 | for (j=i; j<n; j++) { |
---|
187 | if ( xmaxsize < csize (x[j*m+i])) { |
---|
188 | xmaxsize = csize (x[j*m+i]); |
---|
189 | irow[i] = j; |
---|
190 | } |
---|
191 | } |
---|
192 | if (xmaxsize < ee) { |
---|
193 | printf("\nDet goes to zero at %8d!\t\n", i+1); |
---|
194 | return(-1); |
---|
195 | } |
---|
196 | if (irow[i] != i) { |
---|
197 | For(j,m) { |
---|
198 | t = x[i*m+j]; |
---|
199 | x[i*m+j] = x[irow[i]*m+j]; |
---|
200 | x[ irow[i]*m+j] = t; |
---|
201 | } |
---|
202 | } |
---|
203 | t = cdiv (compl(1,0), x[i*m+i]); |
---|
204 | For(j,n) { |
---|
205 | if (j == i) continue; |
---|
206 | t1 = cby (t,x[j*m+i]); |
---|
207 | For(k,m) x[j*m+k] = cminus (x[j*m+k], cby(t1,x[i*m+k])); |
---|
208 | x[j*m+i] = cfactor (t1, -1); |
---|
209 | } |
---|
210 | For(j,m) x[i*m+j] = cby (x[i*m+j], t); |
---|
211 | x[i*m+i] = t; |
---|
212 | } |
---|
213 | for (i=n-1; i>=0; i--) { |
---|
214 | if (irow[i] == i) continue; |
---|
215 | For(j,n) { |
---|
216 | t = x[j*m+i]; |
---|
217 | x[j*m+i] = x[j*m+irow[i]]; |
---|
218 | x[ j*m+irow[i]] = t; |
---|
219 | } |
---|
220 | } |
---|
221 | return (0); |
---|
222 | } |
---|
223 | |
---|
224 | |
---|
225 | void balance(double *mat, int n,int *low, int *hi, double *scale) |
---|
226 | { |
---|
227 | /* Balance a matrix for calculation of eigenvalues and eigenvectors |
---|
228 | */ |
---|
229 | double c,f,g,r,s; |
---|
230 | int i,j,k,l,done; |
---|
231 | /* search for rows isolating an eigenvalue and push them down */ |
---|
232 | for (k = n - 1; k >= 0; k--) { |
---|
233 | for (j = k; j >= 0; j--) { |
---|
234 | for (i = 0; i <= k; i++) { |
---|
235 | if (i != j && fabs(mat[pos(j,i,n)]) != 0) break; |
---|
236 | } |
---|
237 | |
---|
238 | if (i > k) { |
---|
239 | scale[k] = j; |
---|
240 | |
---|
241 | if (j != k) { |
---|
242 | for (i = 0; i <= k; i++) { |
---|
243 | c = mat[pos(i,j,n)]; |
---|
244 | mat[pos(i,j,n)] = mat[pos(i,k,n)]; |
---|
245 | mat[pos(i,k,n)] = c; |
---|
246 | } |
---|
247 | |
---|
248 | for (i = 0; i < n; i++) { |
---|
249 | c = mat[pos(j,i,n)]; |
---|
250 | mat[pos(j,i,n)] = mat[pos(k,i,n)]; |
---|
251 | mat[pos(k,i,n)] = c; |
---|
252 | } |
---|
253 | } |
---|
254 | break; |
---|
255 | } |
---|
256 | } |
---|
257 | if (j < 0) break; |
---|
258 | } |
---|
259 | |
---|
260 | /* search for columns isolating an eigenvalue and push them left */ |
---|
261 | |
---|
262 | for (l = 0; l <= k; l++) { |
---|
263 | for (j = l; j <= k; j++) { |
---|
264 | for (i = l; i <= k; i++) { |
---|
265 | if (i != j && fabs(mat[pos(i,j,n)]) != 0) break; |
---|
266 | } |
---|
267 | if (i > k) { |
---|
268 | scale[l] = j; |
---|
269 | if (j != l) { |
---|
270 | for (i = 0; i <= k; i++) { |
---|
271 | c = mat[pos(i,j,n)]; |
---|
272 | mat[pos(i,j,n)] = mat[pos(i,l,n)]; |
---|
273 | mat[pos(i,l,n)] = c; |
---|
274 | } |
---|
275 | |
---|
276 | for (i = l; i < n; i++) { |
---|
277 | c = mat[pos(j,i,n)]; |
---|
278 | mat[pos(j,i,n)] = mat[pos(l,i,n)]; |
---|
279 | mat[pos(l,i,n)] = c; |
---|
280 | } |
---|
281 | } |
---|
282 | |
---|
283 | break; |
---|
284 | } |
---|
285 | } |
---|
286 | |
---|
287 | if (j > k) break; |
---|
288 | } |
---|
289 | |
---|
290 | *hi = k; |
---|
291 | *low = l; |
---|
292 | |
---|
293 | /* balance the submatrix in rows l through k */ |
---|
294 | |
---|
295 | for (i = l; i <= k; i++) { |
---|
296 | scale[i] = 1; |
---|
297 | } |
---|
298 | |
---|
299 | do { |
---|
300 | for (done = 1,i = l; i <= k; i++) { |
---|
301 | for (c = 0,r = 0,j = l; j <= k; j++) { |
---|
302 | if (j != i) { |
---|
303 | c += fabs(mat[pos(j,i,n)]); |
---|
304 | r += fabs(mat[pos(i,j,n)]); |
---|
305 | } |
---|
306 | } |
---|
307 | |
---|
308 | if (c != 0 && r != 0) { |
---|
309 | g = r / BASE; |
---|
310 | f = 1; |
---|
311 | s = c + r; |
---|
312 | |
---|
313 | while (c < g) { |
---|
314 | f *= BASE; |
---|
315 | c *= BASE * BASE; |
---|
316 | } |
---|
317 | |
---|
318 | g = r * BASE; |
---|
319 | |
---|
320 | while (c >= g) { |
---|
321 | f /= BASE; |
---|
322 | c /= BASE * BASE; |
---|
323 | } |
---|
324 | |
---|
325 | if ((c + r) / f < 0.95 * s) { |
---|
326 | done = 0; |
---|
327 | g = 1 / f; |
---|
328 | scale[i] *= f; |
---|
329 | |
---|
330 | for (j = l; j < n; j++) { |
---|
331 | mat[pos(i,j,n)] *= g; |
---|
332 | } |
---|
333 | |
---|
334 | for (j = 0; j <= k; j++) { |
---|
335 | mat[pos(j,i,n)] *= f; |
---|
336 | } |
---|
337 | } |
---|
338 | } |
---|
339 | } |
---|
340 | } while (!done); |
---|
341 | } |
---|
342 | |
---|
343 | |
---|
344 | /* |
---|
345 | * Transform back eigenvectors of a balanced matrix |
---|
346 | * into the eigenvectors of the original matrix |
---|
347 | */ |
---|
348 | void unbalance(int n,double *vr,double *vi, int low, int hi, double *scale) |
---|
349 | { |
---|
350 | int i,j,k; |
---|
351 | double tmp; |
---|
352 | |
---|
353 | for (i = low; i <= hi; i++) { |
---|
354 | for (j = 0; j < n; j++) { |
---|
355 | vr[pos(i,j,n)] *= scale[i]; |
---|
356 | vi[pos(i,j,n)] *= scale[i]; |
---|
357 | } |
---|
358 | } |
---|
359 | |
---|
360 | for (i = low - 1; i >= 0; i--) { |
---|
361 | if ((k = (int)scale[i]) != i) { |
---|
362 | for (j = 0; j < n; j++) { |
---|
363 | tmp = vr[pos(i,j,n)]; |
---|
364 | vr[pos(i,j,n)] = vr[pos(k,j,n)]; |
---|
365 | vr[pos(k,j,n)] = tmp; |
---|
366 | |
---|
367 | tmp = vi[pos(i,j,n)]; |
---|
368 | vi[pos(i,j,n)] = vi[pos(k,j,n)]; |
---|
369 | vi[pos(k,j,n)] = tmp; |
---|
370 | } |
---|
371 | } |
---|
372 | } |
---|
373 | |
---|
374 | for (i = hi + 1; i < n; i++) { |
---|
375 | if ((k = (int)scale[i]) != i) { |
---|
376 | for (j = 0; j < n; j++) { |
---|
377 | tmp = vr[pos(i,j,n)]; |
---|
378 | vr[pos(i,j,n)] = vr[pos(k,j,n)]; |
---|
379 | vr[pos(k,j,n)] = tmp; |
---|
380 | |
---|
381 | tmp = vi[pos(i,j,n)]; |
---|
382 | vi[pos(i,j,n)] = vi[pos(k,j,n)]; |
---|
383 | vi[pos(k,j,n)] = tmp; |
---|
384 | } |
---|
385 | } |
---|
386 | } |
---|
387 | } |
---|
388 | |
---|
389 | /* |
---|
390 | * Reduce the submatrix in rows and columns low through hi of real matrix mat to |
---|
391 | * Hessenberg form by elementary similarity transformations |
---|
392 | */ |
---|
393 | void elemhess(int job,double *mat,int n,int low,int hi, double *vr, |
---|
394 | double *vi, int *work) |
---|
395 | { |
---|
396 | /* work[n] */ |
---|
397 | int i,j,m; |
---|
398 | double x,y; |
---|
399 | |
---|
400 | for (m = low + 1; m < hi; m++) { |
---|
401 | for (x = 0,i = m,j = m; j <= hi; j++) { |
---|
402 | if (fabs(mat[pos(j,m-1,n)]) > fabs(x)) { |
---|
403 | x = mat[pos(j,m-1,n)]; |
---|
404 | i = j; |
---|
405 | } |
---|
406 | } |
---|
407 | |
---|
408 | if ((work[m] = i) != m) { |
---|
409 | for (j = m - 1; j < n; j++) { |
---|
410 | y = mat[pos(i,j,n)]; |
---|
411 | mat[pos(i,j,n)] = mat[pos(m,j,n)]; |
---|
412 | mat[pos(m,j,n)] = y; |
---|
413 | } |
---|
414 | |
---|
415 | for (j = 0; j <= hi; j++) { |
---|
416 | y = mat[pos(j,i,n)]; |
---|
417 | mat[pos(j,i,n)] = mat[pos(j,m,n)]; |
---|
418 | mat[pos(j,m,n)] = y; |
---|
419 | } |
---|
420 | } |
---|
421 | |
---|
422 | if (x != 0) { |
---|
423 | for (i = m + 1; i <= hi; i++) { |
---|
424 | if ((y = mat[pos(i,m-1,n)]) != 0) { |
---|
425 | y = mat[pos(i,m-1,n)] = y / x; |
---|
426 | |
---|
427 | for (j = m; j < n; j++) { |
---|
428 | mat[pos(i,j,n)] -= y * mat[pos(m,j,n)]; |
---|
429 | } |
---|
430 | |
---|
431 | for (j = 0; j <= hi; j++) { |
---|
432 | mat[pos(j,m,n)] += y * mat[pos(j,i,n)]; |
---|
433 | } |
---|
434 | } |
---|
435 | } |
---|
436 | } |
---|
437 | } |
---|
438 | if (job) { |
---|
439 | for (i=0; i<n; i++) { |
---|
440 | for (j=0; j<n; j++) { |
---|
441 | vr[pos(i,j,n)] = 0.0; vi[pos(i,j,n)] = 0.0; |
---|
442 | } |
---|
443 | vr[pos(i,i,n)] = 1.0; |
---|
444 | } |
---|
445 | |
---|
446 | for (m = hi - 1; m > low; m--) { |
---|
447 | for (i = m + 1; i <= hi; i++) { |
---|
448 | vr[pos(i,m,n)] = mat[pos(i,m-1,n)]; |
---|
449 | } |
---|
450 | |
---|
451 | if ((i = work[m]) != m) { |
---|
452 | for (j = m; j <= hi; j++) { |
---|
453 | vr[pos(m,j,n)] = vr[pos(i,j,n)]; |
---|
454 | vr[pos(i,j,n)] = 0.0; |
---|
455 | } |
---|
456 | vr[pos(i,m,n)] = 1.0; |
---|
457 | } |
---|
458 | } |
---|
459 | } |
---|
460 | } |
---|
461 | |
---|
462 | /* |
---|
463 | * Calculate eigenvalues and eigenvectors of a real upper Hessenberg matrix |
---|
464 | * Return 1 if converges successfully and 0 otherwise |
---|
465 | */ |
---|
466 | |
---|
467 | int realeig(int job,double *mat,int n,int low, int hi, double *valr, |
---|
468 | double *vali, double *vr,double *vi) |
---|
469 | { |
---|
470 | complex v; |
---|
471 | double p=0,q=0,r=0,s=0,t,w,x,y,z=0,ra,sa,norm,eps; |
---|
472 | int niter,en,i,j,k,l,m; |
---|
473 | double precision = pow((double)BASE,(double)(1-DIGITS)); |
---|
474 | |
---|
475 | eps = precision; |
---|
476 | for (i=0; i<n; i++) { |
---|
477 | valr[i]=0.0; |
---|
478 | vali[i]=0.0; |
---|
479 | } |
---|
480 | /* store isolated roots and calculate norm */ |
---|
481 | for (norm = 0,i = 0; i < n; i++) { |
---|
482 | for (j = MAX(0,i-1); j < n; j++) { |
---|
483 | norm += fabs(mat[pos(i,j,n)]); |
---|
484 | } |
---|
485 | if (i < low || i > hi) valr[i] = mat[pos(i,i,n)]; |
---|
486 | } |
---|
487 | t = 0; |
---|
488 | en = hi; |
---|
489 | |
---|
490 | while (en >= low) { |
---|
491 | niter = 0; |
---|
492 | for (;;) { |
---|
493 | |
---|
494 | /* look for single small subdiagonal element */ |
---|
495 | |
---|
496 | for (l = en; l > low; l--) { |
---|
497 | s = fabs(mat[pos(l-1,l-1,n)]) + fabs(mat[pos(l,l,n)]); |
---|
498 | if (s == 0) s = norm; |
---|
499 | if (fabs(mat[pos(l,l-1,n)]) <= eps * s) break; |
---|
500 | } |
---|
501 | |
---|
502 | /* form shift */ |
---|
503 | |
---|
504 | x = mat[pos(en,en,n)]; |
---|
505 | |
---|
506 | if (l == en) { /* one root found */ |
---|
507 | valr[en] = x + t; |
---|
508 | if (job) mat[pos(en,en,n)] = x + t; |
---|
509 | en--; |
---|
510 | break; |
---|
511 | } |
---|
512 | |
---|
513 | y = mat[pos(en-1,en-1,n)]; |
---|
514 | w = mat[pos(en,en-1,n)] * mat[pos(en-1,en,n)]; |
---|
515 | |
---|
516 | if (l == en - 1) { /* two roots found */ |
---|
517 | p = (y - x) / 2; |
---|
518 | q = p * p + w; |
---|
519 | z = sqrt(fabs(q)); |
---|
520 | x += t; |
---|
521 | if (job) { |
---|
522 | mat[pos(en,en,n)] = x; |
---|
523 | mat[pos(en-1,en-1,n)] = y + t; |
---|
524 | } |
---|
525 | if (q < 0) { /* complex pair */ |
---|
526 | valr[en-1] = x+p; |
---|
527 | vali[en-1] = z; |
---|
528 | valr[en] = x+p; |
---|
529 | vali[en] = -z; |
---|
530 | } |
---|
531 | else { /* real pair */ |
---|
532 | z = (p < 0) ? p - z : p + z; |
---|
533 | valr[en-1] = x + z; |
---|
534 | valr[en] = (z == 0) ? x + z : x - w / z; |
---|
535 | if (job) { |
---|
536 | x = mat[pos(en,en-1,n)]; |
---|
537 | s = fabs(x) + fabs(z); |
---|
538 | p = x / s; |
---|
539 | q = z / s; |
---|
540 | r = sqrt(p*p+q*q); |
---|
541 | p /= r; |
---|
542 | q /= r; |
---|
543 | for (j = en - 1; j < n; j++) { |
---|
544 | z = mat[pos(en-1,j,n)]; |
---|
545 | mat[pos(en-1,j,n)] = q * z + p * |
---|
546 | mat[pos(en,j,n)]; |
---|
547 | mat[pos(en,j,n)] = q * mat[pos(en,j,n)] - p*z; |
---|
548 | } |
---|
549 | for (i = 0; i <= en; i++) { |
---|
550 | z = mat[pos(i,en-1,n)]; |
---|
551 | mat[pos(i,en-1,n)] = q * z + p * mat[pos(i,en,n)]; |
---|
552 | mat[pos(i,en,n)] = q * mat[pos(i,en,n)] - p*z; |
---|
553 | } |
---|
554 | for (i = low; i <= hi; i++) { |
---|
555 | z = vr[pos(i,en-1,n)]; |
---|
556 | vr[pos(i,en-1,n)] = q*z + p*vr[pos(i,en,n)]; |
---|
557 | vr[pos(i,en,n)] = q*vr[pos(i,en,n)] - p*z; |
---|
558 | } |
---|
559 | } |
---|
560 | } |
---|
561 | en -= 2; |
---|
562 | break; |
---|
563 | } |
---|
564 | if (niter == MAXITER) return(-1); |
---|
565 | if (niter != 0 && niter % 10 == 0) { |
---|
566 | t += x; |
---|
567 | for (i = low; i <= en; i++) mat[pos(i,i,n)] -= x; |
---|
568 | s = fabs(mat[pos(en,en-1,n)]) + fabs(mat[pos(en-1,en-2,n)]); |
---|
569 | x = y = 0.75 * s; |
---|
570 | w = -0.4375 * s * s; |
---|
571 | } |
---|
572 | niter++; |
---|
573 | /* look for two consecutive small subdiagonal elements */ |
---|
574 | for (m = en - 2; m >= l; m--) { |
---|
575 | z = mat[pos(m,m,n)]; |
---|
576 | r = x - z; |
---|
577 | s = y - z; |
---|
578 | p = (r * s - w) / mat[pos(m+1,m,n)] + mat[pos(m,m+1,n)]; |
---|
579 | q = mat[pos(m+1,m+1,n)] - z - r - s; |
---|
580 | r = mat[pos(m+2,m+1,n)]; |
---|
581 | s = fabs(p) + fabs(q) + fabs(r); |
---|
582 | p /= s; |
---|
583 | q /= s; |
---|
584 | r /= s; |
---|
585 | if (m == l || fabs(mat[pos(m,m-1,n)]) * (fabs(q)+fabs(r)) <= |
---|
586 | eps * (fabs(mat[pos(m-1,m-1,n)]) + fabs(z) + |
---|
587 | fabs(mat[pos(m+1,m+1,n)])) * fabs(p)) break; |
---|
588 | } |
---|
589 | for (i = m + 2; i <= en; i++) mat[pos(i,i-2,n)] = 0; |
---|
590 | for (i = m + 3; i <= en; i++) mat[pos(i,i-3,n)] = 0; |
---|
591 | /* double QR step involving rows l to en and columns m to en */ |
---|
592 | for (k = m; k < en; k++) { |
---|
593 | if (k != m) { |
---|
594 | p = mat[pos(k,k-1,n)]; |
---|
595 | q = mat[pos(k+1,k-1,n)]; |
---|
596 | r = (k == en - 1) ? 0 : mat[pos(k+2,k-1,n)]; |
---|
597 | if ((x = fabs(p) + fabs(q) + fabs(r)) == 0) continue; |
---|
598 | p /= x; |
---|
599 | q /= x; |
---|
600 | r /= x; |
---|
601 | } |
---|
602 | s = sqrt(p*p+q*q+r*r); |
---|
603 | if (p < 0) s = -s; |
---|
604 | if (k != m) { |
---|
605 | mat[pos(k,k-1,n)] = -s * x; |
---|
606 | } |
---|
607 | else if (l != m) { |
---|
608 | mat[pos(k,k-1,n)] = -mat[pos(k,k-1,n)]; |
---|
609 | } |
---|
610 | p += s; |
---|
611 | x = p / s; |
---|
612 | y = q / s; |
---|
613 | z = r / s; |
---|
614 | q /= p; |
---|
615 | r /= p; |
---|
616 | /* row modification */ |
---|
617 | for (j = k; j <= (!job ? en : n-1); j++){ |
---|
618 | p = mat[pos(k,j,n)] + q * mat[pos(k+1,j,n)]; |
---|
619 | if (k != en - 1) { |
---|
620 | p += r * mat[pos(k+2,j,n)]; |
---|
621 | mat[pos(k+2,j,n)] -= p * z; |
---|
622 | } |
---|
623 | mat[pos(k+1,j,n)] -= p * y; |
---|
624 | mat[pos(k,j,n)] -= p * x; |
---|
625 | } |
---|
626 | j = MIN(en,k+3); |
---|
627 | /* column modification */ |
---|
628 | for (i = (!job ? l : 0); i <= j; i++) { |
---|
629 | p = x * mat[pos(i,k,n)] + y * mat[pos(i,k+1,n)]; |
---|
630 | if (k != en - 1) { |
---|
631 | p += z * mat[pos(i,k+2,n)]; |
---|
632 | mat[pos(i,k+2,n)] -= p*r; |
---|
633 | } |
---|
634 | mat[pos(i,k+1,n)] -= p*q; |
---|
635 | mat[pos(i,k,n)] -= p; |
---|
636 | } |
---|
637 | if (job) { /* accumulate transformations */ |
---|
638 | for (i = low; i <= hi; i++) { |
---|
639 | p = x * vr[pos(i,k,n)] + y * vr[pos(i,k+1,n)]; |
---|
640 | if (k != en - 1) { |
---|
641 | p += z * vr[pos(i,k+2,n)]; |
---|
642 | vr[pos(i,k+2,n)] -= p*r; |
---|
643 | } |
---|
644 | vr[pos(i,k+1,n)] -= p*q; |
---|
645 | vr[pos(i,k,n)] -= p; |
---|
646 | } |
---|
647 | } |
---|
648 | } |
---|
649 | } |
---|
650 | } |
---|
651 | |
---|
652 | if (!job) return(0); |
---|
653 | if (norm != 0) { |
---|
654 | /* back substitute to find vectors of upper triangular form */ |
---|
655 | for (en = n-1; en >= 0; en--) { |
---|
656 | p = valr[en]; |
---|
657 | if ((q = vali[en]) < 0) { /* complex vector */ |
---|
658 | m = en - 1; |
---|
659 | if (fabs(mat[pos(en,en-1,n)]) > fabs(mat[pos(en-1,en,n)])) { |
---|
660 | mat[pos(en-1,en-1,n)] = q / mat[pos(en,en-1,n)]; |
---|
661 | mat[pos(en-1,en,n)] = (p - mat[pos(en,en,n)]) / |
---|
662 | mat[pos(en,en-1,n)]; |
---|
663 | } |
---|
664 | else { |
---|
665 | v = cdiv(compl(0.0,-mat[pos(en-1,en,n)]), |
---|
666 | compl(mat[pos(en-1,en-1,n)]-p,q)); |
---|
667 | mat[pos(en-1,en-1,n)] = v.re; |
---|
668 | mat[pos(en-1,en,n)] = v.im; |
---|
669 | } |
---|
670 | mat[pos(en,en-1,n)] = 0; |
---|
671 | mat[pos(en,en,n)] = 1; |
---|
672 | for (i = en - 2; i >= 0; i--) { |
---|
673 | w = mat[pos(i,i,n)] - p; |
---|
674 | ra = 0; |
---|
675 | sa = mat[pos(i,en,n)]; |
---|
676 | for (j = m; j < en; j++) { |
---|
677 | ra += mat[pos(i,j,n)] * mat[pos(j,en-1,n)]; |
---|
678 | sa += mat[pos(i,j,n)] * mat[pos(j,en,n)]; |
---|
679 | } |
---|
680 | if (vali[i] < 0) { |
---|
681 | z = w; |
---|
682 | r = ra; |
---|
683 | s = sa; |
---|
684 | } |
---|
685 | else { |
---|
686 | m = i; |
---|
687 | if (vali[i] == 0) { |
---|
688 | v = cdiv(compl(-ra,-sa),compl(w,q)); |
---|
689 | mat[pos(i,en-1,n)] = v.re; |
---|
690 | mat[pos(i,en,n)] = v.im; |
---|
691 | } |
---|
692 | else { /* solve complex equations */ |
---|
693 | x = mat[pos(i,i+1,n)]; |
---|
694 | y = mat[pos(i+1,i,n)]; |
---|
695 | v.re = (valr[i]- p)*(valr[i]-p) + vali[i]*vali[i] - q*q; |
---|
696 | v.im = (valr[i] - p)*2*q; |
---|
697 | if ((fabs(v.re) + fabs(v.im)) == 0) { |
---|
698 | v.re = eps * norm * (fabs(w) + |
---|
699 | fabs(q) + fabs(x) + fabs(y) + fabs(z)); |
---|
700 | } |
---|
701 | v = cdiv(compl(x*r-z*ra+q*sa,x*s-z*sa-q*ra),v); |
---|
702 | mat[pos(i,en-1,n)] = v.re; |
---|
703 | mat[pos(i,en,n)] = v.im; |
---|
704 | if (fabs(x) > fabs(z) + fabs(q)) { |
---|
705 | mat[pos(i+1,en-1,n)] = |
---|
706 | (-ra - w * mat[pos(i,en-1,n)] + |
---|
707 | q * mat[pos(i,en,n)]) / x; |
---|
708 | mat[pos(i+1,en,n)] = (-sa - w * mat[pos(i,en,n)] - |
---|
709 | q * mat[pos(i,en-1,n)]) / x; |
---|
710 | } |
---|
711 | else { |
---|
712 | v = cdiv(compl(-r-y*mat[pos(i,en-1,n)], |
---|
713 | -s-y*mat[pos(i,en,n)]),compl(z,q)); |
---|
714 | mat[pos(i+1,en-1,n)] = v.re; |
---|
715 | mat[pos(i+1,en,n)] = v.im; |
---|
716 | } |
---|
717 | } |
---|
718 | } |
---|
719 | } |
---|
720 | } |
---|
721 | else if (q == 0) { /* real vector */ |
---|
722 | m = en; |
---|
723 | mat[pos(en,en,n)] = 1; |
---|
724 | for (i = en - 1; i >= 0; i--) { |
---|
725 | w = mat[pos(i,i,n)] - p; |
---|
726 | r = mat[pos(i,en,n)]; |
---|
727 | for (j = m; j < en; j++) { |
---|
728 | r += mat[pos(i,j,n)] * mat[pos(j,en,n)]; |
---|
729 | } |
---|
730 | if (vali[i] < 0) { |
---|
731 | z = w; |
---|
732 | s = r; |
---|
733 | } |
---|
734 | else { |
---|
735 | m = i; |
---|
736 | if (vali[i] == 0) { |
---|
737 | if ((t = w) == 0) t = eps * norm; |
---|
738 | mat[pos(i,en,n)] = -r / t; |
---|
739 | } |
---|
740 | else { /* solve real equations */ |
---|
741 | x = mat[pos(i,i+1,n)]; |
---|
742 | y = mat[pos(i+1,i,n)]; |
---|
743 | q = (valr[i] - p) * (valr[i] - p) + vali[i]*vali[i]; |
---|
744 | t = (x * s - z * r) / q; |
---|
745 | mat[pos(i,en,n)] = t; |
---|
746 | if (fabs(x) <= fabs(z)) { |
---|
747 | mat[pos(i+1,en,n)] = (-s - y * t) / z; |
---|
748 | } |
---|
749 | else { |
---|
750 | mat[pos(i+1,en,n)] = (-r - w * t) / x; |
---|
751 | } |
---|
752 | } |
---|
753 | } |
---|
754 | } |
---|
755 | } |
---|
756 | } |
---|
757 | /* vectors of isolated roots */ |
---|
758 | for (i = 0; i < n; i++) { |
---|
759 | if (i < low || i > hi) { |
---|
760 | for (j = i; j < n; j++) { |
---|
761 | vr[pos(i,j,n)] = mat[pos(i,j,n)]; |
---|
762 | } |
---|
763 | } |
---|
764 | } |
---|
765 | /* multiply by transformation matrix */ |
---|
766 | |
---|
767 | for (j = n-1; j >= low; j--) { |
---|
768 | m = MIN(j,hi); |
---|
769 | for (i = low; i <= hi; i++) { |
---|
770 | for (z = 0,k = low; k <= m; k++) { |
---|
771 | z += vr[pos(i,k,n)] * mat[pos(k,j,n)]; |
---|
772 | } |
---|
773 | vr[pos(i,j,n)] = z; |
---|
774 | } |
---|
775 | } |
---|
776 | } |
---|
777 | /* rearrange complex eigenvectors */ |
---|
778 | for (j = 0; j < n; j++) { |
---|
779 | if (vali[j] != 0) { |
---|
780 | for (i = 0; i < n; i++) { |
---|
781 | vi[pos(i,j,n)] = vr[pos(i,j+1,n)]; |
---|
782 | vr[pos(i,j+1,n)] = vr[pos(i,j,n)]; |
---|
783 | vi[pos(i,j+1,n)] = -vi[pos(i,j,n)]; |
---|
784 | } |
---|
785 | j++; |
---|
786 | } |
---|
787 | } |
---|
788 | return(0); |
---|
789 | } |
---|