int
gsl_sf_multiply_e(const double x, const double y, gsl_sf_result * result)
{
const double ax = fabs(x);
const double ay = fabs(y);
if(x == 0.0 || y == 0.0) {
/* It is necessary to eliminate this immediately.
*/
result->val = 0.0;
result->err = 0.0;
return GSL_SUCCESS;
}
else if((ax <= 1.0 && ay >= 1.0) || (ay <= 1.0 && ax >= 1.0)) {
/* Straddling 1.0 is always safe.
*/
result->val = x*y;
result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
return GSL_SUCCESS;
}
else {
const double f = 1.0 - 2.0 * GSL_DBL_EPSILON;
const double min = GSL_MIN_DBL(fabs(x), fabs(y));
const double max = GSL_MAX_DBL(fabs(x), fabs(y));
if(max < 0.9 * GSL_SQRT_DBL_MAX || min < (f * DBL_MAX)/max) {
result->val = GSL_COERCE_DBL(x*y);
result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
CHECK_UNDERFLOW(result);
return GSL_SUCCESS;
}
else {
OVERFLOW_ERROR(result);
}
}
}
int
gsl_sf_hypot_e(const double x, const double y, gsl_sf_result * result)
{
/* CHECK_POINTER(result) */
if(x == 0.0 && y == 0.0) {
result->val = 0.0;
result->err = 0.0;
return GSL_SUCCESS;
}
else {
const double a = fabs(x);
const double b = fabs(y);
const double min = GSL_MIN_DBL(a,b);
const double max = GSL_MAX_DBL(a,b);
const double rat = min/max;
const double root_term = sqrt(1.0 + rat*rat);
if(max < GSL_DBL_MAX/root_term) {
result->val = max * root_term;
result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
return GSL_SUCCESS;
}
else {
OVERFLOW_ERROR(result);
}
}
}
int
gsl_root_test_interval (double x_lower, double x_upper, double epsabs, double epsrel)
{
const double abs_lower = fabs(x_lower) ;
const double abs_upper = fabs(x_upper) ;
double min_abs, tolerance;
if (epsrel < 0.0)
GSL_ERROR ("relative tolerance is negative", GSL_EBADTOL);
if (epsabs < 0.0)
GSL_ERROR ("absolute tolerance is negative", GSL_EBADTOL);
if (x_lower > x_upper)
GSL_ERROR ("lower bound larger than upper bound", GSL_EINVAL);
if ((x_lower > 0.0 && x_upper > 0.0) || (x_lower < 0.0 && x_upper < 0.0))
{
min_abs = GSL_MIN_DBL(abs_lower, abs_upper) ;
}
else
{
min_abs = 0;
}
tolerance = epsabs + epsrel * min_abs ;
if (fabs(x_upper - x_lower) < tolerance)
return GSL_SUCCESS;
return GSL_CONTINUE ;
}
static VALUE rb_GSL_MIN(VALUE obj, VALUE aa, VALUE bb)
{
double a, b;
double min;
/* Need_Float(aa); Need_Float(bb);*/
a = NUM2DBL(aa);
b = NUM2DBL(bb);
min = GSL_MIN_DBL(a, b);
if (gsl_fcmp(min, a, 1.0e-10) == 0) return aa;
else return bb;
}
static int
iterate (void *vstate, gsl_multifit_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
{
lmder_state_t *state = (lmder_state_t *) vstate;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
gsl_vector *qtf = state->qtf;
gsl_vector *x_trial = state->x_trial;
gsl_vector *f_trial = state->f_trial;
gsl_vector *rptdx = state->rptdx;
gsl_vector *newton = state->newton;
gsl_vector *gradient = state->gradient;
gsl_vector *sdiag = state->sdiag;
gsl_vector *w = state->w;
gsl_vector *work1 = state->work1;
gsl_permutation *perm = state->perm;
double prered, actred;
double pnorm, fnorm1, fnorm1p, gnorm;
double ratio;
double dirder;
int iter = 0;
double p1 = 0.1, p25 = 0.25, p5 = 0.5, p75 = 0.75, p0001 = 0.0001;
if (state->fnorm == 0.0)
{
return GSL_SUCCESS;
}
/* Compute qtf = Q^T f */
gsl_vector_memcpy (qtf, f);
gsl_linalg_QR_QTvec (r, tau, qtf);
/* Compute norm of scaled gradient */
compute_gradient_direction (r, perm, qtf, diag, gradient);
{
size_t iamax = gsl_blas_idamax (gradient);
gnorm = fabs(gsl_vector_get (gradient, iamax) / state->fnorm);
}
/* Determine the Levenberg-Marquardt parameter */
lm_iteration:
iter++ ;
{
int status = lmpar (r, perm, qtf, diag, state->delta, &(state->par), newton, gradient, sdiag, dx, w);
if (status)
return status;
}
/* Take a trial step */
gsl_vector_scale (dx, -1.0); /* reverse the step to go downhill */
compute_trial_step (x, dx, state->x_trial);
pnorm = scaled_enorm (diag, dx);
if (state->iter == 1)
{
if (pnorm < state->delta)
{
#ifdef DEBUG
printf("set delta = pnorm = %g\n" , pnorm);
#endif
state->delta = pnorm;
}
}
/* Evaluate function at x + p */
/* return immediately if evaluation raised error */
{
int status = GSL_MULTIFIT_FN_EVAL_F (fdf, x_trial, f_trial);
if (status)
return status;
}
fnorm1 = enorm (f_trial);
/* Compute the scaled actual reduction */
actred = compute_actual_reduction (state->fnorm, fnorm1);
#ifdef DEBUG
printf("lmiterate: fnorm = %g fnorm1 = %g actred = %g\n", state->fnorm, fnorm1, actred);
printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
printf("dx = "); gsl_vector_fprintf(stdout, dx, "%g");
#endif
/* Compute rptdx = R P^T dx, noting that |J dx| = |R P^T dx| */
compute_rptdx (r, perm, dx, rptdx);
#ifdef DEBUG
printf("rptdx = "); gsl_vector_fprintf(stdout, rptdx, "%g");
#endif
fnorm1p = enorm (rptdx);
/* Compute the scaled predicted reduction = |J dx|^2 + 2 par |D dx|^2 */
{
double t1 = fnorm1p / state->fnorm;
double t2 = (sqrt(state->par) * pnorm) / state->fnorm;
prered = t1 * t1 + t2 * t2 / p5;
dirder = -(t1 * t1 + t2 * t2);
}
/* compute the ratio of the actual to predicted reduction */
if (prered > 0)
{
ratio = actred / prered;
}
else
{
ratio = 0;
}
#ifdef DEBUG
printf("lmiterate: prered = %g dirder = %g ratio = %g\n", prered, dirder,ratio);
#endif
/* update the step bound */
if (ratio > p25)
{
#ifdef DEBUG
printf("ratio > p25\n");
#endif
if (state->par == 0 || ratio >= p75)
{
state->delta = pnorm / p5;
state->par *= p5;
#ifdef DEBUG
printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
#endif
}
}
else
{
double temp = (actred >= 0) ? p5 : p5*dirder / (dirder + p5 * actred);
#ifdef DEBUG
printf("ratio < p25\n");
#endif
if (p1 * fnorm1 >= state->fnorm || temp < p1 )
{
temp = p1;
}
state->delta = temp * GSL_MIN_DBL (state->delta, pnorm/p1);
state->par /= temp;
#ifdef DEBUG
printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
#endif
}
/* test for successful iteration, termination and stringent tolerances */
if (ratio >= p0001)
{
gsl_vector_memcpy (x, x_trial);
gsl_vector_memcpy (f, f_trial);
/* return immediately if evaluation raised error */
{
int status;
if (fdf->df)
status = GSL_MULTIFIT_FN_EVAL_DF (fdf, x_trial, J);
else
status = gsl_multifit_fdfsolver_dif_df(x_trial, fdf, f_trial, J);
if (status)
return status;
}
/* wa2_j = diag_j * x_j */
state->xnorm = scaled_enorm(diag, x);
state->fnorm = fnorm1;
state->iter++;
/* Rescale if necessary */
if (scale)
{
update_diag (J, diag);
}
{
int signum;
gsl_matrix_memcpy (r, J);
gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
}
return GSL_SUCCESS;
}
else if (fabs(actred) <= GSL_DBL_EPSILON && prered <= GSL_DBL_EPSILON
&& p5 * ratio <= 1.0)
{
return GSL_ETOLF ;
}
else if (state->delta <= GSL_DBL_EPSILON * state->xnorm)
{
return GSL_ETOLX;
}
else if (gnorm <= GSL_DBL_EPSILON)
{
return GSL_ETOLG;
}
else if (iter < 10)
{
/* Repeat inner loop if unsuccessful */
goto lm_iteration;
}
return GSL_ENOPROG;
}
static int vector_bfgs3_iterate(void *vstate, gsl_multimin_function_fdf * fdf, gsl_vector * x, double *f, gsl_vector * gradient, gsl_vector * dx)
{
vector_bfgs3_state_t *state = (vector_bfgs3_state_t *) vstate;
double alpha = 0.0, alpha1;
gsl_vector *x0 = state->x0;
gsl_vector *g0 = state->g0;
gsl_vector *p = state->p;
double g0norm = state->g0norm;
double pnorm = state->pnorm;
double delta_f = state->delta_f;
double pg, dir;
int status;
double f0 = *f;
if (pnorm == 0.0 || g0norm == 0.0 || state->fp0 == 0) {
gsl_vector_set_zero(dx);
return GSL_ENOPROG;
}
if (delta_f < 0) {
double del = GSL_MAX_DBL(-delta_f, 10 * GSL_DBL_EPSILON * fabs(f0));
alpha1 = GSL_MIN_DBL(1.0, 2.0 * del / (-state->fp0));
} else {
alpha1 = fabs(state->step);
}
/*
* line minimisation, with cubic interpolation (order = 3)
*/
if (debug)
printf("...call minimize()\n");
status = minimize(&state->wrap.fdf_linear, state->rho, state->sigma, state->tau1, state->tau2, state->tau3, state->order, alpha1, &alpha);
if (debug)
printf("...end minimize()\n");
if (status != GSL_SUCCESS) {
update_position(&(state->wrap), alpha, x, f, gradient); /* YES! hrue */
return status;
}
update_position(&(state->wrap), alpha, x, f, gradient);
state->delta_f = *f - f0;
/*
* Choose a new direction for the next step
*/
{
/*
* This is the BFGS update:
*/
/*
* p' = g1 - A dx - B dg
*/
/*
* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg
*/
/*
* B = dx.g/dx.dg
*/
gsl_vector *dx0 = state->dx0;
gsl_vector *dg0 = state->dg0;
double dxg, dgg, dxdg, dgnorm, A, B;
/*
* dx0 = x - x0
*/
gsl_vector_memcpy(dx0, x);
gsl_blas_daxpy(-1.0, x0, dx0);
gsl_vector_memcpy(dx, dx0); /* keep a copy */
/*
* dg0 = g - g0
*/
gsl_vector_memcpy(dg0, gradient);
gsl_blas_daxpy(-1.0, g0, dg0);
gsl_blas_ddot(dx0, gradient, &dxg);
gsl_blas_ddot(dg0, gradient, &dgg);
gsl_blas_ddot(dx0, dg0, &dxdg);
dgnorm = gsl_blas_dnrm2(dg0);
if (dxdg != 0) {
B = dxg / dxdg;
A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
} else {
B = 0;
A = 0;
}
gsl_vector_memcpy(p, gradient);
gsl_blas_daxpy(-A, dx0, p);
gsl_blas_daxpy(-B, dg0, p);
}
gsl_vector_memcpy(g0, gradient);
gsl_vector_memcpy(x0, x);
state->g0norm = gsl_blas_dnrm2(g0);
state->pnorm = gsl_blas_dnrm2(p);
/*
* update direction and fp0
*/
gsl_blas_ddot(p, gradient, &pg);
dir = (pg >= 0.0) ? -1.0 : +1.0;
gsl_blas_dscal(dir / state->pnorm, p);
state->pnorm = gsl_blas_dnrm2(p);
gsl_blas_ddot(p, g0, &state->fp0);
change_direction(&state->wrap);
return GSL_SUCCESS;
}
static int
bundle_method_iterate (void *vstate, gsl_multimin_function_fsdf * fsdf, gsl_vector * x, double * f,
gsl_vector * subgradient, gsl_vector * dx, double * eps)
{
bundle_method_state_t *state = (bundle_method_state_t *) vstate;
bundle_element *item;
size_t i, debug=0;
int status;
double tmp_d, t_old, t_int_l; /* local variables */
gsl_vector *y; /* a trial point (the next iteration point by the serios step) */
gsl_vector *sgr_y; /* subgradient at y */
double f_y; /* the function value at y */
gsl_vector *p; /* the aggregate subgradient */
double p_norm, lin_error_p; /* norm of p, the aggregate linear. error */
gsl_vector *tmp_v;
/* data for the convex quadratic problem (for the dual problem) */
gsl_vector *q; /* elements of the array are the linearization errors */
gsl_matrix *Q; /* Q=G^T*G (G is matrix which collumns are subgradients) */
gsl_vector *lambda; /* the convex combination coefficients of the subgradients (solution of the dual problem) */
lambda = gsl_vector_alloc(state->bundle_size);
if(lambda == 0)
{
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
q = gsl_vector_alloc(lambda->size);
if(q == 0)
{
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
y = gsl_vector_calloc(x->size);
if(y == 0)
{
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
sgr_y = gsl_vector_calloc(x->size);
if(sgr_y == 0)
{
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
Q = gsl_matrix_alloc(state->bundle_size, state->bundle_size);
if(Q == 0)
{
gsl_vector_free(sgr_y);
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
p = gsl_vector_calloc(x->size);
if(p == 0)
{
gsl_matrix_free(Q);
gsl_vector_free(sgr_y);
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
tmp_v = gsl_vector_calloc(x->size);
if(tmp_v == 0)
{
gsl_vector_free(p);
gsl_matrix_free(Q);
gsl_vector_free(sgr_y);
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
/* solve the dual problem */
status = build_cqp_data(state, Q, q);
status = solve_qp_pdip(Q, q, lambda);
gsl_matrix_free(Q);
gsl_vector_free(q);
/* compute the aggregate subgradient (it is called p in the documantation)*/
/* and the appropriated linearization error */
lin_error_p = 0.0;
item = state->head;
for(i=0; i<lambda->size; i++)
{
status = gsl_blas_daxpy(gsl_vector_get(lambda,i), item->sgr, p);
lin_error_p += gsl_vector_get(lambda,i)*(item->lin_error);
item = item->next;
}
if(debug)
{
printf("the dual problem solution:\n");
for(i=0;i<lambda->size;i++)
printf("%7.6e ",gsl_vector_get(lambda,i));
printf("\n\n");
printf("the aggregate subgradient: \n");
for(i=0;i<p->size;i++)
printf("%.6e ",gsl_vector_get(p,i));
printf("\n");
printf("lin. error for aggr subgradient = %e\n",lin_error_p);
}
/* the norm of the aggr subgradient */
p_norm = gsl_blas_dnrm2(p);
/* search direction dx=-t*p (t is the length of step) */
status = gsl_vector_memcpy(dx,p);
status = gsl_vector_scale(dx,-1.0*state->t);
/* v =-t*norm(p)^2-alpha_p */
state->v = -gsl_pow_2(p_norm)*(state->t)-lin_error_p;
/* the subgradient is the aggegate sungradient */
status = gsl_blas_dcopy(p,subgradient);
/* iteration step */
/* y=x+dx */
status = gsl_blas_dcopy(dx,y);
status = gsl_blas_daxpy(1.0,x,y);
/* function value at y */
f_y = GSL_MULTIMIN_FN_EVAL_F(fsdf, y);
state->f_eval++;
/* for t-update */
if(!state->fixed_step_length)
{
t_old = state->t;
if(fabs(state->v-(f_y-*f)) < state->rg || state->v-(f_y-*f) > state->rg)
t_int_l = state->t_max;
else
t_int_l = 0.5*t_old*(state->v)/(state->v-(f_y-*f));
}
else
{
t_old = state->t;
t_int_l = state->t;
}
if( f_y-*f <= state->m_ss*state->v ) /* Serious-Step */
{
if(debug)
printf("\nSerious-Step\n");
/* the relaxation step */
if(state->relaxation)
{
if(f_y-*f <= state->v*state->m_rel)
{
double f_z;
gsl_vector * z = gsl_vector_alloc(y->size);
/* z = y+dx = x+2*dx */
status = gsl_blas_dcopy(x,z);
status = gsl_blas_daxpy(2.0,dx,z);
f_z = GSL_MULTIMIN_FN_EVAL_F(fsdf, z);
state->f_eval++;
if(0.5*f_z-f_y+0.5*(*f) > state->rg)
state->rel_parameter = GSL_MIN_DBL(-0.5*(-0.5*f_z+2.0*f_y-1.5*(*f))/(0.5*f_z-f_y+0.5*(*f)),1.999);
else if (fabs(0.5*f_z-f_y+0.5*(*f)) > state->rg)
state->rel_parameter = 1.999;
else
/* something is wrong */
state->rel_parameter = 1.0;
/* save the old iteration point */
status = gsl_blas_dcopy(y,z);
/* y = (1-rel_parameter)*x+rel_parameter*y */
gsl_blas_dscal(state->rel_parameter,y);
status = gsl_blas_daxpy(1.0-state->rel_parameter,x,y);
/* f(y) und sgr_f(y) */
tmp_d = GSL_MULTIMIN_FN_EVAL_F(fsdf, y);
state->f_eval++;
if(tmp_d > f_y)
{
/* keep y as the current point */
status = gsl_blas_dcopy(z,y);
state->rel_counter++;
}
else
{
f_y = tmp_d;
/* dx = y-x */
status = gsl_blas_dcopy(y,dx);
status = gsl_blas_daxpy(-1.0,x,dx);
/* if iteration points bevor and after the rel. step are closly,
the rel_step counte will be increased */
/* |1-rel_parameter| <= 0.1*/
if( fabs(1.0-state->rel_parameter) < 0.1)
state->rel_counter++;
}
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
if(state->rel_counter > state->rel_counter_max)
state->relaxation = 0;
/* */
status = gsl_blas_daxpy(-1.0,y,z);
status = gsl_blas_ddot(p, z, &tmp_d);
*eps = f_y-*f-(state->v)+tmp_d;
gsl_vector_free(z);
}
else
{
*eps = f_y-(state->v)-*f;
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
}
}
else
{
*eps = f_y-(state->v)-*f;
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
}
/* calculate linearization errors at new iteration point */
item = state->head;
for(i=0; i<state->bundle_size; i++)
{
status = gsl_blas_ddot(item->sgr, dx, &tmp_d);
item->lin_error += f_y-*f-tmp_d;
item = item->next;
}
/* linearization error at new iteration point */
status = gsl_blas_ddot(p, dx, &tmp_d);
lin_error_p += f_y-*f-tmp_d;
/* update the bundle */
status = update_bundle(state, sgr_y, 0.0, lambda, p, lin_error_p, 1);
/* adapt the step length */
if(!state->fixed_step_length)
{
if(f_y-*f <= state->v*state->m_t && state->step_counter > 0)
state->t = t_int_l;
else if(state->step_counter>3)
state->t=2.0*t_old;
state->t = GSL_MIN_DBL(GSL_MIN_DBL(state->t,10.0*t_old),state->t_max);
/*state->eps_v = GSL_MAX_DBL(state->eps_v,-2.0*state->v);*/
state->step_counter = GSL_MAX_INT(state->step_counter+1,1);
if(fabs(state->t-t_old) > state->rg)
state->step_counter=1;
}
/* x=y, f=f(y) */
status = gsl_blas_dcopy(y,x);
*f = f_y;
}
else /* Null-Step */
{
if(debug)
printf("\nNull-Step\n");
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
/* eps for the eps_subdifferential */
*eps = lin_error_p;
/*calculate the liniarization error at y */
status = gsl_blas_ddot(sgr_y,dx,&tmp_d);
tmp_d += *f-f_y;
/* Bundle update */
status = update_bundle(state, sgr_y, tmp_d, lambda, p, lin_error_p, 0);
/* adapt the step length */
if(!state->fixed_step_length)
{
/*state->eps_v = GSL_MIN_DBL(state->eps_v,lin_error_p);*/
if(tmp_d > GSL_MAX_DBL(p_norm,lin_error_p) && state->step_counter < -1)
state->t = t_int_l;
else if(state->step_counter < -3)
state->t = 0.5*t_old;
state->t = GSL_MAX_DBL(GSL_MAX_DBL(0.1*t_old,state->t),state->t_min);
state->step_counter = GSL_MIN_INT(state->step_counter-1,-1);
if(fabs(state->t-t_old) > state->rg)
state->step_counter = -1;
}
}
state->lambda_min = p_norm * state->lm_accuracy;
if(debug)
{
printf("\nthe new bundle:\n");
bundle_out_liste(state);
printf("\n\n");
printf("the curent itarationspoint (1 x %d)\n",x->size);
for(i=0;i<x->size;i++)
printf("%12.6f ",gsl_vector_get(x,i));
printf("\n\n");
printf("functions value at current point: f=%.8f\n",*f);
printf("\nstep length t=%.5e\n",state->t);
printf("\nstep_counter sc=%d\n",state->step_counter);
printf("\naccuracy: v=%.5e\n",state->v);
printf("\nlambda_min=%e\n",state->lambda_min);
printf("\n");
}
gsl_vector_free(lambda);
gsl_vector_free(y);
gsl_vector_free(sgr_y);
gsl_vector_free(p);
return GSL_SUCCESS;
}
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;
}
