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