static int
dnewton_iterate (void * vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
{
dnewton_state_t * state = (dnewton_state_t *) vstate;
int signum ;
size_t i;
size_t n = function->n ;
gsl_matrix_memcpy (state->lu, state->J);
{
int status = gsl_linalg_LU_decomp (state->lu, state->permutation, &signum);
if (status)
return status;
}
{
int status = gsl_linalg_LU_solve (state->lu, state->permutation, f, dx);
if (status)
return status;
}
for (i = 0; i < n; i++)
{
double e = gsl_vector_get (dx, i);
double y = gsl_vector_get (x, i);
gsl_vector_set (dx, i, -e);
gsl_vector_set (x, i, y - e);
}
{
int status = GSL_MULTIROOT_FN_EVAL (function, x, f);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, state->J);
return GSL_SUCCESS;
}
static int
dnewton_set (void * vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
{
dnewton_state_t * state = (dnewton_state_t *) vstate;
size_t i, n = function->n ;
GSL_MULTIROOT_FN_EVAL (function, x, f);
gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, state->J);
for (i = 0; i < n; i++)
{
gsl_vector_set (dx, i, 0.0);
}
return GSL_SUCCESS;
}
static int
set (void *vstate, gsl_multiroot_function * func, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale)
{
hybrid_state_t *state = (hybrid_state_t *) vstate;
gsl_matrix *J = state->J;
gsl_matrix *q = state->q;
gsl_matrix *r = state->r;
gsl_vector *tau = state->tau;
gsl_vector *diag = state->diag;
GSL_MULTIROOT_FN_EVAL (func, x, f);
gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J) ;
state->iter = 1;
state->fnorm = enorm (f);
state->ncfail = 0;
state->ncsuc = 0;
state->nslow1 = 0;
state->nslow2 = 0;
gsl_vector_set_all (dx, 0.0);
/* Store column norms in diag */
if (scale)
compute_diag (J, diag);
else
gsl_vector_set_all (diag, 1.0);
/* Set delta to factor |D x| or to factor if |D x| is zero */
state->delta = compute_delta (diag, x);
/* Factorize J into QR decomposition */
gsl_linalg_QR_decomp (J, tau);
gsl_linalg_QR_unpack (J, tau, q, r);
return GSL_SUCCESS;
}
static int
iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x, gsl_vector * f, gsl_vector * dx, int scale)
{
hybrid_state_t *state = (hybrid_state_t *) vstate;
const double fnorm = state->fnorm;
gsl_matrix *J = state->J;
gsl_matrix *q = state->q;
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 *df = state->df;
gsl_vector *qtdf = state->qtdf;
gsl_vector *rdx = state->rdx;
gsl_vector *w = state->w;
gsl_vector *v = state->v;
double prered, actred;
double pnorm, fnorm1, fnorm1p;
double ratio;
double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
/* Compute qtf = Q^T f */
compute_qtf (q, f, qtf);
/* Compute dogleg step */
dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
/* Take a trial step */
compute_trial_step (x, dx, state->x_trial);
pnorm = scaled_enorm (diag, dx);
if (state->iter == 1)
{
if (pnorm < state->delta)
{
state->delta = pnorm;
}
}
/* Evaluate function at x + p */
{
int status = GSL_MULTIROOT_FN_EVAL (func, x_trial, f_trial);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
/* Set df = f_trial - f */
compute_df (f_trial, f, df);
/* Compute the scaled actual reduction */
fnorm1 = enorm (f_trial);
actred = compute_actual_reduction (fnorm, fnorm1);
/* Compute rdx = R dx */
compute_rdx (r, dx, rdx);
/* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
fnorm1p = enorm_sum (qtf, rdx);
prered = compute_predicted_reduction (fnorm, fnorm1p);
/* Compute the ratio of the actual to predicted reduction */
if (prered > 0)
{
ratio = actred / prered;
}
else
{
ratio = 0;
}
/* Update the step bound */
if (ratio < p1)
{
state->ncsuc = 0;
state->ncfail++;
state->delta *= p5;
}
else
{
state->ncfail = 0;
state->ncsuc++;
if (ratio >= p5 || state->ncsuc > 1)
state->delta = GSL_MAX (state->delta, pnorm / p5);
if (fabs (ratio - 1) <= p1)
state->delta = pnorm / p5;
}
/* Test for successful iteration */
if (ratio >= p0001)
{
gsl_vector_memcpy (x, x_trial);
gsl_vector_memcpy (f, f_trial);
state->fnorm = fnorm1;
state->iter++;
}
/* Determine the progress of the iteration */
state->nslow1++;
if (actred >= p001)
state->nslow1 = 0;
if (actred >= p1)
state->nslow2 = 0;
if (state->ncfail == 2)
{
gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J) ;
state->nslow2++;
if (state->iter == 1)
{
if (scale)
compute_diag (J, diag);
state->delta = compute_delta (diag, x);
}
else
{
if (scale)
update_diag (J, diag);
}
/* Factorize J into QR decomposition */
gsl_linalg_QR_decomp (J, tau);
gsl_linalg_QR_unpack (J, tau, q, r);
return GSL_SUCCESS;
}
/* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|, v = D^2 dx/|dx| */
compute_qtf (q, df, qtdf);
compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
/* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
gsl_linalg_QR_update (q, r, w, v);
/* No progress as measured by jacobian evaluations */
if (state->nslow2 == 5)
{
return GSL_ENOPROGJ;
}
/* No progress as measured by function evaluations */
if (state->nslow1 == 10)
{
return GSL_ENOPROG;
}
return GSL_SUCCESS;
}
int
gsl_multiroot_fdjacobian (gsl_multiroot_function * F,
const gsl_vector * x, const gsl_vector * f,
double epsrel, gsl_matrix * jacobian)
{
const size_t n = x->size;
const size_t m = f->size;
const size_t n1 = jacobian->size1;
const size_t n2 = jacobian->size2;
if (m != n1 || n != n2)
{
GSL_ERROR ("function and jacobian are not conformant", GSL_EBADLEN);
}
{
size_t i,j;
gsl_vector *x1, *f1;
x1 = gsl_vector_alloc (n);
if (x1 == 0)
{
GSL_ERROR ("failed to allocate space for x1 workspace", GSL_ENOMEM);
}
f1 = gsl_vector_alloc (m);
if (f1 == 0)
{
gsl_vector_free (x1);
GSL_ERROR ("failed to allocate space for f1 workspace", GSL_ENOMEM);
}
gsl_vector_memcpy (x1, x); /* copy x into x1 */
for (j = 0; j < n; j++)
{
double xj = gsl_vector_get (x, j);
double dx = epsrel * fabs (xj);
if (dx == 0)
{
dx = epsrel;
}
gsl_vector_set (x1, j, xj + dx);
{
int status = GSL_MULTIROOT_FN_EVAL (F, x1, f1);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
gsl_vector_set (x1, j, xj);
for (i = 0; i < m; i++)
{
double g1 = gsl_vector_get (f1, i);
double g0 = gsl_vector_get (f, i);
gsl_matrix_set (jacobian, i, j, (g1 - g0) / dx);
}
}
gsl_vector_free (x1);
gsl_vector_free (f1);
}
return GSL_SUCCESS;
}