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