00001
00002
00003
00004
00005
00006 #define WANT_MATH
00007
00008 #include "include.h"
00009 #include "newmat.h"
00010 #include "newmatrm.h"
00011 #include "precisio.h"
00012
00013 #ifdef use_namespace
00014 namespace NEWMAT {
00015 #endif
00016
00017 static void tred2(const SymmetricMatrix& A, DiagonalMatrix& D,
00018 DiagonalMatrix& E, Matrix& Z) {
00019 Tracer et("Evalue(tred2)");
00020 Real tol =
00021 FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
00022 int n = A.Nrows(); Z.ReSize(n,n); Z.Inject(A);
00023 D.ReSize(n); E.ReSize(n);
00024 Real* z = Z.Store(); int i;
00025
00026 for (i=n-1; i > 0; i--) {
00027 Real f = Z.element(i,i-1); Real g = 0.0;
00028 int k = i-1; Real* zik = z + i*n;
00029 while (k--) g += square(*zik++);
00030 Real h = g + square(f);
00031 if (g <= tol) { E.element(i) = f; h = 0.0; }
00032 else {
00033 g = sign(-sqrt(h), f); E.element(i) = g; h -= f*g;
00034 Z.element(i,i-1) = f-g; f = 0.0;
00035 Real* zji = z + i; Real* zij = z + i*n; Real* ej = E.Store();
00036 int j;
00037 for (j=0; j<i; j++) {
00038 *zji = (*zij++)/h; g = 0.0;
00039 Real* zjk = z + j*n; zik = z + i*n;
00040 k = j; while (k--) g += *zjk++ * (*zik++);
00041 k = i-j; while (k--) { g += *zjk * (*zik++); zjk += n; }
00042 *ej++ = g/h; f += g * (*zji); zji += n;
00043 }
00044 Real hh = f / (h + h); zij = z + i*n; ej = E.Store();
00045 for (j=0; j<i; j++) {
00046 f = *zij++; g = *ej - hh * f; *ej++ = g;
00047 Real* zjk = z + j*n; Real* zik = z + i*n;
00048 Real* ek = E.Store(); k = j+1;
00049 while (k--) *zjk++ -= ( f*(*ek++) + g*(*zik++) );
00050 }
00051 }
00052 D.element(i) = h;
00053 }
00054
00055 D.element(0) = 0.0; E.element(0) = 0.0;
00056 for (i=0; i<n; i++) {
00057 if (D.element(i) != 0.0) {
00058 for (int j=0; j<i; j++) {
00059 Real g = 0.0;
00060 Real* zik = z + i*n; Real* zkj = z + j;
00061 int k = i; while (k--) { g += *zik++ * (*zkj); zkj += n; }
00062 Real* zki = z + i; zkj = z + j;
00063 k = i; while (k--) { *zkj -= g * (*zki); zkj += n; zki += n; }
00064 }
00065 }
00066 Real* zij = z + i*n; Real* zji = z + i;
00067 int j = i; while (j--) { *zij++ = 0.0; *zji = 0.0; zji += n; }
00068 D.element(i) = *zij; *zij = 1.0;
00069 }
00070 }
00071
00072 static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z) {
00073 Tracer et("Evalue(tql2)");
00074 Real eps = FloatingPointPrecision::Epsilon();
00075 int n = D.Nrows(); Real* z = Z.Store(); int l;
00076 for (l=1; l<n; l++) E.element(l-1) = E.element(l);
00077 Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
00078 for (l=0; l<n; l++) {
00079 int i,j;
00080 Real& dl = D.element(l); Real& el = E.element(l);
00081 Real h = eps * ( fabs(dl) + fabs(el) );
00082 if (b < h) b = h;
00083 int m;
00084 for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
00085 bool test = false;
00086 for (j=0; j<30; j++) {
00087 if (m==l) { test = true; break; }
00088 Real& dl1 = D.element(l+1);
00089 Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
00090 dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
00091 Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
00092
00093 p = D.element(m); Real c = 1.0; Real s = 0.0;
00094 for (i=m-1; i>=l; i--) {
00095 Real ei = E.element(i); Real di = D.element(i);
00096 Real& ei1 = E.element(i+1);
00097 g = c * ei; h = c * p;
00098 if ( fabs(p) >= fabs(ei)) {
00099 c = ei / p; r = sqrt(c*c + 1.0);
00100 ei1 = s*p*r; s = c/r; c = 1.0/r;
00101 }
00102 else {
00103 c = p / ei; r = sqrt(c*c + 1.0);
00104 ei1 = s * ei * r; s = 1.0/r; c /= r;
00105 }
00106 p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
00107
00108 Real* zki = z + i; Real* zki1 = zki + 1; int k = n;
00109 while (k--) {
00110 h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h;
00111 zki += n; zki1 += n;
00112 }
00113 }
00114 el = s*p; dl = c*p;
00115 if (fabs(el) <= b) { test = true; break; }
00116 }
00117 if (!test) Throw ( ConvergenceException(D) );
00118 dl += f;
00119 }
00120
00121 for (int i=0; i<n; i++) {
00122 int k = i; Real p = D.element(i);
00123 for (int j=i+1; j<n; j++) {
00124 if (D.element(j) < p) {
00125 k = j; p = D.element(j);
00126 }
00127 }
00128 if (k != i) {
00129 D.element(k) = D.element(i); D.element(i) = p; int j = n;
00130 Real* zji = z + i; Real* zjk = z + k;
00131 while (j--) { p = *zji; *zji = *zjk; *zjk = p; zji += n; zjk += n; }
00132 }
00133 }
00134
00135 }
00136
00137 static void tred3(const SymmetricMatrix& X, DiagonalMatrix& D,
00138 DiagonalMatrix& E, SymmetricMatrix& A) {
00139 Tracer et("Evalue(tred3)");
00140 Real tol =
00141 FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
00142 int n = X.Nrows(); A = X; D.ReSize(n); E.ReSize(n);
00143 Real* ei = E.Store() + n;
00144 for (int i = n-1; i >= 0; i--) {
00145 Real h = 0.0; Real f;
00146 Real* d = D.Store(); Real* a = A.Store() + (i*(i+1))/2; int k = i;
00147 while (k--) { f = *a++; *d++ = f; h += square(f); }
00148 if (h <= tol) { *(--ei) = 0.0; h = 0.0; }
00149 else {
00150 Real g = sign(-sqrt(h), f); *(--ei) = g; h -= f*g;
00151 f -= g; *(d-1) = f; *(a-1) = f; f = 0.0;
00152 Real* dj = D.Store(); Real* ej = E.Store(); int j;
00153 for (j = 0; j < i; j++) {
00154 Real* dk = D.Store(); Real* ak = A.Store()+(j*(j+1))/2;
00155 Real g = 0.0; k = j;
00156 while (k--) g += *ak++ * *dk++;
00157 k = i-j; int l = j;
00158 while (k--) { g += *ak * *dk++; ak += ++l; }
00159 g /= h; *ej++ = g; f += g * *dj++;
00160 }
00161 Real hh = f / (2 * h); Real* ak = A.Store();
00162 dj = D.Store(); ej = E.Store();
00163 for (j = 0; j < i; j++) {
00164 f = *dj++; g = *ej - hh * f; *ej++ = g;
00165 Real* dk = D.Store(); Real* ek = E.Store(); k = j+1;
00166 while (k--) { *ak++ -= (f * *ek++ + g * *dk++); }
00167 }
00168 }
00169 *d = *a; *a = h;
00170 }
00171 }
00172
00173 static void tql1(DiagonalMatrix& D, DiagonalMatrix& E) {
00174 Tracer et("Evalue(tql1)");
00175 Real eps = FloatingPointPrecision::Epsilon();
00176 int n = D.Nrows(); int l;
00177 for (l=1; l<n; l++) E.element(l-1) = E.element(l);
00178 Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
00179 for (l=0; l<n; l++) {
00180 int i,j;
00181 Real& dl = D.element(l); Real& el = E.element(l);
00182 Real h = eps * ( fabs(dl) + fabs(el) );
00183 if (b < h) b = h;
00184 int m;
00185 for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
00186 bool test = false;
00187 for (j=0; j<30; j++) {
00188 if (m==l) { test = true; break; }
00189 Real& dl1 = D.element(l+1);
00190 Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
00191 dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
00192 Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
00193
00194 p = D.element(m); Real c = 1.0; Real s = 0.0;
00195 for (i=m-1; i>=l; i--) {
00196 Real ei = E.element(i); Real di = D.element(i);
00197 Real& ei1 = E.element(i+1);
00198 g = c * ei; h = c * p;
00199 if ( fabs(p) >= fabs(ei)) {
00200 c = ei / p; r = sqrt(c*c + 1.0);
00201 ei1 = s*p*r; s = c/r; c = 1.0/r;
00202 }
00203 else {
00204 c = p / ei; r = sqrt(c*c + 1.0);
00205 ei1 = s * ei * r; s = 1.0/r; c /= r;
00206 }
00207 p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
00208 }
00209 el = s*p; dl = c*p;
00210 if (fabs(el) <= b) { test = true; break; }
00211 }
00212 if (!test) Throw ( ConvergenceException(D) );
00213 Real p = dl + f;
00214 test = false;
00215 for (i=l; i>0; i--) {
00216 if (p < D.element(i-1)) D.element(i) = D.element(i-1);
00217 else { test = true; break; }
00218 }
00219 if (!test) i=0;
00220 D.element(i) = p;
00221 }
00222 }
00223
00224 void EigenValues(const SymmetricMatrix& A, DiagonalMatrix& D, Matrix& Z)
00225 { DiagonalMatrix E; tred2(A, D, E, Z); tql2(D, E, Z); }
00226
00227 void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D)
00228 { DiagonalMatrix E; SymmetricMatrix A; tred3(X,D,E,A); tql1(D,E); }
00229
00230 void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D,
00231 SymmetricMatrix& A)
00232 { DiagonalMatrix E; tred3(X,D,E,A); tql1(D,E); }
00233
00234 #ifdef use_namespace
00235 }
00236 #endif
