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;
}
int LuSolve(
size_t n ,
size_t m ,
const FloatVector &A ,
const FloatVector &B ,
FloatVector &X ,
Float &logdet )
{
// check numeric type specifications
CheckNumericType<Float>();
// check simple vector class specifications
CheckSimpleVector<Float, FloatVector>();
size_t p; // index of pivot element (diagonal of L)
int signdet; // sign of the determinant
Float pivot; // pivot element
// the value zero
const Float zero(0);
// pivot row and column order in the matrix
std::vector<size_t> ip(n);
std::vector<size_t> jp(n);
// -------------------------------------------------------
CPPAD_ASSERT_KNOWN(
size_t(A.size()) == n * n,
"Error in LuSolve: A must have size equal to n * n"
);
CPPAD_ASSERT_KNOWN(
size_t(B.size()) == n * m,
"Error in LuSolve: B must have size equal to n * m"
);
CPPAD_ASSERT_KNOWN(
size_t(X.size()) == n * m,
"Error in LuSolve: X must have size equal to n * m"
);
// -------------------------------------------------------
// copy A so that it does not change
FloatVector Lu(A);
// copy B so that it does not change
X = B;
// Lu factor the matrix A
signdet = LuFactor(ip, jp, Lu);
// compute the log of the determinant
logdet = Float(0);
for(p = 0; p < n; p++)
{ // pivot using the max absolute element
pivot = Lu[ ip[p] * n + jp[p] ];
// check for determinant equal to zero
if( pivot == zero )
{ // abort the mission
logdet = Float(0);
return 0;
}
// update the determinant
if( LeqZero ( pivot ) )
{ logdet += log( - pivot );
signdet = - signdet;
}
else logdet += log( pivot );
}
// solve the linear equations
LuInvert(ip, jp, Lu, X);
// return the sign factor for the determinant
return signdet;
}
void OdeGear(
Fun &F ,
size_t m ,
size_t n ,
const Vector &T ,
Vector &X ,
Vector &e )
{
// temporary indices
size_t i, j, k;
typedef typename Vector::value_type Scalar;
// check numeric type specifications
CheckNumericType<Scalar>();
// check simple vector class specifications
CheckSimpleVector<Scalar, Vector>();
CPPAD_ASSERT_KNOWN(
m >= 1,
"OdeGear: m is less than one"
);
CPPAD_ASSERT_KNOWN(
n > 0,
"OdeGear: n is equal to zero"
);
CPPAD_ASSERT_KNOWN(
size_t(T.size()) >= (m+1),
"OdeGear: size of T is not greater than or equal (m+1)"
);
CPPAD_ASSERT_KNOWN(
size_t(X.size()) >= (m+1) * n,
"OdeGear: size of X is not greater than or equal (m+1) * n"
);
for(j = 0; j < m; j++) CPPAD_ASSERT_KNOWN(
T[j] < T[j+1],
"OdeGear: the array T is not monotone increasing"
);
// some constants
Scalar zero(0);
Scalar one(1);
// vectors required by method
Vector alpha(m + 1);
Vector beta(m + 1);
Vector f(n);
Vector f_x(n * n);
Vector x_m0(n);
Vector x_m(n);
Vector b(n);
Vector A(n * n);
// compute alpha[m]
alpha[m] = zero;
for(k = 0; k < m; k++)
alpha[m] += one / (T[m] - T[k]);
// compute beta[m-1]
beta[m-1] = one / (T[m-1] - T[m]);
for(k = 0; k < m-1; k++)
beta[m-1] += one / (T[m-1] - T[k]);
// compute other components of alpha
for(j = 0; j < m; j++)
{ // compute alpha[j]
alpha[j] = one / (T[j] - T[m]);
for(k = 0; k < m; k++)
{ if( k != j )
{ alpha[j] *= (T[m] - T[k]);
alpha[j] /= (T[j] - T[k]);
}
}
}
// compute other components of beta
for(j = 0; j <= m; j++)
{ if( j != m-1 )
{ // compute beta[j]
beta[j] = one / (T[j] - T[m-1]);
for(k = 0; k <= m; k++)
{ if( k != j && k != m-1 )
{ beta[j] *= (T[m-1] - T[k]);
beta[j] /= (T[j] - T[k]);
}
}
}
}
// evaluate f(T[m-1], x_{m-1} )
for(i = 0; i < n; i++)
x_m[i] = X[(m-1) * n + i];
F.Ode(T[m-1], x_m, f);
// solve for x_m^0
for(i = 0; i < n; i++)
{ x_m[i] = f[i];
for(j = 0; j < m; j++)
x_m[i] -= beta[j] * X[j * n + i];
x_m[i] /= beta[m];
}
x_m0 = x_m;
// evaluate partial w.r.t x of f(T[m], x_m^0)
F.Ode_dep(T[m], x_m, f_x);
// compute the matrix A = ( alpha[m] * I - f_x )
for(i = 0; i < n; i++)
{ for(j = 0; j < n; j++)
A[i * n + j] = - f_x[i * n + j];
A[i * n + i] += alpha[m];
}
// LU factor (and overwrite) the matrix A
int sign;
CppAD::vector<size_t> ip(n) , jp(n);
sign = LuFactor(ip, jp, A);
CPPAD_ASSERT_KNOWN(
sign != 0,
"OdeGear: step size is to large"
);
// Iterations of Newton's method
for(k = 0; k < 3; k++)
{
// only evaluate f( T[m] , x_m ) keep f_x during iteration
F.Ode(T[m], x_m, f);
// b = f + f_x x_m - alpha[0] x_0 - ... - alpha[m-1] x_{m-1}
for(i = 0; i < n; i++)
{ b[i] = f[i];
for(j = 0; j < n; j++)
b[i] -= f_x[i * n + j] * x_m[j];
for(j = 0; j < m; j++)
b[i] -= alpha[j] * X[ j * n + i ];
}
LuInvert(ip, jp, A, b);
x_m = b;
}
// return estimate for x( t[k] ) and the estimated error bound
for(i = 0; i < n; i++)
{ X[m * n + i] = x_m[i];
e[i] = x_m[i] - x_m0[i];
if( e[i] < zero )
e[i] = - e[i];
}
}