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) real and imaginary 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 "eigen.h"
24
25
26 #define BASE 2 /* base of floating point arithmetic */
27
28 /* no. of digits to the base BASE in the fraction */
29 #define DIGITS 40
30 /*
31 #define DIGITS 53
32 */
33
34 #define MAXITER 10000 /* max2. no. of iterations to converge */
35
36 #define pos(i,j,n) ((i)*(n)+(j))
37
38
39 /* rr/vr : real parts of eigen values/vectors */
40 /* ri/vi : imaginary part s of eigen values/vectors */
41
42
Eigen(int job,phydbl * A,int n,phydbl * rr,phydbl * ri,phydbl * vr,phydbl * vi,phydbl * work)43 int Eigen(int job, phydbl *A, int n, phydbl *rr, phydbl *ri,
44 phydbl *vr, phydbl *vi, phydbl *work)
45 {
46 /* job=0: eigen values only
47 1: both eigen values and eigen vectors
48 phydbl w[n*2]: work space
49 */
50 int low,hi,i,j,k, it, istate=0;
51 phydbl tiny, t;
52
53 tiny=SQRT(POW((phydbl)BASE,(phydbl)(1-(int)DIGITS)));
54 /* tiny=FLT_MIN; */
55 /* tiny = SMALL; */
56
57 balance(A,n,&low,&hi,work);
58 elemhess(job,A,n,low,hi,vr,vi,(int*)(work+n));
59 if (-1 == realeig(job,A,n,low,hi,rr,ri,vr,vi)) return (-1);
60 if (job) unbalance(n,vr,vi,low,hi,work);
61
62 /* sort, added by Z. Yang */
63 for (i=0; i<n; ++i)
64 {
65 for (j=i+1,it=i,t=rr[i]; j<n; ++j)
66 if (t<rr[j])
67 {
68 t=rr[j];
69 it=j;
70 }
71 rr[it]=rr[i];
72 rr[i]=t;
73 t=ri[it];
74 ri[it]=ri[i];
75 ri[i]=t;
76
77 for (k=0; k<n; ++k)
78 {
79 t=vr[k*n+it];
80 vr[k*n+it]=vr[k*n+i];
81 vr[k*n+i]=t;
82
83 t=vi[k*n+it];
84 vi[k*n+it]=vi[k*n+i];
85 vi[k*n+i]=t;
86 }
87 if (fabs(ri[i])>tiny) istate=1;
88 }
89
90 return (istate) ;
91 }
92
93 /* complex functions
94 */
95
compl(phydbl re,phydbl im)96 complex compl (phydbl re,phydbl im)
97 {
98 complex r;
99
100 r.re = re;
101 r.im = im;
102 return(r);
103 }
104
_conj(complex a)105 complex _conj (complex a)
106 {
107 a.im = -a.im;
108 return(a);
109 }
110
111
cplus(complex a,complex b)112 complex cplus (complex a, complex b)
113 {
114 complex c;
115 c.re = a.re+b.re;
116 c.im = a.im+b.im;
117 return (c);
118 }
119
cminus(complex a,complex b)120 complex cminus (complex a, complex b)
121 {
122 complex c;
123 c.re = a.re-b.re;
124 c.im = a.im-b.im;
125 return (c);
126 }
127
cby(complex a,complex b)128 complex cby (complex a, complex b)
129 {
130 complex c;
131 c.re = a.re*b.re-a.im*b.im ;
132 c.im = a.re*b.im+a.im*b.re ;
133 return (c);
134 }
135
cdiv(complex a,complex b)136 complex cdiv (complex a,complex b)
137 {
138 phydbl ratio, den;
139 complex c;
140
141 if (fabs(b.re) <= fabs(b.im)) {
142 ratio = b.re / b.im;
143 den = b.im * (1 + ratio * ratio);
144 c.re = (a.re * ratio + a.im) / den;
145 c.im = (a.im * ratio - a.re) / den;
146 }
147 else {
148 ratio = b.im / b.re;
149 den = b.re * (1 + ratio * ratio);
150 c.re = (a.re + a.im * ratio) / den;
151 c.im = (a.im - a.re * ratio) / den;
152 }
153 return(c);
154 }
155
156 /* complex local_cexp (complex a) */
157 /* { */
158 /* complex c; */
159 /* c.re = exp(a.re); */
160 /* if (fabs(a.im)==0) c.im = 0; */
161 /* else { c.im = c.re*sin(a.im); c.re*=cos(a.im); } */
162 /* return (c); */
163 /* } */
164
cfactor(complex x,phydbl a)165 complex cfactor (complex x, phydbl a)
166 {
167 complex c;
168 c.re = a*x.re;
169 c.im = a*x.im;
170 return (c);
171 }
172
cxtoy(complex * x,complex * y,int n)173 int cxtoy (complex *x, complex *y, int n)
174 {
175 int i;
176 For (i,n) y[i]=x[i];
177 return (0);
178 }
179
cmatby(complex * a,complex * b,complex * c,int n,int m,int k)180 int cmatby (complex *a, complex *b, complex *c, int n,int m,int k)
181 /* a[n*m], b[m*k], c[n*k] ...... c = a*b
182 */
183 {
184 int i,j,i1;
185 complex t;
186
187 For (i,n) for(j=0;j<k;j++) {
188 for (i1=0,t=compl(0,0); i1<m; i1++)
189 t = cplus (t, cby(a[i*m+i1],b[i1*k+j]));
190 c[i*k+j] = t;
191 }
192 return (0);
193 }
194
cmatinv(complex * x,int n,int m,phydbl * space)195 int cmatinv( complex *x, int n, int m, phydbl *space)
196 {
197 /* x[n*m] ... m>=n
198 */
199 int i,j,k, *irow=(int*) space;
200 phydbl xmaxsize, ee=1e-20;
201 complex t,t1;
202
203 for(i=0;i<n;i++) {
204 xmaxsize = 0.;
205 for (j=i; j<n; j++) {
206 if ( xmaxsize < csize (x[j*m+i])) {
207 xmaxsize = csize (x[j*m+i]);
208 irow[i] = j;
209 }
210 }
211 if (xmaxsize < ee) {
212 PhyML_Printf("\nDet goes to zero at %8d!\t\n", i+1);
213 return(-1);
214 }
215 if (irow[i] != i) {
216 for(j=0;j<m;j++) {
217 t = x[i*m+j];
218 x[i*m+j] = x[irow[i]*m+j];
219 x[ irow[i]*m+j] = t;
220 }
221 }
222 t = cdiv (compl(1,0), x[i*m+i]);
223 for(j=0;j<n;j++) {
224 if (j == i) continue;
225 t1 = cby (t,x[j*m+i]);
226 for(k=0;k<m;k++) x[j*m+k] = cminus (x[j*m+k], cby(t1,x[i*m+k]));
227 x[j*m+i] = cfactor (t1, -1);
228 }
229 for(j=0;j<m;j++) x[i*m+j] = cby (x[i*m+j], t);
230 x[i*m+i] = t;
231 }
232 for (i=n-1; i>=0; i--) {
233 if (irow[i] == i) continue;
234 for(j=0;j<n;j++) {
235 t = x[j*m+i];
236 x[j*m+i] = x[j*m+irow[i]];
237 x[ j*m+irow[i]] = t;
238 }
239 }
240 return (0);
241 }
242
243
balance(phydbl * mat,int n,int * low,int * hi,phydbl * scale)244 void balance(phydbl *mat, int n,int *low, int *hi, phydbl *scale)
245 {
246 /* Balance a matrix for calculation of eigenvalues and eigenvectors
247 */
248 phydbl c,f,g,r,s;
249 int i,j,k,l,done;
250 /* search for rows isolating an eigenvalue and push them down */
251 for (k = n - 1; k >= 0; k--)
252 {
253 for (j = k; j >= 0; j--)
254 {
255 for (i = 0; i <= k; i++)
256 {
257 if (i != j && fabs(mat[pos(j,i,n)]) > SMALL) break;
258 }
259
260 if (i > k) {
261 scale[k] = j;
262
263 if (j != k) {
264 for (i = 0; i <= k; i++) {
265 c = mat[pos(i,j,n)];
266 mat[pos(i,j,n)] = mat[pos(i,k,n)];
267 mat[pos(i,k,n)] = c;
268 }
269
270 for (i = 0; i < n; i++) {
271 c = mat[pos(j,i,n)];
272 mat[pos(j,i,n)] = mat[pos(k,i,n)];
273 mat[pos(k,i,n)] = c;
274 }
275 }
276 break;
277 }
278 }
279 if (j < 0) break;
280 }
281
282 /* search for columns isolating an eigenvalue and push them left */
283
284 for (l = 0; l <= k; l++) {
285 for (j = l; j <= k; j++) {
286 for (i = l; i <= k; i++) {
287 if (i != j && fabs(mat[pos(i,j,n)]) > SMALL) break;
288 }
289 if (i > k) {
290 scale[l] = j;
291 if (j != l) {
292 for (i = 0; i <= k; i++) {
293 c = mat[pos(i,j,n)];
294 mat[pos(i,j,n)] = mat[pos(i,l,n)];
295 mat[pos(i,l,n)] = c;
296 }
297
298 for (i = l; i < n; i++) {
299 c = mat[pos(j,i,n)];
300 mat[pos(j,i,n)] = mat[pos(l,i,n)];
301 mat[pos(l,i,n)] = c;
302 }
303 }
304
305 break;
306 }
307 }
308
309 if (j > k) break;
310 }
311
312 *hi = k;
313 *low = l;
314
315 /* balance the submatrix in rows l through k */
316
317 for (i = l; i <= k; i++) {
318 scale[i] = 1;
319 }
320
321 do {
322 for (done = 1,i = l; i <= k; i++) {
323 for (c = 0,r = 0,j = l; j <= k; j++) {
324 if (j != i) {
325 c += fabs(mat[pos(j,i,n)]);
326 r += fabs(mat[pos(i,j,n)]);
327 }
328 }
329
330 /* if (c != 0 && r != 0) { */
331 if ((fabs(c) > SMALL || fabs(c) < -SMALL) && (fabs(r) > SMALL || fabs(r) < -SMALL))
332 {
333 g = r / BASE;
334 f = 1;
335 s = c + r;
336
337 while (c < g) {
338 f *= BASE;
339 c *= BASE * BASE;
340 }
341
342 g = r * BASE;
343
344 while (c >= g) {
345 f /= BASE;
346 c /= BASE * BASE;
347 }
348
349 if ((c + r) / f < 0.95 * s) {
350 done = 0;
351 g = 1 / f;
352 scale[i] *= f;
353
354 for (j = l; j < n; j++) {
355 mat[pos(i,j,n)] *= g;
356 }
357
358 for (j = 0; j <= k; j++) {
359 mat[pos(j,i,n)] *= f;
360 }
361 }
362 }
363 }
364 } while (!done);
365 }
366
367
368 /*
369 * Transform back eigenvectors of a balanced matrix
370 * into the eigenvectors of the original matrix
371 */
unbalance(int n,phydbl * vr,phydbl * vi,int low,int hi,phydbl * scale)372 void unbalance(int n,phydbl *vr,phydbl *vi, int low, int hi, phydbl *scale)
373 {
374 int i,j,k;
375 phydbl tmp;
376
377 for (i = low; i <= hi; i++) {
378 for (j = 0; j < n; j++) {
379 vr[pos(i,j,n)] *= scale[i];
380 vi[pos(i,j,n)] *= scale[i];
381 }
382 }
383
384 for (i = low - 1; i >= 0; i--) {
385 if ((k = (int)scale[i]) != i) {
386 for (j = 0; j < n; j++) {
387 tmp = vr[pos(i,j,n)];
388 vr[pos(i,j,n)] = vr[pos(k,j,n)];
389 vr[pos(k,j,n)] = tmp;
390
391 tmp = vi[pos(i,j,n)];
392 vi[pos(i,j,n)] = vi[pos(k,j,n)];
393 vi[pos(k,j,n)] = tmp;
394 }
395 }
396 }
397
398 for (i = hi + 1; i < n; i++) {
399 if ((k = (int)scale[i]) != i) {
400 for (j = 0; j < n; j++) {
401 tmp = vr[pos(i,j,n)];
402 vr[pos(i,j,n)] = vr[pos(k,j,n)];
403 vr[pos(k,j,n)] = tmp;
404
405 tmp = vi[pos(i,j,n)];
406 vi[pos(i,j,n)] = vi[pos(k,j,n)];
407 vi[pos(k,j,n)] = tmp;
408 }
409 }
410 }
411 }
412
413 /*
414 * Reduce the submatrix in rows and columns low through hi of real matrix mat to
415 * Hessenberg form by elementary similarity transformations
416 */
elemhess(int job,phydbl * mat,int n,int low,int hi,phydbl * vr,phydbl * vi,int * work)417 void elemhess(int job,phydbl *mat,int n,int low,int hi, phydbl *vr,
418 phydbl *vi, int *work)
419 {
420 /* work[n] */
421 unsigned int i,j,m;
422 phydbl x,y;
423
424 for (m = low + 1; m < hi; m++)
425 {
426 for (x = 0,i = m,j = m; j <= hi; j++)
427 {
428 if(fabs(mat[pos(j,m-1,n)]) > fabs(x))
429 {
430 x = mat[pos(j,m-1,n)];
431 i = j;
432 }
433 }
434
435 if ((work[m] = i) != m)
436 {
437 for (j = m - 1; j < n; j++)
438 {
439 y = mat[pos(i,j,n)];
440 mat[pos(i,j,n)] = mat[pos(m,j,n)];
441 mat[pos(m,j,n)] = y;
442 }
443
444 for (j = 0; j <= hi; j++)
445 {
446 y = mat[pos(j,i,n)];
447 mat[pos(j,i,n)] = mat[pos(j,m,n)];
448 mat[pos(j,m,n)] = y;
449 }
450 }
451
452 if (fabs(x) > SMALL)
453 {
454 for (i = m + 1; i <= hi; i++)
455 {
456 if (fabs(y = mat[pos(i,m-1,n)]) > SMALL)
457 {
458 y = mat[pos(i,m-1,n)] = y / x;
459
460 for (j = m; j < n; j++) {
461 mat[pos(i,j,n)] -= y * mat[pos(m,j,n)];
462 }
463
464 for (j = 0; j <= hi; j++) {
465 mat[pos(j,m,n)] += y * mat[pos(j,i,n)];
466 }
467 }
468 }
469 }
470 }
471
472 if (job)
473 {
474 for (i=0; i<n; i++) {
475 for (j=0; j<n; j++) {
476 vr[pos(i,j,n)] = 0.0;
477 vi[pos(i,j,n)] = 0.0;
478 }
479 vr[pos(i,i,n)] = 1.0;
480 }
481
482 for (m = hi - 1; m > low; m--)
483 {
484 for (i = m + 1; i <= hi; i++)
485 {
486 vr[pos(i,m,n)] = mat[pos(i,m-1,n)];
487 }
488
489 if ((i = work[m]) != m)
490 {
491 for (j = m; j <= hi; j++)
492 {
493 vr[pos(m,j,n)] = vr[pos(i,j,n)];
494 vr[pos(i,j,n)] = 0.0;
495 }
496 vr[pos(i,m,n)] = 1.0;
497 }
498 }
499 }
500 }
501
502 /*
503 * Calculate eigenvalues and eigenvectors of a real upper Hessenberg matrix
504 * Return 1 if converges successfully and 0 otherwise
505 */
506
realeig(int job,phydbl * mat,int n,int low,int hi,phydbl * valr,phydbl * vali,phydbl * vr,phydbl * vi)507 int realeig(int job,phydbl *mat,int n,int low, int hi, phydbl *valr,
508 phydbl *vali, phydbl *vr,phydbl *vi)
509 {
510 complex v;
511 phydbl p=.0,q=.0,r=.0,s=.0,t,w,x,y,z=0,ra,sa,norm,eps;
512 int niter,en,i,j,k,l,m;
513 phydbl precision = POW((phydbl)BASE,(phydbl)(1-(int)DIGITS)); // Won't pass testiphy...
514 /* phydbl precision = SMALL; */
515
516
517 eps = precision;
518 for (i=0; i<n; i++)
519 {
520 valr[i]=0.0;
521 vali[i]=0.0;
522 }
523
524 /* store isolated roots and calculate norm */
525 for (norm = 0,i = 0; i < n; i++) {
526 for (j = MAX(0,i-1); j < n; j++) {
527 norm += fabs(mat[pos(i,j,n)]);
528 }
529 if (i < low || i > hi) valr[i] = mat[pos(i,i,n)];
530 }
531 t = 0;
532 en = hi;
533
534 while (en >= low)
535 {
536 niter = 0;
537 for (;;) {
538
539 /* look for single small subdiagonal element */
540
541 for (l = en; l > low; l--)
542 {
543 s = fabs(mat[pos(l-1,l-1,n)]) + fabs(mat[pos(l,l,n)]);
544 if (fabs(s) < SMALL) s = norm;
545 if (fabs(mat[pos(l,l-1,n)]) <= eps * s) break;
546 }
547
548 /* form shift */
549
550 x = mat[pos(en,en,n)];
551
552 if (l == en)
553 { /* one root found */
554 valr[en] = x + t;
555 if (job) mat[pos(en,en,n)] = x + t;
556 en--;
557 break;
558 }
559
560 y = mat[pos(en-1,en-1,n)];
561 w = mat[pos(en,en-1,n)] * mat[pos(en-1,en,n)];
562
563 if (l == en - 1)
564 { /* two roots found */
565 p = (y - x) / 2;
566 q = p * p + w;
567 z = SQRT(fabs(q));
568 x += t;
569 if (job)
570 {
571 mat[pos(en,en,n)] = x;
572 mat[pos(en-1,en-1,n)] = y + t;
573 }
574 if (q < 0)
575 { /* complex pair */
576 valr[en-1] = x+p;
577 vali[en-1] = z;
578 valr[en] = x+p;
579 vali[en] = -z;
580 }
581 else
582 { /* real pair */
583 z = (p < 0) ? p - z : p + z;
584 valr[en-1] = x + z;
585 valr[en] = (fabs(z) < SMALL) ? x + z : x - w / z;
586 if (job) {
587 x = mat[pos(en,en-1,n)];
588 s = fabs(x) + fabs(z);
589 p = x / s;
590 q = z / s;
591 r = SQRT(p*p+q*q);
592 p /= r;
593 q /= r;
594 for (j = en - 1; j < n; j++)
595 {
596 z = mat[pos(en-1,j,n)];
597 mat[pos(en-1,j,n)] = q * z + p *
598 mat[pos(en,j,n)];
599 mat[pos(en,j,n)] = q * mat[pos(en,j,n)] - p*z;
600 }
601 for (i = 0; i <= en; i++)
602 {
603 z = mat[pos(i,en-1,n)];
604 mat[pos(i,en-1,n)] = q * z + p * mat[pos(i,en,n)];
605 mat[pos(i,en,n)] = q * mat[pos(i,en,n)] - p*z;
606 }
607 for (i = low; i <= hi; i++)
608 {
609 z = vr[pos(i,en-1,n)];
610 vr[pos(i,en-1,n)] = q*z + p*vr[pos(i,en,n)];
611 vr[pos(i,en,n)] = q*vr[pos(i,en,n)] - p*z;
612 }
613 }
614 }
615 en -= 2;
616 break;
617 }
618 if (niter == MAXITER) return(-1);
619 if (niter != 0 && niter % 10 == 0) {
620 t += x;
621 for (i = low; i <= en; i++) mat[pos(i,i,n)] -= x;
622 s = fabs(mat[pos(en,en-1,n)]) + fabs(mat[pos(en-1,en-2,n)]);
623 x = y = 0.75 * s;
624 w = -0.4375 * s * s;
625 }
626 niter++;
627 /* look for two consecutive small subdiagonal elements */
628 for (m = en - 2; m >= l; m--) {
629 z = mat[pos(m,m,n)];
630 r = x - z;
631 s = y - z;
632 p = (r * s - w) / mat[pos(m+1,m,n)] + mat[pos(m,m+1,n)];
633 q = mat[pos(m+1,m+1,n)] - z - r - s;
634 r = mat[pos(m+2,m+1,n)];
635 s = fabs(p) + fabs(q) + fabs(r);
636 p /= s;
637 q /= s;
638 r /= s;
639 if (m == l || fabs(mat[pos(m,m-1,n)]) * (fabs(q)+fabs(r)) <=
640 eps * (fabs(mat[pos(m-1,m-1,n)]) + fabs(z) +
641 fabs(mat[pos(m+1,m+1,n)])) * fabs(p)) break;
642 }
643 for (i = m + 2; i <= en; i++) mat[pos(i,i-2,n)] = 0;
644 for (i = m + 3; i <= en; i++) mat[pos(i,i-3,n)] = 0;
645 /* phydbl QR step involving rows l to en and columns m to en */
646 for (k = m; k < en; k++) {
647 if (k != m) {
648 p = mat[pos(k,k-1,n)];
649 q = mat[pos(k+1,k-1,n)];
650 r = (k == en - 1) ? 0 : mat[pos(k+2,k-1,n)];
651 if (fabs(x = fabs(p) + fabs(q) + fabs(r)) < SMALL) continue;
652 p /= x;
653 q /= x;
654 r /= x;
655 }
656 s = SQRT(p*p+q*q+r*r);
657 if (p < 0) s = -s;
658 if (k != m) {
659 mat[pos(k,k-1,n)] = -s * x;
660 }
661 else if (l != m) {
662 mat[pos(k,k-1,n)] = -mat[pos(k,k-1,n)];
663 }
664 p += s;
665 x = p / s;
666 y = q / s;
667 z = r / s;
668 q /= p;
669 r /= p;
670 /* row modification */
671 for (j = k; j <= (!job ? en : n-1); j++){
672 p = mat[pos(k,j,n)] + q * mat[pos(k+1,j,n)];
673 if (k != en - 1) {
674 p += r * mat[pos(k+2,j,n)];
675 mat[pos(k+2,j,n)] -= p * z;
676 }
677 mat[pos(k+1,j,n)] -= p * y;
678 mat[pos(k,j,n)] -= p * x;
679 }
680 j = MIN(en,k+3);
681 /* column modification */
682 for (i = (!job ? l : 0); i <= j; i++) {
683 p = x * mat[pos(i,k,n)] + y * mat[pos(i,k+1,n)];
684 if (k != en - 1) {
685 p += z * mat[pos(i,k+2,n)];
686 mat[pos(i,k+2,n)] -= p*r;
687 }
688 mat[pos(i,k+1,n)] -= p*q;
689 mat[pos(i,k,n)] -= p;
690 }
691 if (job) { /* accumulate transformations */
692 for (i = low; i <= hi; i++) {
693 p = x * vr[pos(i,k,n)] + y * vr[pos(i,k+1,n)];
694 if (k != en - 1) {
695 p += z * vr[pos(i,k+2,n)];
696 vr[pos(i,k+2,n)] -= p*r;
697 }
698 vr[pos(i,k+1,n)] -= p*q;
699 vr[pos(i,k,n)] -= p;
700 }
701 }
702 }
703 }
704 }
705
706 if (!job) return(0);
707 if (fabs(norm) > SMALL) {
708 /* back substitute to find vectors of upper triangular form */
709 for (en = n-1; en >= 0; en--) {
710 p = valr[en];
711 if ((q = vali[en]) < 0) { /* complex vector */
712 m = en - 1;
713 if (fabs(mat[pos(en,en-1,n)]) > fabs(mat[pos(en-1,en,n)])) {
714 mat[pos(en-1,en-1,n)] = q / mat[pos(en,en-1,n)];
715 mat[pos(en-1,en,n)] = (p - mat[pos(en,en,n)]) /
716 mat[pos(en,en-1,n)];
717 }
718 else {
719 v = cdiv(compl(0.0,-mat[pos(en-1,en,n)]),
720 compl(mat[pos(en-1,en-1,n)]-p,q));
721 mat[pos(en-1,en-1,n)] = v.re;
722 mat[pos(en-1,en,n)] = v.im;
723 }
724 mat[pos(en,en-1,n)] = 0;
725 mat[pos(en,en,n)] = 1;
726 for (i = en - 2; i >= 0; i--) {
727 w = mat[pos(i,i,n)] - p;
728 ra = 0;
729 sa = mat[pos(i,en,n)];
730 for (j = m; j < en; j++) {
731 ra += mat[pos(i,j,n)] * mat[pos(j,en-1,n)];
732 sa += mat[pos(i,j,n)] * mat[pos(j,en,n)];
733 }
734 if (vali[i] < 0) {
735 z = w;
736 r = ra;
737 s = sa;
738 }
739 else {
740 m = i;
741 if (fabs(vali[i]) < SMALL) {
742 v = cdiv(compl(-ra,-sa),compl(w,q));
743 mat[pos(i,en-1,n)] = v.re;
744 mat[pos(i,en,n)] = v.im;
745 }
746 else { /* solve complex equations */
747 x = mat[pos(i,i+1,n)];
748 y = mat[pos(i+1,i,n)];
749 v.re = (valr[i]- p)*(valr[i]-p) + vali[i]*vali[i] - q*q;
750 v.im = (valr[i] - p)*2*q;
751 if (fabs(v.re) + fabs(v.im) < SMALL) {
752 v.re = eps * norm * (fabs(w) +
753 fabs(q) + fabs(x) + fabs(y) + fabs(z));
754 }
755 v = cdiv(compl(x*r-z*ra+q*sa,x*s-z*sa-q*ra),v);
756 mat[pos(i,en-1,n)] = v.re;
757 mat[pos(i,en,n)] = v.im;
758 if (fabs(x) > fabs(z) + fabs(q)) {
759 mat[pos(i+1,en-1,n)] =
760 (-ra - w * mat[pos(i,en-1,n)] +
761 q * mat[pos(i,en,n)]) / x;
762 mat[pos(i+1,en,n)] = (-sa - w * mat[pos(i,en,n)] -
763 q * mat[pos(i,en-1,n)]) / x;
764 }
765 else {
766 v = cdiv(compl(-r-y*mat[pos(i,en-1,n)],
767 -s-y*mat[pos(i,en,n)]),compl(z,q));
768 mat[pos(i+1,en-1,n)] = v.re;
769 mat[pos(i+1,en,n)] = v.im;
770 }
771 }
772 }
773 }
774 }
775 else if (fabs(q) < SMALL) { /* real vector */
776 m = en;
777 mat[pos(en,en,n)] = 1;
778 for (i = en - 1; i >= 0; i--) {
779 w = mat[pos(i,i,n)] - p;
780 r = mat[pos(i,en,n)];
781 for (j = m; j < en; j++) {
782 r += mat[pos(i,j,n)] * mat[pos(j,en,n)];
783 }
784 if (vali[i] < 0) {
785 z = w;
786 s = r;
787 }
788 else {
789 m = i;
790 if (fabs(vali[i]) < SMALL) {
791 if (fabs(t = w) < SMALL) t = eps * norm;
792 mat[pos(i,en,n)] = -r / t;
793 }
794 else { /* solve real equations */
795 x = mat[pos(i,i+1,n)];
796 y = mat[pos(i+1,i,n)];
797 q = (valr[i] - p) * (valr[i] - p) + vali[i]*vali[i];
798 t = (x * s - z * r) / q;
799 mat[pos(i,en,n)] = t;
800 if (fabs(x) <= fabs(z)) {
801 mat[pos(i+1,en,n)] = (-s - y * t) / z;
802 }
803 else {
804 mat[pos(i+1,en,n)] = (-r - w * t) / x;
805 }
806 }
807 }
808 }
809 }
810 }
811 /* vectors of isolated roots */
812 for (i = 0; i < n; i++) {
813 if (i < low || i > hi) {
814 for (j = i; j < n; j++) {
815 vr[pos(i,j,n)] = mat[pos(i,j,n)];
816 }
817 }
818 }
819 /* multiply by transformation matrix */
820
821 for (j = n-1; j >= low; j--) {
822 m = MIN(j,hi);
823 for (i = low; i <= hi; i++) {
824 for (z = 0,k = low; k <= m; k++) {
825 z += vr[pos(i,k,n)] * mat[pos(k,j,n)];
826 }
827 vr[pos(i,j,n)] = z;
828 }
829 }
830 }
831 /* rearrange complex eigenvectors */
832 for (j = 0; j < n; j++) {
833 if (fabs(vali[j]) > SMALL) {
834 for (i = 0; i < n; i++) {
835 vi[pos(i,j,n)] = vr[pos(i,j+1,n)];
836 vr[pos(i,j+1,n)] = vr[pos(i,j,n)];
837 vi[pos(i,j+1,n)] = -vi[pos(i,j,n)];
838 }
839 j++;
840 }
841 }
842 return(0);
843 }
844
845
846 #define LUDCMP_TINY 1.0e-20;
847
ludcmp(phydbl ** a,int n,phydbl * d)848 int ludcmp(phydbl **a, int n, phydbl *d)
849 {
850 int i,imax,j,k;
851 phydbl big,dum,sum,temp;
852 phydbl *vv;
853
854 imax = 0;
855 vv = (phydbl *)mCalloc(n,sizeof(phydbl));
856
857 *d=1.0;
858 for (i=0;i<n;i++)
859 {
860 big=0.0;
861 for (j=0;j<n;j++)
862 if ((temp=fabs(a[i][j])) > big) big=temp;
863 if (fabs(big) < SMALL) Exit("\n. Singular matrix in routine LUDCMP");
864 vv[i]=1.0/big;
865 }
866 for (j=0;j<n;j++)
867 {
868 for (i=0;i<j;i++)
869 {
870 sum=a[i][j];
871 for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
872 a[i][j]=sum;
873 }
874 big=0.0;
875 for (i=j;i<n;i++) {
876 sum=a[i][j];
877 for (k=0;k<j;k++)
878 sum -= a[i][k]*a[k][j];
879 a[i][j]=sum;
880 if ((dum=vv[i]*fabs(sum)) >= big)
881 {
882 big=dum;
883 imax=i;
884 }
885 }
886 if (j != imax)
887 {
888 for (k=0;k<n;k++)
889 {
890 dum=a[imax][k];
891 a[imax][k]=a[j][k];
892 a[j][k]=dum;
893 }
894 *d = -(*d);
895 vv[imax]=vv[j];
896 }
897 if (fabs(a[j][j]) < SMALL) a[j][j]=LUDCMP_TINY;
898 if (j != n) {
899 dum=1.0/(a[j][j]);
900 for (i=j+1;i<n;i++) a[i][j] *= dum;
901 }
902 }
903 Free(vv);
904 return(0);
905 }
906
det(phydbl ** a,int n,phydbl * d)907 void det(phydbl **a, int n, phydbl *d)
908 {
909 int j;
910 ludcmp(a,n,d);
911 for(j=0;j<n;j++) *d *= a[j][j];
912 }
913
914
915
ludcmp_1D(phydbl * a,int n,phydbl * d)916 int ludcmp_1D(phydbl *a, int n, phydbl *d)
917 {
918 int i,imax,j,k;
919 phydbl big,dum,sum,temp;
920 phydbl *vv;
921
922 imax = 0;
923 vv = (phydbl *)mCalloc(n,sizeof(phydbl));
924
925 *d=1.0;
926 for (i=0;i<n;i++)
927 {
928 big=0.0;
929 for (j=0;j<n;j++)
930 if ((temp=fabs(a[i*n+j])) > big) big=temp;
931 if (fabs(big) < SMALL) Exit("\n. Singular matrix in routine LUDCMP");
932 vv[i]=1.0/big;
933 }
934 for (j=0;j<n;j++)
935 {
936 for (i=0;i<j;i++)
937 {
938 sum=a[i*n+j];
939 for (k=0;k<i;k++) sum -= a[i*n+k]*a[k*n+j];
940 a[i*n+j]=sum;
941 }
942 big=0.0;
943 for (i=j;i<n;i++) {
944 sum=a[i*n+j];
945 for (k=0;k<j;k++)
946 sum -= a[i*n+k]*a[k*n+j];
947 a[i*n+j]=sum;
948 if ((dum=vv[i]*fabs(sum)) >= big)
949 {
950 big=dum;
951 imax=i;
952 }
953 }
954 if (j != imax)
955 {
956 for (k=0;k<n;k++)
957 {
958 dum=a[imax*n+k];
959 a[imax*n+k]=a[j*n+k];
960 a[j*n+k]=dum;
961 }
962 *d = -(*d);
963 vv[imax]=vv[j];
964 }
965 if (fabs(a[j*n+j]) < SMALL) a[j*n+j]=LUDCMP_TINY;
966 if (j != n) {
967 dum=1.0/(a[j*n+j]);
968 for (i=j+1;i<n;i++) a[i*n+j] *= dum;
969 }
970 }
971 Free(vv);
972 return(0);
973 }
974
det_1D(phydbl * a,int n,phydbl * d)975 void det_1D(phydbl *a, int n, phydbl *d)
976 {
977 int j;
978 ludcmp_1D(a,n,d);
979 for(j=0;j<n;j++) *d *= a[j*n+j];
980 }
981
982 /* Find L such that L.L' = A */
Cholesky_Decomp(phydbl * A,int dim)983 phydbl *Cholesky_Decomp(phydbl *A, int dim)
984 {
985 int i,j,k;
986 phydbl sum;
987 phydbl *L;
988
989 L = (phydbl *)mCalloc(dim*dim,sizeof(phydbl));
990
991 for(i=0;i<dim;i++)
992 {
993 for(j=i;j<dim;j++)
994 {
995 sum = A[j*dim+i];
996 for(k=0;k<i;k++) sum -= L[i*dim+k] * L[j*dim+k];
997
998 if(i == j)
999 {
1000 if(sum < 1.E-20)
1001 {
1002 PhyML_Printf("\n== sum=%G i=%d j=%d",sum,i,j);
1003 PhyML_Printf("\n== Numerical precision issue detected...");
1004 PhyML_Printf("\n== Err in file %s at line %d\n\n",__FILE__,__LINE__);
1005 Warn_And_Exit("");
1006 }
1007 L[j*dim+i] = SQRT(sum);
1008 }
1009
1010 else L[j*dim+i] = sum / L[i*dim+i];
1011
1012 }
1013 }
1014
1015 return L;
1016 }
1017