source: trunk/GDE/PHYML20130708/phyml/src/eigen.c

Last change on this file was 10307, checked in by aboeckma, 11 years ago

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