static void tql1(DiagonalMatrix& D, DiagonalMatrix& E) { Tracer et("Evalue(tql1)"); Real eps = FloatingPointPrecision::Epsilon(); int n = D.Nrows(); int l; for (l=1; l<n; l++) E.element(l-1) = E.element(l); Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0; for (l=0; l<n; l++) { int i,j; Real& dl = D.element(l); Real& el = E.element(l); Real h = eps * ( fabs(dl) + fabs(el) ); if (b < h) b = h; int m; for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break; bool test = false; for (j=0; j<30; j++) { if (m==l) { test = true; break; } Real& dl1 = D.element(l+1); Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0); dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h; Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h; p = D.element(m); Real c = 1.0; Real s = 0.0; for (i=m-1; i>=l; i--) { Real ei = E.element(i); Real di = D.element(i); Real& ei1 = E.element(i+1); g = c * ei; h = c * p; if ( fabs(p) >= fabs(ei)) { c = ei / p; r = sqrt(c*c + 1.0); ei1 = s*p*r; s = c/r; c = 1.0/r; } else { c = p / ei; r = sqrt(c*c + 1.0); ei1 = s * ei * r; s = 1.0/r; c /= r; } p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di); } el = s*p; dl = c*p; if (fabs(el) <= b) { test = true; break; } } if (!test) Throw ( ConvergenceException(D) ); Real p = dl + f; test = false; for (i=l; i>0; i--) { if (p < D.element(i-1)) D.element(i) = D.element(i-1); else { test = true; break; } } if (!test) i=0; D.element(i) = p; } }
static void MyQuickSortDescending(Real* first, Real* last, int depth) { for (;;) { const int length = last - first + 1; if (length < DoSimpleSort) return; if (depth++ > MaxDepth) Throw(ConvergenceException("QuickSortDescending fails: ")); Real* centre = first + length/2; const Real test = SortThreeDescending(first, centre, last); Real* f = first; Real* l = last; for (;;) { while (*(++f) > test) {} while (*(--l) < test) {} if (l <= f) break; const Real temp = *f; *f = *l; *l = temp; } if (f > centre) { MyQuickSortDescending(l+1, last, depth); last = f-1; } else { MyQuickSortDescending(first, f-1, depth); first = l+1; } } }
void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A, Matrix& V, bool eivec) { Real epsilon = FloatingPointPrecision::Epsilon(); Tracer et("Jacobi"); REPORT int n = X.Nrows(); DiagonalMatrix B(n), Z(n); D.resize(n); A = X; if (eivec) { REPORT V.resize(n,n); D = 1.0; V = D; } B << A; D = B; Z = 0.0; A.Inject(Z); bool converged = false; for (int i=1; i<=50; i++) { Real sm=0.0; Real* a = A.Store(); int p = A.Storage(); while (p--) sm += fabs(*a++); // have previously zeroed diags if (sm==0.0) { REPORT converged = true; break; } Real tresh = (i<4) ? 0.2 * sm / square(n) : 0.0; a = A.Store(); for (p = 0; p < n; p++) { Real* ap1 = a + (p*(p+1))/2; Real& zp = Z.element(p); Real& dp = D.element(p); for (int q = p+1; q < n; q++) { Real* ap = ap1; Real* aq = a + (q*(q+1))/2; Real& zq = Z.element(q); Real& dq = D.element(q); Real& apq = A.element(q,p); Real g = 100 * fabs(apq); Real adp = fabs(dp); Real adq = fabs(dq); if (i>4 && g < epsilon*adp && g < epsilon*adq) { REPORT apq = 0.0; } else if (fabs(apq) > tresh) { REPORT Real t; Real h = dq - dp; Real ah = fabs(h); if (g < epsilon*ah) { REPORT t = apq / h; } else { REPORT Real theta = 0.5 * h / apq; t = 1.0 / ( fabs(theta) + sqrt(1.0 + square(theta)) ); if (theta<0.0) { REPORT t = -t; } } Real c = 1.0 / sqrt(1.0 + square(t)); Real s = t * c; Real tau = s / (1.0 + c); h = t * apq; zp -= h; zq += h; dp -= h; dq += h; apq = 0.0; int j = p; while (j--) { g = *ap; h = *aq; *ap++ = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau); } int ip = p+1; j = q-ip; ap += ip++; aq++; while (j--) { g = *ap; h = *aq; *ap = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau); ap += ip++; } if (q < n-1) // last loop is non-empty { int iq = q+1; j = n-iq; ap += ip++; aq += iq++; for (;;) { g = *ap; h = *aq; *ap = g-s*(h+g*tau); *aq = h+s*(g-h*tau); if (!(--j)) break; ap += ip++; aq += iq++; } } if (eivec) { REPORT RectMatrixCol VP(V,p); RectMatrixCol VQ(V,q); Rotate(VP, VQ, tau, s); } } } } B = B + Z; D = B; Z = 0.0; } if (!converged) Throw(ConvergenceException(X)); if (eivec) SortSV(D, V, true); else SortAscending(D); }
void FindMaximum2::Fit(ColumnVector& Theta, int n_it) { Tracer tr("FindMaximum2::Fit"); enum State {Start, Restart, Continue, Interpolate, Extrapolate, Fail, Convergence}; State TheState = Start; Real z,w,x,x2,g,l1,l2,l3,d1,d2=0,d3; ColumnVector Theta1, Theta2, Theta3; int np = Theta.Nrows(); ColumnVector H1(np), H3, HP(np), K, K1(np); bool oorg, conv; int counter = 0; Theta1 = Theta; HP = 0.0; g = 0.0; // This is really a set of gotos and labels, but they do not work // correctly in AT&T C++ and Sun 4.01 C++. for(;;) { switch (TheState) { case Start: tr.ReName("FindMaximum2::Fit/Start"); Value(Theta1, true, l1, oorg); if (oorg) Throw(ProgramException("invalid starting value\n")); case Restart: tr.ReName("FindMaximum2::Fit/ReStart"); conv = NextPoint(H1, d1); if (conv) { TheState = Convergence; break; } if (counter++ > n_it) { TheState = Fail; break; } z = 1.0 / sqrt(d1); H3 = H1 * z; K = (H3 - HP) * g; HP = H3; g = 0.0; // de-activate to use curved projection if (g==0.0) K1 = 0.0; else K1 = K * 0.2 + K1 * 0.6; // (K - K1) * alpha + K1 * (1 - alpha) // = K * alpha + K1 * (1 - 2 * alpha) K = K1 * d1; g = z; case Continue: tr.ReName("FindMaximum2::Fit/Continue"); Theta2 = Theta1 + H1 + K; Value(Theta2, false, l2, oorg); if (counter++ > n_it) { TheState = Fail; break; } if (oorg) { H1 *= 0.5; K *= 0.25; d1 *= 0.5; g *= 2.0; TheState = Continue; break; } d2 = LastDerivative(H1 + K * 2.0); case Interpolate: tr.ReName("FindMaximum2::Fit/Interpolate"); z = d1 + d2 - 3.0 * (l2 - l1); w = z * z - d1 * d2; if (w < 0.0) { TheState = Extrapolate; break; } w = z + sqrt(w); if (1.5 * w + d1 < 0.0) { TheState = Extrapolate; break; } if (d2 > 0.0 && l2 > l1 && w > 0.0) { TheState = Extrapolate; break; } x = d1 / (w + d1); x2 = x * x; g /= x; Theta3 = Theta1 + H1 * x + K * x2; Value(Theta3, true, l3, oorg); if (counter++ > n_it) { TheState = Fail; break; } if (oorg) { if (x <= 1.0) { x *= 0.5; x2 = x*x; g *= 2.0; d1 *= x; H1 *= x; K *= x2; } else { x = 0.5 * (x-1.0); x2 = x*x; Theta1 = Theta2; H1 = (H1 + K * 2.0) * x; K *= x2; g = 0.0; d1 = x * d2; l1 = l2; } TheState = Continue; break; } if (l3 >= l1 && l3 >= l2) { Theta1 = Theta3; l1 = l3; TheState = Restart; break; } d3 = LastDerivative(H1 + K * 2.0); if (l1 > l2) { H1 *= x; K *= x2; Theta2 = Theta3; d1 *= x; d2 = d3*x; } else { Theta1 = Theta2; Theta2 = Theta3; x -= 1.0; x2 = x*x; g = 0.0; H1 = (H1 + K * 2.0) * x; K *= x2; l1 = l2; l2 = l3; d1 = x*d2; d2 = x*d3; if (d1 <= 0.0) { TheState = Start; break; } } TheState = Interpolate; break; case Extrapolate: tr.ReName("FindMaximum2::Fit/Extrapolate"); Theta1 = Theta2; g = 0.0; K *= 4.0; H1 = (H1 * 2.0 + K); d1 = 2.0 * d2; l1 = l2; TheState = Continue; break; case Fail: Throw(ConvergenceException(Theta)); case Convergence: Theta = Theta1; return; } } }
static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z) { Tracer et("Evalue(tql2)"); Real eps = FloatingPointPrecision::Epsilon(); int n = D.Nrows(); Real* z = Z.Store(); int l; for (l=1; l<n; l++) E.element(l-1) = E.element(l); Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0; for (l=0; l<n; l++) { int i,j; Real& dl = D.element(l); Real& el = E.element(l); Real h = eps * ( fabs(dl) + fabs(el) ); if (b < h) b = h; int m; for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break; bool test = false; for (j=0; j<30; j++) { if (m==l) { test = true; break; } Real& dl1 = D.element(l+1); Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0); dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h; Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h; p = D.element(m); Real c = 1.0; Real s = 0.0; for (i=m-1; i>=l; i--) { Real ei = E.element(i); Real di = D.element(i); Real& ei1 = E.element(i+1); g = c * ei; h = c * p; if ( fabs(p) >= fabs(ei)) { c = ei / p; r = sqrt(c*c + 1.0); ei1 = s*p*r; s = c/r; c = 1.0/r; } else { c = p / ei; r = sqrt(c*c + 1.0); ei1 = s * ei * r; s = 1.0/r; c /= r; } p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di); Real* zki = z + i; Real* zki1 = zki + 1; int k = n; while (k--) { h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h; zki += n; zki1 += n; } } el = s*p; dl = c*p; if (fabs(el) <= b) { test = true; break; } } if (!test) Throw ( ConvergenceException(D) ); dl += f; } for (int i=0; i<n; i++) { int k = i; Real p = D.element(i); for (int j=i+1; j<n; j++) { if (D.element(j) < p) { k = j; p = D.element(j); } } if (k != i) { D.element(k) = D.element(i); D.element(i) = p; int j = n; Real* zji = z + i; Real* zjk = z + k; while (j--) { p = *zji; *zji = *zjk; *zjk = p; zji += n; zjk += n; } } } }