// same as above for X a ColumnVector, length n, element j = 1; otherwise 0
ReturnMatrix Helmert(int n, int j, bool full)
{
REPORT
Tracer et("Helmert:single element ");
if (n <= 0) Throw(ProgramException("X Vector of length <= 0"));
if (j > n || j <= 0)
Throw(ProgramException("Out of range element number "));
ColumnVector Y; if (full) Y.resize(n); else Y.resize(n-1);
Y = 0.0;
if (j > 1) Y(j-1) = sqrt((Real)(j-1) / (Real)j);
for (int i = j; i < n; ++i) Y(i) = - 1.0 / sqrt((Real)i * (i+1));
if (full) Y(n) = 1.0 / sqrt((Real)n);
Y.release(); return Y.for_return();
}
void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
{
// Fourier transform of a real series
Tracer trace("RealFFT");
REPORT
const int n = U.Nrows(); // length of arrays
const int n2 = n / 2;
if (n != 2 * n2)
Throw(ProgramException("Vector length not multiple of 2", U));
ColumnVector A(n2), B(n2);
Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
while (i--) { *a++ = *u++; *b++ = *u++; }
FFT(A,B,A,B);
int n21 = n2 + 1;
X.ReSize(n21); Y.ReSize(n21);
i = n2 - 1;
a = A.Store(); b = B.Store(); // first els of A and B
Real* an = a + i; Real* bn = b + i; // last els of A and B
Real* x = X.Store(); Real* y = Y.Store(); // first els of X and Y
Real* xn = x + n2; Real* yn = y + n2; // last els of X and Y
*x++ = *a + *b; *y++ = 0.0; // first complex element
*xn-- = *a++ - *b++; *yn-- = 0.0; // last complex element
int j = -1; i = n2/2;
while (i--)
{
Real c,s; cossin(j--,n,c,s);
Real am = *a - *an; Real ap = *a++ + *an--;
Real bm = *b - *bn; Real bp = *b++ + *bn--;
Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
*x++ = 0.5 * ( ap + samcbp); *y++ = 0.5 * ( bm + sbpcam);
*xn-- = 0.5 * ( ap - samcbp); *yn-- = 0.5 * (-bm + sbpcam);
}
}
//load, compile and link
void Program::LoadShaders(const std::string &name, const std::string &defines)
{
const std::string filename = std::string("shaders/opengl/") + name;
//load, create and compile shaders
Shader vs(GL_VERTEX_SHADER, filename + ".vert", defines);
Shader fs(GL_FRAGMENT_SHADER, filename + ".frag", defines);
//create program, attach shaders and link
m_program = glCreateProgram();
if(glIsProgram(m_program)!=GL_TRUE)
throw ProgramException();
glAttachShader(m_program, vs.shader);
glAttachShader(m_program, fs.shader);
//extra attribs, if they exist
glBindAttribLocation(m_program, 0, "a_vertex");
glBindAttribLocation(m_program, 1, "a_normal");
glBindAttribLocation(m_program, 2, "a_color");
glBindAttribLocation(m_program, 3, "a_uv0");
glBindAttribLocation(m_program, 4, "a_transform");
glBindFragDataLocation(m_program, 0, "frag_color");
glLinkProgram(m_program);
check_glsl_errors(name.c_str(), m_program);
//shaders may now be deleted by Shader destructor
}
void GetSubMatrix::operator-=(const BaseMatrix& bmx)
{
REPORT
Tracer tr("SubMatrix(-=)"); GeneralMatrix* gmx = 0;
// MatrixConversionCheck mcc; // Check for loss of info
Try
{
SetUpLHS(); gmx = ((BaseMatrix&)bmx).Evaluate();
if (row_number != gmx->Nrows() || col_number != gmx->Ncols())
Throw(IncompatibleDimensionsException());
if (gm->type().is_symmetric() &&
( ! gmx->type().is_symmetric() || row_skip != col_skip) )
Throw(ProgramException("Illegal operation on symmetric"));
MatrixRow mrx(gmx, LoadOnEntry);
MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
// do need LoadOnEntry
MatrixRowCol sub; int i = row_number;
while (i--)
{
mr.SubRowCol(sub, col_skip, col_number); // put values in sub
sub.Check(mrx); // check for loss of info
sub.Sub(mrx); mr.Next(); mrx.Next();
}
gmx->tDelete();
}
CatchAll
{
if (gmx) gmx->tDelete();
ReThrow;
}
}
void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
{
REPORT
Tracer et("QRZ(2)");
int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
if (Y.Nrows() != n)
{ Throw(ProgramException("Unequal column lengths",X,Y)); }
M.resize(s,t); M = 0;Real* m0 = M.Store(); Real* m;
Real* xi0 = X.Store();
int j, k; int i = s;
while (i--)
{
Real* xj0 = Y.Store(); Real* xi = xi0; k = n;
if (k) for (;;)
{
m = m0; Real Xi = *xi; Real* xj = xj0;
j = t; while(j--) *m++ += Xi * *xj++;
if (!(--k)) break;
xi += s; xj0 += t;
}
xj0 = Y.Store(); xi = xi0++; k = n;
if (k) for (;;)
{
m = m0; Real Xi = *xi; Real* xj = xj0;
j = t; while(j--) *xj++ -= *m++ * Xi;
if (!(--k)) break;
xi += s; xj0 += t;
}
m0 += t;
}
}
void MatrixRowCol::Check(const MatrixRowCol& mrc1)
// Throw an exception if +=, -=, copy etc would lead to a loss of data
{
REPORT
int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
if (f < skip || l > lx) Throw(ProgramException("Illegal Conversion"));
}
void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
{
// inverse of a Fourier transform of a real series
Tracer trace("RealFFTI");
REPORT
const int n21 = A.Nrows(); // length of arrays
if (n21 != B.Nrows() || n21 == 0)
Throw(ProgramException("Vector lengths unequal or zero", A, B));
const int n2 = n21 - 1; const int n = 2 * n2; int i = n2 - 1;
ColumnVector X(n2), Y(n2);
Real* a = A.Store(); Real* b = B.Store(); // first els of A and B
Real* an = a + n2; Real* bn = b + n2; // last els of A and B
Real* x = X.Store(); Real* y = Y.Store(); // first els of X and Y
Real* xn = x + i; Real* yn = y + i; // last els of X and Y
Real hn = 0.5 / n2;
*x++ = hn * (*a + *an); *y++ = - hn * (*a - *an);
a++; an--; b++; bn--;
int j = -1; i = n2/2;
while (i--)
{
Real c,s; cossin(j--,n,c,s);
Real am = *a - *an; Real ap = *a++ + *an--;
Real bm = *b - *bn; Real bp = *b++ + *bn--;
Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
*x++ = hn * ( ap + samcbp); *y++ = - hn * ( bm + sbpcam);
*xn-- = hn * ( ap - samcbp); *yn-- = - hn * (-bm + sbpcam);
}
FFT(X,Y,X,Y); // have done inverting elsewhere
U.ReSize(n); i = n2;
x = X.Store(); y = Y.Store(); Real* u = U.Store();
while (i--) { *u++ = *x++; *u++ = - *y++; }
}
void BandMatrix::ReSize(int n, int lb, int ub)
{
REPORT
Tracer tr("BandMatrix::ReSize");
if (lb<0 || ub<0) Throw(ProgramException("Undefined bandwidth"));
lower = (lb<=n) ? lb : n-1; upper = (ub<=n) ? ub : n-1;
GeneralMatrix::ReSize(n,n,n*(lower+1+upper)); CornerClear();
}
// Multiply X by n-1 x n matrix to give n-1 contrasts
// Return a ColumnVector
ReturnMatrix Helmert(const ColumnVector& X, bool full)
{
REPORT
Tracer et("Helmert * CV");
int n = X.nrows();
if (n == 0) Throw(ProgramException("X Vector of length 0", X));
Real sum = 0.0; ColumnVector Y;
if (full) Y.resize(n); else Y.resize(n-1);
for (int i = 1; i < n; ++i)
{ sum += X(i); Y(i) = (i * X(i+1) - sum) / sqrt((Real)i * (i+1)); }
if (full) { sum += X(n); Y(n) = sum / sqrt((Real)n); }
Y.release(); return Y.for_return();
}
void GetSubMatrix::SetUpLHS() {
REPORT
Tracer tr("SubMatrix(LHS)");
const BaseMatrix* bm1 = bm;
GeneralMatrix* gm1 = ((BaseMatrix*&)bm)->Evaluate();
if ((BaseMatrix*)gm1!=bm1)
Throw(ProgramException("Invalid LHS"));
if (row_number < 0) row_number = gm1->Nrows();
if (col_number < 0) col_number = gm1->Ncols();
if (row_skip+row_number > gm1->Nrows()
|| col_skip+col_number > gm1->Ncols())
Throw(SubMatrixDimensionException());
}
void updateQRZ(const Matrix& X, Matrix& MX, Matrix& MU)
{
REPORT
Tracer et("updateQRZ(2)");
int s = X.Ncols(); int n = X.Nrows();
if (n != MX.Nrows())
Throw(ProgramException("Incompatible dimensions",X,MX));
if (s != MU.Nrows())
Throw(ProgramException("Incompatible dimensions",X,MU));
int t = MX.Ncols();
if (t != MU.Ncols())
Throw(ProgramException("Incompatible dimensions",MX,MU));
if (s == 0) return;
const Real* xi0 = X.data(); Real* mx = MX.data(); Real* muj = MU.data();
for (int i=1; i<=s; ++i)
{
Real sum = 0.0;
{
const Real* xi=xi0; int k=n;
while(k--) { sum += square(*xi); xi+= s;}
}
Real a0 = sqrt(2.0 - sum); Real* mxj0 = mx;
for (int j=1; j<=t; ++j)
{
Real sum = 0.0;
const Real* xi=xi0; Real* mxj=mxj0; int k=n;
while(--k) { sum += *xi * *mxj; xi += s; mxj += t; }
sum += *xi * *mxj; // last line of loop
sum += a0 * *muj;
xi=xi0; mxj=mxj0; k=n;
while(--k) { *mxj -= sum * *xi; xi += s; mxj += t; }
*mxj -= sum * *xi; // last line of loop
*muj -= sum * a0; ++mxj0; ++muj;
}
++xi0;
}
}
void MatrixRowCol::CopyCheck(const MatrixRowCol& mrc1)
// Throw an exception if this would lead to a loss of data
{
REPORT
int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
if (f < skip || l > lx) Throw(ProgramException("Illegal Conversion"));
Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
int l1 = f-skip; while (l1--) *elx++ = 0.0;
l1 = l-f; while (l1--) *elx++ = *ely++;
lx -= l; while (lx--) *elx++ = 0.0;
}
void FFT(const ColumnVector& U, const ColumnVector& V,
ColumnVector& X, ColumnVector& Y)
{
// from Carl de Boor (1980), Siam J Sci Stat Comput, 1 173-8
// but first try Sande and Gentleman
Tracer trace("FFT");
REPORT
const int n = U.Nrows(); // length of arrays
if (n != V.Nrows() || n == 0)
Throw(ProgramException("Vector lengths unequal or zero", U, V));
if (n == 1) { REPORT X = U; Y = V; return; }
// see if we can use the newfft routine
if (!FFT_Controller::OnlyOldFFT && FFT_Controller::CanFactor(n))
{
REPORT
X = U; Y = V;
if ( FFT_Controller::ar_1d_ft(n,X.Store(),Y.Store()) ) return;
}
ColumnVector B = V;
ColumnVector A = U;
X.ReSize(n); Y.ReSize(n);
const int nextmx = 8;
#ifndef ATandT
int prime[8] = { 2,3,5,7,11,13,17,19 };
#else
int prime[8];
prime[0]=2; prime[1]=3; prime[2]=5; prime[3]=7;
prime[4]=11; prime[5]=13; prime[6]=17; prime[7]=19;
#endif
int after = 1; int before = n; int next = 0; bool inzee = true;
int now = 0; int b1; // initialised to keep gnu happy
do
{
for (;;)
{
if (next < nextmx) { REPORT now = prime[next]; }
b1 = before / now; if (b1 * now == before) { REPORT break; }
next++; now += 2;
}
before = b1;
if (inzee) { REPORT fftstep(A, B, X, Y, after, now, before); }
else { REPORT fftstep(X, Y, A, B, after, now, before); }
inzee = !inzee; after *= now;
}
ReturnMatrix Helmert(const Matrix& X, bool full)
{
REPORT
Tracer et("Helmert * Matrix");
int m = X.nrows(); int n = X.ncols();
if (m == 0) Throw(ProgramException("Matrix has 0 rows ", X));
Matrix Y;
if (full) Y.resize(m,n); else Y.resize(m-1, n);
for (int j = 1; j <= n; ++j)
{
ColumnVector CV = X.Column(j);
Y.Column(j) = Helmert(CV, full);
}
Y.release(); return Y.for_return();
}
ReturnMatrix Helmert(int n, bool full)
{
REPORT
Tracer et("Helmert ");
if (n <= 0) Throw(ProgramException("Dimension <= 0 "));
Matrix H;
if (full) H.resize(n,n); else H.resize(n-1,n);
H = 0.0;
for (int i = 1; i < n; ++i)
{
Real f = 1.0 / sqrt((Real)i * (i+1));
H.submatrix(i,i,1,i) = -f; H(i,i+1) = f * i;
}
if (full) { H.row(n) = 1.0 / sqrt((Real)n); }
H.release(); return H.for_return();
}
GeneralMatrix* MatrixType::New(int nr, int nc, BaseMatrix* bm) const
{
// make a new matrix with the given attributes
Tracer tr("New"); GeneralMatrix* gm;
switch (attribute)
{
case Valid:
if (nc==1) { gm = new ColumnVector(nr); break; }
if (nr==1) { gm = new RowVector(nc); break; }
gm = new Matrix(nr, nc); break;
case Valid+Symmetric:
gm = new SymmetricMatrix(nr); break;
case Valid+Band:
{
MatrixBandWidth bw = bm->BandWidth();
gm = new BandMatrix(nr,bw.lower,bw.upper); break;
}
case Valid+Symmetric+Band:
gm = new SymmetricBandMatrix(nr,bm->BandWidth().lower); break;
case Valid+Upper:
gm = new UpperTriangularMatrix(nr); break;
case Valid+Diagonal+Symmetric+Band+Upper+Lower:
gm = new DiagonalMatrix(nr); break;
case Valid+Band+Upper:
gm = new UpperBandMatrix(nr,bm->BandWidth().upper); break;
case Valid+Lower:
gm = new LowerTriangularMatrix(nr); break;
case Valid+Band+Lower:
gm = new LowerBandMatrix(nr,bm->BandWidth().lower); break;
default:
Throw(ProgramException("Invalid matrix type"));
}
MatrixErrorNoSpace(gm); gm->Protect(); return gm;
}
void updateQRZ(UpperTriangularMatrix& X, UpperTriangularMatrix& U)
{
REPORT
Tracer et("updateQRZ(3)");
int s = X.Ncols();
if (s != U.Ncols())
Throw(ProgramException("Incompatible dimensions",X,U));
if (s == 0) return;
Real* xi0 = X.data(); Real* u = U.data();
for (int i=1; i<=s; ++i)
{
Real r = *u; Real sum = 0.0;
{
Real* xi=xi0; int k=i; int l=s;
while(k--) { sum += square(*xi); xi+= --l;}
}
sum = sqrt(sum + square(r));
if (sum == 0.0) { REPORT X.column(i) = 0.0; *u = 0.0; }
else
{
Real frs = fabs(r) + sum;
Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
if (r <= 0) { REPORT *u = sum; alpha = -alpha; }
else { REPORT *u = -sum; }
{
Real* xj0=xi0; int k=i; int l=s;
while(k--) { *xj0 *= alpha; --l; xj0 += l;}
}
Real* xj0=xi0; Real* uj=u;
for (int j=i+1; j<=s; ++j)
{
Real sum = 0.0; ++xj0; ++uj;
Real* xi=xi0; Real* xj=xj0; int k=i; int l=s;
while(k--) { sum += *xi * *xj; --l; xi += l; xj += l; }
sum += a0 * *uj;
xi=xi0; xj=xj0; k=i; l=s;
while(k--) { *xj -= sum * *xi; --l; xi += l; xj += l; }
*uj -= sum * a0;
}
}
++xi0; u += s-i+1;
}
}
void QRZT(const Matrix& X, Matrix& Y, Matrix& M)
{
REPORT
Tracer et("QRZT(2)");
int n = X.Ncols(); int s = X.Nrows(); int t = Y.Nrows();
if (Y.Ncols() != n)
{ Throw(ProgramException("Unequal row lengths",X,Y)); }
M.resize(t,s);
Real* xi = X.Store(); int k;
for (int i=0; i<s; i++)
{
Real* xj0 = Y.Store(); Real* xi0 = xi;
for (int j=0; j<t; j++)
{
Real sum=0.0;
xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
M.element(j,i) = sum;
}
}
}
// produces the Cholesky decomposition of A - x.t() * x where A = chol.t() * chol
void downdate_Cholesky(UpperTriangularMatrix &chol, RowVector x)
{
int nRC = chol.Nrows();
// solve R^T a = x
LowerTriangularMatrix L = chol.t();
ColumnVector a(nRC); a = 0.0;
int i, j;
for (i = 1; i <= nRC; ++i)
{
// accumulate subtr sum
Real subtrsum = 0.0;
for(int k = 1; k < i; ++k) subtrsum += a(k) * L(i,k);
a(i) = (x(i) - subtrsum) / L(i,i);
}
// test that l2 norm of a is < 1
Real squareNormA = a.SumSquare();
if (squareNormA >= 1.0)
Throw(ProgramException("downdate_Cholesky() fails", chol));
Real alpha = sqrt(1.0 - squareNormA);
// compute and apply Givens rotations to the vector a
ColumnVector cGivens(nRC); cGivens = 0.0;
ColumnVector sGivens(nRC); sGivens = 0.0;
for(i = nRC; i >= 1; i--)
alpha = pythag(alpha, a(i), cGivens(i), sGivens(i));
// apply Givens rotations to the jth column of chol
ColumnVector xtilde(nRC); xtilde = 0.0;
for(j = nRC; j >= 1; j--)
{
// only the first j rotations have an affect on chol,0
for(int k = j; k >= 1; k--)
GivensRotation(cGivens(k), -sGivens(k), chol(k,j), xtilde(j));
}
}
void updateQRZT(Matrix& X, LowerTriangularMatrix& L)
{
REPORT
Tracer et("updateQRZT");
int n = X.Ncols(); int s = X.Nrows();
if (s != L.Nrows())
Throw(ProgramException("Incompatible dimensions",X,L));
if (n == 0 || s == 0) return;
Real* xi = X.Store(); int k;
for (int i=0; i<s; i++)
{
Real r = L.element(i,i);
Real sum = 0.0;
Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
sum = sqrt(sum + square(r));
if (sum == 0.0)
{
REPORT
k=n; while(k--) { *xi0++ = 0.0; }
for (int j=i; j<s; j++) L.element(j,i) = 0.0;
}
else
{
Real frs = fabs(r) + sum;
Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
if (r <= 0) { REPORT L.element(i,i) = sum; alpha = -alpha; }
else { REPORT L.element(i,i) = -sum; }
Real* xj0=xi0; k=n; while(k--) { *xj0++ *= alpha; }
for (int j=i+1; j<s; j++)
{
sum = 0.0;
xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
sum += a0 * L.element(j,i);
xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
L.element(j,i) -= sum * a0;
}
}
}
}
void updateQRZ(Matrix& X, UpperTriangularMatrix& U)
{
REPORT
Tracer et("updateQRZ");
int n = X.Nrows(); int s = X.Ncols();
if (s != U.Ncols())
Throw(ProgramException("Incompatible dimensions",X,U));
if (n == 0 || s == 0) return;
Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
RowVector V(s); Real* v0 = V.Store(); Real* v; V = 0.0;
int j, k; int J = s; int i = s;
while (i--)
{
Real* xj0 = xi0; Real* xi = xi0; k = n;
if (k) for (;;)
{
v = v0; Real Xi = *xi; Real* xj = xj0;
j = J; while(j--) *v++ += Xi * *xj++;
if (!(--k)) break;
xi += s; xj0 += s;
}
Real r = *u0;
Real sum = sqrt(*v0 + square(r));
if (sum == 0.0)
{
REPORT
u = u0; v = v0;
j = J; while(j--) { *u++ = 0.0; *v++ = 0.0; }
xj0 = xi0++; k = n;
if (k) for (;;)
{
*xj0 = 0.0;
if (!(--k)) break;
xj0 += s;
}
u0 += J--;
}
else
{
Real frs = fabs(r) + sum;
Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
if (r <= 0) { REPORT alpha = -alpha; *u0 = sum; }
else { REPORT *u0 = -sum; }
j = J - 1; v = v0 + 1; u = u0 + 1;
while (j--)
{ *v = a0 * *u + alpha * *v; *u -= a0 * *v; ++v; ++u; }
xj0 = xi0; xi = xi0++; k = n;
if (k) for (;;)
{
v = v0 + 1; Real Xi = *xi; Real* xj = xj0;
Xi *= alpha; *xj++ = Xi;
j = J - 1; while(j--) *xj++ -= *v++ * Xi;
if (!(--k)) break;
xi += s; xj0 += s;
}
j = J; v = v0;
while (j--) *v++ = 0.0;
u0 += J--;
}
}
}
static void NullMatrixError(const GeneralMatrix* gm)
{
((GeneralMatrix&)*gm).tDelete();
Throw(ProgramException("Maximum or minimum of null matrix"));
}
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;
}
}
}
void MatrixRowCol::Check(const MatrixRowCol& mrc1)
// Throw an exception if +=, -=, copy etc would lead to a loss of data
{
REPORT
int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
if (f < skip || l > lx) Throw(ProgramException("Illegal Conversion"));
}
void MatrixRowCol::Check()
// Throw an exception if +=, -= of constant would lead to a loss of data
// that is: check full row is present
// may not be appropriate for symmetric matrices
{
REPORT
if (skip!=0 || storage!=length)
Throw(ProgramException("Illegal Conversion"));
}
void MatrixRowCol::Negate(const MatrixRowCol& mrc1)
{
// THIS = -mrc1
REPORT
int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
if (f < skip) { f = skip; if (l < f) l = f; }
if (l > lx) { l = lx; if (f > lx) f = lx; }
Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
int l1 = f-skip; while (l1--) *elx++ = 0.0;
l1 = l-f; while (l1--) *elx++ = - *ely++;
lx -= l; while (lx--) *elx++ = 0.0;
ReturnMatrixX::ReturnMatrixX(const ReturnMatrixX& tm)
: gm(tm.gm) { Throw(ProgramException("ReturnMatrixX error")); }
void BandMatrix::ReSize(int n, int lb, int ub)
{
REPORT
Tracer tr("BandMatrix::ReSize");
if (lb<0 || ub<0) Throw(ProgramException("Undefined bandwidth"));
lower = (lb<=n) ? lb : n-1; upper = (ub<=n) ? ub : n-1;
GeneralMatrix::ReSize(n,n,n*(lower+1+upper)); CornerClear();
}
void UpperBandMatrix::ReSize(int n, int lb, int ub)
{
REPORT
if (lb != 0)
{
Tracer tr("UpperBandMatrix::ReSize");
Throw(ProgramException("UpperBandMatrix with non-zero lower band" ));
}
BandMatrix::ReSize(n, lb, ub);
}
void LowerBandMatrix::ReSize(int n, int lb, int ub)
{
REPORT
if (lb != 0)
{
Tracer tr("LowerBandMatrix::ReSize");
Throw(ProgramException("LowerBandMatrix with non-zero upper band" ));
}
BandMatrix::ReSize(n, lb, ub);
}
