home *** CD-ROM | disk | FTP | other *** search
/ Usenet 1994 October / usenetsourcesnewsgroupsinfomagicoctober1994disk2.iso / misc / volume34 / newmat06 / part06 / evalue.cxx < prev    next >
C/C++ Source or Header  |  1992-12-06  |  8KB  |  253 lines
  1. //$$evalue.cxx                           eigen-value decomposition
  2.  
  3. // Copyright (C) 1991,2: R B Davies
  4.  
  5. #define WANT_MATH
  6.  
  7. #include "include.h"
  8. #include "newmat.h"
  9. #include "newmatrm.h"
  10. #include "precisio.h"
  11.  
  12.  
  13. static void tred2(const SymmetricMatrix& A, DiagonalMatrix& D,
  14.    DiagonalMatrix& E, Matrix& Z)
  15. {
  16.    Tracer et("Evalue(tred2)");
  17.    Real tol =
  18.       FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
  19.    int n = A.Nrows(); Z.ReDimension(n,n); Z.Inject(A);
  20.    D.ReDimension(n); E.ReDimension(n);
  21.    Real* z = Z.Store();
  22.  
  23.    for (int i=n-1; i > 0; i--)                   // i=0 is excluded
  24.    {
  25.       Real f = Z.element(i,i-1); Real g = 0.0;
  26.       int k = i-1; Real* zik = z + i*n;
  27.       while (k--) g += square(*zik++);
  28.       Real h = g + square(f);
  29.       if (g <= tol) { E.element(i) = f; h = 0.0; }
  30.       else
  31.       {
  32.      g = sign(-sqrt(h), f); E.element(i) = g; h -= f*g;
  33.      Z.element(i,i-1) = f-g; f = 0.0;
  34.          Real* zji = z + i; Real* zij = z + i*n; Real* ej = E.Store();
  35.      for (int j=0; j<i; j++)
  36.      {
  37.         *zji = (*zij++)/h; g = 0.0;
  38.             Real* zjk = z + j*n; zik = z + i*n;
  39.             k = j; while (k--) g += *zjk++ * (*zik++);
  40.             k = i-j; while (k--) { g += *zjk * (*zik++); zjk += n; }
  41.         *ej++ = g/h; f += g * (*zji); zji += n;
  42.      }
  43.      Real hh = f / (h + h); zij = z + i*n; ej = E.Store();
  44.      for (j=0; j<i; j++)
  45.      {
  46.         f = *zij++; g = *ej - hh * f; *ej++ = g;
  47.             Real* zjk = z + j*n; Real* zik = z + i*n;
  48.             Real* ek = E.Store(); k = j+1;
  49.             while (k--)  *zjk++ -= ( f*(*ek++) + g*(*zik++) ); 
  50.      }
  51.       }
  52.       D.element(i) = h;
  53.    }
  54.  
  55.    D.element(0) = 0.0; E.element(0) = 0.0;
  56.    for (i=0; i<n; i++)
  57.    {
  58.       if (D.element(i) != 0.0)
  59.       {
  60.      for (int j=0; j<i; j++)
  61.      {
  62.         Real g = 0.0;
  63.             Real* zik = z + i*n; Real* zkj = z + j;
  64.             int k = i; while (k--) { g += *zik++ * (*zkj); zkj += n; }
  65.             Real* zki = z + i; zkj = z + j;
  66.             k = i; while (k--) { *zkj -= g * (*zki); zkj += n; zki += n; }
  67.      }
  68.       }
  69.       Real* zij = z + i*n; Real* zji = z + i;
  70.       int j = i; while (j--)  { *zij++ = 0.0; *zji = 0.0; zji += n; }
  71.       D.element(i) = *zij; *zij = 1.0;
  72.    }
  73. }
  74.  
  75. static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z)
  76. {
  77.    Tracer et("Evalue(tql2)");
  78.    Real eps = FloatingPointPrecision::Epsilon();
  79.    int n = D.Nrows(); Real* z = Z.Store();
  80.    for (int l=1; l<n; l++) E.element(l-1) = E.element(l);
  81.    Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
  82.    for (l=0; l<n; l++)
  83.    {
  84.       int i,j;
  85.       Real& dl = D.element(l); Real& el = E.element(l);
  86.       Real h = eps * ( fabs(dl) + fabs(el) );
  87.       if (b < h) b = h;
  88.       for (int m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
  89.       Boolean test = FALSE;
  90.       for (j=0; j<30; j++)
  91.       {
  92.      if (m==l) { test = TRUE; break; }
  93.      Real& dl1 = D.element(l+1);
  94.      Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
  95.      dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
  96.      Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
  97.  
  98.      p = D.element(m); Real c = 1.0; Real s = 0.0;
  99.      for (i=m-1; i>=l; i--)
  100.      {
  101.         Real ei = E.element(i); Real di = D.element(i);
  102.         Real& ei1 = E.element(i+1);
  103.         g = c * ei; h = c * p;
  104.         if ( fabs(p) >= fabs(ei))
  105.         {
  106.            c = ei / p; r = sqrt(c*c + 1.0);
  107.            ei1 = s*p*r; s = c/r; c = 1.0/r;
  108.         }
  109.         else
  110.         {
  111.            c = p / ei; r = sqrt(c*c + 1.0);
  112.            ei1 = s * ei * r; s = 1.0/r; c /= r;
  113.         }
  114.         p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
  115.  
  116.         Real* zki = z + i; Real* zki1 = zki + 1; int k = n;
  117.         while (k--)
  118.         {
  119.            h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h;
  120.            zki += n; zki1 += n;
  121.         }
  122.      }
  123.      el = s*p; dl = c*p;
  124.      if (fabs(el) <= b) { test = TRUE; break; }
  125.       }
  126.       if (!test) Throw ( ConvergenceException(D) );
  127.       dl += f;
  128.    }
  129.  
  130.    for (int i=0; i<n; i++)
  131.    {
  132.       int k = i; Real p = D.element(i);
  133.       for (int j=i+1; j<n; j++)
  134.          { if (D.element(j) < p) { k = j; p = D.element(j); } }
  135.       if (k != i)
  136.       {
  137.          D.element(k) = D.element(i); D.element(i) = p; int j = n;
  138.      Real* zji = z + i; Real* zjk = z + k;
  139.          while (j--) { p = *zji; *zji = *zjk; *zjk = p; zji += n; zjk += n; }
  140.       }
  141.    }
  142.  
  143. }
  144.  
  145. static void tred3(const SymmetricMatrix& X, DiagonalMatrix& D,
  146.    DiagonalMatrix& E, SymmetricMatrix& A)
  147. {
  148.    Tracer et("Evalue(tred3)");
  149.    Real tol =
  150.       FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
  151.    int n = X.Nrows(); A = X; D.ReDimension(n); E.ReDimension(n);
  152.    Real* ei = E.Store() + n;
  153.    for (int i = n-1; i >= 0; i--)
  154.    {
  155.       Real h = 0.0; Real f;
  156.       Real* d = D.Store(); Real* a = A.Store() + (i*(i+1))/2; int k = i;
  157.       while (k--) { f = *a++; *d++ = f; h += square(f); }
  158.       if (h <= tol) { *(--ei) = 0.0; h = 0.0; }
  159.       else
  160.       {
  161.      Real g = sign(-sqrt(h), f); *(--ei) = g; h -= f*g;
  162.          f -= g; *(d-1) = f; *(a-1) = f; f = 0.0;
  163.          Real* dj = D.Store(); Real* ej = E.Store();
  164.          for (int j = 0; j < i; j++)
  165.          {
  166.             Real* dk = D.Store(); Real* ak = A.Store()+(j*(j+1))/2;
  167.             Real g = 0.0; k = j;
  168.             while (k--)  g += *ak++ * *dk++;
  169.             k = i-j; int l = j; 
  170.             while (k--) { g += *ak * *dk++; ak += ++l; }
  171.         g /= h; *ej++ = g; f += g * *dj++;
  172.          }  
  173.      Real hh = f / (2 * h); Real* ak = A.Store();
  174.          dj = D.Store(); ej = E.Store();
  175.          for (j = 0; j < i; j++)
  176.          {
  177.         f = *dj++; g = *ej - hh * f; *ej++ = g;
  178.             Real* dk = D.Store(); Real* ek = E.Store(); k = j+1;
  179.         while (k--) { *ak++ -= (f * *ek++ + g * *dk++); }
  180.      }
  181.       }
  182.       *d = *a; *a = h;
  183.    }
  184. }
  185.  
  186. static void tql1(DiagonalMatrix& D, DiagonalMatrix& E)
  187. {
  188.    Tracer et("Evalue(tql1)");
  189.    Real eps = FloatingPointPrecision::Epsilon();
  190.    int n = D.Nrows();
  191.    for (int l=1; l<n; l++) E.element(l-1) = E.element(l);
  192.    Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
  193.    for (l=0; l<n; l++)
  194.    {
  195.       int i,j;
  196.       Real& dl = D.element(l); Real& el = E.element(l);
  197.       Real h = eps * ( fabs(dl) + fabs(el) );
  198.       if (b < h) b = h;
  199.       for (int m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
  200.       Boolean test = FALSE;
  201.       for (j=0; j<30; j++)
  202.       {
  203.          if (m==l) { test = TRUE; break; }
  204.          Real& dl1 = D.element(l+1);
  205.      Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
  206.      dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
  207.          Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
  208.  
  209.      p = D.element(m); Real c = 1.0; Real s = 0.0;
  210.      for (i=m-1; i>=l; i--)
  211.      {
  212.             Real ei = E.element(i); Real di = D.element(i);
  213.             Real& ei1 = E.element(i+1);
  214.         g = c * ei; h = c * p;
  215.         if ( fabs(p) >= fabs(ei))
  216.         {
  217.            c = ei / p; r = sqrt(c*c + 1.0); 
  218.                ei1 = s*p*r; s = c/r; c = 1.0/r;
  219.         }
  220.         else
  221.         {
  222.            c = p / ei; r = sqrt(c*c + 1.0);
  223.            ei1 = s * ei * r; s = 1.0/r; c /= r;
  224.         }
  225.         p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
  226.      }
  227.      el = s*p; dl = c*p;
  228.      if (fabs(el) <= b) { test = TRUE; break; }
  229.       }
  230.       if (!test) Throw ( ConvergenceException(D) );
  231.       Real p = dl + f;
  232.       test = FALSE;
  233.       for (i=l; i>0; i--)
  234.       {
  235.          if (p < D.element(i-1)) D.element(i) = D.element(i-1);
  236.          else { test = TRUE; break; }
  237.       }
  238.       if (!test) i=0;
  239.       D.element(i) = p;
  240.    }
  241. }
  242.  
  243. void EigenValues(const SymmetricMatrix& A, DiagonalMatrix& D, Matrix& Z)
  244. { DiagonalMatrix E; tred2(A, D, E, Z); tql2(D, E, Z); }
  245.  
  246. void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D)
  247. { DiagonalMatrix E; SymmetricMatrix A; tred3(X,D,E,A); tql1(D,E); }
  248.  
  249. void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D,
  250.    SymmetricMatrix& A)
  251. { DiagonalMatrix E; tred3(X,D,E,A); tql1(D,E); }
  252.  
  253.