std::pair<std::vector<T>,std::vector<T> > FindRootsGSL(
const TF &f,
const T *guess,
const gsl_multiroot_fdfsolver_type *solveType= gsl_multiroot_fdfsolver_hybridsj
){
boost::shared_ptr<gsl_multiroot_fdfsolver> fsolve(
gsl_multiroot_fdfsolver_alloc(solveType, f.arity()),
gsl_multiroot_fdfsolver_free
);
if(fsolve==0)
throw std::logic_error("Couldn't allocate GSL solver.");
boost::shared_ptr<gsl_vector> gslGuess=detail::ToGSL(guess, guess+f.arity());
detail::GSLFindRootsWrapper<TF> wrapper(f);
gsl_multiroot_fdfsolver_set(fsolve.get(), &wrapper.m_fdf, gslGuess.get());
while(true){
int code=gsl_multiroot_fdfsolver_iterate(fsolve.get());
if((code==GSL_ENOPROG) || (code==GSL_ENOPROGJ))
break;
if(code!=0)
throw std::logic_error("Error while trying to find function roots using GSL.");
}
return std::make_pair(
detail::FromGSL<T>(gsl_multiroot_fdfsolver_root(fsolve.get())),
detail::FromGSL<T>(gsl_multiroot_fdfsolver_f(fsolve.get()))
);
}
int binom_solver(const double* fq, double* rs, const double* ival, double epsabs, double epsrel, int max_iter)
{
#ifdef USE_R
gsl_set_error_handler_off ();
#endif
double params[2]; memmove(params, fq, 2 * sizeof(double));
// fq[0] = prior[0]; fq[1] = prior[1];
const gsl_multiroot_fdfsolver_type *T;
gsl_multiroot_fdfsolver *s;
const size_t n = 2;
// Set up F.
gsl_multiroot_function_fdf F = {&binom_transform_gsl,
&binom_transform_df,
&binom_transform_fdf,
n, (void *)params};
// Set up initial vector.
gsl_vector* x = gsl_vector_alloc(2);
memcpy(x->data, ival, 2 * sizeof(double));
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc (T, n);
gsl_multiroot_fdfsolver_set (s, &F, x);
// Rprintf("x: %g, %g \t f: %g, %g\n", s->x->data[0], s->x->data[1], s->f->data[0], s->f->data[0]);
int i = 0;
int msg = GSL_CONTINUE;
for(i = 0; i < max_iter && msg != GSL_SUCCESS; i++) {
msg = gsl_multiroot_fdfsolver_iterate(s);
if (msg == GSL_EBADFUNC || msg == GSL_ENOPROG) break;
// Rprintf("x: %g, %g \t f: %g, %g \t dx: %g, %g\n", s->x->data[0], s->x->data[1],
// s->f->data[0], s->f->data[0], s->dx->data[0], s->dx->data[1]);
// check |dx| < epsabs + epsrel * |x|
msg = gsl_multiroot_test_delta(s->dx, s->x, epsabs, epsrel);
}
// You can turn off GSL error handling so it doesn't crash things.
if (msg != GSL_SUCCESS) {
Rprintf( "CUBS_udpate.cpp::solver Error %i. Break on %i.\n", msg, i);
Rprintf( "error: %s\n", gsl_strerror (msg));
Rprintf( "Init: r=%g, s=%g, f=%g, q=%g\n", ival[0], ival[1], fq[0], fq[1]);
Rprintf( "Exit: r=%g, s=%g, ", s->x->data[0], s->x->data[1]);
Rprintf( "F0=%g, F1=%g, ", s->f->data[0], s->f->data[1]);
Rprintf( "D0=%g, D1=%g\n", s->dx->data[0], s->dx->data[1]);
}
memmove(rs, s->x->data, 2 * sizeof(double));
// Free mem.
gsl_multiroot_fdfsolver_free (s);
gsl_vector_free (x);
return msg;
}
static void cv(gsl_vector * lean, gsl_vector * pitch,
const gsl_vector * steer, Whipple * bike)
{
int i, N = lean->size, iter, iter_max = ITER_MAX, status;
double ftol = FTOL;
gsl_vector * x = gsl_vector_alloc(2); // vector to store the solution
const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
gsl_multiroot_function_fdf f = {&cv_f, &cv_df, &cv_fdf, 2, bike};
gsl_vector_set(x, 0, bike->q1);
gsl_vector_set(x, 1, bike->q2);
bike->q3 = gsl_vector_get(steer, 0);
gsl_multiroot_fdfsolver_set(s, &f, x);
gsl_vector_set(lean, 0, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, 0, gsl_vector_get(s->x, 1));
// for loop to loop over all values of steer
for (i = 1; i < N - 1; ++i) {
bike->q3 = gsl_vector_get(steer, i); // steer as a parameter
iter = 0;
do
{
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "cv()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while (status == GSL_CONTINUE && ++iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
//if (iter == iter_max) {
// gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
// gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
// gsl_multiroot_fdfsolver_set(s, &f, x);
// increaseftol(&ftol, &i, iter_max, "cv()", bike->q3);
// continue;
//} // if
// Store the lean into the lean vector
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
// cout << gsl_vector_get(lean, i) << ", "
// << gsl_vector_get(pitch, i) << ", "
// << gsl_vector_get(steer, i) << '\n';
//ftol = FTOL;
} // for
bike->q1 = 0.0; bike->q3 = M_PI; bike->calcPitch();
gsl_vector_set(lean, i, 0.0);
gsl_vector_set(pitch, i, bike->q2);
// Free dynamically allocated variables
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(x);
} // cv()
CAMLprim value ml_gsl_multiroot_fdfsolver_set(value S, value fun, value X)
{
CAMLparam2(S,X);
struct callback_params *p=CALLBACKPARAMS_VAL(S);
_DECLARE_VECTOR(X);
_CONVERT_VECTOR(X);
p->closure = fun;
if(v_X.size != p->gslfun.mrfdf.n)
GSL_ERROR("wrong number of dimensions for function", GSL_EBADLEN);
gsl_multiroot_fdfsolver_set(GSLMULTIROOTFDFSOLVER_VAL(S),
&(p->gslfun.mrfdf), &v_X);
CAMLreturn(Val_unit);
}
int main(void) {
const gsl_multiroot_fdfsolver_type *T;
gsl_multiroot_fdfsolver *s;
int status;
size_t i, iter = 0;
const size_t n = 2;
struct rparams p = {1.0, 10.0};
gsl_multiroot_function_fdf f = {&rosenbrock_f,
&rosenbrock_df,
&rosenbrock_fdf,
n, &p};
double x_init[2] = {-10.0, -5.0};
gsl_vector *x = gsl_vector_alloc (n);
gsl_vector_set (x, 0, x_init[0]);
gsl_vector_set (x, 1, x_init[1]);
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc (T, n);
gsl_multiroot_fdfsolver_set (s, &f, x);
print_state (iter, s);
do {
iter++;
status = gsl_multiroot_fdfsolver_iterate (s);
print_state (iter, s);
if (status)
break;
status = gsl_multiroot_test_residual (s->f, 1e-7);
} while (status == GSL_CONTINUE && iter < 1000);
printf ("status = %s\n", gsl_strerror (status));
gsl_multiroot_fdfsolver_free (s);
gsl_vector_free (x);
return EXIT_SUCCESS;
}
SteamState freesteam_solver2_region1(FREESTEAM_CHAR A, FREESTEAM_CHAR B, double atarget, double btarget, SteamState guess, int *retstatus){
const gsl_multiroot_fdfsolver_type *T;
gsl_multiroot_fdfsolver *s;
int status;
size_t iter = 0;
const size_t n = 2;
//fprintf(stderr,"region 1 solver...\n");
Solver2Data D = {A,B,solver2_region1_propfn(A), solver2_region1_propfn(B), atarget,btarget};
gsl_multiroot_function_fdf f = {®ion1_f, ®ion1_df, ®ion1_fdf, n, &D};
gsl_vector *x = gsl_vector_alloc(n);
gsl_vector_set(x, 0, freesteam_rho(guess));
gsl_vector_set(x, 1, freesteam_T(guess));
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc(T, n);
gsl_multiroot_fdfsolver_set(s, &f, x);
//region1_print_state(iter, s);
do{
iter++;
status = gsl_multiroot_fdfsolver_iterate(s);
//region1_print_state(iter, s);
if(status){
/* check if solver is stuck */
break;
}
status = gsl_multiroot_test_residual(s->f, 1e-6);
} while(status == GSL_CONTINUE && iter < 20);
SteamState S = freesteam_region1_set_pT(gsl_vector_get(s->x,0), gsl_vector_get(s->x,1));
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(x);
*retstatus = status;
if(status){
fprintf(stderr,"%s (%s:%d): %s: ",__func__,__FILE__,__LINE__,gsl_strerror(status));
freesteam_fprint(stderr,S);
}
return S;
}
static void infspeed(gsl_vector * lean, gsl_vector * pitch,
double lean_ig, double pitch_ig, int ig_index,
const gsl_vector * steer, Whipple * bike)
{
int i, N = lean->size, iter, status, iter_max = ITER_MAX;
double ftol = FTOL;
gsl_vector * x = gsl_vector_alloc(2); // vector to store the solution
const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
gsl_multiroot_function_fdf f = {&inf_f, &inf_df, &inf_fdf, 2, bike};
// Setup the initial conditions
bike->q1 = lean_ig;
bike->q2 = pitch_ig;
bike->q3 = gsl_vector_get(steer, ig_index);
bike->calcPitch();
gsl_vector_set(x, 0, lean_ig);
gsl_vector_set(x, 1, bike->q2);
gsl_multiroot_fdfsolver_set(s, &f, x);
for (i = ig_index; i > 0; --i) {
bike->q3 = gsl_vector_get(steer, i);
iter = 0;
do {
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "infspeed()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while(status == GSL_CONTINUE && ++iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
//if (iter == iter_max) {
// gsl_vector_set(x, 0, gsl_vector_get(lean, i+1));
// gsl_vector_set(x, 1, gsl_vector_get(pitch, i+1));
// gsl_multiroot_fdfsolver_set(s, &f, x);
// increaseftol(&ftol, &i, iter_max, "infspeed()", bike->q3);
// continue;
//} // if
// Store the lean into the lean vector
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
ftol = FTOL; // reset ftol
} // for
gsl_vector_set(lean, i, gsl_vector_get(lean, 1));
gsl_vector_set(pitch, i, gsl_vector_get(pitch, 1));
// Setup the initial conditions
bike->q1 = lean_ig;
bike->q2 = pitch_ig;
bike->q3 = gsl_vector_get(steer, ig_index);
bike->calcPitch();
gsl_vector_set(x, 0, lean_ig);
gsl_vector_set(x, 1, bike->q2);
gsl_multiroot_fdfsolver_set(s, &f, x);
for (i = ig_index + 1; i < N - 1; ++i) {
bike->q3 = gsl_vector_get(steer, i);
iter = 0;
do {
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "infspeed()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while (status == GSL_CONTINUE && ++iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
if (iter == iter_max) {
gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
gsl_multiroot_fdfsolver_set(s, &f, x);
increaseftol(&ftol, &i, iter_max, "infspeed()", bike->q3);
continue;
} // if
// Store the lean into the lean vector
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
ftol = FTOL; // reset ftol
} // for
gsl_vector_set(lean, i, gsl_vector_get(lean, i - 1));
gsl_vector_set(pitch, i, gsl_vector_get(pitch, i - 1));
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(x);
}
static void cfglim(gsl_vector * lean_max, gsl_vector * pitch_max,
gsl_vector * lean_min, gsl_vector * pitch_min,
const gsl_vector * steer, Whipple * bike)
{
int i, N = steer->size, iter = 0, iter_max = ITER_MAX, status;
double ftol = FTOL;
gsl_vector * x = gsl_vector_alloc(2); // vector to store the solution
gsl_vector * lean, * pitch;
const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
gsl_multiroot_function_fdf f = {&cfglim_f, &cfglim_df,
&cfglim_fdf, 2, bike};
// Maximum lean initial guess
gsl_vector_set(x, 0, M_PI/3.0);
gsl_vector_set(x, 1, M_PI/2.0);
lean = lean_max; // set lean to point at max lean vector
pitch = pitch_max; // set pitch to point at max pitch vector
for (int c = 0; c < 2; gsl_vector_set(x, 0, -M_PI/3.0), // min lean i.g.
gsl_vector_set(x, 1, M_PI/2.0),
lean = lean_min, // point at min lean vector
pitch = pitch_min, // point at min pitch vector
++c) {
gsl_multiroot_fdfsolver_set(s, &f, x);
for (i = N / 2; i < N - 1; ++i) {
bike->q3 = gsl_vector_get(steer, i); // steer as a parameter
iter = 0;
do
{
++iter;
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "cfglim()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while(status == GSL_CONTINUE && iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
//if (iter == iter_max) {
// gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
// gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
// gsl_multiroot_fdfsolver_set(s, &f, x);
// increaseftol(&ftol, &i, iter_max, "cfglim()", bike->q3);
// continue;
//} // if
// Store the lean and pitch
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
ftol = FTOL; // reset FTOL
} // for i (steer from PI/2 to PI)
gsl_vector_set(lean, i, gsl_vector_get(lean, i-1));
gsl_vector_set(pitch, i, gsl_vector_get(pitch, i-1));
gsl_vector_set(x, 0, gsl_vector_get(lean, N/2));
gsl_vector_set(x, 1, gsl_vector_get(pitch, N/2));
gsl_multiroot_fdfsolver_set(s, &f, x);
for (i = N / 2 - 1; i > 0; --i) {
bike->q3 = gsl_vector_get(steer, i); // steer as a parameter
iter = 0;
do
{
++iter;
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "cfglim()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while(status == GSL_CONTINUE && iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
//if (iter == iter_max) {
// gsl_vector_set(x, 0, gsl_vector_get(lean, i+1));
// gsl_vector_set(x, 1, gsl_vector_get(pitch, i+1));
// gsl_multiroot_fdfsolver_set(s, &f, x);
// increaseftol(&ftol, &i, iter_max, "cfglim()", bike->q3);
// continue;
//} // if
// Store the lean and pitch
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
// Reset ftol in case it had been increased due to convergence issues
ftol = FTOL;
} // for i (steer from PI/2 to O)
gsl_vector_set(lean, 0, gsl_vector_get(lean, 1));
gsl_vector_set(pitch, 0, gsl_vector_get(pitch, 1));
} // for c
// Free dynamically allocated variables
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(x);
} // cfglim()
// Given a vector of steer values, calculate the lean values associated with
// static equilibrium. Also, return the indices of the steer/lean vectors
// which most nearly cause the u5^2 coefficient to go to zero.
static int staticEq(gsl_vector * lean, gsl_vector * pitch,
const gsl_vector * steer, Whipple * bike)
{
int i, N = lean->size, iter, iter_max = ITER_MAX, status;
double ftol = FTOL;
gsl_vector * x = gsl_vector_alloc(2); // vector to store the solution
gsl_vector * u5s_coefs = zeros(steer->size);
const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
gsl_multiroot_function_fdf f = {&static_f, &static_df, &static_fdf, 2, bike};
bike->q1 = bike->q3 = 0.0;
bike->calcPitch();
gsl_vector_set(x, 0, bike->q1);
gsl_vector_set(x, 1, bike->q2);
gsl_multiroot_fdfsolver_set(s, &f, x);
// for loop to loop over all values of steer
for (i = 0; i < N; ++i) {
bike->q3 = gsl_vector_get(steer, i); // steer as a parameter
iter = 0;
do
{
++iter;
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "staticEq()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while (status == GSL_CONTINUE && iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
if (iter == iter_max) {
gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
gsl_multiroot_fdfsolver_set(s, &f, x);
increaseftol(&ftol, &i, iter_max, "staticEq()", bike->q3);
continue;
} // if
// Store the lean into the lean vector
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
// Store the square of the coefficient of the u5^2 term;
gsl_vector_set(u5s_coefs, i, bike->F[10] * bike->F[10]);
ftol = FTOL; // reset the error tolerance
} // for
// Assign a large value to the u5s_coefs vector near steer = 0 and steer = PI
// This ensure the minimum will be near PI/2 where the two boudary curves
// cross
for (i = 0; i < 5; ++i) {
gsl_vector_set(u5s_coefs, i, 10000.0);
gsl_vector_set(u5s_coefs, u5s_coefs->size - 1 - i, 10000.0);
}
// Free dynamically allocated variables
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(x);
i = gsl_vector_min_index(u5s_coefs);
gsl_vector_free(u5s_coefs);
return i;
} // staticEq()
int
test_fdf (const char * desc, gsl_multiroot_function_fdf * function,
initpt_function initpt, double factor,
const gsl_multiroot_fdfsolver_type * T)
{
int status;
double residual = 0;
size_t i, n = function->n, iter = 0;
gsl_vector *x = gsl_vector_alloc (n);
gsl_matrix *J = gsl_matrix_alloc (n, n);
gsl_multiroot_fdfsolver *s;
(*initpt) (x);
if (factor != 1.0) scale(x, factor);
s = gsl_multiroot_fdfsolver_alloc (T, n);
gsl_multiroot_fdfsolver_set (s, function, x);
do
{
iter++;
status = gsl_multiroot_fdfsolver_iterate (s);
if (status)
break ;
status = gsl_multiroot_test_residual (s->f, 0.0000001);
}
while (status == GSL_CONTINUE && iter < 1000);
#ifdef DEBUG
printf("x ");
gsl_vector_fprintf (stdout, s->x, "%g");
printf("\n");
printf("f ");
gsl_vector_fprintf (stdout, s->f, "%g");
printf("\n");
#endif
#ifdef TEST_JACOBIAN
{
double r,sum;
size_t j;
gsl_multiroot_function f1 ;
f1.f = function->f ;
f1.n = function->n ;
f1.params = function->params ;
gsl_multiroot_fdjacobian (&f1, s->x, s->f, GSL_SQRT_DBL_EPSILON, J);
/* compare J and s->J */
r=0;
sum=0;
for (i = 0; i < n; i++)
for (j = 0; j< n ; j++)
{
double u = gsl_matrix_get(J,i,j);
double su = gsl_matrix_get(s->J, i, j);
r = fabs(u - su)/(1e-6 + 1e-6 * fabs(u));
sum+=r;
if (fabs(u - su) > 1e-6 + 1e-6 * fabs(u))
printf("broken jacobian %g\n", r);
}
printf("avg r = %g\n", sum/(n*n));
}
#endif
for (i = 0; i < n ; i++)
{
residual += fabs(gsl_vector_get(s->f, i));
}
gsl_multiroot_fdfsolver_free (s);
gsl_matrix_free(J);
gsl_vector_free(x);
gsl_test(status, "%s on %s (%g), %u iterations, residual = %.2g", T->name, desc, factor, iter, residual);
return status;
}
void newton_remap(double xres[], long *success)
{
double *x_init, zero[2], def1i, def2i, fctgrid, dx;
const gsl_multiroot_fdfsolver_type *T;
gsl_multiroot_fdfsolver *s;
int status;
size_t iter = 0;
const size_t n = 2;
gsl_vector * x = gsl_vector_alloc (NDIM_N);
gsl_multiroot_function_fdf f = {&lenseq_f_remap,
&lenseq_df_remap,
&lenseq_fdf_remap,
n,NULL}; /* first is the function, then the derivative, then both, then number of simultaneous equations, then params -- Added comment AH 10/29/2015 */
fctgrid = ((double) lens_grid.nedge-1)/(LX);
dx = (LX / ((double) lens_grid.nedge-1));
if ((x_init = (double *) malloc((n)*sizeof(double))) == NULL)
error("main","can't allocate memory for caustic");
x_init[0] = xres[0];
x_init[1] = xres[1];
gsl_vector_set (x, 0, x_init[0]);
gsl_vector_set (x, 1, x_init[1]);
T = gsl_multiroot_fdfsolver_newton;
s = gsl_multiroot_fdfsolver_alloc (T, n);
gsl_multiroot_fdfsolver_set (s, &f, x);
do
{
iter++;
status = gsl_multiroot_fdfsolver_iterate (s);
if (status)
break;
status = gsl_multiroot_test_residual (s->f, 0.001);
}
while (status == GSL_CONTINUE && iter < 100);
xres[0] = gsl_vector_get (s->x, 0);
xres[1] = gsl_vector_get (s->x, 1);
def1i = interp(lens_grid.def1l, xres[0], xres[1], lens_grid.nedge, lens_grid.nedge, LX, LX);
def2i = interp(lens_grid.def2l, xres[0], xres[1], lens_grid.nedge, lens_grid.nedge, LX, LX);
zero[0] = gsl_vector_get (s->f, 0);
zero[1] = gsl_vector_get (s->f, 1);
gsl_multiroot_fdfsolver_free(s);
zero[0] = xres[0]*fctgrid - def1i - lens_grid.y01;
zero[1] = xres[1]*fctgrid - def2i - lens_grid.y02;
if (iter > 98 || fabs(zero[0]) > 10.0 || fabs(zero[1]) > 10.0) *success = 0;
else *success = 1;
free(x_init);
return;
}
/** *************************************************************************************
*****************************************************************************************
*****************************************************************************************/
double g_inner_gaus( gsl_vector *beta, const datamatrix *designdata, int groupid, double epsabs, int maxiters, int verbose){
/** this function perform a Laplace approx on a single data group given fixed beta, so only integrate over single term epsilon **/
/* const gsl_multiroot_fdfsolver_type *T;
gsl_multiroot_fdfsolver *s;
gsl_multiroot_function_fdf FDF;*/
struct fnparams gparams;/** for passing to the gsl zero finding functions */
/*double epsilon=0;*//** the variable we want to find the root of **/
gsl_vector *epsilon = gsl_vector_alloc (1);
gsl_vector *dgvalues = gsl_vector_alloc (1);
gsl_matrix *hessgvalue = gsl_matrix_alloc (1,1);
/*int iter=0;*/
/*int status;*/
/*double epsabs=1e-5;
int maxiters=100;*/
/*int verbose=1;*/
gsl_vector *vectmp1 = gsl_vector_alloc (designdata->numparams+1);/** scratch space same length as number of params inc precision **/
gsl_vector *vectmp1long = gsl_vector_alloc ( ((designdata->array_of_Y)[groupid])->size);/** scratch space same length as number of obs in group j**/
gsl_vector *vectmp2long = gsl_vector_alloc ( ((designdata->array_of_Y)[groupid])->size);
double logscore;
double gvalue;int n,m;
/*for(i=0;i<beta->size;i++){Rprintf("g_inner_gaus=%f\n",gsl_vector_get(beta,i));}*/
/*Rprintf("I HAVE epsabs_inner=%f maxiters_inner=%d verbose=%d\n",epsabs,maxiters,verbose);*/
/*
FDF.f = &rv_dg_inner_gaus;
FDF.df = &rv_hessg_inner_gaus;
FDF.fdf = &wrapper_rv_fdf_inner_gaus;
FDF.n = 1;
FDF.params = &gparams;
*/
gparams.Y=designdata->array_of_Y[groupid];
gparams.X=designdata->array_of_designs[groupid];
gparams.beta=beta;/** inc group and residual precision **/
/*Rprintf("tau in g_inner=%f\n",gsl_vector_get(beta,beta->size-1));
if(gsl_vector_get(beta,beta->size-1)<0.0){Rprintf("got negative tau!!=%f\n",gsl_vector_get(beta,beta->size-1));error("");}*/
gparams.vectmp1=vectmp1;/** same length as beta but used as scratch space */
gparams.vectmp1long=vectmp1long;
gparams.vectmp2long=vectmp2long;
/** ******************** FIRST TRY for a root using hybridsj *******************************************************/
#ifdef NO
iter=0;
/*T = gsl_root_fdfsolver_newton;
s = gsl_root_fdfsolver_alloc (T);*/
T = gsl_multiroot_fdfsolver_hybridsj;
s = gsl_multiroot_fdfsolver_alloc (T, 1);
status=GSL_FAILURE;/** just set it to something not equal to GSL_SUCCESS */
/*status_inits=generate_inits_rv_n(x,&gparams);*/
gsl_vector_set(epsilon,0,0.0);/** initial guess */
/*gsl_root_fdfsolver_set (s, &FDF, epsilon);*/
gsl_multiroot_fdfsolver_set (s, &FDF, epsilon);
/*Rprintf ("using %s method\n",
gsl_root_fdfsolver_name (s));
Rprintf ("%-5s %10s %10s %10s\n",
"iter", "root", "err", "err(est)");
*/
/*print_state (iter, s);*/
iter=0;
do
{
iter++;
status = gsl_multiroot_fdfsolver_iterate (s);
/*print_state (iter, s);*/
if (status)
break;
status = gsl_multiroot_test_residual (s->f, epsabs);
}
while (status == GSL_CONTINUE && iter < maxiters);
if( status != GSL_SUCCESS){Rprintf ("Zero finding warning: internal--- epsilon status = %s\n", gsl_strerror (status));
/*for(i=0;i<s->x->size;i++){Rprintf("0epsilon=%f ",gsl_vector_get(s->x,i));}Rprintf("\n");*/}
gsl_vector_memcpy(epsilon,s->x);
Rprintf("modes: %f\n",gsl_vector_get(epsilon,0));
gsl_multiroot_fdfsolver_free(s);
/*Rprintf("x=%5.10f f=%5.10f\n",gsl_root_fdfsolver_root(s),rv_dg_inner(gsl_root_fdfsolver_root(s),&gparams));*/
/* if(status != GSL_SUCCESS){*//*error("no root\n");*//*Rprintf("binary no root at node %d\n",groupid+1);*//*logscore= DBL_MAX;*/ /** root finding failed so discard model by setting fit to worst possible */
/*} else {*/
/*gsl_vector_set(epsilon,0,0.3);*/
#endif
rv_dg_inner_gaus(epsilon,&gparams, dgvalues);/** value is returned in dgvalues - first entry **/
gsl_vector_memcpy(epsilon,dgvalues);/** copy value dgvalues into epsilon */
/*Rprintf("mode for epsilon=%f\n",gsl_vector_get(epsilon,0));*/
rv_g_inner_gaus(epsilon,&gparams, &gvalue);/*Rprintf("==>g()=%e %f tau=%f\n",gvalue,gsl_vector_get(epsilon,0),gsl_vector_get(beta,2));*/
/*if(status != GSL_SUCCESS){Rprintf("1epsilon=%f %f\n",gsl_vector_get(epsilon,0), gvalue);}*/
rv_hessg_inner_gaus(epsilon,&gparams, hessgvalue);
/* Rprintf("node=%d hessian at g\n",nodeid+1);
for(j=0;j<myBeta->size;j++){Rprintf("%f ",gsl_vector_get(myBeta,j));}Rprintf("\n");
for(j=0;j<hessgvalue->size1;j++){
for(k=0;k<hessgvalue->size2;k++){Rprintf("%f ",gsl_matrix_get(hessgvalue,j,k));} Rprintf("\n");}*/
/*Rprintf("epsilon=%f\n",epsilon);*/
n=((designdata->array_of_designs)[groupid])->size1;/** number of obs in group */
m=1;/** number of params */
/*Rprintf("gvalue in g_inner=|%f| n=|%d| |%f|\n",gvalue,n,-n*gvalue);*/
/*if(status != GSL_SUCCESS){Rprintf("2epsilon=%f %f\n",gsl_vector_get(epsilon,0), gvalue);}*/
logscore= -n*gvalue-0.5*log(gsl_matrix_get(hessgvalue,0,0))+(m/2.0)*log((2.0*M_PI)/n); /** this is the final value */
if(gsl_isnan(logscore)){error("nan in g_inner hessmat=%f epsilon=%f gvalue=%f\n",gsl_matrix_get(hessgvalue,0,0),gsl_vector_get(epsilon,0),gvalue);}
/*}*/
/*Rprintf("group=%d logscore=%f\n",groupid+1,logscore);*/
gsl_vector_free(dgvalues);
gsl_vector_free(epsilon);
gsl_matrix_free(hessgvalue);
gsl_vector_free(vectmp1);
gsl_vector_free(vectmp1long);
gsl_vector_free(vectmp2long);
return(logscore);
}
int lua_multiroot_solve(lua_State * L) {
double eps=0.00001;
int maxiter=1000;
bool print=false;
array<double> * x=0;
const gsl_multiroot_fsolver_type *Tf = 0;
const gsl_multiroot_fdfsolver_type *Tdf = 0;
multi_param mp;
mp.L=L;
mp.fdf_index=-1;
lua_pushstring(L,"f");
lua_gettable(L,-2);
if(lua_isfunction(L,-1)) {
mp.f_index=luaL_ref(L, LUA_REGISTRYINDEX);
} else {
luaL_error(L,"%s\n","missing function");
}
lua_pushstring(L,"df");
lua_gettable(L,-2);
if(lua_isfunction(L,-1)) {
mp.df_index=luaL_ref(L, LUA_REGISTRYINDEX);
Tdf= gsl_multiroot_fdfsolver_hybridsj;
} else {
lua_pop(L,1);
Tf= gsl_multiroot_fsolver_hybrids;
}
lua_pushstring(L,"fdf");
lua_gettable(L,-2);
if(lua_isfunction(L,-1)) {
mp.fdf_index=luaL_ref(L, LUA_REGISTRYINDEX);
} else {
lua_pop(L,1);
mp.fdf_index=-1;
}
lua_pushstring(L,"algorithm");
lua_gettable(L,-2);
if(lua_isstring(L,-1)) {
if(Tf) {
if(!strcmp(lua_tostring(L,-1),"hybrid")) {
Tf = gsl_multiroot_fsolver_hybrid;
} else if(!strcmp(lua_tostring(L,-1),"dnewton")) {
Tf = gsl_multiroot_fsolver_dnewton;
} else if(!strcmp(lua_tostring(L,-1),"hybrids")) {
Tf = gsl_multiroot_fsolver_hybrids;
} else if(!strcmp(lua_tostring(L,-1),"broyden")) {
Tf = gsl_multiroot_fsolver_broyden;
} else {
luaL_error(L,"%s\n","invalid algorithm");
}
} else {
if(!strcmp(lua_tostring(L,-1),"hybridj")) {
Tdf = gsl_multiroot_fdfsolver_hybridj;
} else if(!strcmp(lua_tostring(L,-1),"newton")) {
Tdf = gsl_multiroot_fdfsolver_newton;
} else if(!strcmp(lua_tostring(L,-1),"hybridsj")) {
Tdf = gsl_multiroot_fdfsolver_hybridsj;
} else if(!strcmp(lua_tostring(L,-1),"gnewton")) {
Tdf = gsl_multiroot_fdfsolver_gnewton;
} else {
luaL_error(L,"%s\n","invalid algorithm");
}
}
}
lua_pop(L,1);
lua_pushstring(L,"show_iterations");
lua_gettable(L,-2);
if(lua_isboolean(L,-1)) {
print=(lua_toboolean(L,-1)==1);
}
lua_pop(L,1);
lua_pushstring(L,"eps");
lua_gettable(L,-2);
if(lua_isnumber(L,-1)) {
eps=lua_tonumber(L,-1);
}
lua_pop(L,1);
lua_pushstring(L,"maxiter");
lua_gettable(L,-2);
if(lua_isnumber(L,-1)) {
maxiter=(int)lua_tonumber(L,-1);
}
lua_pop(L,1);
lua_pushstring(L,"starting_point");
lua_gettable(L,-2);
if(!lua_isuserdata(L,-1)) lua_error(L);
if (!SWIG_IsOK(SWIG_ConvertPtr(L,-1,(void**)&x,SWIGTYPE_p_arrayT_double_t,0))){
lua_error(L);
}
lua_pop(L,1);
lua_pop(L,1);
if(Tf) {
gsl_multiroot_fsolver *s = NULL;
gsl_vector X;
gsl_multiroot_function sol_func;
int iter = 0;
int status;
double size;
int N=x->size();
/* Starting point */
X.size=x->size();
X.stride=1;
X.data=x->data();
X.owner=0;
/* Initialize method and iterate */
sol_func.n = N;
sol_func.f = multiroot_f_cb;
sol_func.params = ∓
s = gsl_multiroot_fsolver_alloc (Tf, N);
gsl_multiroot_fsolver_set (s, &sol_func, &X);
if(print) printf ("running algorithm '%s'\n",
gsl_multiroot_fsolver_name (s));
do
{
iter++;
status = gsl_multiroot_fsolver_iterate(s);
if (status)
break;
status = gsl_multiroot_test_residual (s->f, eps);
if(print) {
printf ("%5d f() = ", iter);
gsl_vector_fprintf(stdout, s->f, "%f");
}
} while (status == GSL_CONTINUE && iter < maxiter);
for(int i=0;i<N;++i) x->set(i,gsl_vector_get(s->x,i));
luaL_unref(L, LUA_REGISTRYINDEX, mp.f_index);
gsl_multiroot_fsolver_free (s);
} else {
gsl_multiroot_fdfsolver *s = NULL;
gsl_vector X;
gsl_multiroot_function_fdf sol_func;
int iter = 0;
int status;
double size;
int N=x->size();
/* Starting point */
X.size=x->size();
X.stride=1;
X.data=x->data();
X.owner=0;
/* Initialize method and iterate */
sol_func.n = N;
sol_func.f = multiroot_f_cb;
sol_func.df = multiroot_df_cb;
sol_func.fdf = multiroot_fdf_cb;
sol_func.params = ∓
s = gsl_multiroot_fdfsolver_alloc (Tdf, N);
gsl_multiroot_fdfsolver_set (s, &sol_func, &X);
if(print) printf ("running algorithm '%s'\n",
gsl_multiroot_fdfsolver_name (s));
do
{
iter++;
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
break;
status = gsl_multiroot_test_residual (s->f, eps);
if(print) {
printf ("%5d f() = ", iter);
gsl_vector_fprintf(stdout, s->f, "%f");
}
} while (status == GSL_CONTINUE && iter < maxiter);
for(int i=0;i<N;++i) x->set(i,gsl_vector_get(s->x,i));
luaL_unref(L, LUA_REGISTRYINDEX, mp.f_index);
luaL_unref(L, LUA_REGISTRYINDEX, mp.df_index);
gsl_multiroot_fdfsolver_free (s);
}
if(mp.fdf_index>=0) luaL_unref(L, LUA_REGISTRYINDEX, mp.fdf_index);
return 0;
}
