| 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 | } |
|---|