evalue.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00006 
00007 // Copyright (C) 1991,2,3,4: R B Davies
00008 
00009 #define WANT_MATH
00010 
00011 #include "include.h"
00012 #include "newmatap.h"
00013 #include "newmatrm.h"
00014 #include "precisio.h"
00015 
00016 #ifdef use_namespace
00017 namespace NEWMAT {
00018 #endif
00019 
00020 #ifdef DO_REPORT
00021 #define REPORT { static ExeCounter ExeCount(__LINE__,17); ++ExeCount; }
00022 #else
00023 #define REPORT {}
00024 #endif
00025 
00026 
00027 
00028 static void tred2(const SymmetricMatrix& A, DiagonalMatrix& D,
00029    DiagonalMatrix& E, Matrix& Z)
00030 {
00031    Tracer et("Evalue(tred2)");
00032    REPORT
00033    Real tol =
00034       FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
00035    int n = A.Nrows(); Z.resize(n,n); Z.Inject(A);
00036    D.resize(n); E.resize(n);
00037    Real* z = Z.Store(); int i;
00038 
00039    for (i=n-1; i > 0; i--)                   // i=0 is excluded
00040    {
00041       Real f = Z.element(i,i-1); Real g = 0.0;
00042       int k = i-1; Real* zik = z + i*n;
00043       while (k--) g += square(*zik++);
00044       Real h = g + square(f);
00045       if (g <= tol) { REPORT E.element(i) = f; h = 0.0; }
00046       else
00047       {
00048          REPORT
00049          g = sign(-sqrt(h), f); E.element(i) = g; h -= f*g;
00050          Z.element(i,i-1) = f-g; f = 0.0;
00051          Real* zji = z + i; Real* zij = z + i*n; Real* ej = E.Store();
00052          int j;
00053          for (j=0; j<i; j++)
00054          {
00055             *zji = (*zij++)/h; g = 0.0;
00056             Real* zjk = z + j*n; zik = z + i*n;
00057             k = j; while (k--) g += *zjk++ * (*zik++);
00058             k = i-j;
00059             if (k) for(;;)
00060                { g += *zjk * (*zik++); if (!(--k)) break; zjk += n; }
00061             *ej++ = g/h; f += g * (*zji); zji += n;
00062          }
00063          Real hh = f / (h + h); zij = z + i*n; ej = E.Store();
00064          for (j=0; j<i; j++)
00065          {
00066             f = *zij++; g = *ej - hh * f; *ej++ = g;
00067             Real* zjk = z + j*n; Real* zik = z + i*n;
00068             Real* ek = E.Store(); k = j+1;
00069             while (k--)  *zjk++ -= ( f*(*ek++) + g*(*zik++) ); 
00070          }
00071       }
00072       D.element(i) = h;
00073    }
00074 
00075    D.element(0) = 0.0; E.element(0) = 0.0;
00076    for (i=0; i<n; i++)
00077    {
00078       if (D.element(i) != 0.0)
00079       {
00080          REPORT
00081          for (int j=0; j<i; j++)
00082          {
00083             Real g = 0.0;
00084             Real* zik = z + i*n; Real* zkj = z + j;
00085             int k = i;
00086             if (k) for (;;)
00087                { g += *zik++ * (*zkj); if (!(--k)) break; zkj += n; }
00088             Real* zki = z + i; zkj = z + j;
00089             k = i;
00090             if (k) for (;;)
00091                { *zkj -= g * (*zki); if (!(--k)) break; zkj += n; zki += n; }
00092          }
00093       }
00094       Real* zij = z + i*n; Real* zji = z + i;
00095       int j = i;
00096       if (j) for (;;)
00097          { *zij++ = 0.0; *zji = 0.0; if (!(--j)) break; zji += n; }
00098       D.element(i) = *zij; *zij = 1.0;
00099    }
00100 }
00101 
00102 static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z)
00103 {
00104    Tracer et("Evalue(tql2)");
00105    REPORT
00106    Real eps = FloatingPointPrecision::Epsilon();
00107    int n = D.Nrows(); Real* z = Z.Store(); int l;
00108    for (l=1; l<n; l++) E.element(l-1) = E.element(l);
00109    Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
00110    for (l=0; l<n; l++)
00111    {
00112       int i,j;
00113       Real& dl = D.element(l); Real& el = E.element(l);
00114       Real h = eps * ( fabs(dl) + fabs(el) );
00115       if (b < h) { REPORT b = h; }
00116       int m;
00117       for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
00118       bool test = false;
00119       for (j=0; j<30; j++)
00120       {
00121          if (m==l) { REPORT test = true; break; }
00122          Real& dl1 = D.element(l+1);
00123          Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
00124          dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
00125          Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
00126 
00127          p = D.element(m); Real c = 1.0; Real s = 0.0;
00128          for (i=m-1; i>=l; i--)
00129          {
00130             Real ei = E.element(i); Real di = D.element(i);
00131             Real& ei1 = E.element(i+1);
00132             g = c * ei; h = c * p;
00133             if ( fabs(p) >= fabs(ei))
00134             {
00135                REPORT
00136                c = ei / p; r = sqrt(c*c + 1.0);
00137                ei1 = s*p*r; s = c/r; c = 1.0/r;
00138             }
00139             else
00140             {
00141                REPORT
00142                c = p / ei; r = sqrt(c*c + 1.0);
00143                ei1 = s * ei * r; s = 1.0/r; c /= r;
00144             }
00145             p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
00146 
00147             Real* zki = z + i; Real* zki1 = zki + 1; int k = n;
00148             if (k) for (;;)
00149             {
00150                REPORT
00151                h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h;
00152                if (!(--k)) break;
00153                zki += n; zki1 += n;
00154             }
00155          }
00156          el = s*p; dl = c*p;
00157          if (fabs(el) <= b) { REPORT; test = true; break; }
00158       }
00159       if (!test) Throw ( ConvergenceException(D) );
00160       dl += f;
00161    }
00162 /*
00163    for (int i=0; i<n; i++)
00164    {
00165       int k = i; Real p = D.element(i);
00166       for (int j=i+1; j<n; j++)
00167          { if (D.element(j) < p) { k = j; p = D.element(j); } }
00168       if (k != i)
00169       {
00170          D.element(k) = D.element(i); D.element(i) = p; int j = n;
00171          Real* zji = z + i; Real* zjk = z + k;
00172          if (j) for(;;)
00173          {
00174             p = *zji; *zji = *zjk; *zjk = p;
00175             if (!(--j)) break;
00176             zji += n; zjk += n;
00177          }
00178       }
00179    }
00180 */
00181 }
00182 
00183 static void tred3(const SymmetricMatrix& X, DiagonalMatrix& D,
00184    DiagonalMatrix& E, SymmetricMatrix& A)
00185 {
00186    Tracer et("Evalue(tred3)");
00187    REPORT
00188    Real tol =
00189       FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
00190    int n = X.Nrows(); A = X; D.resize(n); E.resize(n);
00191    Real* ei = E.Store() + n;
00192    for (int i = n-1; i >= 0; i--)
00193    {
00194       Real h = 0.0; Real f = - FloatingPointPrecision::Maximum();
00195       Real* d = D.Store(); Real* a = A.Store() + (i*(i+1))/2; int k = i;
00196       while (k--) { f = *a++; *d++ = f; h += square(f); }
00197       if (h <= tol) { REPORT *(--ei) = 0.0; h = 0.0; }
00198       else
00199       {
00200          REPORT
00201          Real g = sign(-sqrt(h), f); *(--ei) = g; h -= f*g;
00202          f -= g; *(d-1) = f; *(a-1) = f; f = 0.0;
00203          Real* dj = D.Store(); Real* ej = E.Store(); int j;
00204          for (j = 0; j < i; j++)
00205          {
00206             Real* dk = D.Store(); Real* ak = A.Store()+(j*(j+1))/2;
00207             Real g = 0.0; k = j;
00208             while (k--)  g += *ak++ * *dk++;
00209             k = i-j; int l = j; 
00210             if (k) for (;;) { g += *ak * *dk++; if (!(--k)) break; ak += ++l; }
00211             g /= h; *ej++ = g; f += g * *dj++;
00212          }  
00213          Real hh = f / (2 * h); Real* ak = A.Store();
00214          dj = D.Store(); ej = E.Store();
00215          for (j = 0; j < i; j++)
00216          {
00217             f = *dj++; g = *ej - hh * f; *ej++ = g;
00218             Real* dk = D.Store(); Real* ek = E.Store(); k = j+1;
00219             while (k--) { *ak++ -= (f * *ek++ + g * *dk++); }
00220          }
00221       }
00222       *d = *a; *a = h;
00223    }
00224 }
00225 
00226 static void tql1(DiagonalMatrix& D, DiagonalMatrix& E)
00227 {
00228    Tracer et("Evalue(tql1)");
00229    REPORT
00230    Real eps = FloatingPointPrecision::Epsilon();
00231    int n = D.Nrows(); int l;
00232    for (l=1; l<n; l++) E.element(l-1) = E.element(l);
00233    Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
00234    for (l=0; l<n; l++)
00235    {
00236       int i,j;
00237       Real& dl = D.element(l); Real& el = E.element(l);
00238       Real h = eps * ( fabs(dl) + fabs(el) );
00239       if (b < h) b = h;
00240       int m;
00241       for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
00242       bool test = false;
00243       for (j=0; j<30; j++)
00244       {
00245          if (m==l) { REPORT test = true; break; }
00246          Real& dl1 = D.element(l+1);
00247          Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
00248          dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
00249          Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
00250 
00251          p = D.element(m); Real c = 1.0; Real s = 0.0;
00252          for (i=m-1; i>=l; i--)
00253          {
00254             Real ei = E.element(i); Real di = D.element(i);
00255             Real& ei1 = E.element(i+1);
00256             g = c * ei; h = c * p;
00257             if ( fabs(p) >= fabs(ei))
00258             {
00259                REPORT
00260                c = ei / p; r = sqrt(c*c + 1.0);
00261                ei1 = s*p*r; s = c/r; c = 1.0/r;
00262             }
00263             else
00264             {
00265                REPORT
00266                c = p / ei; r = sqrt(c*c + 1.0);
00267                ei1 = s * ei * r; s = 1.0/r; c /= r;
00268             }
00269             p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
00270          }
00271          el = s*p; dl = c*p;
00272          if (fabs(el) <= b) { REPORT test = true; break; }
00273       }
00274       if (!test) Throw ( ConvergenceException(D) );
00275       Real p = dl + f;
00276       test = false;
00277       for (i=l; i>0; i--)
00278       {
00279          if (p < D.element(i-1)) { REPORT D.element(i) = D.element(i-1); }
00280          else { REPORT test = true; break; }
00281       }
00282       if (!test) i=0;
00283       D.element(i) = p;
00284    }
00285 }
00286 
00287 void eigenvalues(const SymmetricMatrix& A, DiagonalMatrix& D, Matrix& Z)
00288 { REPORT DiagonalMatrix E; tred2(A, D, E, Z); tql2(D, E, Z); SortSV(D,Z,true); }
00289 
00290 void eigenvalues(const SymmetricMatrix& X, DiagonalMatrix& D)
00291 { REPORT DiagonalMatrix E; SymmetricMatrix A; tred3(X,D,E,A); tql1(D,E); }
00292 
00293 void eigenvalues(const SymmetricMatrix& X, DiagonalMatrix& D,
00294    SymmetricMatrix& A)
00295 { REPORT DiagonalMatrix E; tred3(X,D,E,A); tql1(D,E); }
00296 
00297 
00298 #ifdef use_namespace
00299 }
00300 #endif
00301 
00303 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:53