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));
}
static VALUE MSolver_test_delta(VALUE self, VALUE dx, VALUE x, VALUE epsabs,
VALUE epsrel) {
gsl_vector * mydx, * myx;
Data_Get_Struct(dx, gsl_vector, mydx);
Data_Get_Struct(x, gsl_vector, myx);
return INT2FIX(gsl_multiroot_test_delta(mydx, myx, NUM2DBL(epsabs), NUM2DBL(epsrel)));
}
CAMLprim value ml_gsl_multiroot_test_delta_fdf(value S, value epsabs, value epsrel)
{
int status;
status = gsl_multiroot_test_delta(GSLMULTIROOTFDFSOLVER_VAL(S)->dx,
GSLMULTIROOTFDFSOLVER_VAL(S)->x,
Double_val(epsabs), Double_val(epsrel));
return Val_negbool(status);
}
int binom_solver(const double* fq, double* rs, const double* ival, double epsabs, double epsrel, int max_iter)
{
#ifdef USE_R
gsl_set_error_handler_off ();
#endif
double params[2]; memmove(params, fq, 2 * sizeof(double));
// fq[0] = prior[0]; fq[1] = prior[1];
const gsl_multiroot_fdfsolver_type *T;
gsl_multiroot_fdfsolver *s;
const size_t n = 2;
// Set up F.
gsl_multiroot_function_fdf F = {&binom_transform_gsl,
&binom_transform_df,
&binom_transform_fdf,
n, (void *)params};
// Set up initial vector.
gsl_vector* x = gsl_vector_alloc(2);
memcpy(x->data, ival, 2 * sizeof(double));
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc (T, n);
gsl_multiroot_fdfsolver_set (s, &F, x);
// Rprintf("x: %g, %g \t f: %g, %g\n", s->x->data[0], s->x->data[1], s->f->data[0], s->f->data[0]);
int i = 0;
int msg = GSL_CONTINUE;
for(i = 0; i < max_iter && msg != GSL_SUCCESS; i++) {
msg = gsl_multiroot_fdfsolver_iterate(s);
if (msg == GSL_EBADFUNC || msg == GSL_ENOPROG) break;
// Rprintf("x: %g, %g \t f: %g, %g \t dx: %g, %g\n", s->x->data[0], s->x->data[1],
// s->f->data[0], s->f->data[0], s->dx->data[0], s->dx->data[1]);
// check |dx| < epsabs + epsrel * |x|
msg = gsl_multiroot_test_delta(s->dx, s->x, epsabs, epsrel);
}
// You can turn off GSL error handling so it doesn't crash things.
if (msg != GSL_SUCCESS) {
Rprintf( "CUBS_udpate.cpp::solver Error %i. Break on %i.\n", msg, i);
Rprintf( "error: %s\n", gsl_strerror (msg));
Rprintf( "Init: r=%g, s=%g, f=%g, q=%g\n", ival[0], ival[1], fq[0], fq[1]);
Rprintf( "Exit: r=%g, s=%g, ", s->x->data[0], s->x->data[1]);
Rprintf( "F0=%g, F1=%g, ", s->f->data[0], s->f->data[1]);
Rprintf( "D0=%g, D1=%g\n", s->dx->data[0], s->dx->data[1]);
}
memmove(rs, s->x->data, 2 * sizeof(double));
// Free mem.
gsl_multiroot_fdfsolver_free (s);
gsl_vector_free (x);
return msg;
}
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;
}
static PyObject*
PyGSL_multiroot_fdfsolver_test_delta(PyGSL_solver *self, PyObject *args)
{
int flag;
double epsabs, epsrel;
gsl_multiroot_fdfsolver *s = self->solver;
if(!PyArg_ParseTuple(args, "dd", &epsabs, &epsrel))
return NULL;
flag = gsl_multiroot_test_delta(s->dx, s->x, epsabs, epsrel);
return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}
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);
}
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;
}
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 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;
}
