Vector Rosen34(
Fun &F ,
size_t M ,
const Scalar &ti ,
const Scalar &tf ,
const Vector &xi ,
Vector &e )
{
CPPAD_ASSERT_FIRST_CALL_NOT_PARALLEL;
// check numeric type specifications
CheckNumericType<Scalar>();
// check simple vector class specifications
CheckSimpleVector<Scalar, Vector>();
// Parameters for Shampine's Rosenbrock method
// are static to avoid recalculation on each call and
// do not use Vector to avoid possible memory leak
static Scalar a[3] = {
Scalar(0),
Scalar(1),
Scalar(3) / Scalar(5)
};
static Scalar b[2 * 2] = {
Scalar(1),
Scalar(0),
Scalar(24) / Scalar(25),
Scalar(3) / Scalar(25)
};
static Scalar ct[4] = {
Scalar(1) / Scalar(2),
- Scalar(3) / Scalar(2),
Scalar(121) / Scalar(50),
Scalar(29) / Scalar(250)
};
static Scalar cg[3 * 3] = {
- Scalar(4),
Scalar(0),
Scalar(0),
Scalar(186) / Scalar(25),
Scalar(6) / Scalar(5),
Scalar(0),
- Scalar(56) / Scalar(125),
- Scalar(27) / Scalar(125),
- Scalar(1) / Scalar(5)
};
static Scalar d3[3] = {
Scalar(97) / Scalar(108),
Scalar(11) / Scalar(72),
Scalar(25) / Scalar(216)
};
static Scalar d4[4] = {
Scalar(19) / Scalar(18),
Scalar(1) / Scalar(4),
Scalar(25) / Scalar(216),
Scalar(125) / Scalar(216)
};
CPPAD_ASSERT_KNOWN(
M >= 1,
"Error in Rosen34: the number of steps is less than one"
);
CPPAD_ASSERT_KNOWN(
e.size() == xi.size(),
"Error in Rosen34: size of e not equal to size of xi"
);
size_t i, j, k, l, m; // indices
size_t n = xi.size(); // number of components in X(t)
Scalar ns = Scalar(double(M)); // number of steps as Scalar object
Scalar h = (tf - ti) / ns; // step size
Scalar zero = Scalar(0); // some constants
Scalar one = Scalar(1);
Scalar two = Scalar(2);
// permutation vectors needed for LU factorization routine
CppAD::vector<size_t> ip(n), jp(n);
// vectors used to store values returned by F
Vector E(n * n), Eg(n), f_t(n);
Vector g(n * 3), x3(n), x4(n), xf(n), ftmp(n), xtmp(n), nan_vec(n);
// initialize e = 0, nan_vec = nan
for(i = 0; i < n; i++)
{ e[i] = zero;
nan_vec[i] = nan(zero);
}
xf = xi; // initialize solution
for(m = 0; m < M; m++)
{ // time at beginning of this interval
Scalar t = ti * (Scalar(int(M - m)) / ns)
+ tf * (Scalar(int(m)) / ns);
// value of x at beginning of this interval
x3 = x4 = xf;
// evaluate partial derivatives at beginning of this interval
F.Ode_ind(t, xf, f_t);
F.Ode_dep(t, xf, E); // E = f_x
if( hasnan(f_t) || hasnan(E) )
{ e = nan_vec;
return nan_vec;
}
// E = I - f_x * h / 2
for(i = 0; i < n; i++)
{ for(j = 0; j < n; j++)
E[i * n + j] = - E[i * n + j] * h / two;
E[i * n + i] += one;
}
// LU factor the matrix E
# ifndef NDEBUG
int sign = LuFactor(ip, jp, E);
# else
LuFactor(ip, jp, E);
# endif
CPPAD_ASSERT_KNOWN(
sign != 0,
"Error in Rosen34: I - f_x * h / 2 not invertible"
);
// loop over integration steps
for(k = 0; k < 3; k++)
{ // set location for next function evaluation
xtmp = xf;
for(l = 0; l < k; l++)
{ // loop over previous function evaluations
Scalar bkl = b[(k-1)*2 + l];
for(i = 0; i < n; i++)
{ // loop over elements of x
xtmp[i] += bkl * g[i*3 + l] * h;
}
}
// ftmp = F(t + a[k] * h, xtmp)
F.Ode(t + a[k] * h, xtmp, ftmp);
if( hasnan(ftmp) )
{ e = nan_vec;
return nan_vec;
}
// Form Eg for this integration step
for(i = 0; i < n; i++)
Eg[i] = ftmp[i] + ct[k] * f_t[i] * h;
for(l = 0; l < k; l++)
{ for(i = 0; i < n; i++)
Eg[i] += cg[(k-1)*3 + l] * g[i*3 + l];
}
// Solve the equation E * g = Eg
LuInvert(ip, jp, E, Eg);
// save solution and advance x3, x4
for(i = 0; i < n; i++)
{ g[i*3 + k] = Eg[i];
x3[i] += h * d3[k] * Eg[i];
x4[i] += h * d4[k] * Eg[i];
}
}
// Form Eg for last update to x4 only
for(i = 0; i < n; i++)
Eg[i] = ftmp[i] + ct[3] * f_t[i] * h;
for(l = 0; l < 3; l++)
{ for(i = 0; i < n; i++)
Eg[i] += cg[2*3 + l] * g[i*3 + l];
}
// Solve the equation E * g = Eg
LuInvert(ip, jp, E, Eg);
// advance x4 and accumulate error bound
for(i = 0; i < n; i++)
{ x4[i] += h * d4[3] * Eg[i];
// cant use abs because cppad.hpp may not be included
Scalar diff = x4[i] - x3[i];
if( diff < zero )
e[i] -= diff;
else e[i] += diff;
}
// advance xf for this step using x4
xf = x4;
}
return xf;
}
Vector OdeErrControl(
Method &method,
const Scalar &ti ,
const Scalar &tf ,
const Vector &xi ,
const Scalar &smin ,
const Scalar &smax ,
Scalar &scur ,
const Vector &eabs ,
const Scalar &erel ,
Vector &ef ,
Vector &maxabs,
size_t &nstep )
{
// check simple vector class specifications
CheckSimpleVector<Scalar, Vector>();
size_t n = size_t(xi.size());
CPPAD_ASSERT_KNOWN(
smin <= smax,
"Error in OdeErrControl: smin > smax"
);
CPPAD_ASSERT_KNOWN(
size_t(eabs.size()) == n,
"Error in OdeErrControl: size of eabs is not equal to n"
);
CPPAD_ASSERT_KNOWN(
size_t(maxabs.size()) == n,
"Error in OdeErrControl: size of maxabs is not equal to n"
);
size_t m = method.order();
CPPAD_ASSERT_KNOWN(
m > 1,
"Error in OdeErrControl: m is less than or equal one"
);
bool ok;
bool minimum_step;
size_t i;
Vector xa(n), xb(n), eb(n), nan_vec(n);
// initialization
Scalar zero(0);
Scalar one(1);
Scalar two(2);
Scalar three(3);
Scalar m1(m-1);
Scalar ta = ti;
for(i = 0; i < n; i++)
{ nan_vec[i] = nan(zero);
ef[i] = zero;
xa[i] = xi[i];
if( zero <= xi[i] )
maxabs[i] = xi[i];
else maxabs[i] = - xi[i];
}
nstep = 0;
Scalar tb, step, lambda, axbi, a, r, root;
while( ! (ta == tf) )
{ // start with value suggested by error criteria
step = scur;
// check maximum
if( smax <= step )
step = smax;
// check minimum
minimum_step = step <= smin;
if( minimum_step )
step = smin;
// check if near the end
if( tf <= ta + step * three / two )
tb = tf;
else tb = ta + step;
// try using this step size
nstep++;
method.step(ta, tb, xa, xb, eb);
step = tb - ta;
// check if this steps error estimate is ok
ok = ! (hasnan(xb) || hasnan(eb));
if( (! ok) && minimum_step )
{ ef = nan_vec;
return nan_vec;
}
// compute value of lambda for this step
lambda = Scalar(10) * scur / step;
for(i = 0; i < n; i++)
{ if( zero <= xb[i] )
axbi = xb[i];
else axbi = - xb[i];
a = eabs[i] + erel * axbi;
if( ! (eb[i] == zero) )
{ r = ( a / eb[i] ) * step / (tf - ti);
root = exp( log(r) / m1 );
if( root <= lambda )
lambda = root;
}
}
if( ok && ( one <= lambda || step <= smin * three / two) )
{ // this step is within error limits or
// close to the minimum size
ta = tb;
for(i = 0; i < n; i++)
{ xa[i] = xb[i];
ef[i] = ef[i] + eb[i];
if( zero <= xb[i] )
axbi = xb[i];
else axbi = - xb[i];
if( axbi > maxabs[i] )
maxabs[i] = axbi;
}
}
if( ! ok )
{ // decrease step an see if method will work this time
scur = step / two;
}
else if( ! (ta == tf) )
{ // step suggested by the error criteria is not used
// on the last step because it may be very small.
scur = lambda * step / two;
}
}
return xa;
}
