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;
}
void binom_solver(const double* fq, double* rs, double epsabs, double epsrel, int max_iter)
{
double params[2]; memmove(params, fq, 2 * sizeof(double));
// fq[0] = prior[0]; fq[1] = prior[1];
const gsl_multiroot_fsolver_type * T = gsl_multiroot_fsolver_hybrid;
gsl_multiroot_fsolver * s = gsl_multiroot_fsolver_alloc(T, 2);
gsl_multiroot_function F;
// Set up F.
F.f = &binom_transform_gsl;
F.n = 2;
F.params = (void *)params;
// Set up initial vector.
gsl_vector* x = gsl_vector_alloc(2);
gsl_vector_set_all(x, 1.0);
gsl_multiroot_fsolver_set(s, &F, x);
// printf("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++) {
gsl_multiroot_fsolver_iterate(s);
// printf("x: %g, %g \t f: %g, %g\n", s->x->data[0], s->x->data[1], s->f->data[0], s->f->data[0]);
// check |dx| < epsabs + epsrel * |x|
msg = gsl_multiroot_test_delta(s->dx, s->x, epsabs, epsrel);
}
memmove(rs, s->x->data, 2 * sizeof(double));
}
int solver(const double* fq, double* rs, const double* ival, double epsabs, double epsrel, int max_iter,
int (*gsl_transform) (const gsl_vector*, void*, gsl_vector*))
{
#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_fsolver_type * T = gsl_multiroot_fsolver_hybrid;
gsl_multiroot_fsolver * s = gsl_multiroot_fsolver_alloc(T, 2);
gsl_multiroot_function F;
// Set up F.
F.f = gsl_transform;
F.n = 2;
F.params = (void *)params;
// Set up initial vector.
gsl_vector* x = gsl_vector_alloc(2);
memcpy(x->data, ival, 2 * sizeof(double));
gsl_multiroot_fsolver_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_fsolver_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_fsolver_free (s);
gsl_vector_free (x);
return msg;
}
int MultidimensionalRootFinder(const int dimension,
gsl_multiroot_function *f,
gsl_vector *initial_guess,
double abs_error,
double rel_error,
int max_iterations,
gsl_vector *results)
{
int status;
size_t iter = 0;
const gsl_multiroot_fsolver_type * solver_type = gsl_multiroot_fsolver_broyden;
gsl_multiroot_fsolver * solver = gsl_multiroot_fsolver_alloc(solver_type,
dimension);
gsl_multiroot_fsolver_set(solver, f, initial_guess);
do {
iter++;
status = gsl_multiroot_fsolver_iterate(solver);
if (status == GSL_EBADFUNC){
printf("TwodimensionalRootFinder: Error: Infinity or division by zero.\n");
abort();
}
else if (status == GSL_ENOPROG){
printf("TwodimensionalRootFinder: Error: Solver is stuck. Try a different initial guess.\n");
abort();
}
// Check if the root is good enough:
// tests for the convergence of the sequence by comparing the last step dx with the
// absolute error epsabs and relative error epsrel to the current position x. The test
// returns GSL_SUCCESS if the following condition is achieved,
//
// |dx_i| < epsabs + epsrel |x_i|
gsl_vector * x = gsl_multiroot_fsolver_root(solver); // current root
gsl_vector * dx = gsl_multiroot_fsolver_dx(solver); // last step
status = gsl_multiroot_test_delta(dx,
x,
abs_error,
rel_error);
} while (status == GSL_CONTINUE
&& iter < max_iterations);
// Save results in return variables
gsl_vector_memcpy(results, gsl_multiroot_fsolver_root(solver));
// Free vectors
gsl_multiroot_fsolver_free(solver);
return 0;
}
CAMLprim value ml_gsl_multiroot_fsolver_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.mrf.n)
GSL_ERROR("wrong number of dimensions for function", GSL_EBADLEN);
gsl_multiroot_fsolver_set(GSLMULTIROOTSOLVER_VAL(S), &(p->gslfun.mrf), &v_X);
CAMLreturn(Val_unit);
}
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);
}
}
void CUBSSolver::solve(const double* fq, double* rs, 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];
gsl_multiroot_function F;
// Set up F.
F.f = gsl_transform;
F.n = 2;
F.params = (void *)params;
// Set up initial vector.
gsl_vector* x = gsl_vector_alloc(2);
gsl_vector_set_all(x, 0.01);
gsl_multiroot_fsolver_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_fsolver_iterate(s);
if (msg == GSL_EBADFUNC || msg == GSL_ENOPROG) break;
// 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]);
// 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( "CUBSSolver::solve: Error %i. Break on %i.\n", msg, i);
Rprintf( "error: %s\n", gsl_strerror (msg));
Rprintf( "r=%g, s=%g\n", s->x->data[0], s->x->data[1]);
Rprintf( "f=%g, q=%g\n", s->f->data[0], s->f->data[1]);
}
memmove(rs, s->x->data, 2 * sizeof(double));
// Free mem.
gsl_vector_free (x);
}
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;
}
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_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;
}
/*
* 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;
}
static int
frootN(lua_State *L, int idx_x)
{
const gsl_multiroot_fsolver_type *T = NULL;
gsl_multiroot_fsolver *s = NULL;
char *name = NULL;
struct frootN_params params;
gsl_multiroot_function func;
int ndim = lua_objlen(L, idx_x);
int max_iter;
int logging;
double rel_err;
double abs_err;
gsl_vector *x = NULL;
int iter;
int i;
int status = 1;
if (ndim < 1)
luaL_error(L, "Dimension too small in solver");
switch (luaL_checkint(L, lua_upvalueindex(QS_kind))) {
case QROOT_dnewton:
T = gsl_multiroot_fsolver_dnewton;
name = "dnewton";
break;
case QROOT_broyden:
T = gsl_multiroot_fsolver_broyden;
name = "broyden";
break;
case QROOT_hybrid:
T = gsl_multiroot_fsolver_hybrid;
name = "hybrid";
break;
case QROOT_hybrids:
T = gsl_multiroot_fsolver_hybrids;
name = "hybrids";
break;
default:
luaL_error(L, "internal error: unexpected solver");
return 0;
}
x = new_gsl_vector(L, ndim);
for (i = 0; i < ndim; i++) {
double xi;
lua_pushnumber(L, i + 1);
lua_gettable(L, idx_x);
xi = luaL_checknumber(L, -1);
lua_pop(L, 1);
gsl_vector_set(x, i, xi);
}
max_iter = luaL_optint(L, lua_upvalueindex(QS_max_iter), 100);
rel_err = luaL_optnumber(L, lua_upvalueindex(QS_rel_err), 0.0);
abs_err = luaL_optnumber(L, lua_upvalueindex(QS_abs_err), 0.0);
logging = lua_toboolean(L, lua_upvalueindex(QS_logging));
params.func = ¶ms;
params.ndim = ndim;
params.L = L;
lua_pushlightuserdata(L, ¶ms);
lua_pushvalue(L, 1);
lua_settable(L, LUA_REGISTRYINDEX);
func.params = ¶ms;
func.n = ndim;
func.f = frootN_func;
s = gsl_multiroot_fsolver_alloc (T, ndim);
if (s == NULL) {
lua_gc(L, LUA_GCCOLLECT, 0);
s = gsl_multiroot_fsolver_alloc (T, ndim);
if (s == NULL)
luaL_error(L, "not enough memory");
}
gsl_multiroot_fsolver_set(s, &func, x);
lua_pushnil(L);
lua_createtable(L, 0, logging?4:3);
lua_pushstring(L, name);
lua_setfield(L, -2, "Name");
if (logging) {
lua_createtable(L, 0, 2); /* Logs */
lua_createtable(L, max_iter, 0); /* X */
lua_createtable(L, max_iter, 0); /* f */
}
for (iter = 1; iter < max_iter; iter++) {
if (gsl_multiroot_fsolver_iterate(s) != 0) {
iter --;
status = 2;
break;
}
if (logging) {
lua_createtable(L, ndim, 0); /* x */
lua_createtable(L, ndim, 0); /* f */
for (i = 0; i < ndim; i++) {
lua_pushnumber(L, gsl_vector_get(s->x, i));
lua_rawseti(L, -3, i+1);
lua_pushnumber(L, gsl_vector_get(s->f, i));
lua_rawseti(L, -2, i+1);
}
lua_rawseti(L, -3, iter);
lua_rawseti(L, -3, iter);
}
if (gsl_multiroot_test_delta(s->dx, s->x, abs_err, rel_err) == GSL_SUCCESS) {
status = 0;
break;
}
}
if (logging) {
lua_setfield(L, -3, "f");
lua_setfield(L, -2, "x");
lua_setfield(L, -2, "Logs");
}
lua_pushstring(L, status == 0? "OK": "FAILED");
lua_setfield(L, -2, "Status");
lua_pushinteger(L, iter);
lua_setfield(L, -2, "Iterations");
lua_createtable(L, ndim, 0);
for (i = 0; i < ndim; i++) {
lua_pushnumber(L, gsl_vector_get(s->x, i));
lua_rawseti(L, -2, i+1);
}
lua_replace(L, -3);
lua_pushlightuserdata(L, ¶ms);
lua_pushnil(L);
lua_settable(L, LUA_REGISTRYINDEX);
gsl_multiroot_fsolver_free(s);
gsl_vector_free(x);
return 2;
}
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;
}
}
}
}
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;
}
const Tle convertCartesianStateToTwoLineElements(
const Vector6& cartesianState,
const DateTime& epoch,
std::string& solverStatusSummary,
int& numberOfIterations,
const Tle& referenceTle,
const Real earthGravitationalParameter,
const Real earthMeanRadius,
const Real absoluteTolerance,
const Real relativeTolerance,
const int maximumIterations )
{
// Store reference TLE as the template TLE and update epoch.
Tle templateTle = referenceTle;
templateTle.updateEpoch( epoch );
// Set up parameters for residual function.
CartesianToTwoLineElementsParameters< Vector6 > parameters( cartesianState, templateTle );
// Set up residual function.
gsl_multiroot_function cartesianToTwoLineElementsFunction
= { &computeCartesianToTwoLineElementResiduals< Real, Vector6 >,
6,
¶meters };
// Compute current state in Keplerian elements, for use as initial guess for the TLE mean
// elements.
const Vector6 initialKeplerianElements = astro::convertCartesianToKeplerianElements(
parameters.targetState, earthGravitationalParameter );
// Compute initial guess for TLE mean elements.
const Vector6 initialTleMeanElements
= computeInitialGuessTleMeanElements( initialKeplerianElements,
earthGravitationalParameter );
// Set initial guess.
gsl_vector* initialGuessTleMeanElements = gsl_vector_alloc( 6 );
for ( int i = 0; i < 6; i++ )
{
gsl_vector_set( initialGuessTleMeanElements, i, initialTleMeanElements[ i ] );
}
// Set up solver type (derivative free).
const gsl_multiroot_fsolver_type* solverType = gsl_multiroot_fsolver_hybrids;
// Allocate memory for solver.
gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc( solverType, 6 );
// Set solver to use residual function with initial guess for TLE mean elements.
gsl_multiroot_fsolver_set( solver,
&cartesianToTwoLineElementsFunction,
initialGuessTleMeanElements );
// Declare current solver status and iteration counter.
int solverStatus = false;
int counter = 0;
// Set up buffer to store solver status summary table.
std::ostringstream summary;
// Print header for summary table to buffer.
summary << printCartesianToTleSolverStateTableHeader( );
do
{
// Print current state of solver for summary table.
summary << printCartesianToTleSolverState( counter, solver );
// Increment iteration counter.
++counter;
// Execute solver iteration.
solverStatus = gsl_multiroot_fsolver_iterate( solver );
// Check if solver is stuck; if it is stuck, break from loop.
if ( solverStatus )
{
solverStatusSummary = summary.str( );
throw std::runtime_error( "ERROR: Non-linear solver is stuck!" );
}
// Check if root has been found (within tolerance).
solverStatus = gsl_multiroot_test_delta(
solver->dx, solver->x, absoluteTolerance, relativeTolerance );
} while ( solverStatus == GSL_CONTINUE && counter < maximumIterations );
// Save number of iterations.
numberOfIterations = counter - 1;
// Print final status of solver to buffer.
summary << std::endl;
summary << "Status of non-linear solver: " << gsl_strerror( solverStatus ) << std::endl;
summary << std::endl;
// Write buffer contents to solver status summary string.
solverStatusSummary = summary.str( );
// Generate TLE with converged mean elements.
Tle virtualTle = templateTle;
Real convergedMeanEccentricity = gsl_vector_get( solver->x, 2 );
if ( convergedMeanEccentricity < 0.0 )
{
convergedMeanEccentricity = std::fabs( gsl_vector_get( solver->x, 2 ) );
}
if ( convergedMeanEccentricity > 0.999 )
{
convergedMeanEccentricity = 0.99;
}
virtualTle.updateMeanElements( sml::computeModulo( std::fabs( gsl_vector_get( solver->x, 0 ) ), 180.0 ),
sml::computeModulo( gsl_vector_get( solver->x, 1 ), 360.0 ),
convergedMeanEccentricity,
sml::computeModulo( gsl_vector_get( solver->x, 3 ), 360.0 ),
sml::computeModulo( gsl_vector_get( solver->x, 4 ), 360.0 ),
std::fabs( gsl_vector_get( solver->x, 5 ) ) );
// Free up memory.
gsl_multiroot_fsolver_free( solver );
gsl_vector_free( initialGuessTleMeanElements );
return virtualTle;
}
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;
}
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;
}
const std::pair< Vector3, Vector3 > executeAtomSolver(
const Vector3& departurePosition,
const DateTime& departureEpoch,
const Vector3& arrivalPosition,
const Real timeOfFlight,
const Vector3& departureVelocityGuess,
std::string& solverStatusSummary,
int& numberOfIterations,
const Tle& referenceTle,
const Real earthGravitationalParameter,
const Real earthMeanRadius,
const Real absoluteTolerance,
const Real relativeTolerance,
const int maximumIterations )
{
// Set up parameters for residual function.
AtomParameters< Real, Vector3 > parameters( departurePosition,
departureEpoch,
arrivalPosition,
timeOfFlight,
earthGravitationalParameter,
earthMeanRadius,
referenceTle,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Set up residual function.
gsl_multiroot_function atomFunction
= {
&computeAtomResiduals< Real, Vector3 >, 3, ¶meters
};
// Set initial guess.
gsl_vector* initialGuess = gsl_vector_alloc( 3 );
for ( int i = 0; i < 3; i++ )
{
gsl_vector_set( initialGuess, i, departureVelocityGuess[ i ] );
}
// Set up solver type (derivative free).
const gsl_multiroot_fsolver_type* solverType = gsl_multiroot_fsolver_hybrids;
// Allocate memory for solver.
gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc( solverType, 3 );
// Set solver to use residual function with initial guess.
gsl_multiroot_fsolver_set( solver, &atomFunction, initialGuess );
// Declare current solver status and iteration counter.
int solverStatus = false;
int counter = 0;
// Set up buffer to store solver status summary table.
std::ostringstream summary;
// Print header for summary table to buffer.
summary << printAtomSolverStateTableHeader( );
do
{
// Print current state of solver for summary table.
summary << printAtomSolverState( counter, solver );
// Increment iteration counter.
++counter;
// Execute solver iteration.
solverStatus = gsl_multiroot_fsolver_iterate( solver );
// Check if solver is stuck; if it is stuck, break from loop.
if ( solverStatus )
{
std::cerr << "GSL solver status: " << solverStatus << std::endl;
std::cerr << summary.str( ) << std::endl;
std::cerr << std::endl;
throw std::runtime_error( "ERROR: Non-linear solver is stuck!" );
}
// Check if root has been found (within tolerance).
solverStatus = gsl_multiroot_test_delta(
solver->dx, solver->x, absoluteTolerance, relativeTolerance );
} while ( solverStatus == GSL_CONTINUE && counter < maximumIterations );
// Save number of iterations.
numberOfIterations = counter - 1;
// Print final status of solver to buffer.
summary << std::endl;
summary << "Status of non-linear solver: " << gsl_strerror( solverStatus ) << std::endl;
summary << std::endl;
// Write buffer contents to solver status summary string.
solverStatusSummary = summary.str( );
// Store final departure velocity.
Vector3 departureVelocity = departureVelocityGuess;
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( solver->x, i );
}
// Set departure state [km/s].
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements< Real >(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
Vector3 arrivalVelocity = departureVelocity;
arrivalVelocity[ 0 ] = arrivalState.Velocity( ).x;
arrivalVelocity[ 1 ] = arrivalState.Velocity( ).y;
arrivalVelocity[ 2 ] = arrivalState.Velocity( ).z;
// Free up memory.
gsl_multiroot_fsolver_free( solver );
gsl_vector_free( initialGuess );
// Return departure and arrival velocities.
return std::make_pair< Vector3, Vector3 >( departureVelocity, arrivalVelocity );
}
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;
}
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);
}
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;
}