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; }
}
}
}
