static int
rk2imp_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[], const gsl_odeiv_system * sys)
{
rk2imp_state_t *state = (rk2imp_state_t *) vstate;
const int iter_steps = 3;
int status = 0;
int nu;
size_t i;
double *const knu = state->knu;
double *const ytmp = state->ytmp;
/* initialization step */
if (dydt_in != NULL)
{
DBL_MEMCPY (knu, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, knu);
GSL_STATUS_UPDATE (&status, s);
}
/* iterative solution */
for (nu = 0; nu < iter_steps; nu++)
{
for (i = 0; i < dim; i++)
{
ytmp[i] = y[i] + 0.5 * h * knu[i];
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, knu);
GSL_STATUS_UPDATE (&status, s);
}
}
/* assignment */
for (i = 0; i < dim; i++)
{
y[i] += h * knu[i];
yerr[i] = h * h * knu[i];
if (dydt_out != NULL)
dydt_out[i] = knu[i];
}
return status;
}
static int
eulerplus_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[],
const gsl_odeiv_system * sys)
{
eulerplus_state_t *state = (eulerplus_state_t *) vstate;
double temp;
size_t i;
int status = 0;
double *const k1 = state->k1;
double *const k2 = state->k2;
double *const ytmp = state->ytmp;
if (dydt_in != NULL)
{
DBL_MEMCPY (k1, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
GSL_STATUS_UPDATE (&status, s);
}
/* aplicamos euler */
for (i = 0; i < dim; i++)
{
ytmp[i] = y[i] + h * k1[i] ;
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t+h, ytmp, k2);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
{
temp = (k1[i]+k2[i]) / 2; /* la pendiente en x + h */
if (dydt_out != NULL)
dydt_out[i] = temp;
yerr[i] = h * temp;
y[i] += h*temp;
}
return status;
}
static int
rk2imp_step (double *y, rk2imp_state_t *state,
const double h, const double t,
const size_t dim, const gsl_odeiv_system *sys)
{
/* Makes a Runge-Kutta 2nd order implicit advance with step size h.
y0 is initial values of variables y.
The implicit matrix equations to solve are:
Y1 = y0 + h/2 * f(t + h/2, Y1)
y = y0 + h * f(t + h/2, Y1)
*/
const double *y0 = state->y0;
double *Y1 = state->Y1;
double *ytmp = state->ytmp;
int max_iter=3;
int nu;
size_t i;
/* iterative solution of Y1 = y0 + h/2 * f(t + h/2, Y1)
Y1 should include initial values at call.
Note: This method does not check for convergence of the
iterative solution!
*/
for (nu = 0; nu < max_iter; nu++)
{
for (i = 0; i < dim; i++)
{
ytmp[i] = y0[i] + 0.5 * h * Y1[i];
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, Y1);
if (s != GSL_SUCCESS)
{
return s;
}
}
}
/* assignment */
for (i = 0; i < dim; i++)
{
y[i] = y0[i] + h * Y1[i];
}
return GSL_SUCCESS;
}
static int
euler_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[],
const gsl_odeiv_system * sys)
{
euler_state_t *state = (euler_state_t *) vstate;
double temp;
size_t i;
int status = 0;
double *const k = state->k;
if (dydt_in != NULL)
{
DBL_MEMCPY (k, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, k);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
{
temp = y[i]; /* save y[i] */
y[i] = h * k[i];
yerr[i] = h * y[i];
y[i] += temp;
if (dydt_out != NULL)
dydt_out[i] = k[i];
}
return status;
}
static int
gear2_step (double *y, gear2_state_t * state,
const double h, const double t,
const size_t dim, const gsl_odeiv_system * sys)
{
/* Makes a Gear2 advance with step size h.
y0 is the initial values of variables y.
The implicit matrix equations to solve are:
k = y0 + h * f(t + h, k)
y = y0 + h * f(t + h, k)
*/
const int iter_steps = 3;
int nu;
size_t i;
double *y0 = state->y0;
double *yim1 = state->yim1;
double *k = state->k;
/* Iterative solution of k = y0 + h * f(t + h, k)
Note: This method does not check for convergence of the
iterative solution!
*/
for (nu = 0; nu < iter_steps; nu++)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, k);
if (s != GSL_SUCCESS)
{
return s;
}
for (i = 0; i < dim; i++)
{
y[i] = ((4.0 * y0[i] - yim1[i]) + 2.0 * h * k[i]) / 3.0;
}
}
return GSL_SUCCESS;
}
static int
rk4_step (double *y, const rk4_state_t *state,
const double h, const double t, const size_t dim,
const gsl_odeiv_system *sys)
{
/* Makes a Runge-Kutta 4th order advance with step size h. */
/* initial values of variables y. */
const double *y0 = state->y0;
/* work space */
double *ytmp = state->ytmp;
/* Runge-Kutta coefficients. Contains values of coefficient k1
in the beginning
*/
double *k = state->k;
size_t i;
/* k1 step */
for (i = 0; i < dim; i++)
{
y[i] += h / 6.0 * k[i];
ytmp[i] = y0[i] + 0.5 * h * k[i];
}
/* k2 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
{
y[i] += h / 3.0 * k[i];
ytmp[i] = y0[i] + 0.5 * h * k[i];
}
/* k3 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
{
y[i] += h / 3.0 * k[i];
ytmp[i] = y0[i] + h * k[i];
}
/* k4 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
{
y[i] += h / 6.0 * k[i];
}
return GSL_SUCCESS;
}
static int
rkck_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[], const gsl_odeiv2_system * sys)
{
rkck_state_t *state = (rkck_state_t *) vstate;
size_t i;
double *const k1 = state->k1;
double *const k2 = state->k2;
double *const k3 = state->k3;
double *const k4 = state->k4;
double *const k5 = state->k5;
double *const k6 = state->k6;
double *const ytmp = state->ytmp;
double *const y0 = state->y0;
DBL_MEMCPY (y0, y, dim);
/* k1 step */
if (dydt_in != NULL)
{
DBL_MEMCPY (k1, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + b21 * h * k1[i];
/* k2 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);
/* k3 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[1] * k2[i] + b4[2] * k3[i]);
/* k4 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b5[0] * k1[i] + b5[1] * k2[i] + b5[2] * k3[i] +
b5[3] * k4[i]);
/* k5 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b6[0] * k1[i] + b6[1] * k2[i] + b6[2] * k3[i] +
b6[3] * k4[i] + b6[4] * k5[i]);
/* k6 step and final sum */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);
if (s != GSL_SUCCESS)
{
return s;
}
}
for (i = 0; i < dim; i++)
{
const double d_i = c1 * k1[i] + c3 * k3[i] + c4 * k4[i] + c6 * k6[i];
y[i] += h * d_i;
}
/* Evaluate dydt_out[]. */
if (dydt_out != NULL)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS)
{
/* Restore initial values */
DBL_MEMCPY (y, y0, dim);
return s;
}
}
/* difference between 4th and 5th order */
for (i = 0; i < dim; i++)
{
yerr[i] = h * (ec[1] * k1[i] + ec[3] * k3[i] + ec[4] * k4[i]
+ ec[5] * k5[i] + ec[6] * k6[i]);
}
return GSL_SUCCESS;
}
/*This is the main function of the integration program that calls the gsl Runge-Kutta integrator:*/
void integrator(function F, int D, void *params, double x[], double dxdt[], double x0[], double t[], int iters, double s_noise, double abstol, double reltol) {
/*Temporary variables*/
double *y = (double*) malloc( sizeof(double)*D ); /*state variable at time t-1 (input) and then at time t(output)*/
double *dydt_in = (double*) malloc( sizeof(double)*D ); /*rate of change at time point t-1*/
double *dydt_out= (double*) malloc( sizeof(double)*D ); /*rate of change at time point t*/
double *yerr = (double*) malloc( sizeof(double)*D );/*error*/
double t0,tf,tc,dt=(t[1]-t[0])/2,noise;/*initial time point, final time point, current time point, current time step, noise*/
int j,ii;/*State variable and iteration indexes*/
int status;/*integrator success flag*/
/*Definitions and initializations of gsl integrator necessary inputs and parameters:*/
/*Prepare noise generator*/
const gsl_rng_type *Q;
gsl_rng *r;
gsl_rng_env_setup();
Q = gsl_rng_default;
r = gsl_rng_alloc(Q);
/*Create a stepping function*/
const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T, D);
/*Create an adaptive control function*/
gsl_odeiv_control *c = gsl_odeiv_control_y_new(abstol, reltol);
/*Create the system to be integrated (with NULL jacobian)*/
gsl_odeiv_system sys = {F, NULL, D, params};
/*The integration loop:*/
/*Initialize*/
/*Calculate dx/dt for x0*/
tc=t[0];
/* initialise dydt_in from system parameters */
/*GSL_ODEIV_FN_EVAL(&sys, t, y, dydt_in);*/
GSL_ODEIV_FN_EVAL(&sys, tc, x0, dydt_in);
for (j=0;j<D;j++) {
y[j] = x[j] = x0[j];
}
/*Integration*/
for (ii=1; ii<iters; ii++) {
/*Call the integrator*/
/*int gsl_odeiv_step_apply(gsl_odeiv_step * s, double t, double h, double y[], double yerr[], const double dydt_in[], double dydt_out[], const gsl_odeiv_system * dydt)*/
t0=t[ii-1];
tf=t[ii];
tc=t0;
while (tc<tf) {
/*Constraint time step h such as that tc+h<=tf*/
if (tc+dt>tf) dt=tf-tc;
/*Advance a h time step*/
status=gsl_odeiv_step_apply(s, tc, dt, y, yerr, dydt_in, dydt_out, &sys);
if (status != GSL_SUCCESS) break;
/*Modify time sep*/
gsl_odeiv_control_hadjust(c,s,y,yerr,dydt_in,&dt);
/*Increase current time*/
tc += dt;
/*Add noise*/
for (j=0;j<D;j++) {
noise=gsl_ran_gaussian_ziggurat(r, s_noise);
y[j] += sqrt(dt)*noise;
//dydt_in[j]+=noise;
}
}
/*Unpack and store result for this time point*/
if (status != GSL_SUCCESS) break;
for (j=0;j<D;j++) {
x[ii*D+j] = y[j];
dxdt[(ii-1)*D+j] = dydt_in[j];
/*Prepare next step*/
dydt_in[j] = dydt_out[j];
}
}
/*Get dxdt for the last time point*/
for (j=0;j<D;j++) dxdt[(iters-1)*D+j] = dydt_out[j];
/*Free dynamically allocated memory*/
gsl_odeiv_control_free(c);
printf("c freed\n");
gsl_odeiv_step_free(s);
printf("s freed\n");
gsl_rng_free(r);
printf("rng freed\n");
free(yerr);
printf("yerr freed\n");
free(dydt_out);
printf("dydt_out freed\n");
free(dydt_in);
printf("dydt_in freed\n");
free(y);
printf("y freed\n");
}
/* Perform the basic semi-implicit extrapolation
* step, of size h, at a Deuflhard determined order.
*/
static int
bsimp_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[], const gsl_odeiv2_system * sys)
{
bsimp_state_t *state = (bsimp_state_t *) vstate;
double *const x = state->x;
double *const yp = state->yp;
double *const y_save = state->y_save;
double *const yerr_save = state->yerr_save;
double *const y_extrap_sequence = state->y_extrap_sequence;
double *const y_extrap_save = state->y_extrap_save;
double *const extrap_work = state->extrap_work;
double *const dfdt = state->dfdt;
gsl_matrix *d = state->d;
gsl_matrix *dfdy = state->dfdy;
const double t_local = t;
size_t i, k;
if (h + t_local == t_local)
{
return GSL_EUNDRFLW; /* FIXME: error condition */
}
DBL_MEMCPY (y_extrap_save, y, dim);
/* Save inputs */
DBL_MEMCPY (y_save, y, dim);
DBL_MEMCPY (yerr_save, yerr, dim);
/* Evaluate the derivative. */
if (dydt_in != NULL)
{
DBL_MEMCPY (yp, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t_local, y, yp);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Evaluate the Jacobian for the system. */
{
int s = GSL_ODEIV_JA_EVAL (sys, t_local, y, dfdy->data, dfdt);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Make a series of refined extrapolations,
* up to the specified maximum order, which
* was calculated based on the Deuflhard
* criterion upon state initialization. */
for (k = 0; k <= state->k_current; k++)
{
const unsigned int N = bd_sequence[k];
const double r = (h / N);
const double x_k = r * r;
int status = bsimp_step_local (state,
dim, t_local, h, N,
y_extrap_save, yp,
dfdt, dfdy,
y_extrap_sequence,
sys);
if (status == GSL_EFAILED)
{
/* If the local step fails, set the error to infinity in
order to force a reduction in the step size */
for (i = 0; i < dim; i++)
{
yerr[i] = GSL_POSINF;
}
break;
}
else if (status != GSL_SUCCESS)
{
return status;
}
x[k] = x_k;
poly_extrap (d, x, k, x_k, y_extrap_sequence, y, yerr, extrap_work,
dim);
}
/* Evaluate dydt_out[]. */
if (dydt_out != NULL)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS)
{
DBL_MEMCPY (y, y_save, dim);
DBL_MEMCPY (yerr, yerr_save, dim);
return s;
}
}
return GSL_SUCCESS;
}
static int
bsimp_step_local (void *vstate,
size_t dim,
const double t0,
const double h_total,
const unsigned int n_step,
const double y[],
const double yp[],
const double dfdt[],
const gsl_matrix * dfdy,
double y_out[], const gsl_odeiv2_system * sys)
{
bsimp_state_t *state = (bsimp_state_t *) vstate;
gsl_matrix *const a_mat = state->a_mat;
gsl_permutation *const p_vec = state->p_vec;
double *const delta = state->delta;
double *const y_temp = state->y_temp;
double *const delta_temp = state->delta_temp;
double *const rhs_temp = state->rhs_temp;
double *const w = state->weight;
gsl_vector_view y_temp_vec = gsl_vector_view_array (y_temp, dim);
gsl_vector_view delta_temp_vec = gsl_vector_view_array (delta_temp, dim);
gsl_vector_view rhs_temp_vec = gsl_vector_view_array (rhs_temp, dim);
const double h = h_total / n_step;
double t = t0 + h;
double sum;
/* This is the factor sigma referred to in equation 3.4 of the
paper. A relative change in y exceeding sigma indicates a
runaway behavior. According to the authors suitable values for
sigma are >>1. I have chosen a value of 100*dim. BJG */
const double max_sum = 100.0 * dim;
int signum, status;
size_t i, j;
size_t n_inter;
/* Calculate the matrix for the linear system. */
for (i = 0; i < dim; i++)
{
for (j = 0; j < dim; j++)
{
gsl_matrix_set (a_mat, i, j, -h * gsl_matrix_get (dfdy, i, j));
}
gsl_matrix_set (a_mat, i, i, gsl_matrix_get (a_mat, i, i) + 1.0);
}
/* LU decomposition for the linear system. */
gsl_linalg_LU_decomp (a_mat, p_vec, &signum);
/* Compute weighting factors */
compute_weights (y, w, dim);
/* Initial step. */
for (i = 0; i < dim; i++)
{
y_temp[i] = h * (yp[i] + h * dfdt[i]);
}
gsl_linalg_LU_solve (a_mat, p_vec, &y_temp_vec.vector,
&delta_temp_vec.vector);
sum = 0.0;
for (i = 0; i < dim; i++)
{
const double di = delta_temp[i];
delta[i] = di;
y_temp[i] = y[i] + di;
sum += fabs (di) / w[i];
}
if (sum > max_sum)
{
return GSL_EFAILED;
}
/* Intermediate steps. */
status = GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);
if (status)
{
return status;
}
for (n_inter = 1; n_inter < n_step; n_inter++)
{
for (i = 0; i < dim; i++)
{
rhs_temp[i] = h * y_out[i] - delta[i];
}
gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector,
&delta_temp_vec.vector);
sum = 0.0;
for (i = 0; i < dim; i++)
{
delta[i] += 2.0 * delta_temp[i];
y_temp[i] += delta[i];
sum += fabs (delta[i]) / w[i];
}
if (sum > max_sum)
{
return GSL_EFAILED;
}
t += h;
status = GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);
if (status)
{
return status;
}
}
/* Final step. */
for (i = 0; i < dim; i++)
{
rhs_temp[i] = h * y_out[i] - delta[i];
}
gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector,
&delta_temp_vec.vector);
sum = 0.0;
for (i = 0; i < dim; i++)
{
y_out[i] = y_temp[i] + delta_temp[i];
sum += fabs (delta_temp[i]) / w[i];
}
if (sum > max_sum)
{
return GSL_EFAILED;
}
return GSL_SUCCESS;
}
static int
rk1imp_apply (void *vstate, size_t dim, double t, double h,
double y[], double yerr[],
const double dydt_in[], double dydt_out[],
const gsl_odeiv2_system * sys)
{
/* Makes an implicit Euler step with size h and estimates the local
error of the step by step doubling.
*/
rk1imp_state_t *state = (rk1imp_state_t *) vstate;
double *const y_onestep = state->y_onestep;
double *const y_twostep = state->y_twostep;
double *const ytmp = state->ytmp;
double *const y_save = state->y_save;
double *const YZ = state->YZ;
double *const fYZ = state->fYZ;
gsl_matrix *const dfdy = state->dfdy;
double *const dfdt = state->dfdt;
double *const errlev = state->errlev;
const modnewton1_state_t *esol = state->esol;
/* Runge-Kutta coefficients */
gsl_matrix *A = state->A;
const double b[] = { 1.0 };
const double c[] = { 1.0 };
gsl_matrix_set (A, 0, 0, 1.0);
if (esol == NULL)
{
GSL_ERROR ("no non-linear equation solver speficied", GSL_EINVAL);
}
/* Get desired error levels via gsl_odeiv2_control object through driver
object, which is a requirement for this stepper.
*/
if (state->driver == NULL)
{
return GSL_EFAULT;
}
else
{
size_t i;
for (i = 0; i < dim; i++)
{
if (dydt_in != NULL)
{
gsl_odeiv2_control_errlevel (state->driver->c, y[i],
dydt_in[i], h, i, &errlev[i]);
}
else
{
gsl_odeiv2_control_errlevel (state->driver->c, y[i],
0.0, h, i, &errlev[i]);
}
}
}
/* Evaluate Jacobian for modnewton1 */
{
int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Calculate a single step with size h */
{
int s = modnewton1_init ((void *) esol, A, h, dfdy, sys);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s = modnewton1_solve ((void *) esol, A, c, t, h, y,
sys, YZ, errlev);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h, YZ, fYZ);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s = rksubs (y_onestep, h, y, fYZ, b, RK1IMP_STAGE, dim);
if (s != GSL_SUCCESS)
return s;
}
/* Error estimation by step doubling */
{
int s = modnewton1_init ((void *) esol, A, h / 2.0, dfdy, sys);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* 1st half step */
{
int s = modnewton1_solve ((void *) esol, A, c, t, h / 2.0, y,
sys, YZ, errlev);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h / 2.0, YZ, fYZ);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s = rksubs (ytmp, h / 2.0, y, fYZ, b, RK1IMP_STAGE, dim);
if (s != GSL_SUCCESS)
return s;
}
/* Save original y values in case of error */
DBL_MEMCPY (y_save, y, dim);
/* 2nd half step */
{
int s = modnewton1_solve ((void *) esol, A, c, t + h / 2.0, h / 2.0,
ytmp, sys, YZ, errlev);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0 + c[0] * h / 2.0, YZ, fYZ);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
/* Note: rk1imp returns y using the results from two half steps
instead of the single step since the results are freely
available and more precise.
*/
int s = rksubs (y_twostep, h / 2.0, ytmp, fYZ, b, RK1IMP_STAGE, dim);
if (s != GSL_SUCCESS)
{
DBL_MEMCPY (y, y_save, dim);
return s;
}
}
DBL_MEMCPY (y, y_twostep, dim);
/* Error estimation */
{
size_t i;
for (i = 0; i < dim; i++)
{
yerr[i] = ODEIV_ERR_SAFETY * 0.5 * fabs (y_twostep[i] - y_onestep[i]);
}
}
/* Derivatives at output */
if (dydt_out != NULL)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS)
{
/* Restore original values */
DBL_MEMCPY (y, y_save, dim);
return s;
}
}
return GSL_SUCCESS;
}
static int
modnewton1_solve (void *vstate, const gsl_matrix * A,
const double c[], const double t, const double h,
const double y0[], const gsl_odeiv2_system * sys,
double YZ[], const double errlev[])
{
/* Solves the non-linear equation system resulting from implicit
Runge-Kutta methods by a modified Newton iteration. The
modified Newton iteration with this problem is stated in the
form
IhAJ * dYk = rhs
in which IhAJ is the iteration matrix: IhAJ = (I - hA (*) J) in
which (*) is the Kronecker matrix product (size of IhAJ =
dim*stage, dim*stage), dYk is the Runge-Kutta point (Y)
difference vector for kth Newton iteration: dYk = Y(k+1) - Y(k),
and rhs = Y(k) - y0 - h * sum j=1..stage (a_j * f(Y(k)))
This function solves dYk by LU-decomposition of IhAJ with partial
pivoting.
*/
modnewton1_state_t *state = (modnewton1_state_t *) vstate;
gsl_matrix *const IhAJ = state->IhAJ;
gsl_permutation *const p = state->p;
gsl_vector *const dYk = state->dYk;
double *const Yk = state->Yk;
double *const fYk = state->fYk;
gsl_vector *const rhs = state->rhs;
double *const eeta_prev = &(state->eeta_prev);
gsl_vector *const dScal = state->dScal;
const size_t dim = sys->dimension;
const size_t stage = A->size1;
int status = GSL_CONTINUE; /* Convergence status for Newton iteration */
/* Set starting values for iteration. Use starting values of Y(k) =
y0. FIXME: Implement better initial guess described in Hairer and
Wanner.
*/
{
size_t j, m;
gsl_vector_set_zero (dYk);
for (j = 0; j < stage; j++)
for (m = 0; m < dim; m++)
Yk[j * dim + m] = y0[m];
}
/* Loop modified Newton iterations */
{
size_t iter = 0;
size_t j, m, n;
/* Maximum number of Newton iterations. */
const size_t max_iter = 7;
double dScal_norm = 0.0; /* Newton iteration step length */
double dScal_norm_prev = 0.0; /* Previous dScal_norm */
while (status == GSL_CONTINUE && iter < max_iter)
{
iter++;
/* Update Y(k) and evaluate function */
for (j = 0; j < stage; j++)
{
for (m = 0; m < dim; m++)
{
Yk[j * dim + m] += gsl_vector_get (dYk, j * dim + m);
}
{
int s = GSL_ODEIV_FN_EVAL (sys, t + c[j] * h, &Yk[j * dim],
&fYk[j * dim]);
if (s != GSL_SUCCESS)
{
return s;
}
}
}
/* Calculate rhs */
for (j = 0; j < stage; j++)
for (m = 0; m < dim; m++)
{
double sum = 0;
for (n = 0; n < stage; n++)
sum += gsl_matrix_get (A, j, n) * fYk[n * dim + m];
gsl_vector_set (rhs, j * dim + m,
-1.0 * Yk[j * dim + m] + y0[m] + h * sum);
}
/* Solve dYk */
{
int s = gsl_linalg_LU_solve (IhAJ, p, rhs, dYk);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Evaluate convergence according to method by Hairer and
Wanner, section IV.8. The iteration step is normalized by
desired error level before calculation of the norm, to take
the tolerance on each component of y into account.
*/
{
/* relative change in two Newton iteration steps */
double theta_k;
/* transformation of theta_k */
double eeta_k = 0.0;
for (j = 0; j < stage; j++)
for (m = 0; m < dim; m++)
{
gsl_vector_set (dScal, j * dim + m,
gsl_vector_get (dYk, j * dim + m)
/ errlev[m]);
}
dScal_norm_prev = dScal_norm;
dScal_norm = gsl_blas_dnrm2 (dScal);
theta_k = dScal_norm / dScal_norm_prev;
if (iter > 1)
{
/* check for increase in step size, which indicates divergence */
if (theta_k >= 1.0)
{
return GSL_FAILURE;
}
eeta_k = theta_k / (1.0 - theta_k);
}
else
{
eeta_k = pow (GSL_MAX_DBL (*eeta_prev, GSL_DBL_EPSILON), 0.8);
}
*eeta_prev = eeta_k;
if (eeta_k * dScal_norm < 1.0)
{
status = GSL_SUCCESS;
}
}
}
}
/* No convergence reached within allowed iterations */
if (status != GSL_SUCCESS)
{
return GSL_FAILURE;
}
/* Give solution in YZ */
else
{
size_t j, m;
for (j = 0; j < stage; j++)
for (m = 0; m < dim; m++)
YZ[j * dim + m] =
Yk[j * dim + m] + gsl_vector_get (dYk, j * dim + m);
return status;
}
}
static int
msbdf_corrector (void *vstate, const gsl_odeiv2_system * sys,
const double t, const double h, const size_t dim,
const double z[], const double errlev[],
const double l[], const double errcoeff,
gsl_vector * abscor, gsl_vector * relcor,
double ytmp[], double ytmp2[],
gsl_matrix * dfdy, double dfdt[], gsl_matrix * M,
gsl_permutation * p, gsl_vector * rhs,
size_t * nJ, size_t * nM,
const double tprev, const double failt,
const double gamma, const double gammaprev,
const double hprev0)
{
/* Calculates the correction step (abscor). Equation
system M = I - gamma * dfdy = -G is solved by Newton iteration.
*/
size_t mi, i;
const size_t max_iter = 3; /* Maximum number of iterations */
double convrate = 1.0; /* convergence rate */
double stepnorm = 0.0; /* norm of correction step */
double stepnormprev = 0.0; /* previous norm value */
/* Evaluate at predicted values */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, ytmp);
if (s == GSL_EBADFUNC)
{
return s;
}
if (s != GSL_SUCCESS)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL at user function evaluation\n");
#endif
return s;
}
}
/* Calculate correction step (abscor) */
gsl_vector_set_zero (abscor);
for (mi = 0; mi < max_iter; mi++)
{
const double safety = 0.3;
const double safety2 = 0.1;
/* Generate or update Jacobian and/or iteration matrix M if needed */
if (mi == 0)
{
int s = msbdf_update (vstate, dim, dfdy, dfdt, t + h, z,
sys, M, p, mi,
nJ, nM, tprev, failt,
gamma, gammaprev,
h / hprev0);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Evaluate the right hand side (-G) */
for (i = 0; i < dim; i++)
{
const double r = -1.0 * gsl_vector_get (abscor, i) -
z[1 * dim + i] / l[1] + gamma * ytmp[i];
gsl_vector_set (rhs, i, r);
}
/* Solve system of equations */
{
int s = gsl_linalg_LU_solve (M, p, rhs, relcor);
if (s != GSL_SUCCESS)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL at LU_solve\n");
#endif
return GSL_FAILURE;
}
}
#ifdef DEBUG
{
size_t di;
printf ("-- dstep: ");
for (di = 0; di < dim; di++)
{
printf ("%.5e ", gsl_vector_get (relcor, di));
}
printf ("\n");
}
#endif
/* Add iteration results */
for (i = 0; i < dim; i++)
{
const double r =
gsl_vector_get (abscor, i) + gsl_vector_get (relcor, i);
gsl_vector_set (abscor, i, r);
ytmp2[i] = z[i] + r;
gsl_vector_set (relcor, i, gsl_vector_get (relcor, i) / errlev[i]);
}
#ifdef DEBUG
{
size_t di;
printf ("-- abscor: ");
for (di = 0; di < dim; di++)
{
printf ("%.5e ", gsl_vector_get (abscor, di));
}
printf ("\n");
}
#endif
/* Convergence test. Norms used are root-mean-square norms. */
stepnorm = gsl_blas_dnrm2 (relcor) / sqrt ((double)dim);
if (mi > 0)
{
convrate = GSL_MAX_DBL (safety * convrate, stepnorm / stepnormprev);
}
else
{
convrate = 1.0;
}
{
const double convtest =
GSL_MIN_DBL (convrate, 1.0) * stepnorm * errcoeff / safety2;
#ifdef DEBUG
printf
("-- newt iter loop %d, errcoeff=%.5e, stepnorm =%.5e, convrate = %.5e, convtest = %.5e\n",
(int) mi, errcoeff, stepnorm, convrate, convtest);
#endif
if (convtest <= 1.0)
{
break;
}
}
/* Check for divergence during iteration */
{
const double div_const = 2.0;
if (mi > 1 && stepnorm > div_const * stepnormprev)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL, diverging Newton iteration\n");
#endif
return GSL_FAILURE;
}
}
/* Evaluate at new y */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp2, ytmp);
if (s == GSL_EBADFUNC)
{
return s;
}
if (s != GSL_SUCCESS)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL at user function evaluation\n");
#endif
return s;
}
}
stepnormprev = stepnorm;
}
#ifdef DEBUG
printf ("-- Newton iteration exit at mi=%d\n", (int) mi);
#endif
/* Handle convergence failure */
if (mi == max_iter)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL, max_iter reached\n");
#endif
return GSL_FAILURE;
}
return GSL_SUCCESS;
}
static int
rk8pd_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[], const gsl_odeiv_system * sys)
{
rk8pd_state_t *state = (rk8pd_state_t *) vstate;
size_t i;
int status = 0;
double *const ytmp = state->ytmp;
/* Note that k1 is stored in state->k[0] due to zero-based indexing */
double *const k1 = state->k[0];
double *const k2 = state->k[1];
double *const k3 = state->k[2];
double *const k4 = state->k[3];
double *const k5 = state->k[4];
double *const k6 = state->k[5];
double *const k7 = state->k[6];
double *const k8 = state->k[7];
double *const k9 = state->k[8];
double *const k10 = state->k[9];
double *const k11 = state->k[10];
double *const k12 = state->k[11];
double *const k13 = state->k[12];
/* k1 step */
if (dydt_in != NULL)
{
DBL_MEMCPY (k1, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + b21 * h * k1[i];
/* k2 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);
/* k3 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[2] * k3[i]);
/* k4 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + h * (b5[0] * k1[i] + b5[2] * k3[i] + b5[3] * k4[i]);
/* k5 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] = y[i] + h * (b6[0] * k1[i] + b6[3] * k4[i] + b6[4] * k5[i]);
/* k6 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b7[0] * k1[i] + b7[3] * k4[i] + b7[4] * k5[i] +
b7[5] * k6[i]);
/* k7 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[5] * h, ytmp, k7);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b8[0] * k1[i] + b8[3] * k4[i] + b8[4] * k5[i] +
b8[5] * k6[i] + b8[6] * k7[i]);
/* k8 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[6] * h, ytmp, k8);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b9[0] * k1[i] + b9[3] * k4[i] + b9[4] * k5[i] +
b9[5] * k6[i] + b9[6] * k7[i] + b9[7] * k8[i]);
/* k9 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[7] * h, ytmp, k9);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b10[0] * k1[i] + b10[3] * k4[i] + b10[4] * k5[i] +
b10[5] * k6[i] + b10[6] * k7[i] + b10[7] * k8[i] +
b10[8] * k9[i]);
/* k10 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[8] * h, ytmp, k10);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b11[0] * k1[i] + b11[3] * k4[i] + b11[4] * k5[i] +
b11[5] * k6[i] + b11[6] * k7[i] + b11[7] * k8[i] +
b11[8] * k9[i] + b11[9] * k10[i]);
/* k11 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + ah[9] * h, ytmp, k11);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b12[0] * k1[i] + b12[3] * k4[i] + b12[4] * k5[i] +
b12[5] * k6[i] + b12[6] * k7[i] + b12[7] * k8[i] +
b12[8] * k9[i] + b12[9] * k10[i] + b12[10] * k11[i]);
/* k12 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k12);
GSL_STATUS_UPDATE (&status, s);
}
for (i = 0; i < dim; i++)
ytmp[i] =
y[i] + h * (b13[0] * k1[i] + b13[3] * k4[i] + b13[4] * k5[i] +
b13[5] * k6[i] + b13[6] * k7[i] + b13[7] * k8[i] +
b13[8] * k9[i] + b13[9] * k10[i] + b13[10] * k11[i] +
b13[11] * k12[i]);
/* k13 step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k13);
GSL_STATUS_UPDATE (&status, s);
}
/* final sum and error estimate */
for (i = 0; i < dim; i++)
{
const double ksum8 =
Abar[0] * k1[i] + Abar[5] * k6[i] + Abar[6] * k7[i] +
Abar[7] * k8[i] + Abar[8] * k9[i] + Abar[9] * k10[i] +
Abar[10] * k11[i] + Abar[11] * k12[i] + Abar[12] * k13[i];
const double ksum7 =
A[0] * k1[i] + A[5] * k6[i] + A[6] * k7[i] + A[7] * k8[i] +
A[8] * k9[i] + A[9] * k10[i] + A[10] * k11[i] + A[11] * k12[i];
y[i] += h * ksum8;
yerr[i] = h * (ksum7 - ksum8);
if (dydt_out != NULL)
dydt_out[i] = ksum8;
}
return status;
}
/* Evolution framework method.
*
* Uses an adaptive step control object
*/
int
gsl_odeiv_evolve_apply (gsl_odeiv_evolve * e,
gsl_odeiv_control * con,
gsl_odeiv_step * step,
const gsl_odeiv_system * dydt,
double *t, double t1, double *h, double y[])
{
const double t0 = *t;
double h0 = *h;
int step_status;
int final_step = 0;
double dt = t1 - t0; /* remaining time, possibly less than h */
if (e->dimension != step->dimension)
{
GSL_ERROR ("step dimension must match evolution size", GSL_EINVAL);
}
if ((dt < 0.0 && h0 > 0.0) || (dt > 0.0 && h0 < 0.0))
{
GSL_ERROR ("step direction must match interval direction", GSL_EINVAL);
}
/* No need to copy if we cannot control the step size. */
if (con != NULL)
{
DBL_MEMCPY (e->y0, y, e->dimension);
}
/* Calculate initial dydt once if the method can benefit. */
if (step->type->can_use_dydt_in)
{
int status = GSL_ODEIV_FN_EVAL (dydt, t0, y, e->dydt_in);
if (status)
{
return status;
}
}
try_step:
if ((dt >= 0.0 && h0 > dt) || (dt < 0.0 && h0 < dt))
{
h0 = dt;
final_step = 1;
}
else
{
final_step = 0;
}
if (step->type->can_use_dydt_in)
{
step_status =
gsl_odeiv_step_apply (step, t0, h0, y, e->yerr, e->dydt_in,
e->dydt_out, dydt);
}
else
{
step_status =
gsl_odeiv_step_apply (step, t0, h0, y, e->yerr, NULL, e->dydt_out,
dydt);
}
/* Check for stepper internal failure */
if (step_status != GSL_SUCCESS)
{
return step_status;
}
e->count++;
e->last_step = h0;
if (final_step)
{
*t = t1;
}
else
{
*t = t0 + h0;
}
if (con != NULL)
{
/* Check error and attempt to adjust the step. */
const int hadjust_status
= gsl_odeiv_control_hadjust (con, step, y, e->yerr, e->dydt_out, &h0);
if (hadjust_status == GSL_ODEIV_HADJ_DEC)
{
/* Step was decreased. Undo and go back to try again. */
DBL_MEMCPY (y, e->y0, dydt->dimension);
e->failed_steps++;
goto try_step;
}
}
*h = h0; /* suggest step size for next time-step */
return step_status;
}
static int
rk4imp_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[],
const gsl_odeiv_system * sys)
{
rk4imp_state_t *state = (rk4imp_state_t *) vstate;
size_t i;
double *y0 = state->y0;
double *y0_orig = state->y0_orig;
double *y_onestep = state->y_onestep;
double *k1nu = state->k1nu;
double *k2nu = state->k2nu;
/* Initialization step */
DBL_MEMCPY (y0, y, dim);
/* Save initial values in case of failure */
DBL_MEMCPY (y0_orig, y, dim);
if (dydt_in != 0)
{
DBL_MEMCPY (k1nu, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1nu);
if (s != GSL_SUCCESS)
{
return s;
}
}
DBL_MEMCPY (k2nu, k1nu, dim);
/* First traverse h with one step (save to y_onestep) */
DBL_MEMCPY (y_onestep, y, dim);
{
int s = rk4imp_step (y_onestep, state, h, t, dim, sys);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Then with two steps with half step length (save to y) */
{
int s = rk4imp_step (y, state, h/2.0, t, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
DBL_MEMCPY (y0, y, dim);
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h/2.0, y, k1nu);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
DBL_MEMCPY (k2nu, k1nu, dim);
{
int s = rk4imp_step (y, state, h/2.0, t + h/2.0, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
/* Derivatives at output */
if (dydt_out != NULL)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS) {
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
/* Error estimation */
/* Denominator in step doubling error equation
* yerr = 0.5 * | y(onestep) - y(twosteps) | / (2^order - 1)
*/
for (i = 0; i < dim; i++)
{
yerr[i] = 8.0 * 0.5 * (y[i] - y_onestep[i]) / 15.0;
}
return GSL_SUCCESS;
}
static int
rk4imp_step (double *y, rk4imp_state_t *state,
const double h, const double t,
const size_t dim, const gsl_odeiv_system *sys)
{
/* Makes a Runge-Kutta 4th order implicit advance with step size h.
y0 is initial values of variables y.
The implicit matrix equations to solve are:
Y1 = y0 + h * a11 * f(t + h * c1, Y1) + h * a12 * f(t + h * c2, Y2)
Y2 = y0 + h * a21 * f(t + h * c1, Y1) + h * a22 * f(t + h * c2, Y2)
y = y0 + h * b1 * f(t + h * c1, Y1) + h * b2 * f(t + h * c2, Y2)
with constant coefficients a, b and c. For this method
they are: b=[0.5 0.5] c=[(3-sqrt(3))/6 (3+sqrt(3))/6]
a11=1/4, a12=(3-2*sqrt(3))/12, a21=(3+2*sqrt(3))/12 and a22=1/4
*/
const double ir3 = 1.0 / M_SQRT3;
const int iter_steps = 3;
int nu;
size_t i;
double *const k1nu = state->k1nu;
double *const k2nu = state->k2nu;
double *const ytmp1 = state->ytmp1;
double *const ytmp2 = state->ytmp2;
/* iterative solution of Y1 and Y2.
Note: This method does not check for convergence of the
iterative solution!
*/
for (nu = 0; nu < iter_steps; nu++)
{
for (i = 0; i < dim; i++)
{
ytmp1[i] =
y[i] + h * (0.25 * k1nu[i] + 0.5 * (0.5 - ir3) * k2nu[i]);
ytmp2[i] =
y[i] + h * (0.25 * k2nu[i] + 0.5 * (0.5 + ir3) * k1nu[i]);
}
{
int s =
GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 - ir3), ytmp1, k1nu);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
int s =
GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 + ir3), ytmp2, k2nu);
if (s != GSL_SUCCESS)
{
return s;
}
}
}
/* assignment */
for (i = 0; i < dim; i++)
{
const double d_i = 0.5 * (k1nu[i] + k2nu[i]);
y[i] += h * d_i;
}
return GSL_SUCCESS;
}
int
gsl_odeiv2_evolve_apply_fixed_step (gsl_odeiv2_evolve * e,
gsl_odeiv2_control * con,
gsl_odeiv2_step * step,
const gsl_odeiv2_system * dydt,
double *t, const double h, double y[])
{
const double t0 = *t;
int step_status;
if (e->dimension != step->dimension)
{
GSL_ERROR ("step dimension must match evolution size", GSL_EINVAL);
}
/* Save y in case of failure in a step */
DBL_MEMCPY (e->y0, y, e->dimension);
/* Calculate initial dydt once if the method can benefit. */
if (step->type->can_use_dydt_in)
{
int status = GSL_ODEIV_FN_EVAL (dydt, t0, y, e->dydt_in);
if (status)
{
return status;
}
}
if (step->type->can_use_dydt_in)
{
step_status =
gsl_odeiv2_step_apply (step, t0, h, y, e->yerr, e->dydt_in,
e->dydt_out, dydt);
}
else
{
step_status =
gsl_odeiv2_step_apply (step, t0, h, y, e->yerr, NULL, e->dydt_out,
dydt);
}
/* Return the stepper return value in case of an error */
if (step_status != GSL_SUCCESS)
{
return step_status;
}
if (con != NULL)
{
/* Calculate error level. Fail if error level exceeds desired
error level. */
double htemp = h;
const int hadjust_status
= gsl_odeiv2_control_hadjust (con, step, y, e->yerr,
e->dydt_out, &htemp);
if (hadjust_status == GSL_ODEIV_HADJ_DEC)
{
DBL_MEMCPY (y, e->y0, dydt->dimension);
e->failed_steps++;
return GSL_FAILURE;
}
}
/* Step is accepted, update status */
e->count++;
e->last_step = h;
*t = t0 + h;
return GSL_SUCCESS;
}
/* Evolution framework method.
*
* Uses an adaptive step control object
*/
int
gsl_odeiv2_evolve_apply (gsl_odeiv2_evolve * e,
gsl_odeiv2_control * con,
gsl_odeiv2_step * step,
const gsl_odeiv2_system * dydt,
double *t, double t1, double *h, double y[])
{
const double t0 = *t;
double h0 = *h;
int step_status;
int final_step = 0;
double dt = t1 - t0; /* remaining time, possibly less than h */
if (e->dimension != step->dimension)
{
GSL_ERROR ("step dimension must match evolution size", GSL_EINVAL);
}
if ((dt < 0.0 && h0 > 0.0) || (dt > 0.0 && h0 < 0.0))
{
GSL_ERROR ("step direction must match interval direction", GSL_EINVAL);
}
/* Save y in case of failure in a step */
DBL_MEMCPY (e->y0, y, e->dimension);
/* Calculate initial dydt once or reuse previous value if the method
can benefit. */
if (step->type->can_use_dydt_in)
{
if (e->count == 0)
{
int status = GSL_ODEIV_FN_EVAL (dydt, t0, y, e->dydt_in);
if (status)
{
return status;
}
}
else
{
DBL_MEMCPY (e->dydt_in, e->dydt_out, e->dimension);
}
}
try_step:
if ((dt >= 0.0 && h0 > dt) || (dt < 0.0 && h0 < dt))
{
h0 = dt;
final_step = 1;
}
else
{
final_step = 0;
}
if (step->type->can_use_dydt_in)
{
step_status =
gsl_odeiv2_step_apply (step, t0, h0, y, e->yerr, e->dydt_in,
e->dydt_out, dydt);
}
else
{
step_status =
gsl_odeiv2_step_apply (step, t0, h0, y, e->yerr, NULL, e->dydt_out,
dydt);
}
/* Return if stepper indicates a pointer or user function failure */
if (step_status == GSL_EFAULT || step_status == GSL_EBADFUNC)
{
return step_status;
}
/* Check for stepper internal failure */
if (step_status != GSL_SUCCESS)
{
/* Stepper was unable to calculate step. Try decreasing step size. */
double h_old = h0;
h0 *= 0.5;
#ifdef DEBUG
printf ("-- gsl_odeiv2_evolve_apply h0=%.5e\n", h0);
#endif
/* Check that an actual decrease in h0 occured and the
suggested h0 will change the time by at least 1 ulp */
{
double t_curr = GSL_COERCE_DBL (*t);
double t_next = GSL_COERCE_DBL ((*t) + h0);
if (fabs (h0) < fabs (h_old) && t_next != t_curr)
{
/* Step was decreased. Undo step, and try again with new h0. */
DBL_MEMCPY (y, e->y0, dydt->dimension);
e->failed_steps++;
goto try_step;
}
else
{
#ifdef DEBUG
printf
("-- gsl_odeiv2_evolve_apply h0=%.5e, t0=%.5e, step_status=%d\n",
h0, t0, step_status);
#endif
*h = h0; /* notify user of step-size which caused the failure */
*t = t0; /* restore original t value */
return step_status;
}
}
}
e->count++;
e->last_step = h0;
if (final_step)
{
*t = t1;
}
else
{
*t = t0 + h0;
}
if (con != NULL)
{
/* Check error and attempt to adjust the step. */
double h_old = h0;
const int hadjust_status
=
gsl_odeiv2_control_hadjust (con, step, y, e->yerr, e->dydt_out, &h0);
if (hadjust_status == GSL_ODEIV_HADJ_DEC)
{
/* Check that the reported status is correct (i.e. an actual
decrease in h0 occured) and the suggested h0 will change
the time by at least 1 ulp */
double t_curr = GSL_COERCE_DBL (*t);
double t_next = GSL_COERCE_DBL ((*t) + h0);
if (fabs (h0) < fabs (h_old) && t_next != t_curr)
{
/* Step was decreased. Undo step, and try again with new h0. */
DBL_MEMCPY (y, e->y0, dydt->dimension);
e->failed_steps++;
goto try_step;
}
else
{
/* Can not obtain required error tolerance, and can not
decrease step-size any further, so give up and return
GSL_FAILURE.
*/
*h = h0; /* notify user of step-size which caused the failure */
return GSL_FAILURE;
}
}
}
/* Suggest step size for next time-step. Change of step size is not
suggested in the final step, because that step can be very
small compared to previous step, to reach t1.
*/
if (final_step == 0)
{
*h = h0;
}
return step_status;
}
static int
rk4_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[],
const gsl_odeiv_system * sys)
{
rk4_state_t *state = (rk4_state_t *) vstate;
size_t i;
double *const k = state->k;
double *const k1 = state->k1;
double *const y0 = state->y0;
double *const y_onestep = state->y_onestep;
DBL_MEMCPY (y0, y, dim);
if (dydt_in != NULL)
{
DBL_MEMCPY (k, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y0, k);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Error estimation is done by step doubling procedure */
/* Save first point derivatives*/
DBL_MEMCPY (k1, k, dim);
/* First traverse h with one step (save to y_onestep) */
DBL_MEMCPY (y_onestep, y, dim);
{
int s = rk4_step (y_onestep, state, h, t, dim, sys);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Then with two steps with half step length (save to y) */
DBL_MEMCPY (k, k1, dim);
{
int s = rk4_step (y, state, h/2.0, t, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original values */
DBL_MEMCPY (y, y0, dim);
return s;
}
}
/* Update before second step */
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h/2.0, y, k);
if (s != GSL_SUCCESS)
{
/* Restore original values */
DBL_MEMCPY (y, y0, dim);
return s;
}
}
/* Save original y0 to k1 for possible failures */
DBL_MEMCPY (k1, y0, dim);
/* Update y0 for second step */
DBL_MEMCPY (y0, y, dim);
{
int s = rk4_step (y, state, h/2.0, t + h/2.0, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original values */
DBL_MEMCPY (y, k1, dim);
return s;
}
}
/* Derivatives at output */
if (dydt_out != NULL) {
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS)
{
/* Restore original values */
DBL_MEMCPY (y, k1, dim);
return s;
}
}
/* Error estimation
yerr = C * 0.5 * | y(onestep) - y(twosteps) | / (2^order - 1)
constant C is approximately 8.0 to ensure 90% of samples lie within
the error (assuming a gaussian distribution with prior p(sigma)=1/sigma.)
*/
for (i = 0; i < dim; i++)
{
yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 15.0;
}
return GSL_SUCCESS;
}
/* Evolution framework method.
*
* Uses an adaptive step control object
*/
int
gsl_odeiv_evolve_apply (gsl_odeiv_evolve * e,
gsl_odeiv_control * con,
gsl_odeiv_step * step,
const gsl_odeiv_system * dydt,
double *t, double t1, double *h, double y[])
{
const double t0 = *t;
double h0 = *h;
int step_status;
int final_step = 0;
double dt = t1 - t0; /* remaining time, possibly less than h */
if (e->dimension != step->dimension)
{
GSL_ERROR ("step dimension must match evolution size", GSL_EINVAL);
}
if ((dt < 0.0 && h0 > 0.0) || (dt > 0.0 && h0 < 0.0))
{
GSL_ERROR ("step direction must match interval direction", GSL_EINVAL);
}
/* No need to copy if we cannot control the step size. */
if (con != NULL)
{
DBL_MEMCPY (e->y0, y, e->dimension);
}
/* Calculate initial dydt once if the method can benefit. */
if (step->type->can_use_dydt_in)
{
int status = GSL_ODEIV_FN_EVAL (dydt, t0, y, e->dydt_in);
if (status)
{
return status;
}
}
try_step:
if ((dt >= 0.0 && h0 > dt) || (dt < 0.0 && h0 < dt))
{
h0 = dt;
final_step = 1;
}
else
{
final_step = 0;
}
if (step->type->can_use_dydt_in)
{
step_status =
gsl_odeiv_step_apply (step, t0, h0, y, e->yerr, e->dydt_in,
e->dydt_out, dydt);
}
else
{
step_status =
gsl_odeiv_step_apply (step, t0, h0, y, e->yerr, NULL, e->dydt_out,
dydt);
}
/* Check for stepper internal failure */
if (step_status != GSL_SUCCESS)
{
*h = h0; /* notify user of step-size which caused the failure */
return step_status;
}
e->count++;
e->last_step = h0;
if (final_step)
{
*t = t1;
}
else
{
*t = t0 + h0;
}
if (con != NULL)
{
/* Check error and attempt to adjust the step. */
double h_old = h0;
const int hadjust_status
= gsl_odeiv_control_hadjust (con, step, y, e->yerr, e->dydt_out, &h0);
if (hadjust_status == GSL_ODEIV_HADJ_DEC)
{
/* Check that the reported status is correct (i.e. an actual
decrease in h0 occured) and the suggested h0 will change
the time by at least 1 ulp */
double t_curr = GSL_COERCE_DBL(*t);
double t_next = GSL_COERCE_DBL((*t) + h0);
if (fabs(h0) < fabs(h_old) && t_next != t_curr)
{
/* Step was decreased. Undo step, and try again with new h0. */
DBL_MEMCPY (y, e->y0, dydt->dimension);
e->failed_steps++;
goto try_step;
}
else
{
h0 = h_old; /* keep current step size */
}
}
}
*h = h0; /* suggest step size for next time-step */
return step_status;
}
static int
rk2imp_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[], const gsl_odeiv_system * sys)
{
rk2imp_state_t *state = (rk2imp_state_t *) vstate;
size_t i;
double *Y1 = state->Y1;
double *y0 = state->y0;
double *y_onestep = state->y_onestep;
double *y0_orig = state->y0_orig;
/* Error estimation is done by step doubling procedure */
/* initialization step */
DBL_MEMCPY (y0, y, dim);
/* Save initial values for possible failures */
DBL_MEMCPY (y0_orig, y, dim);
if (dydt_in != NULL)
{
DBL_MEMCPY (Y1, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, Y1);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* First traverse h with one step (save to y_onestep) */
DBL_MEMCPY (y_onestep, y, dim);
{
int s = rk2imp_step (y_onestep, state, h, t, dim, sys);
if (s != GSL_SUCCESS)
{
return s;
}
}
/* Then with two steps with half step length (save to y) */
{
int s = rk2imp_step (y, state, h / 2.0, t, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
DBL_MEMCPY (y0, y, dim);
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0, y, Y1);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
{
int s = rk2imp_step (y, state, h / 2.0, t + h / 2.0, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
/* Derivatives at output */
if (dydt_out != NULL)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
/* Error estimation */
for (i = 0; i < dim; i++)
{
yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 3.0;
}
return GSL_SUCCESS;
}
static int
gear2_apply (void *vstate,
size_t dim,
double t,
double h,
double y[],
double yerr[],
const double dydt_in[],
double dydt_out[], const gsl_odeiv_system * sys)
{
gear2_state_t *state = (gear2_state_t *) vstate;
state->stutter = 0;
if (state->primed == 0 || t == state->t_primed || h != state->last_h)
{
/* Execute a single-step method to prime the process. Note that
* we do this if the step size changes, so frequent step size
* changes will cause the method to stutter.
*
* Note that we reuse this method if the time has not changed,
* which can occur when the adaptive driver is attempting to find
* an appropriate step-size on its first iteration */
int status;
DBL_MEMCPY (state->yim1, y, dim);
status =
gsl_odeiv_step_apply (state->primer, t, h, y, yerr, dydt_in, dydt_out,
sys);
/* Make note of step size and indicate readiness for a Gear step. */
state->primed = 1;
state->t_primed = t;
state->last_h = h;
state->stutter = 1;
return status;
}
else
{
/* We have a previous y value in the buffer, and the step
* sizes match, so we go ahead with the Gear step.
*/
double *const k = state->k;
double *const y0 = state->y0;
double *const y0_orig = state->y0_orig;
double *const yim1 = state->yim1;
double *y_onestep = state->y_onestep;
int s;
size_t i;
DBL_MEMCPY (y0, y, dim);
/* iterative solution */
if (dydt_out != NULL)
{
DBL_MEMCPY (k, dydt_out, dim);
}
/* First traverse h with one step (save to y_onestep) */
DBL_MEMCPY (y_onestep, y, dim);
s = gear2_step (y_onestep, state, h, t, dim, sys);
if (s != GSL_SUCCESS)
{
return s;
}
/* Then with two steps with half step length (save to y) */
s = gear2_step (y, state, h / 2.0, t, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
DBL_MEMCPY (y0, y, dim);
s = gear2_step (y, state, h / 2.0, t + h / 2.0, dim, sys);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
/* Cleanup update */
if (dydt_out != NULL)
{
s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);
if (s != GSL_SUCCESS)
{
/* Restore original y vector */
DBL_MEMCPY (y, y0_orig, dim);
return s;
}
}
/* Estimate error and update the state buffer. */
for (i = 0; i < dim; i++)
{
yerr[i] = 4.0 * (y[i] - y_onestep[i]);
yim1[i] = y0[i];
}
/* Make note of step size. */
state->last_h = h;
return 0;
}
}
static int
msbdf_apply (void *vstate, size_t dim, double t, double h,
double y[], double yerr[],
const double dydt_in[], double dydt_out[],
const gsl_odeiv2_system * sys)
{
/* Carries out a step by BDF linear multistep methods. */
msbdf_state_t *state = (msbdf_state_t *) vstate;
double *const z = state->z;
double *const zbackup = state->zbackup;
double *const ytmp = state->ytmp;
double *const ytmp2 = state->ytmp2;
double *const l = state->l;
double *const hprev = state->hprev;
double *const hprevbackup = state->hprevbackup;
size_t *const ordprev = state->ordprev;
size_t *const ordprevbackup = state->ordprevbackup;
double *const errlev = state->errlev;
gsl_vector *const abscor = state->abscor;
gsl_vector *const relcor = state->relcor;
gsl_vector *const svec = state->svec;
gsl_vector *const tempvec = state->tempvec;
size_t ord = state->ord; /* order for this step */
double ordm1coeff = 0.0;
double ordp1coeff = 0.0;
double ordp2coeff = 0.0;
double errcoeff = 0.0; /* error coefficient */
double gamma = 0.0; /* gamma coefficient */
const size_t max_failcount = 3;
size_t i;
#ifdef DEBUG
{
size_t di;
printf ("msbdf_apply: t=%.5e, ord=%d, h=%.5e, y:", t, (int) ord, h);
for (di = 0; di < dim; di++)
{
printf ("%.5e ", y[di]);
}
printf ("\n");
}
#endif
/* Check if t is the same as on previous stepper call (or last
failed call). This means that calculation of previous step failed
or the step was rejected, and therefore previous state will be
restored or the method will be reset.
*/
if (state->ni > 0 && (t == state->tprev || t == state->failt))
{
if (state->ni == 1)
{
/* No step has been accepted yet, reset method */
msbdf_reset (vstate, dim);
#ifdef DEBUG
printf ("-- first step was REJECTED, msbdf_reset called\n");
#endif
}
else
{
/* A succesful step has been saved, restore previous state. */
/* If previous step suggests order increase, but the step was
rejected, then do not increase order.
*/
if (ord > ordprev[0])
{
state->ord = ordprev[0];
ord = state->ord;
}
/* Restore previous state */
DBL_MEMCPY (z, zbackup, (MSBDF_MAX_ORD + 1) * dim);
DBL_MEMCPY (hprev, hprevbackup, MSBDF_MAX_ORD);
for (i = 0; i < MSBDF_MAX_ORD; i++)
{
ordprev[i] = ordprevbackup[i];
}
state->ordwait = state->ordwaitbackup;
state->gammaprev = state->gammaprevbackup;
#ifdef DEBUG
printf ("-- previous step was REJECTED, state restored\n");
#endif
}
/* If step is repeatedly rejected, then reset method */
state->failcount++;
if (state->failcount > max_failcount && state->ni > 1)
{
msbdf_reset (vstate, dim);
ord = state->ord;
#ifdef DEBUG
printf ("-- max_failcount reached, msbdf_reset called\n");
#endif
}
}
else
{
/* The previous step was accepted. Backup current state. */
DBL_MEMCPY (zbackup, z, (MSBDF_MAX_ORD + 1) * dim);
DBL_MEMCPY (hprevbackup, hprev, MSBDF_MAX_ORD);
for (i = 0; i < MSBDF_MAX_ORD; i++)
{
ordprevbackup[i] = ordprev[i];
}
state->ordwaitbackup = state->ordwait;
state->gammaprevbackup = state->gammaprev;
state->failcount = 0;
#ifdef DEBUG
if (state->ni > 0)
{
printf ("-- previous step was ACCEPTED, state saved\n");
}
#endif
}
#ifdef DEBUG
printf ("-- ord=%d, ni=%ld, ordwait=%d\n", (int) ord, state->ni,
(int) state->ordwait);
size_t di;
printf ("-- ordprev: ");
for (di = 0; di < MSBDF_MAX_ORD; di++)
{
printf ("%d ", (int) ordprev[di]);
}
printf ("\n");
#endif
/* Get desired error levels via gsl_odeiv2_control object through driver
object, which is a requirement for this stepper.
*/
if (state->driver == NULL)
{
return GSL_EFAULT;
}
else
{
size_t i;
for (i = 0; i < dim; i++)
{
if (dydt_in != NULL)
{
gsl_odeiv2_control_errlevel (state->driver->c, y[i],
dydt_in[i], h, i, &errlev[i]);
}
else
{
gsl_odeiv2_control_errlevel (state->driver->c, y[i],
0.0, h, i, &errlev[i]);
}
}
}
#ifdef DEBUG
{
size_t di;
printf ("-- errlev: ");
for (di = 0; di < dim; di++)
{
printf ("%.5e ", errlev[di]);
}
printf ("\n");
}
#endif
/* On first call initialize Nordsieck matrix */
if (state->ni == 0)
{
size_t i;
DBL_ZERO_MEMSET (z, (MSBDF_MAX_ORD + 1) * dim);
if (dydt_in != NULL)
{
DBL_MEMCPY (ytmp, dydt_in, dim);
}
else
{
int s = GSL_ODEIV_FN_EVAL (sys, t, y, ytmp);
if (s != GSL_SUCCESS)
{
return s;
}
}
DBL_MEMCPY (&z[0 * dim], y, dim);
DBL_MEMCPY (&z[1 * dim], ytmp, dim);
for (i = 0; i < dim; i++)
{
z[1 * dim + i] *= h;
}
}
/* Stability enhancement heuristic for msbdf: If order > 1 and order
has not been changed, check for decrease in step size, that is
not accompanied by a decrease in method order. This condition may
be indication of BDF method stability problems, a change in ODE
system, or convergence problems in Newton iteration. In all
cases, the strategy is to decrease method order.
*/
#ifdef DEBUG
printf ("-- check_no_order_decrease %d, check_step_size_decrease %d\n",
msbdf_check_no_order_decrease (ordprev),
msbdf_check_step_size_decrease (hprev));
#endif
if (ord > 1 &&
ord - ordprev[0] == 0 &&
msbdf_check_no_order_decrease (ordprev) &&
msbdf_check_step_size_decrease (hprev))
{
state->ord--;
state->ordwait = ord + 2;
ord = state->ord;
#ifdef DEBUG
printf ("-- stability enhancement decreased order to %d\n", (int) ord);
#endif
}
/* Sanity check */
{
const int deltaord = ord - ordprev[0];
if (deltaord > 1 || deltaord < -1)
{
printf ("-- order change %d\n", deltaord);
GSL_ERROR_NULL ("msbdf_apply too large order change", GSL_ESANITY);
}
/* Modify Nordsieck matrix if order or step length has been changed */
/* If order increased by 1, adjust Nordsieck matrix */
if (deltaord == 1)
{
if (ord > 2)
{
size_t i, j;
double hsum = h;
double coeff1 = -1.0;
double coeff2 = 1.0;
double hrelprev = 1.0;
double hrelprod = 1.0;
double hrel = 0.0;
/* Calculate coefficients used in adjustment to l */
DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);
l[2] = 1.0;
for (i = 1; i < ord - 1; i++)
{
hsum += hprev[i];
hrel = hsum / h;
hrelprod *= hrel;
coeff1 -= 1.0 / (i + 1);
coeff2 += 1.0 / hrel;
for (j = i + 2; j > 1; j--)
{
l[j] *= hrelprev;
l[j] += l[j - 1];
}
hrelprev = hrel;
}
/* Scale Nordsieck matrix */
{
const double c = (-coeff1 - coeff2) / hrelprod;
for (i = 0; i < dim; i++)
{
z[ord * dim + i] = c * gsl_vector_get (abscor, i);
}
}
for (i = 2; i < ord; i++)
for (j = 0; j < dim; j++)
{
z[i * dim + j] += l[i] * z[ord * dim + j];
}
}
else
{
/* zero new vector for order incease from 1 to 2 */
DBL_ZERO_MEMSET (&z[ord * dim], dim);
}
#ifdef DEBUG
printf ("-- order increase detected, Nordsieck modified\n");
#endif
}
/* If order decreased by 1, adjust Nordsieck matrix */
if (deltaord == -1)
{
size_t i, j;
double hsum = 0.0;
/* Calculate coefficients used in adjustment to l */
DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);
l[2] = 1.0;
for (i = 1; i < ord; i++)
{
hsum += hprev[i - 1];
for (j = i + 2; j > 1; j--)
{
l[j] *= hsum / h;
l[j] += l[j - 1];
}
}
/* Scale Nordsieck matrix */
for (i = 2; i < ord + 1; i++)
for (j = 0; j < dim; j++)
{
z[i * dim + j] += -l[i] * z[(ord + 1) * dim + j];
}
#ifdef DEBUG
printf ("-- order decrease detected, Nordsieck modified\n");
#endif
}
/* Scale Nordsieck vectors if step size has been changed */
if (state->ni > 0 && h != hprev[0])
{
size_t i, j;
const double hrel = h / hprev[0];
double coeff = hrel;
for (i = 1; i < ord + 1; i++)
{
for (j = 0; j < dim; j++)
{
z[i * dim + j] *= coeff;
}
coeff *= hrel;
}
#ifdef DEBUG
printf ("-- h != hprev, Nordsieck modified\n");
#endif
}
/* Calculate polynomial coefficients (l), error coefficient and
auxiliary coefficients
*/
msbdf_calccoeffs (ord, state->ordwait, h, hprev, l, &errcoeff,
º1coeff, &ordp1coeff, &ordp2coeff, &gamma);
/* Carry out the prediction step */
{
size_t i, j, k;
for (i = 1; i < ord + 1; i++)
for (j = ord; j > i - 1; j--)
for (k = 0; k < dim; k++)
{
z[(j - 1) * dim + k] += z[j * dim + k];
}
#ifdef DEBUG
{
size_t di;
printf ("-- predicted y: ");
for (di = 0; di < dim; di++)
{
printf ("%.5e ", z[di]);
}
printf ("\n");
}
#endif
}
/* Calculate correction step to abscor */
{
int s;
s = msbdf_corrector (vstate, sys, t, h, dim, z, errlev, l, errcoeff,
abscor, relcor, ytmp, ytmp2,
state->dfdy, state->dfdt, state->M,
state->p, state->rhs,
&(state->nJ), &(state->nM),
state->tprev, state->failt, gamma,
state->gammaprev, hprev[0]);
if (s != GSL_SUCCESS)
{
return s;
}
}
{
/* Add accepted final correction step to Nordsieck matrix */
size_t i, j;
for (i = 0; i < ord + 1; i++)
for (j = 0; j < dim; j++)
{
z[i * dim + j] += l[i] * gsl_vector_get (abscor, j);
}
#ifdef DEBUG
{
size_t di;
printf ("---- l: ");
for (di = 0; di < ord + 1; di++)
{
printf ("%.5e ", l[di]);
}
printf ("\n");
printf ("-- corrected y: ");
for (di = 0; di < dim; di++)
{
printf ("%.5e ", z[di]);
}
printf ("\n");
}
#endif
/* Derivatives at output */
if (dydt_out != NULL)
{
int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, dydt_out);
if (s == GSL_EBADFUNC)
{
return s;
}
if (s != GSL_SUCCESS)
{
msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
printf ("-- FAIL at user function evaluation\n");
#endif
return s;
}
}
/* Calculate error estimate */
for (i = 0; i < dim; i++)
{
yerr[i] = fabs (gsl_vector_get (abscor, i)) * errcoeff;
}
#ifdef DEBUG
{
size_t di;
printf ("-- yerr: ");
for (di = 0; di < dim; di++)
{
printf ("%.5e ", yerr[di]);
}
printf ("\n");
}
#endif
/* Save y values */
for (i = 0; i < dim; i++)
{
y[i] = z[0 * dim + i];
}
}
/* Scale abscor with errlev for later use in norm calculations */
{
size_t i;
for (i = 0; i < dim; i++)
{
gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) / errlev[i]);
}
}
/* Save items needed for evaluation of order increase on next
call, if needed
*/
if (state->ordwait == 1 && ord < MSBDF_MAX_ORD)
{
size_t i;
state->ordp1coeffprev = ordp1coeff;
for (i = 0; i < dim; i++)
{
gsl_vector_set (svec, i, gsl_vector_get (abscor, i));
}
}
/* Consider and execute order change for next step */
if (state->ordwait == 0)
{
msbdf_eval_order (abscor, tempvec, svec, errcoeff, dim, errlev,
ordm1coeff, ordp1coeff,
state->ordp1coeffprev, ordp2coeff,
hprev, h, z, &(state->ord), &(state->ordwait));
}
/* Undo scaling of abscor for possible order increase on next step */
{
size_t i;
for (i = 0; i < dim; i++)
{
gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) * errlev[i]);
}
}
/* Save information about current step in state and update counters */
{
size_t i;
for (i = MSBDF_MAX_ORD - 1; i > 0; i--)
{
hprev[i] = hprev[i - 1];
ordprev[i] = ordprev[i - 1];
}
}
hprev[0] = h;
ordprev[0] = ord;
#ifdef DEBUG
{
size_t di;
printf ("-- hprev: ");
for (di = 0; di < MSBDF_MAX_ORD; di++)
{
printf ("%.5e ", hprev[di]);
}
printf ("\n");
}
#endif
state->tprev = t;
state->ordwait--;
state->ni++;
state->gammaprev = gamma;
state->nJ++;
state->nM++;
#ifdef DEBUG
printf ("-- nJ=%d, nM=%d\n", (int) state->nJ, (int) state->nM);
#endif
}
return GSL_SUCCESS;
}
