CAMLprim value ml_gsl_multiroot_test_residual_fdf(value S, value epsabs)
{
int status;
status = gsl_multiroot_test_residual(GSLMULTIROOTFDFSOLVER_VAL(S)->f,
Double_val(epsabs));
return Val_negbool(status);
}
static void
BD_imp_GSL_MULTIROOT_wrap (struct BD_imp *BDimp,
double *x, double *q)
{
/**
* set the initial guess
*/
BD_imp_GSL_set_guess (BDimp, x, q, BDimp->guess);
gsl_multiroot_fsolver_set (BDimp->S, BDimp->F, BDimp->guess);
int status;
int iter = 0;
do
{
iter++;
status = gsl_multiroot_fsolver_iterate (BDimp->S);
if (status) /* check if solver is stuck */
break;
status = gsl_multiroot_test_residual (BDimp->S->f, BDimp->eps);
}
while (status == GSL_CONTINUE && iter < BDimp->itmax);
/**
* retreive the solution
*/
gsl_vector *root = gsl_multiroot_fsolver_root (BDimp->S);
BD_imp_GSL_get_root (BDimp, root, x, q);
/* it looks like the "root" is just a pointer, so we don't need to free it.
gsl_vector_free (root);
*/
}
int iterate( const gsl_multiroot_fsolver_type* st, struct reac_info *ri,
int maxIter )
{
int status = 0;
gsl_vector* x = gsl_vector_calloc( ri->num_mols );
gsl_multiroot_fsolver *solver =
gsl_multiroot_fsolver_alloc( st, ri->num_mols );
gsl_multiroot_function func = {&ss_func, ri->num_mols, ri};
// This gives the starting point for finding the solution
for ( unsigned int i = 0; i < ri->num_mols; ++i )
gsl_vector_set( x, i, invop( ri->nVec[i] ) );
gsl_multiroot_fsolver_set( solver, &func, x );
ri->nIter = 0;
do {
ri->nIter++;
status = gsl_multiroot_fsolver_iterate( solver );
if (status ) break;
status = gsl_multiroot_test_residual(
solver->f, ri->convergenceCriterion);
} while (status == GSL_CONTINUE && ri->nIter < maxIter );
gsl_multiroot_fsolver_free( solver );
gsl_vector_free( x );
return status;
}
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()
static PyObject*
PyGSL_multiroot_fdfsolver_test_residual(PyGSL_solver *self, PyObject *args)
{
int flag;
double epsabs;
gsl_multiroot_fdfsolver *s = self->solver;
if(!PyArg_ParseTuple(args, "d", &epsabs))
return NULL;
flag = gsl_multiroot_test_residual(s->f, epsabs);
return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}
void
fastSI_GSL_MULTIROOT_wrap (struct BD_imp *b,
const double *q0,
double *q)
{
int np3 = 3 * b->BD->sys->np;
// set the initial connector in b->x0[]
fastSI_set_Q0 (b, q0);
/**
* set the initial guess
*/
int i;
for (i = 0; i < np3; i ++)
{
gsl_vector_set (b->guess, i, q0[i]);
}
gsl_multiroot_fsolver_set (b->S, b->F, b->guess);
int status;
int iter = 0;
do
{
iter++;
status = gsl_multiroot_fsolver_iterate (b->S);
if (status) /* check if solver is stuck */
break;
status = gsl_multiroot_test_residual (b->S->f, b->eps);
}
while (status == GSL_CONTINUE && iter < b->itmax);
if (status != GSL_SUCCESS)
{
fprintf (stdout, "status = %s\n", gsl_strerror (status));
}
/**
* retreive the solution
*/
gsl_vector *root = gsl_multiroot_fsolver_root (b->S);
for (i = 0; i < np3; i ++)
{
q[i] = gsl_vector_get (root, i);
}
}
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
intersect_polish_root (Curve const &A, double &s,
Curve const &B, double &t) {
int status;
size_t iter = 0;
const size_t n = 2;
struct rparams p = {A, B};
gsl_multiroot_function f = {&intersect_polish_f, n, &p};
double x_init[2] = {s, t};
gsl_vector *x = gsl_vector_alloc (n);
gsl_vector_set (x, 0, x_init[0]);
gsl_vector_set (x, 1, x_init[1]);
const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrids;
gsl_multiroot_fsolver *sol = gsl_multiroot_fsolver_alloc (T, 2);
gsl_multiroot_fsolver_set (sol, &f, x);
do
{
iter++;
status = gsl_multiroot_fsolver_iterate (sol);
if (status) /* check if solver is stuck */
break;
status =
gsl_multiroot_test_residual (sol->f, 1e-12);
}
while (status == GSL_CONTINUE && iter < 1000);
s = gsl_vector_get (sol->x, 0);
t = gsl_vector_get (sol->x, 1);
gsl_multiroot_fsolver_free (sol);
gsl_vector_free (x);
}
double getDecay(int nnod, double x1, double x2, double y1, double y2) {
const gsl_multiroot_fsolver_type *T;
gsl_multiroot_fsolver *s;
int status;
size_t i, iter = 0;
const size_t n = 2;
struct pair p = {x1, y1, x2, y2};
gsl_multiroot_function f = {&ExponentialRootF, n, &p};
double x_init[2] = {y2, sqrt(nnod)};
gsl_vector *x = gsl_vector_alloc(n);
double result;
for (i=0; i<n; i++)
gsl_vector_set(x, i, x_init[i]);
T = gsl_multiroot_fsolver_hybrids;
s = gsl_multiroot_fsolver_alloc (T, n);
gsl_multiroot_fsolver_set (s, &f, x);
do {
iter++;
status = gsl_multiroot_fsolver_iterate (s);
if (status) /* check if solver is stuck */
break;
status =gsl_multiroot_test_residual(s->f, 1e-7);
} while (status == GSL_CONTINUE && iter < 1000);
if (strcmp(gsl_strerror(status), "success") != 0)
result = -1;
else
result = gsl_vector_get(s->x, 1);
gsl_multiroot_fsolver_free(s);
gsl_vector_free(x);
return result;
}
int
main (void)
{
const SOLVER_TYPE *T;
SOLVER *s;
int status;
size_t i, iter = 0;
const size_t n = 2;
struct rparams p = {1.0, 10.0};
#ifdef DERIV
gsl_multiroot_function_fdf f = {&rosenbrock_f, &rosenbrock_df, &rosenbrock_fdf, n, &p};
#else
gsl_multiroot_function f = {&rosenbrock_f, n, &p};
#endif
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]);
#ifdef DERIV
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc (T, &f, x);
#else
T = gsl_multiroot_fsolver_hybrids;
s = gsl_multiroot_fsolver_alloc (T, &f, x);
#endif
print_state (iter, s);
do
{
iter++;
#ifdef DERIV
status = gsl_multiroot_fdfsolver_iterate (s);
#else
status = gsl_multiroot_fsolver_iterate (s);
#endif
print_state (iter, s);
if (status)
break;
status = gsl_multiroot_test_residual (s->f, 0.0000001);
}
while (status == GSL_CONTINUE && iter < 1000);
printf ("status = %s\n", gsl_strerror (status));
#ifdef DERIV
gsl_multiroot_fdfsolver_free (s);
#else
gsl_multiroot_fsolver_free (s);
#endif
gsl_vector_free (x);
}
int
test_f (const char * desc, gsl_multiroot_function_fdf * fdf,
initpt_function initpt, double factor,
const gsl_multiroot_fsolver_type * T)
{
int status;
size_t i, n = fdf->n, iter = 0;
double residual = 0;
gsl_vector *x;
gsl_multiroot_fsolver *s;
gsl_multiroot_function function;
function.f = fdf->f;
function.params = fdf->params;
function.n = n ;
x = gsl_vector_alloc (n);
(*initpt) (x);
if (factor != 1.0) scale(x, factor);
s = gsl_multiroot_fsolver_alloc (T, n);
gsl_multiroot_fsolver_set (s, &function, x);
/* printf("x "); gsl_vector_fprintf (stdout, s->x, "%g"); printf("\n"); */
/* printf("f "); gsl_vector_fprintf (stdout, s->f, "%g"); printf("\n"); */
do
{
iter++;
status = gsl_multiroot_fsolver_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
for (i = 0; i < n ; i++)
{
residual += fabs(gsl_vector_get(s->f, i));
}
gsl_multiroot_fsolver_free (s);
gsl_vector_free(x);
gsl_test(status, "%s on %s (%g), %u iterations, residual = %.2g", T->name, desc, factor, iter, residual);
return status;
}
/** *************************************************************************************
*****************************************************************************************
*****************************************************************************************/
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);
}
std::vector<double> operator() (FUNCTION func, const std::vector<double>& xg,
const int nFunc) {
gsl_set_error_handler_off();
// Build the GSL type functions from the passed function
// Similar to stack overflow 13289311
gsl_multiroot_function F;
F.n = nFunc;
F.f = [] (const gsl_vector * x, void * p, gsl_vector * f)->int {
std::vector<double> xin;
for (unsigned int i=0; i < x->size; ++i) {
double xt = gsl_vector_get(x, i);
if (xt != xt) return GSL_EDOM;
xin.push_back(xt);
}
std::vector<double> ff = (*static_cast<FUNCTION*>(p))(xin);
for (unsigned int i=0; i < x->size; ++i) {
if (ff[i] != ff[i]) return GSL_ERANGE;
gsl_vector_set(f, i, ff[i]);
}
return GSL_SUCCESS;
};
F.params = &func;
gsl_vector *x = gsl_vector_alloc(nFunc);
for (unsigned int i=0; i<nFunc; ++i) {
gsl_vector_set(x, i, xg[i]);
}
//const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrids;
//const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrid;
const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_dnewton;
gsl_multiroot_fsolver *s = gsl_multiroot_fsolver_alloc(T, nFunc);
int status = gsl_multiroot_fsolver_set(s, &F, x);
//if (status) printf(" GSL Error: %s\n", gsl_strerror(status));
int iter = 0;
do {
iter++;
status = gsl_multiroot_fsolver_iterate(s);
if (status) break; // Solver is stuck
status = gsl_multiroot_test_residual(s->f, mTol);
} while (status == GSL_CONTINUE && iter < mMaxIter);
gsl_set_error_handler(NULL);
if (iter >= mMaxIter || status) {
std::vector<double> ferr, xx;
for (int i=0; i<nFunc; i++) {
ferr.push_back(gsl_vector_get(s->f, i));
xx.push_back(gsl_vector_get(s->x, i));
}
throw MultiDRootException("Multi root find did not converge", iter,
status, xx, ferr);
}
std::vector<double> out;
for (unsigned int i=0; i<nFunc; ++i) {
out.push_back(gsl_vector_get(s->x, i));
}
return out;
}
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 fsolver (double *xfree, int nelem, double epsabs, int method)
{
gsl_multiroot_fsolver_type *T;
gsl_multiroot_fsolver *s;
int status;
size_t i, iter = 0;
size_t n = nelem;
double p[1] = { nelem };
int iloop;
// struct func_params p = {1.0, 10.0};
gsl_multiroot_function func = {&my_f, n, p};
gsl_vector *x = gsl_vector_alloc (n);
for (iloop=0;iloop<nelem; iloop++) {
//printf("in fsovler2D, C side, input is %g \n",xfree[iloop]);
gsl_vector_set (x, iloop, xfree[iloop]);
}
switch (method){
case 0 : T = (gsl_multiroot_fsolver_type *) gsl_multiroot_fsolver_hybrids; break;
case 1 : T = (gsl_multiroot_fsolver_type *) gsl_multiroot_fsolver_hybrid; break;
case 2 : T = (gsl_multiroot_fsolver_type *) gsl_multiroot_fsolver_dnewton; break;
case 3 : T = (gsl_multiroot_fsolver_type *) gsl_multiroot_fsolver_broyden; break;
default: barf("Something is wrong: could not assing fsolver type...\n"); break;
}
s = gsl_multiroot_fsolver_alloc (T, nelem);
gsl_multiroot_fsolver_set (s, &func, x);
do
{
iter++;
//printf("GSL iter %d \n",iter);
status = gsl_multiroot_fsolver_iterate (s);
if (status) /* check if solver is stuck */
break;
status =
gsl_multiroot_test_residual (s->f, epsabs);
}
while (status == GSL_CONTINUE && iter < 1000);
if (status)
warn ("Final status = %s\n", gsl_strerror (status));
for (iloop=0;iloop<nelem; iloop++) {
xfree[iloop] = gsl_vector_get (s->x, iloop);
}
gsl_multiroot_fsolver_free (s);
gsl_vector_free (x);
return 0;
}
int main(int argc, char** args) {
int ext = 0,c;
double ra,dec;
double sol[2];
const gsl_multiroot_fsolver_type *T;
gsl_multiroot_fsolver *s;
int status;
size_t iter=0;
const size_t n=2;
gsl_multiroot_function f={&fvec,n,NULL};
gsl_vector *x = gsl_vector_alloc(n);
char *wcsfn1=NULL, *wcsfn2=NULL;
while ((c = getopt(argc, args, OPTIONS)) != -1) {
switch(c) {
case 'v':
loglvl++;
break;
case 'h':
print_help(args[0]);
exit(0);
case '1':
wcsfn1 = optarg;
break;
case '2':
wcsfn2 = optarg;
break;
}
}
log_init(loglvl);
if (optind != argc) {
print_help(args[0]);
exit(-1);
}
if (!(wcsfn1) || !(wcsfn2)) {
print_help(args[0]);
exit(-1);
}
/* open the two wcs systems */
wcs1 = anwcs_open(wcsfn1, ext);
if (!wcs1) {
ERROR("Failed to read WCS file");
exit(-1);
}
logverb("Read WCS:\n");
if (log_get_level() >= LOG_VERB) {
anwcs_print(wcs1, log_get_fid());
}
wcs2 = anwcs_open(wcsfn2, ext);
if (!wcs2) {
ERROR("Failed to read WCS file");
exit(-1);
}
logverb("Read WCS:\n");
if (log_get_level() >= LOG_VERB) {
anwcs_print(wcs2, log_get_fid());
}
/* setup the solver, start in the middle */
gsl_vector_set(x,0,anwcs_imagew(wcs1)/2.0);
gsl_vector_set(x,1,anwcs_imageh(wcs1)/2.0);
T = gsl_multiroot_fsolver_hybrids;
s = gsl_multiroot_fsolver_alloc (T,2);
gsl_multiroot_fsolver_set(s,&f,x);
print_state(iter,s);
do {
iter++;
status = gsl_multiroot_fsolver_iterate(s);
print_state(iter,s);
if (status) break;
status = gsl_multiroot_test_residual(s->f,1e-7);
} while (status == GSL_CONTINUE && iter < 1000);
sol[0]=gsl_vector_get(s->x,0);
sol[1]=gsl_vector_get(s->x,1);
/* write some diagnostics on stderr */
/* transform to ra/dec */
anwcs_pixelxy2radec(wcs1, sol[0], sol[1], &ra, &dec);
if (loglvl > LOG_MSG)
fprintf(stderr,"Pixel (%.10f, %.10f) -> RA,Dec (%.10f, %.10f)\n",
sol[0], sol[1], ra, dec);
/* transform to x/y with second wcs
center of rotation should stay the same x/y */
anwcs_radec2pixelxy(wcs2, ra, dec, &sol[0], &sol[1]);
if (loglvl > LOG_MSG)
fprintf(stderr,"RA,Dec (%.10f, %.10f) -> Pixel (%.10f, %.10f) \n",
ra, dec, sol[0], sol[1]);
/* write the solution */
fprintf(stdout,"%f\n",sol[0]);
fprintf(stdout,"%f\n",sol[1]);
return(0);
}
static void intersect_polish_root (D2<SBasis> const &A, double &s,
D2<SBasis> const &B, double &t) {
#ifdef HAVE_GSL
const gsl_multiroot_fsolver_type *T;
gsl_multiroot_fsolver *sol;
int status;
size_t iter = 0;
#endif
std::vector<Point> as, bs;
as = A.valueAndDerivatives(s, 2);
bs = B.valueAndDerivatives(t, 2);
Point F = as[0] - bs[0];
double best = dot(F, F);
for(int i = 0; i < 4; i++) {
/**
we want to solve
J*(x1 - x0) = f(x0)
|dA(s)[0] -dB(t)[0]| (X1 - X0) = A(s) - B(t)
|dA(s)[1] -dB(t)[1]|
**/
// We're using the standard transformation matricies, which is numerically rather poor. Much better to solve the equation using elimination.
Affine jack(as[1][0], as[1][1],
-bs[1][0], -bs[1][1],
0, 0);
Point soln = (F)*jack.inverse();
double ns = s - soln[0];
double nt = t - soln[1];
as = A.valueAndDerivatives(ns, 2);
bs = B.valueAndDerivatives(nt, 2);
F = as[0] - bs[0];
double trial = dot(F, F);
if (trial > best*0.1) {// we have standards, you know
// At this point we could do a line search
break;
}
best = trial;
s = ns;
t = nt;
}
#ifdef HAVE_GSL
const size_t n = 2;
struct rparams p = {A, B};
gsl_multiroot_function f = {&intersect_polish_f, n, &p};
double x_init[2] = {s, t};
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_fsolver_hybrids;
sol = gsl_multiroot_fsolver_alloc (T, 2);
gsl_multiroot_fsolver_set (sol, &f, x);
do
{
iter++;
status = gsl_multiroot_fsolver_iterate (sol);
if (status) /* check if solver is stuck */
break;
status =
gsl_multiroot_test_residual (sol->f, 1e-12);
}
while (status == GSL_CONTINUE && iter < 1000);
s = gsl_vector_get (sol->x, 0);
t = gsl_vector_get (sol->x, 1);
gsl_multiroot_fsolver_free (sol);
gsl_vector_free (x);
#endif
{
// This code does a neighbourhood search for minor improvements.
double best_v = L1(A(s) - B(t));
//std::cout << "------\n" << best_v << std::endl;
Point best(s,t);
while (true) {
Point trial = best;
double trial_v = best_v;
for(int nsi = -1; nsi < 2; nsi++) {
for(int nti = -1; nti < 2; nti++) {
Point n(EpsilonBy(best[0], nsi),
EpsilonBy(best[1], nti));
double c = L1(A(n[0]) - B(n[1]));
//std::cout << c << "; ";
if (c < trial_v) {
trial = n;
trial_v = c;
}
}
}
if(trial == best) {
//std::cout << "\n" << s << " -> " << s - best[0] << std::endl;
//std::cout << t << " -> " << t - best[1] << std::endl;
//std::cout << best_v << std::endl;
s = best[0];
t = best[1];
return;
} else {
best = trial;
best_v = trial_v;
}
}
}
}
static int
XLALSimIMRSpinEOBInitialConditionsPrec(
REAL8Vector * initConds, /**<< OUTPUT, Initial dynamical variables */
const REAL8 mass1, /**<< mass 1 */
const REAL8 mass2, /**<< mass 2 */
const REAL8 fMin, /**<< Initial frequency (given) */
const REAL8 inc, /**<< Inclination */
const REAL8 spin1[], /**<< Initial spin vector 1 */
const REAL8 spin2[], /**<< Initial spin vector 2 */
SpinEOBParams * params /**<< Spin EOB parameters */
)
{
#ifndef LAL_NDEBUG
if (!initConds) {
XLAL_ERROR(XLAL_EINVAL);
}
#endif
int debugPK = 0; int printPK = 0;
FILE* UNUSED out = NULL;
if (printPK) {
XLAL_PRINT_INFO("Inside the XLALSimIMRSpinEOBInitialConditionsPrec function!\n");
XLAL_PRINT_INFO(
"Inputs: m1 = %.16e, m2 = %.16e, fMin = %.16e, inclination = %.16e\n",
mass1, mass2, (double)fMin, (double)inc);
XLAL_PRINT_INFO("Inputs: mSpin1 = {%.16e, %.16e, %.16e}\n",
spin1[0], spin1[1], spin1[2]);
XLAL_PRINT_INFO("Inputs: mSpin2 = {%.16e, %.16e, %.16e}\n",
spin2[0], spin2[1], spin2[2]);
fflush(NULL);
}
static const int UNUSED lMax = 8;
int i;
/* Variable to keep track of whether the user requested the tortoise */
int tmpTortoise;
UINT4 SpinAlignedEOBversion;
REAL8 mTotal;
REAL8 eta;
REAL8 omega , v0; /* Initial velocity and angular
* frequency */
REAL8 ham; /* Hamiltonian */
REAL8 LnHat [3]; /* Initial orientation of angular
* momentum */
REAL8 rHat [3]; /* Initial orientation of radial
* vector */
REAL8 vHat [3]; /* Initial orientation of velocity
* vector */
REAL8 Lhat [3]; /* Direction of relativistic ang mom */
REAL8 qHat [3];
REAL8 pHat [3];
/* q and p vectors in Cartesian and spherical coords */
REAL8 qCart [3], pCart[3];
REAL8 qSph [3], pSph[3];
/* We will need to manipulate the spin vectors */
/* We will use temporary vectors to do this */
REAL8 tmpS1 [3];
REAL8 tmpS2 [3];
REAL8 tmpS1Norm[3];
REAL8 tmpS2Norm[3];
REAL8Vector qCartVec, pCartVec;
REAL8Vector s1Vec, s2Vec, s1VecNorm, s2VecNorm;
REAL8Vector sKerr, sStar;
REAL8 sKerrData[3], sStarData[3];
REAL8 a = 0.;
//, chiS, chiA;
//REAL8 chi1, chi2;
/*
* We will need a full values vector for calculating derivs of
* Hamiltonian
*/
REAL8 sphValues[12];
REAL8 cartValues[12];
/* Matrices for rotating to the new basis set. */
/* It is more convenient to calculate the ICs in a simpler basis */
gsl_matrix *rotMatrix = NULL;
gsl_matrix *invMatrix = NULL;
gsl_matrix *rotMatrix2 = NULL;
gsl_matrix *invMatrix2 = NULL;
/* Root finding stuff for finding the spherical orbit */
SEOBRootParams rootParams;
const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrid;
gsl_multiroot_fsolver *rootSolver = NULL;
gsl_multiroot_function rootFunction;
gsl_vector *initValues = NULL;
gsl_vector *finalValues = NULL;
INT4 gslStatus;
INT4 cntGslNoProgress = 0, MAXcntGslNoProgress = 5;
//INT4 cntGslNoProgress = 0, MAXcntGslNoProgress = 50;
REAL8 multFacGslNoProgress = 3./5.;
//const int maxIter = 2000;
const int maxIter = 10000;
memset(&rootParams, 0, sizeof(rootParams));
mTotal = mass1 + mass2;
eta = mass1 * mass2 / (mTotal * mTotal);
memcpy(tmpS1, spin1, sizeof(tmpS1));
memcpy(tmpS2, spin2, sizeof(tmpS2));
memcpy(tmpS1Norm, spin1, sizeof(tmpS1Norm));
memcpy(tmpS2Norm, spin2, sizeof(tmpS2Norm));
for (i = 0; i < 3; i++) {
tmpS1Norm[i] /= mTotal * mTotal;
tmpS2Norm[i] /= mTotal * mTotal;
}
SpinAlignedEOBversion = params->seobCoeffs->SpinAlignedEOBversion;
/* We compute the ICs for the non-tortoise p, and convert at the end */
tmpTortoise = params->tortoise;
params->tortoise = 0;
EOBNonQCCoeffs *nqcCoeffs = NULL;
nqcCoeffs = params->nqcCoeffs;
/*
* STEP 1) Rotate to LNhat0 along z-axis and N0 along x-axis frame,
* where LNhat0 and N0 are initial normal to orbital plane and
* initial orbital separation;
*/
/* Set the initial orbital ang mom direction. Taken from STPN code */
LnHat[0] = sin(inc);
LnHat[1] = 0.;
LnHat[2] = cos(inc);
/*
* Set the radial direction - need to take care to avoid singularity
* if L is along z axis
*/
if (LnHat[2] > 0.9999) {
rHat[0] = 1.;
rHat[1] = rHat[2] = 0.;
} else {
REAL8 theta0 = atan(-LnHat[2] / LnHat[0]); /* theta0 is between 0
* and Pi */
rHat[0] = sin(theta0);
rHat[1] = 0;
rHat[2] = cos(theta0);
}
/* Now we can complete the triad */
vHat[0] = CalculateCrossProductPrec(0, LnHat, rHat);
vHat[1] = CalculateCrossProductPrec(1, LnHat, rHat);
vHat[2] = CalculateCrossProductPrec(2, LnHat, rHat);
NormalizeVectorPrec(vHat);
/* Vectors BEFORE rotation */
if (printPK) {
for (i = 0; i < 3; i++)
XLAL_PRINT_INFO(" LnHat[%d] = %.16e, rHat[%d] = %.16e, vHat[%d] = %.16e\n",
i, LnHat[i], i, rHat[i], i, vHat[i]);
XLAL_PRINT_INFO("\n\n");
for (i = 0; i < 3; i++)
XLAL_PRINT_INFO(" s1[%d] = %.16e, s2[%d] = %.16e\n", i, tmpS1[i], i, tmpS2[i]);
fflush(NULL);
}
/* Allocate and compute the rotation matrices */
XLAL_CALLGSL(rotMatrix = gsl_matrix_alloc(3, 3));
XLAL_CALLGSL(invMatrix = gsl_matrix_alloc(3, 3));
if (!rotMatrix || !invMatrix) {
if (rotMatrix)
gsl_matrix_free(rotMatrix);
if (invMatrix)
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_ENOMEM);
}
if (CalculateRotationMatrixPrec(rotMatrix, invMatrix, rHat, vHat, LnHat) == XLAL_FAILURE) {
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_ENOMEM);
}
/* Rotate the orbital vectors and spins */
ApplyRotationMatrixPrec(rotMatrix, rHat);
ApplyRotationMatrixPrec(rotMatrix, vHat);
ApplyRotationMatrixPrec(rotMatrix, LnHat);
ApplyRotationMatrixPrec(rotMatrix, tmpS1);
ApplyRotationMatrixPrec(rotMatrix, tmpS2);
ApplyRotationMatrixPrec(rotMatrix, tmpS1Norm);
ApplyRotationMatrixPrec(rotMatrix, tmpS2Norm);
/* See if Vectors have been rotated fine */
if (printPK) {
XLAL_PRINT_INFO("\nAfter applying rotation matrix:\n\n");
for (i = 0; i < 3; i++)
XLAL_PRINT_INFO(" LnHat[%d] = %.16e, rHat[%d] = %.16e, vHat[%d] = %.16e\n",
i, LnHat[i], i, rHat[i], i, vHat[i]);
XLAL_PRINT_INFO("\n");
for (i = 0; i < 3; i++)
XLAL_PRINT_INFO(" s1[%d] = %.16e, s2[%d] = %.16e\n", i, tmpS1[i], i, tmpS2[i]);
fflush(NULL);
}
/*
* STEP 2) After rotation in STEP 1, in spherical coordinates, phi0
* and theta0 are given directly in Eq. (4.7), r0, pr0, ptheta0 and
* pphi0 are obtained by solving Eqs. (4.8) and (4.9) (using
* gsl_multiroot_fsolver). At this step, we find initial conditions
* for a spherical orbit without radiation reaction.
*/
/* Initialise the gsl stuff */
XLAL_CALLGSL(rootSolver = gsl_multiroot_fsolver_alloc(T, 3));
if (!rootSolver) {
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_ENOMEM);
}
XLAL_CALLGSL(initValues = gsl_vector_calloc(3));
if (!initValues) {
gsl_multiroot_fsolver_free(rootSolver);
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_ENOMEM);
}
rootFunction.f = XLALFindSphericalOrbitPrec;
rootFunction.n = 3;
rootFunction.params = &rootParams;
/* Calculate the initial velocity from the given initial frequency */
omega = LAL_PI * mTotal * LAL_MTSUN_SI * fMin;
v0 = cbrt(omega);
/* Given this, we can start to calculate the initial conditions */
/* for spherical coords in the new basis */
rootParams.omega = omega;
rootParams.params = params;
/* To start with, we will just assign Newtonian-ish ICs to the system */
rootParams.values[0] = scale1 * 1. / (v0 * v0); /* Initial r */
rootParams.values[4] = scale2 * v0; /* Initial p */
rootParams.values[5] = scale3 * 1e-3;
//PK
memcpy(rootParams.values + 6, tmpS1, sizeof(tmpS1));
memcpy(rootParams.values + 9, tmpS2, sizeof(tmpS2));
if (printPK) {
XLAL_PRINT_INFO("ICs guess: x = %.16e, py = %.16e, pz = %.16e\n",
rootParams.values[0]/scale1, rootParams.values[4]/scale2,
rootParams.values[5]/scale3);
fflush(NULL);
}
gsl_vector_set(initValues, 0, rootParams.values[0]);
gsl_vector_set(initValues, 1, rootParams.values[4]);
gsl_vector_set(initValues, 2, rootParams.values[5]);
gsl_multiroot_fsolver_set(rootSolver, &rootFunction, initValues);
/* We are now ready to iterate to find the solution */
i = 0;
if(debugPK){ out = fopen("ICIterations.dat", "w"); }
do {
XLAL_CALLGSL(gslStatus = gsl_multiroot_fsolver_iterate(rootSolver));
if (debugPK) {
fprintf( out, "%d\t", i );
/* Write to file */
fprintf( out, "%.16e\t%.16e\t%.16e\t",
rootParams.values[0]/scale1, rootParams.values[4]/scale2,
rootParams.values[5]/scale3 );
/* Residual Function values whose roots we are trying to find */
finalValues = gsl_multiroot_fsolver_f(rootSolver);
/* Write to file */
fprintf( out, "%.16e\t%.16e\t%.16e\t",
gsl_vector_get(finalValues, 0),
gsl_vector_get(finalValues, 1),
gsl_vector_get(finalValues, 2) );
/* Step sizes in each of function variables */
finalValues = gsl_multiroot_fsolver_dx(rootSolver);
/* Write to file */
fprintf( out, "%.16e\t%.16e\t%.16e\t%d\n",
gsl_vector_get(finalValues, 0)/scale1,
gsl_vector_get(finalValues, 1)/scale2,
gsl_vector_get(finalValues, 2)/scale3,
gslStatus );
}
if (gslStatus == GSL_ENOPROG || gslStatus == GSL_ENOPROGJ) {
XLAL_PRINT_INFO(
"\n NO PROGRESS being made by Spherical orbit root solver\n");
/* Print Residual Function values whose roots we are trying to find */
finalValues = gsl_multiroot_fsolver_f(rootSolver);
XLAL_PRINT_INFO("Function value here given by the following:\n");
XLAL_PRINT_INFO(" F1 = %.16e, F2 = %.16e, F3 = %.16e\n",
gsl_vector_get(finalValues, 0),
gsl_vector_get(finalValues, 1), gsl_vector_get(finalValues, 2));
/* Print Step sizes in each of function variables */
finalValues = gsl_multiroot_fsolver_dx(rootSolver);
// XLAL_PRINT_INFO("Stepsizes in each dimension:\n");
// XLAL_PRINT_INFO(" x = %.16e, py = %.16e, pz = %.16e\n",
// gsl_vector_get(finalValues, 0)/scale1,
// gsl_vector_get(finalValues, 1)/scale2,
// gsl_vector_get(finalValues, 2)/scale3);
/* Only allow this flag to be caught MAXcntGslNoProgress no. of times */
cntGslNoProgress += 1;
if (cntGslNoProgress >= MAXcntGslNoProgress) {
cntGslNoProgress = 0;
if(multFacGslNoProgress < 1.){ multFacGslNoProgress *= 1.02; }
else{ multFacGslNoProgress /= 1.01; }
}
/* Now that no progress is being made, we need to reset the initial guess
* for the (r,pPhi, pTheta) and reset the integrator */
rootParams.values[0] = scale1 * 1. / (v0 * v0); /* Initial r */
rootParams.values[4] = scale2 * v0; /* Initial p */
if( cntGslNoProgress % 2 )
rootParams.values[5] = scale3 * 1e-3 / multFacGslNoProgress;
else
rootParams.values[5] = scale3 * 1e-3 * multFacGslNoProgress;
//PK
memcpy(rootParams.values + 6, tmpS1, sizeof(tmpS1));
memcpy(rootParams.values + 9, tmpS2, sizeof(tmpS2));
if (printPK) {
XLAL_PRINT_INFO("New ICs guess: x = %.16e, py = %.16e, pz = %.16e\n",
rootParams.values[0]/scale1, rootParams.values[4]/scale2,
rootParams.values[5]/scale3);
fflush(NULL);
}
gsl_vector_set(initValues, 0, rootParams.values[0]);
gsl_vector_set(initValues, 1, rootParams.values[4]);
gsl_vector_set(initValues, 2, rootParams.values[5]);
gsl_multiroot_fsolver_set(rootSolver, &rootFunction, initValues);
}
else if (gslStatus == GSL_EBADFUNC) {
XLALPrintError(
"Inf or Nan encountered in evaluluation of spherical orbit Eqn\n");
gsl_multiroot_fsolver_free(rootSolver);
gsl_vector_free(initValues);
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_EDOM);
}
else if (gslStatus != GSL_SUCCESS) {
XLALPrintError("Error in GSL iteration function!\n");
gsl_multiroot_fsolver_free(rootSolver);
gsl_vector_free(initValues);
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_EDOM);
}
/* different ways to test convergence of the method */
XLAL_CALLGSL(gslStatus = gsl_multiroot_test_residual(rootSolver->f, 1.0e-8));
/*XLAL_CALLGSL(gslStatus= gsl_multiroot_test_delta(
gsl_multiroot_fsolver_dx(rootSolver),
gsl_multiroot_fsolver_root(rootSolver),
1.e-8, 1.e-5));*/
i++;
}
while (gslStatus == GSL_CONTINUE && i <= maxIter);
if(debugPK) { fflush(NULL); fclose(out); }
if (i > maxIter && gslStatus != GSL_SUCCESS) {
gsl_multiroot_fsolver_free(rootSolver);
gsl_vector_free(initValues);
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
//XLAL_ERROR(XLAL_EMAXITER);
XLAL_ERROR(XLAL_EDOM);
}
finalValues = gsl_multiroot_fsolver_root(rootSolver);
if (printPK) {
XLAL_PRINT_INFO("Spherical orbit conditions here given by the following:\n");
XLAL_PRINT_INFO(" x = %.16e, py = %.16e, pz = %.16e\n",
gsl_vector_get(finalValues, 0)/scale1,
gsl_vector_get(finalValues, 1)/scale2,
gsl_vector_get(finalValues, 2)/scale3);
}
memset(qCart, 0, sizeof(qCart));
memset(pCart, 0, sizeof(pCart));
qCart[0] = gsl_vector_get(finalValues, 0)/scale1;
pCart[1] = gsl_vector_get(finalValues, 1)/scale2;
pCart[2] = gsl_vector_get(finalValues, 2)/scale3;
/* Free the GSL root finder, since we're done with it */
gsl_multiroot_fsolver_free(rootSolver);
gsl_vector_free(initValues);
/*
* STEP 3) Rotate to L0 along z-axis and N0 along x-axis frame, where
* L0 is the initial orbital angular momentum and L0 is calculated
* using initial position and linear momentum obtained in STEP 2.
*/
/* Now we can calculate the relativistic L and rotate to a new basis */
memcpy(qHat, qCart, sizeof(qCart));
memcpy(pHat, pCart, sizeof(pCart));
NormalizeVectorPrec(qHat);
NormalizeVectorPrec(pHat);
Lhat[0] = CalculateCrossProductPrec(0, qHat, pHat);
Lhat[1] = CalculateCrossProductPrec(1, qHat, pHat);
Lhat[2] = CalculateCrossProductPrec(2, qHat, pHat);
NormalizeVectorPrec(Lhat);
XLAL_CALLGSL(rotMatrix2 = gsl_matrix_alloc(3, 3));
XLAL_CALLGSL(invMatrix2 = gsl_matrix_alloc(3, 3));
if (CalculateRotationMatrixPrec(rotMatrix2, invMatrix2, qHat, pHat, Lhat) == XLAL_FAILURE) {
gsl_matrix_free(rotMatrix);
gsl_matrix_free(invMatrix);
XLAL_ERROR(XLAL_ENOMEM);
}
ApplyRotationMatrixPrec(rotMatrix2, rHat);
ApplyRotationMatrixPrec(rotMatrix2, vHat);
ApplyRotationMatrixPrec(rotMatrix2, LnHat);
ApplyRotationMatrixPrec(rotMatrix2, tmpS1);
ApplyRotationMatrixPrec(rotMatrix2, tmpS2);
ApplyRotationMatrixPrec(rotMatrix2, tmpS1Norm);
ApplyRotationMatrixPrec(rotMatrix2, tmpS2Norm);
ApplyRotationMatrixPrec(rotMatrix2, qCart);
ApplyRotationMatrixPrec(rotMatrix2, pCart);
gsl_matrix_free(rotMatrix);
gsl_matrix_free(rotMatrix2);
if (printPK) {
XLAL_PRINT_INFO("qCart after rotation2 %3.10f %3.10f %3.10f\n", qCart[0], qCart[1], qCart[2]);
XLAL_PRINT_INFO("pCart after rotation2 %3.10f %3.10f %3.10f\n", pCart[0], pCart[1], pCart[2]);
XLAL_PRINT_INFO("S1 after rotation2 %3.10f %3.10f %3.10f\n", tmpS1Norm[0], tmpS1Norm[1], tmpS1Norm[2]);
XLAL_PRINT_INFO("S2 after rotation2 %3.10f %3.10f %3.10f\n", tmpS2Norm[0], tmpS2Norm[1], tmpS2Norm[2]);
}
/*
* STEP 4) In the L0-N0 frame, we calculate (dE/dr)|sph using Eq.
* (4.14), then initial dr/dt using Eq. (4.10), and finally pr0 using
* Eq. (4.15).
*/
/* Now we can calculate the flux. Change to spherical co-ords */
CartesianToSphericalPrec(qSph, pSph, qCart, pCart);
memcpy(sphValues, qSph, sizeof(qSph));
memcpy(sphValues + 3, pSph, sizeof(pSph));
memcpy(sphValues + 6, tmpS1, sizeof(tmpS1));
memcpy(sphValues + 9, tmpS2, sizeof(tmpS2));
memcpy(cartValues, qCart, sizeof(qCart));
memcpy(cartValues + 3, pCart, sizeof(pCart));
memcpy(cartValues + 6, tmpS1, sizeof(tmpS1));
memcpy(cartValues + 9, tmpS2, sizeof(tmpS2));
REAL8 dHdpphi , d2Hdr2, d2Hdrdpphi;
REAL8 rDot , dHdpr, flux, dEdr;
d2Hdr2 = XLALCalculateSphHamiltonianDeriv2Prec(0, 0, sphValues, params);
d2Hdrdpphi = XLALCalculateSphHamiltonianDeriv2Prec(0, 5, sphValues, params);
if (printPK)
XLAL_PRINT_INFO("d2Hdr2 = %.16e, d2Hdrdpphi = %.16e\n", d2Hdr2, d2Hdrdpphi);
/* New code to compute derivatives w.r.t. cartesian variables */
REAL8 tmpDValues[14];
int UNUSED status;
for (i = 0; i < 3; i++) {
cartValues[i + 6] /= mTotal * mTotal;
cartValues[i + 9] /= mTotal * mTotal;
}
UINT4 oldignoreflux = params->ignoreflux;
params->ignoreflux = 1;
status = XLALSpinPrecHcapNumericalDerivative(0, cartValues, tmpDValues, params);
params->ignoreflux = oldignoreflux;
for (i = 0; i < 3; i++) {
cartValues[i + 6] *= mTotal * mTotal;
cartValues[i + 9] *= mTotal * mTotal;
}
dHdpphi = tmpDValues[1] / sqrt(cartValues[0] * cartValues[0] + cartValues[1] * cartValues[1] + cartValues[2] * cartValues[2]);
//XLALSpinPrecHcapNumDerivWRTParam(4, cartValues, params) / sphValues[0];
dEdr = -dHdpphi * d2Hdr2 / d2Hdrdpphi;
if (printPK)
XLAL_PRINT_INFO("d2Hdr2 = %.16e d2Hdrdpphi = %.16e dHdpphi = %.16e\n",
d2Hdr2, d2Hdrdpphi, dHdpphi);
if (d2Hdr2 != 0.0) {
/* We will need to calculate the Hamiltonian to get the flux */
s1Vec.length = s2Vec.length = s1VecNorm.length = s2VecNorm.length = sKerr.length = sStar.length = 3;
s1Vec.data = tmpS1;
s2Vec.data = tmpS2;
s1VecNorm.data = tmpS1Norm;
s2VecNorm.data = tmpS2Norm;
sKerr.data = sKerrData;
sStar.data = sStarData;
qCartVec.length = pCartVec.length = 3;
qCartVec.data = qCart;
pCartVec.data = pCart;
//chi1 = tmpS1[0] * LnHat[0] + tmpS1[1] * LnHat[1] + tmpS1[2] * LnHat[2];
//chi2 = tmpS2[0] * LnHat[0] + tmpS2[1] * LnHat[1] + tmpS2[2] * LnHat[2];
//if (debugPK)
//XLAL_PRINT_INFO("magS1 = %.16e, magS2 = %.16e\n", chi1, chi2);
//chiS = 0.5 * (chi1 / (mass1 * mass1) + chi2 / (mass2 * mass2));
//chiA = 0.5 * (chi1 / (mass1 * mass1) - chi2 / (mass2 * mass2));
XLALSimIMRSpinEOBCalculateSigmaKerr(&sKerr, mass1, mass2, &s1Vec, &s2Vec);
XLALSimIMRSpinEOBCalculateSigmaStar(&sStar, mass1, mass2, &s1Vec, &s2Vec);
/*
* The a in the flux has been set to zero, but not in the
* Hamiltonian
*/
a = sqrt(sKerr.data[0] * sKerr.data[0] + sKerr.data[1] * sKerr.data[1] + sKerr.data[2] * sKerr.data[2]);
//XLALSimIMREOBCalcSpinPrecFacWaveformCoefficients(params->eobParams->hCoeffs, mass1, mass2, eta, /* a */ 0.0, chiS, chiA);
//XLALSimIMRCalculateSpinPrecEOBHCoeffs(params->seobCoeffs, eta, a);
ham = XLALSimIMRSpinPrecEOBHamiltonian(eta, &qCartVec, &pCartVec, &s1VecNorm, &s2VecNorm, &sKerr, &sStar, params->tortoise, params->seobCoeffs);
if (printPK)
XLAL_PRINT_INFO("Stas: hamiltonian in ICs at this point is %.16e\n", ham);
/* And now, finally, the flux */
REAL8Vector polarDynamics, cartDynamics;
REAL8 polarData[4], cartData[12];
polarDynamics.length = 4;
polarDynamics.data = polarData;
polarData[0] = qSph[0];
polarData[1] = 0.;
polarData[2] = pSph[0];
polarData[3] = pSph[2];
cartDynamics.length = 12;
cartDynamics.data = cartData;
memcpy(cartData, qCart, 3 * sizeof(REAL8));
memcpy(cartData + 3, pCart, 3 * sizeof(REAL8));
memcpy(cartData + 6, tmpS1Norm, 3 * sizeof(REAL8));
memcpy(cartData + 9, tmpS2Norm, 3 * sizeof(REAL8));
//XLAL_PRINT_INFO("Stas: starting FLux calculations\n");
flux = XLALInspiralPrecSpinFactorizedFlux(&polarDynamics, &cartDynamics, nqcCoeffs, omega, params, ham, lMax, SpinAlignedEOBversion);
/*
* flux = XLALInspiralSpinFactorizedFlux( &polarDynamics,
* nqcCoeffs, omega, params, ham, lMax, SpinAlignedEOBversion
* );
*/
//XLAL_PRINT_INFO("Stas flux = %.16e \n", flux);
//exit(0);
flux = flux / eta;
rDot = -flux / dEdr;
if (debugPK) {
XLAL_PRINT_INFO("Stas here I am 2 \n");
}
/*
* We now need dHdpr - we take it that it is safely linear up
* to a pr of 1.0e-3 PK: Ideally, the pr should be of the
* order of other momenta, in order for its contribution to
* the Hamiltonian to not get buried in the numerical noise
* in the numerically larger momenta components
*/
cartValues[3] = 1.0e-3;
for (i = 0; i < 3; i++) {
cartValues[i + 6] /= mTotal * mTotal;
cartValues[i + 9] /= mTotal * mTotal;
}
oldignoreflux = params->ignoreflux;
params->ignoreflux = 1;
params->seobCoeffs->updateHCoeffs = 1;
status = XLALSpinPrecHcapNumericalDerivative(0, cartValues, tmpDValues, params);
params->ignoreflux = oldignoreflux;
for (i = 0; i < 3; i++) {
cartValues[i + 6] *= mTotal * mTotal;
cartValues[i + 9] *= mTotal * mTotal;
}
REAL8 csi = sqrt(XLALSimIMRSpinPrecEOBHamiltonianDeltaT(params->seobCoeffs, qSph[0], eta, a)*XLALSimIMRSpinPrecEOBHamiltonianDeltaR(params->seobCoeffs, qSph[0], eta, a)) / (qSph[0] * qSph[0] + a * a);
dHdpr = csi*tmpDValues[0];
//XLALSpinPrecHcapNumDerivWRTParam(3, cartValues, params);
if (debugPK) {
XLAL_PRINT_INFO("Ingredients going into prDot:\n");
XLAL_PRINT_INFO("flux = %.16e, dEdr = %.16e, dHdpr = %.16e, dHdpr/pr = %.16e\n", flux, dEdr, dHdpr, dHdpr / cartValues[3]);
}
/*
* We can now calculate what pr should be taking into account
* the flux
*/
pSph[0] = rDot / (dHdpr / cartValues[3]);
} else {
/*
* Since d2Hdr2 has evaluated to zero, we cannot do the
* above. Just set pr to zero
*/
//XLAL_PRINT_INFO("d2Hdr2 is zero!\n");
pSph[0] = 0;
}
/* Now we are done - convert back to cartesian coordinates ) */
SphericalToCartesianPrec(qCart, pCart, qSph, pSph);
/*
* STEP 5) Rotate back to the original inertial frame by inverting
* the rotation of STEP 3 and then inverting the rotation of STEP 1.
*/
/* Undo rotations to get back to the original basis */
/* Second rotation */
ApplyRotationMatrixPrec(invMatrix2, rHat);
ApplyRotationMatrixPrec(invMatrix2, vHat);
ApplyRotationMatrixPrec(invMatrix2, LnHat);
ApplyRotationMatrixPrec(invMatrix2, tmpS1);
ApplyRotationMatrixPrec(invMatrix2, tmpS2);
ApplyRotationMatrixPrec(invMatrix2, tmpS1Norm);
ApplyRotationMatrixPrec(invMatrix2, tmpS2Norm);
ApplyRotationMatrixPrec(invMatrix2, qCart);
ApplyRotationMatrixPrec(invMatrix2, pCart);
/* First rotation */
ApplyRotationMatrixPrec(invMatrix, rHat);
ApplyRotationMatrixPrec(invMatrix, vHat);
ApplyRotationMatrixPrec(invMatrix, LnHat);
ApplyRotationMatrixPrec(invMatrix, tmpS1);
ApplyRotationMatrixPrec(invMatrix, tmpS2);
ApplyRotationMatrixPrec(invMatrix, tmpS1Norm);
ApplyRotationMatrixPrec(invMatrix, tmpS2Norm);
ApplyRotationMatrixPrec(invMatrix, qCart);
ApplyRotationMatrixPrec(invMatrix, pCart);
gsl_matrix_free(invMatrix);
gsl_matrix_free(invMatrix2);
/* If required, apply the tortoise transform */
if (tmpTortoise) {
REAL8 r = sqrt(qCart[0] * qCart[0] + qCart[1] * qCart[1] + qCart[2] * qCart[2]);
REAL8 deltaR = XLALSimIMRSpinPrecEOBHamiltonianDeltaR(params->seobCoeffs, r, eta, a);
REAL8 deltaT = XLALSimIMRSpinPrecEOBHamiltonianDeltaT(params->seobCoeffs, r, eta, a);
REAL8 csi = sqrt(deltaT * deltaR) / (r * r + a * a);
REAL8 pr = (qCart[0] * pCart[0] + qCart[1] * pCart[1] + qCart[2] * pCart[2]) / r;
params->tortoise = tmpTortoise;
if (debugPK) {
XLAL_PRINT_INFO("Applying the tortoise to p (csi = %.26e)\n", csi);
XLAL_PRINT_INFO("pCart = %3.10f %3.10f %3.10f\n", pCart[0], pCart[1], pCart[2]);
}
for (i = 0; i < 3; i++) {
pCart[i] = pCart[i] + qCart[i] * pr * (csi - 1.) / r;
}
}
/* Now copy the initial conditions back to the return vector */
memcpy(initConds->data, qCart, sizeof(qCart));
memcpy(initConds->data + 3, pCart, sizeof(pCart));
memcpy(initConds->data + 6, tmpS1Norm, sizeof(tmpS1Norm));
memcpy(initConds->data + 9, tmpS2Norm, sizeof(tmpS2Norm));
for (i=0; i<12; i++) {
if (fabs(initConds->data[i]) <=1.0e-15) {
initConds->data[i] = 0.;
}
}
if (debugPK) {
XLAL_PRINT_INFO("THE FINAL INITIAL CONDITIONS:\n");
XLAL_PRINT_INFO(" %.16e %.16e %.16e\n%.16e %.16e %.16e\n%.16e %.16e %.16e\n%.16e %.16e %.16e\n", initConds->data[0], initConds->data[1], initConds->data[2],
initConds->data[3], initConds->data[4], initConds->data[5], initConds->data[6], initConds->data[7], initConds->data[8],
initConds->data[9], initConds->data[10], initConds->data[11]);
}
return XLAL_SUCCESS;
}
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;
}
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);
}
/*
* Function: distort_point
* The function finds the distorted coordinates
* for a given undistorted coordinate and the
* transformations to get the undistorted coordinates.
* This function makes the inverse transformation
* to the drizzle transformation.
*
* Parameters:
* @param coeffs - the drizzle coefficients
* @param pixmax - the image dimensions
* @param xy_image - the undistorted (x,y)
*
* Returns:
* @return xy_ret - the distorted (x,y)
*/
d_point
distort_point(gsl_matrix *coeffs, const px_point pixmax, d_point xy_image)
{
const gsl_multiroot_fsolver_type *msolve_type;
gsl_multiroot_fsolver *msolve;
int status;
size_t i, iter = 0;
const size_t n = 2;
gsl_multiroot_function mult_func;
drz_pars *drzpars;
gsl_vector *xy_in = gsl_vector_alloc(2);
gsl_vector *xy_out = gsl_vector_alloc(2);
d_point xy_ret;
// set up the parameters for the
// multi-d function
drzpars = (drz_pars *) malloc(sizeof(drz_pars));
drzpars->coeffs = coeffs;
drzpars->offset = xy_image;
drzpars->npixels = pixmax;
// set up the multi-d function
mult_func.f = &drizzle_distort;
mult_func.n = 2;
mult_func.params = drzpars;
// set the starting coordinates
gsl_vector_set(xy_in, 0, xy_image.x);
gsl_vector_set(xy_in, 1, xy_image.y);
// allocate and initialize the multi-d solver
msolve_type = gsl_multiroot_fsolver_dnewton;
msolve = gsl_multiroot_fsolver_alloc (msolve_type, 2);
gsl_multiroot_fsolver_set (msolve, &mult_func, xy_in);
// print_state (iter, msolve);
// iterate
do
{
// count the number of iterations
iter++;
// do an iteration
status = gsl_multiroot_fsolver_iterate (msolve);
// print_state (iter, msolve);
// check if solver is stuck
if (status)
break;
// evaluate the iteration
status = gsl_multiroot_test_residual (msolve->f, 1e-7);
}
while (status == GSL_CONTINUE && iter < 1000);
// chek for the break conditions
// transfer the result to the return struct
xy_ret.x = gsl_vector_get(msolve->x,0);
xy_ret.y = gsl_vector_get(msolve->x,1);
// deallocate the different structures
gsl_multiroot_fsolver_free (msolve);
gsl_vector_free(xy_in);
gsl_vector_free(xy_out);
// return the result
return xy_ret;
}
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;
}
static VALUE MSolver_test_residual(VALUE self, VALUE f, VALUE epsabs) {
gsl_vector * myf;
Data_Get_Struct(f, gsl_vector, myf);
return INT2FIX(gsl_multiroot_test_residual(myf, NUM2DBL(epsabs)));
}
static Obj FIND_BARYCENTER (Obj self, Obj gap_points, Obj gap_init, Obj gap_iter, Obj gap_tol)
{
#ifdef MALLOC_HACK
old_malloc_hook = __malloc_hook;
old_free_hook = __free_hook;
__malloc_hook = my_malloc_hook;
__free_hook = my_free_hook;
#endif
UInt i, j, n = LEN_PLIST(gap_points);
Double __points[n][3];
bparams bparam = { n, __points };
for (i = 0; i < n; i++)
for (j = 0; j < 3; j++)
bparam.points[i][j] = VAL_FLOAT(ELM_PLIST(ELM_PLIST(gap_points,i+1),j+1));
const gsl_multiroot_fsolver_type *T;
gsl_multiroot_fsolver *s;
int status;
size_t iter = 0, max_iter = INT_INTOBJ(gap_iter);
double precision = VAL_FLOAT(gap_tol);
gsl_multiroot_function f = {&barycenter, 3, &bparam};
gsl_vector *x = gsl_vector_alloc (3);
for (i = 0; i < 3; i++) gsl_vector_set (x, i, VAL_FLOAT(ELM_PLIST(gap_init,i+1)));
T = gsl_multiroot_fsolver_hybrids;
s = gsl_multiroot_fsolver_alloc (T, 3);
gsl_multiroot_fsolver_set (s, &f, x);
do {
iter++;
status = gsl_multiroot_fsolver_iterate (s);
if (status) /* check if solver is stuck */
break;
status = gsl_multiroot_test_residual (s->f, precision);
}
while (status == GSL_CONTINUE && iter < max_iter);
Obj result = ALLOC_PLIST(2);
Obj list = ALLOC_PLIST(3); set_elm_plist(result, 1, list);
for (i = 0; i < 3; i++)
set_elm_plist(list, i+1, NEW_FLOAT(gsl_vector_get (s->x, i)));
list = ALLOC_PLIST(3); set_elm_plist(result, 2, list);
for (i = 0; i < 3; i++)
set_elm_plist(list, i+1, NEW_FLOAT(gsl_vector_get (s->f, i)));
gsl_multiroot_fsolver_free (s);
gsl_vector_free (x);
if (status != 0) {
const char *s = gsl_strerror (status);
C_NEW_STRING(result, strlen(s), s);
}
#ifdef MALLOC_HACK
__malloc_hook = old_malloc_hook;
__free_hook = old_free_hook;
#endif
return result;
}
static void
intersect_polish_root (Curve const &A, double &s,
Curve const &B, double &t) {
std::vector<Point> as, bs;
as = A.pointAndDerivatives(s, 2);
bs = B.pointAndDerivatives(t, 2);
Point F = as[0] - bs[0];
double best = dot(F, F);
for(int i = 0; i < 4; i++) {
/**
we want to solve
J*(x1 - x0) = f(x0)
|dA(s)[0] -dB(t)[0]| (X1 - X0) = A(s) - B(t)
|dA(s)[1] -dB(t)[1]|
**/
// We're using the standard transformation matricies, which is numerically rather poor. Much better to solve the equation using elimination.
Matrix jack(as[1][0], as[1][1],
-bs[1][0], -bs[1][1],
0, 0);
Point soln = (F)*jack.inverse();
double ns = s - soln[0];
double nt = t - soln[1];
if (ns<0) ns=0;
else if (ns>1) ns=1;
if (nt<0) nt=0;
else if (nt>1) nt=1;
as = A.pointAndDerivatives(ns, 2);
bs = B.pointAndDerivatives(nt, 2);
F = as[0] - bs[0];
double trial = dot(F, F);
if (trial > best*0.1) // we have standards, you know
// At this point we could do a line search
break;
best = trial;
s = ns;
t = nt;
}
#ifdef HAVE_GSL
if(0) { // the GSL version is more accurate, but taints this with GPL
const size_t n = 2;
struct rparams p = {A, B};
gsl_multiroot_function f = {&intersect_polish_f, n, &p};
double x_init[2] = {s, t};
gsl_vector *x = gsl_vector_alloc (n);
gsl_vector_set (x, 0, x_init[0]);
gsl_vector_set (x, 1, x_init[1]);
const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrids;
gsl_multiroot_fsolver *sol = gsl_multiroot_fsolver_alloc (T, 2);
gsl_multiroot_fsolver_set (sol, &f, x);
int status = 0;
size_t iter = 0;
do
{
iter++;
status = gsl_multiroot_fsolver_iterate (sol);
if (status) /* check if solver is stuck */
break;
status =
gsl_multiroot_test_residual (sol->f, 1e-12);
}
while (status == GSL_CONTINUE && iter < 1000);
s = gsl_vector_get (sol->x, 0);
t = gsl_vector_get (sol->x, 1);
gsl_multiroot_fsolver_free (sol);
gsl_vector_free (x);
}
#endif
}
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;
}
