/**
* The Brent minimization algorithm combines a parabolic interpolation with the golden section algorithm.
* This produces a fast algorithm which is still robust. The outline of the algorithm can be summarized as
* follows: on each iteration Brent's method approximates the function using an interpolating parabola
* through three existing points. The minimum of the parabola is taken as a guess for the minimum.
* If it lies within the bounds of the current interval then the interpolating point is accepted,
* and used to generate a smaller interval. If the interpolating point is not accepted then the
* algorithm falls back to an ordinary golden section step. The full details of Brent's method
* include some additional checks to improve convergence.
*
* @author Sharmila Prasad (8/15/2011)
*
* @param x_lower - x_lower interval
* @param x_upper - x_upper interval
* @param Func - gsl_function, high-level driver for the algorithm
* Continuous function of one variable for the minimizers to operate on
* @param x_minimum - x_minimum calculated parabola min value
* @return double - status
*/
int Photometry::brentminimizer(double x_lower, double x_upper, gsl_function *Func,
double & x_minimum, double tolerance) {
int status;
int iter=0, max_iter=100;
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
//double m_expected = M_PI;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc(T);
// This function sets, or resets, an existing minimizer s to use the function Func and
// the initial search interval [x_lower, x_upper], with a guess for the location of
// the minimum x_minimum. If the interval given does not contain a minimum, then
// the function returns an error code of GSL_EINVAL.
gsl_min_fminimizer_set(s, Func, x_minimum, x_lower, x_upper);
do {
iter++;
status = gsl_min_fminimizer_iterate(s);
x_minimum = gsl_min_fminimizer_x_minimum(s);
x_lower = gsl_min_fminimizer_x_lower(s);
x_upper = gsl_min_fminimizer_x_upper(s);
status = gsl_min_test_interval(x_lower, x_upper, tolerance, 0.0);
} while(status == GSL_CONTINUE && iter < max_iter);
// This function frees all the memory associated with the minimizer s.
gsl_min_fminimizer_free(s);
return status;
}
void
test_f (const gsl_min_fminimizer_type * T,
const char * description, gsl_function *f,
double lower_bound, double middle, double upper_bound,
double correct_minimum)
{
int status;
size_t iterations = 0;
double m, a, b;
double x_lower, x_upper;
gsl_min_fminimizer * s;
x_lower = lower_bound;
x_upper = upper_bound;
s = gsl_min_fminimizer_alloc (T) ;
gsl_min_fminimizer_set (s, f, middle, x_lower, x_upper) ;
do
{
iterations++ ;
status = gsl_min_fminimizer_iterate (s);
m = gsl_min_fminimizer_x_minimum(s);
a = gsl_min_fminimizer_x_lower(s);
b = gsl_min_fminimizer_x_upper(s);
#ifdef DEBUG
printf("%.12f %.18f %.12f %.18f %.12f %.18f status=%d\n",
a, GSL_FN_EVAL(f, a), m, GSL_FN_EVAL(f, m), b, GSL_FN_EVAL(f, b), status);
#endif
if (a > b)
gsl_test (GSL_FAILURE, "interval is invalid (%g,%g)", a, b);
if (m < a || m > b)
gsl_test (GSL_FAILURE, "m lies outside interval %g (%g,%g)", m, a, b);
if (status) break ;
status = gsl_min_test_interval (a, b, EPSABS, EPSREL);
}
while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);
gsl_test (status, "%s, %s (%g obs vs %g expected) ",
gsl_min_fminimizer_name(s), description,
gsl_min_fminimizer_x_minimum(s), correct_minimum);
/* check the validity of the returned result */
if (!WITHIN_TOL (m, correct_minimum, EPSREL, EPSABS))
{
gsl_test (GSL_FAILURE, "incorrect precision (%g obs vs %g expected)",
m, correct_minimum);
}
gsl_min_fminimizer_free (s);
}
CAMLprim value ml_gsl_min_fminimizer_set(value s, value f, value min, value lo, value up)
{
CAMLparam1(s);
Mparams_val(s)->closure = f;
gsl_min_fminimizer_set(Minimizer_val(s), &(Mparams_val(s)->gslfun.gf),
Double_val(min), Double_val(lo), Double_val(up));
CAMLreturn(Val_unit);
}
void Minimiser::setup(Problem problem) {
gslFunction.function = problem.function;
gslFunction.params = 0;
int retCode = gsl_min_fminimizer_set(s, &gslFunction, problem.x.minimum, problem.x.lower, problem.x.upper);
if (retCode==GSL_EINVAL) {
throw Exception("GSL Minimiser could not be set up");
}
}
int
main (void)
{
int status;
int iter = 0, max_iter = 100;
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
double m = 2.0, m_expected = M_PI;
double a = 0.0, b = 6.0;
gsl_function F;
F.function = &fn1;
F.params = 0;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc (T);
gsl_min_fminimizer_set (s, &F, m, a, b);
printf ("using %s method\n",
gsl_min_fminimizer_name (s));
printf ("%5s [%9s, %9s] %9s %10s %9s\n",
"iter", "lower", "upper", "min",
"err", "err(est)");
printf ("%5d [%.7f, %.7f] %.7f %+.7f %.7f\n",
iter, a, b,
m, m - m_expected, b - a);
do
{
iter++;
status = gsl_min_fminimizer_iterate (s);
m = gsl_min_fminimizer_x_minimum (s);
a = gsl_min_fminimizer_x_lower (s);
b = gsl_min_fminimizer_x_upper (s);
status
= gsl_min_test_interval (a, b, 0.001, 0.0);
if (status == GSL_SUCCESS)
printf ("Converged:\n");
printf ("%5d [%.7f, %.7f] "
"%.7f %+.7f %.7f\n",
iter, a, b,
m, m - m_expected, b - a);
}
while (status == GSL_CONTINUE && iter < max_iter);
gsl_min_fminimizer_free (s);
return status;
}
/*=============================================================================
* MINIMIZATION
*=============================================================================*/
double minimize(double a,double b, double m){
int status;
int iter = 0, max_iter = 100;
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
double epsabs= 0.005; //eps of solution
double epsrel= 0.0; //eps of solution
gsl_function F;
F.function = &min_func;
F.params = 0;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc (T);
gsl_min_fminimizer_set (s, &F, m, a, b);
printf ("using %s method\n",
gsl_min_fminimizer_name (s));
printf ("%5s [%9s, %9s] %9s %9s\n",
"iter", "lower", "upper", "min",
"err(est)");
printf ("%5d [%.7f, %.7f] %.7f %.7f\n",
iter, a, b,
m, b - a);
do
{
iter++;
status = gsl_min_fminimizer_iterate (s);
m = gsl_min_fminimizer_x_minimum (s);
a = gsl_min_fminimizer_x_lower (s);
b = gsl_min_fminimizer_x_upper (s);
status
= gsl_min_test_interval (a, b, epsabs, epsrel);
if (status == GSL_SUCCESS)
printf ("Converged:\n");
printf ("%5d [%.7f, %.7f] "
"%.7f %.7f\n",
iter, a, b,
m, b - a);
}
while (status == GSL_CONTINUE && iter < max_iter);
gsl_min_fminimizer_free (s);
return m;
}
static VALUE rb_gsl_min_fminimizer_set(VALUE obj, VALUE ff, VALUE xmin,
VALUE xl, VALUE xu)
{
gsl_min_fminimizer *gmf = NULL;
gsl_function *f = NULL;
Need_Float(xmin); Need_Float(xl); Need_Float(xu);
CHECK_FUNCTION(ff);
Data_Get_Struct(obj, gsl_min_fminimizer, gmf);
Data_Get_Struct(ff, gsl_function, f);
return INT2FIX(gsl_min_fminimizer_set(gmf, f, NUM2DBL(xmin),
NUM2DBL(xl), NUM2DBL(xu)));
}
double iteractive_max(Params *params,double a, double b) {
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
gsl_function F;
int status;
int iter = 0, max_iter = 100;
double m=(a+b)/2.0;
printf("a=%lg b=%lg\n",a,b);
printf("f(a)=%lg f(b)=%lg f((a+b)/2)=%lg\n",max_fun(a,params),max_fun(b,params),max_fun((a+b)/2.0,params));
F.function = &max_fun;
F.params = (void*)params;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc (T);
gsl_min_fminimizer_set (s, &F, m, a, b);
printf ("using %s method\n",gsl_min_fminimizer_name (s));
fflush(stdout);
printf("Start Interactions\n");
fflush(stdout);
do
{
iter++;
status = gsl_min_fminimizer_iterate (s);
m = gsl_min_fminimizer_x_minimum (s);
a = gsl_min_fminimizer_x_lower (s);
b = gsl_min_fminimizer_x_upper (s);
status = gsl_min_test_interval (a, b, 0.001, 0.0);
if (status == GSL_SUCCESS)
printf ("Converged:\n");
printf ("%5d [%.7f, %.7f] "
"%.7f %.7f\n",
iter, a, b,
m, b - a);
}
while (status == GSL_CONTINUE && iter < max_iter);
gsl_min_fminimizer_free (s);
return m;
}
int main ()
{
int status;
int iter = 0, max_iter = 100; /*Max. number of iterations*/
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
double m = 0.7; /* Starting point for the search*/
double a = -4.0, b = 1.0; /* The interval in which the minimum lies*/
gsl_function F;
F.function = &fn_1; /* Function to Minimize*/
F.params = 0;
T = gsl_min_fminimizer_goldensection; /*Set the minimization algorithm - Uses Golden Section*/
s = gsl_min_fminimizer_alloc (T); /* Initialize the minimizer*/
gsl_min_fminimizer_set (s, &F, m, a, b); /*Set up the minimizer*/
printf ("Using %s method\n", gsl_min_fminimizer_name (s));
printf ("%5s [%9s, %9s] %9s \n","iter", "lower", "upper", "min", "err", "err(est)");
printf ("%5d [%.7f, %.7f] %.7f \n", iter, a, b, m);
/* Set up the iterative minimization procedure*/
do
{
iter++;
status = gsl_min_fminimizer_iterate(s);
m = gsl_min_fminimizer_x_minimum (s);
a = gsl_min_fminimizer_x_lower (s);
b = gsl_min_fminimizer_x_upper (s);
status = gsl_min_test_interval (a, b, 0.001, 0.0);
if (status == GSL_SUCCESS)
printf ("Converged:\n");
printf ("%5d [%.7f, %.7f] %.7f\n",iter, a, b, m);
} while (status == GSL_CONTINUE && iter < max_iter);
gsl_min_fminimizer_free (s);
return status;
}
void
test_f_e (const gsl_min_fminimizer_type * T,
const char * description, gsl_function *f,
double lower_bound, double middle, double upper_bound,
double correct_minimum)
{
int status;
size_t iterations = 0;
double x_lower, x_upper;
double a, b;
gsl_min_fminimizer * s;
x_lower = lower_bound;
x_upper = upper_bound;
s = gsl_min_fminimizer_alloc (T) ;
status = gsl_min_fminimizer_set (s, f, middle, x_lower, x_upper) ;
if (status != GSL_SUCCESS)
{
gsl_min_fminimizer_free (s) ;
gsl_test (status == GSL_SUCCESS, "%s, %s", T->name, description);
return ;
}
do
{
iterations++ ;
gsl_min_fminimizer_iterate (s);
a = gsl_min_fminimizer_x_lower(s);
b = gsl_min_fminimizer_x_upper(s);
status = gsl_min_test_interval (a, b, EPSABS, EPSREL);
}
while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);
gsl_test (!status, "%s, %s", gsl_min_fminimizer_name(s), description,
gsl_min_fminimizer_x_minimum(s) - correct_minimum);
gsl_min_fminimizer_free (s);
}
/** ***********************************************************************************************/
double get_best_stepsize(double delta,double lower,double upper,int maxiters_hessian, struct fnparams *gparams,
double (* compute_mlik_nm_brent) (double finitestepsize, void *params), gsl_min_fminimizer *s1, double *finitestepsize,double *saverror)
{
gsl_function F1;
/*const gsl_min_fminimizer_type *T1;
gsl_min_fminimizer *s1;*/
int status=GSL_SUCCESS;
int iter;double myerror=0.0;
*finitestepsize=delta;/** --copy-- current delta to finitestepsize as finitstepsize will be changed in here **/
/** could probably do the memory withouth alloc free in here but outside of function to avoid extra mallocs */
F1.function = compute_mlik_brent;
F1.params = gparams;
gsl_min_fminimizer_set (s1, &F1, *finitestepsize,lower,upper);
iter=0;
do
{
iter++;
/* Rprintf("iter=%d\n",iter);*/
status = gsl_min_fminimizer_iterate (s1);
*finitestepsize = gsl_min_fminimizer_x_minimum (s1);
lower=gsl_min_fminimizer_x_lower (s1);
upper=gsl_min_fminimizer_x_upper (s1);
status = gsl_min_test_interval (lower,upper, 0.00001, 0.0);
/*Rprintf("[%e,%e] min=%e\n",lower,upper,finitestepsize);*/
/*if (status==GSL_SUCCESS)*/
/*break;*/
}
while (status == GSL_CONTINUE && iter < maxiters_hessian);
/** now get the error in hessian **/
/*compute_mlik_nm_brent(finitestepsize, &gparams);*/
/*gsl_min_fminimizer_free (s1);*/
myerror=compute_mlik_brent(*finitestepsize, gparams);
*saverror=myerror;/** this is a pointer to the dag->HessianError[nodeid] */
Rprintf("Brent minimiser: error in mlik=%e in [%e,%e] with best h=%e\n",myerror,lower,upper,*finitestepsize);
return(myerror);
}
void FC_FUNC_(oct_1dminimize, OCT_1DMINIMIZE)(double *a, double *b, double *m, func1 f, int *status)
{
int iter = 0;
int max_iter = 100;
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
gsl_function F;
param_f1_t p;
p.func = f;
F.function = &fn1;
F.params = (void *) &p;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc (T);
*status = gsl_min_fminimizer_set (s, &F, *m, *a, *b);
gsl_set_error_handler_off();
do
{
iter++;
*status = gsl_min_fminimizer_iterate (s);
*m = gsl_min_fminimizer_x_minimum (s);
*a = gsl_min_fminimizer_x_lower (s);
*b = gsl_min_fminimizer_x_upper (s);
*status = gsl_min_test_interval (*a, *b, 0.00001, 0.0);
/*if (*status == GSL_SUCCESS) printf ("Converged:\n");*/
/*printf ("%5d [%.7f, %.7f] %.7f \n", iter, *a, *b,*m);*/
}
while (*status == GSL_CONTINUE && iter < max_iter);
gsl_min_fminimizer_free(s);
}
void interpolate_minimum(interp_info *interp,
double x_lo_in,
double x_guess_in,
double x_hi_in,
double threshold,
double * x_minima,
double * y_minima) {
double x_lo;
double x_hi;
double x_guess;
double r_val;
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer * s;
gsl_function F;
int status;
int iter = 0;
int max_iter;
x_lo = x_lo_in;
x_guess = x_guess_in;
x_hi = x_hi_in;
max_iter = GBP_MAX(100, interp->n);
F.function = interpolate_minimum_function;
F.params = (void *)interp;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc(T);
gsl_min_fminimizer_set(s, &F, x_guess, x_lo, x_hi);
do {
iter++;
status = gsl_min_fminimizer_iterate(s);
x_lo = gsl_min_fminimizer_x_lower(s);
x_guess = gsl_min_fminimizer_x_minimum(s);
x_hi = gsl_min_fminimizer_x_upper(s);
status = gsl_min_test_interval(x_lo, x_hi, 0., threshold);
} while(status == GSL_CONTINUE && iter < max_iter);
(*x_minima) = x_guess;
(*y_minima) = interpolate(interp, x_guess);
}
/* this function minimizes the one-dimensional function, starting at point
x0, in the interval [x_min, x_max]. */
int f_min (double x_min, double x0, double x_max, double *minimum, struct f_min_params *par) {
size_t iter;
int status;
double x_lo, x_hi;
gsl_function func;
/* allocates the memory for the minimizer */
gsl_min_fminimizer *s = gsl_min_fminimizer_alloc (par->type);
/* defines the function to minimize and to be handed to the minimizer */
func.function = par->func;
func.params = par->func_p;
/* initializes the minimizer */
gsl_min_fminimizer_set (s, &func, x0, x_min, x_max);
/* iterate */
iter = 0;
do {
iter++;
status = gsl_min_fminimizer_iterate (s);
/* check interval for convergence */
x_lo = gsl_min_fminimizer_x_lower (s);
x_hi = gsl_min_fminimizer_x_upper (s);
status = gsl_min_test_interval (x_lo, x_hi, par->eps_abs, par->eps_rel);
if (par->verbose)
printf ("iter %lu: x_lo = %f x_hi = %f\n", iter, x_lo, x_hi);
} while (status==GSL_CONTINUE && iter<par->max_iter);
/* get the value of the minimum and free the memory */
*minimum = gsl_min_fminimizer_x_minimum (s);
gsl_min_fminimizer_free (s);
return status;
}
int Minimizer::minimize(gsl_function F, double startValue, double lowerBound,
double upperBound) {
gsl_min_fminimizer_set(s, &F, startValue, lowerBound, upperBound);
int iter = 0;
int status;
double a, b;
do {
iter++;
status = gsl_min_fminimizer_iterate(s);
if (status == GSL_EBADFUNC || status == GSL_FAILURE) {
return -1;
}
finalX = gsl_min_fminimizer_x_minimum(s);
a = gsl_min_fminimizer_x_lower(s);
b = gsl_min_fminimizer_x_upper(s);
// stopping rule
// see
// http://www.gnu.org/software/gsl/manual/html_node/Minimization-Stopping-Parameters.html
status = gsl_min_test_interval(a, b, epsabs, epsrel);
if (status == GSL_SUCCESS) {
// printf ("Converged:\n");
finalY = gsl_min_fminimizer_f_minimum(s);
return 0;
}
// printf("%5d\ta=%g\tb=%g\tfinalX=%g\n", iter, a, b, finalX);
// printf ("%5d .7f, [.7f] "
// "%.7f %+.7f %.7f\n",
// iter, a, b,
// m, m - m_expected, b - a);
} while (status == GSL_CONTINUE && iter < this->maxIter);
return -1;
}
bool PairMatchingCut::Get_CDA_FromCalculation( EventClass &E, int t, int a, double &z, double &cda, int &iterations)
{
//=====================================================================//
double z1,z2;
//------------------------------------------------------------------//
// Find the range where to look for the CDA //
//------------------------------------------------------------------//
if( E.dcmin[t] < E.dcmin[a])
{
z1 = E.geo->dc(std::min(E.dcmax[t], E.dcmin[a])).center().z();
z2 = E.geo->dc(std::max(E.dcmax[t], E.dcmin[a])).center().z();
}
else
{
z1 = E.geo->dc(std::min(E.dcmax[a], E.dcmin[t])).center().z();
z2 = E.geo->dc(std::max(E.dcmax[a], E.dcmin[t])).center().z();
}
z1 = z1 -0.2;
z2 = z2 +0.2;
// L corresponding to the maximum frequency:
// L1 --> omega1, L2 --> omega2, omega_max = w1 + w2 --> Lmin
double lmin = fabs(E.wavelen[t])*fabs(E.wavelen[a])/(fabs(E.wavelen[t])+fabs(E.wavelen[a]));
// cout<<" In Get_CDA_FromCalculation 2 lmin ="<<lmin<<endl;
// cout<<" In Get_CDA_FromCalculation 2 wavelen ="<<E.wavelen[t]<<" "<<E.wavelen[a]<<endl;
// cout<<" In Get_CDA_FromCalculation 2 ptot ="<<E.ptot[t]<<" "<<E.costh[a]<<endl;
//------------------------------------------------------------------//
// Sample the distance to isolate local minima //
// First decide about the number of points in the sample //
//------------------------------------------------------------------//
int nsample = int(12*(1.+fabs(z1-z2)/lmin));
TrackDistanceUV distance(E, t, a, z1, z2);
vector<double> dd(nsample);
for(int i = 0; i<nsample; i++) {
double z = z1 + (z2-z1)*i/double(nsample-1);
dd[i] = distance(z);
}
double A,M,B;
vector<MinBracket> brackets;
for(int i=1; i<nsample-1; i++) {
if( (dd[i] < dd[i-1]) && (dd[i] <= dd[i+1]) ) {
A = z1 + (z2-z1)*(i-1)/double(nsample-1);
M = z1 + (z2-z1)*i/double(nsample-1);
B = z1 + (z2-z1)*(i+1)/double(nsample-1);
brackets.push_back(MinBracket(A,M,B));
}
}
//------------------------------------------------------------------//
// Check the ends of the search interval //
//------------------------------------------------------------------//
// cout<<" In Get_CDA_FromCalculation 3"<<endl;
// The distance of approach
vector<double> Alldist;
// Position of minimal distance of approach
vector<double> Allz;
vector<int> Alliter;
if(dd[0] <= dd[1]) {
double ztest = z1 + std::min(z2-z1, tolerance_abs);
double ftest = distance(ztest);
if( (ftest <= dd[0]) && (ftest <= dd[1])) {
//std::cerr<<" z1 bracket"<<std::endl;
brackets.push_back(MinBracket(z1,ztest, z1 + (z2-z1)/double(nsample-1)));
}
else {
//std::cerr<<" z1 min"<<std::endl;
// Extract the distances of approach here
Alldist.push_back(dd[0]);
// Position of minimal distance of approach
Allz.push_back(z1);
Alliter.push_back(0);
}
}
if(dd[nsample-1] <= dd[nsample-2]) {
double ztest = z2 - std::min(z2-z1, tolerance_abs);
double ftest = distance(ztest);
if( (ftest <= dd[nsample-1]) && (ftest <= dd[nsample-2])) {
//std::cerr<<" z2 bracket"<<std::endl;
brackets.push_back(MinBracket(z1 + (z2-z1)*(nsample-2)/double(nsample-1), ztest, z2));
}
else {
//std::cerr<<" z2 min"<<std::endl;
// Extract the distances of approach here
Alldist.push_back(dd[nsample-1]);
// Position of minimal distance of approach
Allz.push_back(z2);
Alliter.push_back(0);
}
}
// cout<<" In Get_CDA_FromCalculation 4 brackets size ="<<brackets.size()<<endl;
//------------------------------------------------------------------//
// Loop over the brackets and for each one of them, find a minimum. //
//------------------------------------------------------------------//
for(unsigned i=0; i<brackets.size(); i++) {
//std::cerr<<"Bracket{"<<brackets[i].a<<", "<<brackets[i].m<<", "<<brackets[i].b<<std::endl;
const gsl_min_fminimizer_type *T = gsl_min_fminimizer_brent;
gsl_min_fminimizer *s = gsl_min_fminimizer_alloc (T);
gsl_function F;
F.function = &TrackDistanceUV::fwrapper;
F.params = &distance;
gsl_min_fminimizer_set(s, &F, brackets[i].m, brackets[i].a, brackets[i].b);
int iter=0;
while(1)
{
iter++;
int status;
if(GSL_SUCCESS != (status = gsl_min_fminimizer_iterate (s)) )
{
gsl_min_fminimizer_free(s);
Log->warn("PairMatchingCut: Get_CDA_FromCalculation(): failure from gsl_min_fminimizer_iterate(): status==%i", status);
return false;
}
M = gsl_min_fminimizer_x_minimum (s);
A = gsl_min_fminimizer_x_lower (s);
B = gsl_min_fminimizer_x_upper (s);
status = gsl_min_test_interval (A, B, tolerance_abs, tolerance_rel);
if (status == GSL_SUCCESS) {
//printf ("Converged:\n");
//printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, m, b - a);
// Extract the distances of approach here
Alldist.push_back(gsl_min_fminimizer_f_minimum(s));
// Position of minimal distance of approach
Allz.push_back(M);
Alliter.push_back(iter);
break;
}
else if(status == GSL_CONTINUE) {
if(iter>max_iter) {
gsl_min_fminimizer_free(s);
Log->warn("PairMatchingCut: Get_CDA_FromCalculation(): exceeded max_iter=");
return false;
}
}
else {
gsl_min_fminimizer_free(s);
Log->warn("PairMatchingCut: Get_CDA_FromCalculation(); failure from gsl_min_test_interval(): status=");
return false;
}
} // while(1) minimize
gsl_min_fminimizer_free(s);
} // for(brackets)
// cout<<" In Get_CDA_FromCalculation 5 Alldist size ="<<Alldist.size()<<endl;
//------------------------------------------------------------------//
// The CDA is the minimum of all the distances of approach //
//------------------------------------------------------------------//
int MinIndex = -1;
cda = 500.;
z = 1000.;
iterations = 0;
// cout<<" In Get_CDA_FromCalculation 6 cda = "<<cda<<endl;
for (int i = 0; i < Alldist.size(); i++)
{
// cout<<" In Get_CDA_FromCalculation 6.1 AllDist[0] = "<<Alldist[0]<<endl;
if ( Alldist[i] < cda )
{
MinIndex = i;
cda = Alldist[i];
// cout<<" In Get_CDA_FromCalculation 6.2 cda = "<<cda<<endl;
}
}
if (MinIndex == -1)
{
Log->warn("PairMatchingCut: Get_CDA_FromCalculation(); The cda could not be found.");
cda = 500; // Safer with a large number since we cut on the small cda.
return false;
}
else
{
z = Allz[MinIndex];
iterations = Alliter[MinIndex];
}
// cout<<" In Get_CDA_FromCalculation 7 cda = "<<cda<<endl;
return true;
}
void shearlayerkcrit_driver(char *input_file_name)
{
PARAMS_STRUCT *params;
GRID_STRUCT *grid;
ROTATION_STRUCT *rotation;
COMPRESSED_MATRIX *matrix;
ARPACK_CONTROL *arpack_params;
RESULTS_STRUCT *results;
OUTPUT_CONTROL *output_control;
double shear_width, shear_radius, E;
//Parameters needed for the root-finding routine
int status;
int iter=0, max_iter=50;
const gsl_root_fsolver_type *Troot;
const gsl_min_fminimizer_type *Tmin;
gsl_root_fsolver *sroot;
gsl_min_fminimizer *smin;
double k_low, k_high, k_guess;
double k_min = NAN;
double k_max = NAN;
double k_peak = NAN;
double gr_peak;
double errabs, errrel;
double width_prefactor;
gsl_function F;
struct FUNCTION_PARAMS function_params;
//Get the physical parameters for the computation
params = malloc(sizeof(PARAMS_STRUCT));
probgen(input_file_name, params);
//Set up the grid, based on the physical parameters
grid = gridgen(params);
//Set up the rotation profile of a shear layer. Derive the width
//from the Ekman number, E=\nu/\Omega r^2, width = rE^(1/4)
//Use r = (r2-r1) and Omega = (Omega1-Omega2)/2.
shear_radius = get_dparam("shear_radius", input_file_name);
/*
width_prefactor = get_dparam("width_prefactor", input_file_name);
E = params->nu/(0.5*fabs(params->omega1 - params->omega2) *
pow((params->r2-params->r1),2));
shear_width = width_prefactor*(params->r2-params->r1)*pow(E, 0.25);
printf("Using shear layer width %g cm\n", shear_width);
*/
shear_width = get_dparam("shear_width", input_file_name);
rotation = shearlayer(params, grid, shear_width, shear_radius);
//Set up the matrix structure for the computations.
matrix = create_matrix(5*grid->numcells);
//Setup the ARPACK parameters
arpack_params = setup_arpack(input_file_name);
//Setup the output control structure
output_control = malloc(sizeof(OUTPUT_CONTROL));
//Pull the error params from the input file to decide when
//we have converged
errabs = get_dparam("errabs", input_file_name);
errrel = get_dparam("errrel", input_file_name);
//Put pointers to all of our control structures in function_params
function_params.params = params;
function_params.grid = grid;
function_params.rotation = rotation;
function_params.matrix = matrix;
function_params.arpack_params = arpack_params;
//Assign the evaluation function and params structure to
//the gsl_function
F.function = &mindampingrate;
F.params = &function_params;
gsl_set_error_handler(&err_handler);
/* Now we find the peak of the growth rate, by minimizing the
damping rate. We set what we hope are reasonable numbers
for the bounds and initial guess.
*/
k_low = 0.01;
k_high = 1000;
k_guess = params->k;
Tmin = gsl_min_fminimizer_brent;
smin = gsl_min_fminimizer_alloc(Tmin);
status = gsl_min_fminimizer_set(smin, &F, k_guess, k_low, k_high);
//Make sure that we didn't thrown an error on initialization
if (status == GSL_SUCCESS) {
//Now iterate!
iter = 0;
do
{
iter++;
status = gsl_min_fminimizer_iterate(smin);
//Make sure that we didn't thrown an error in the iteration routine
if (status != GSL_SUCCESS) {
fprintf(stderr, "Aborted attempt to find k_peak.\n");
break;
}
params->k = gsl_min_fminimizer_x_minimum(smin);
k_low = gsl_min_fminimizer_x_lower(smin);
k_high = gsl_min_fminimizer_x_upper(smin);
status = gsl_min_test_interval(k_low, k_high, errabs, errrel);
if(status == GSL_SUCCESS && params->VERBOSE) {
printf("Converged with k_peak=%g\n", params->k);
}
}
while (status == GSL_CONTINUE && iter < max_iter);
//Save the peak growth rate for printing later, then free the solver
gr_peak = -gsl_min_fminimizer_f_minimum(smin);
} else {
fprintf(stderr, "Aborted attempt to find k_peak.\n");
}
gsl_min_fminimizer_free(smin);
//Check to make sure we converged. If not, don't save the results.
if (status == GSL_SUCCESS) {
k_peak = params->k;
//Make sure everything is set up correctly for normal run
params->kva = params->k*params->va;
free(grid->r);
free(grid->x);
free(grid->r2inv);
free(grid);
grid = gridgen(params);
//Now do a normal run with the chosen k
arpack_params->sigma = find_sigma(matrix, params, grid, rotation,
arpack_params);
results = eigensolve(matrix, params, grid, rotation, arpack_params);
//Setup the structures needed to output the data files, and write them.
get_sparam("basefilename", input_file_name, output_control->basefilename);
strcat(output_control->basefilename, "_kpeak");
wnetcdf(params, grid, rotation, output_control, arpack_params, results);
free(results->lambda);
free(results->z);
free(results->residual);
free(results);
}
/* Now do a root finding search for k_min. */
/*
//Set up the root solver.
Troot = gsl_root_fsolver_brent;
sroot = gsl_root_fsolver_alloc(Troot);
//Set the initial bounds for the search. We're searching for k_min,
//so search from 0 up to k_peak.
k_low = 0;
k_high = k_peak;
status = gsl_root_fsolver_set(sroot, &F, k_low, k_high);
//Make sure that we didn't thrown an error on initialization
if (status == GSL_SUCCESS) {
//Now iterate!
iter = 0;
do
{
iter++;
status = gsl_root_fsolver_iterate(sroot);
//Make sure that we didn't thrown an error in the iteration routine
if (status != GSL_SUCCESS) {
fprintf(stderr, "Aborted attempt to find k_min.\n");
break;
}
params->k = gsl_root_fsolver_root(sroot);
k_low = gsl_root_fsolver_x_lower(sroot);
k_high = gsl_root_fsolver_x_upper(sroot);
status = gsl_root_test_interval(k_low, k_high, errabs, errrel);
if(status == GSL_SUCCESS && params->VERBOSE) {
printf("Converged with k_min=%g\n", params->k);
}
}
while (status == GSL_CONTINUE && iter < max_iter);
} else {
fprintf(stderr, "Aborted attempt to find k_min.\n");
}
gsl_root_fsolver_free (sroot);
//Check to make sure we converged. If not, don't save the results.
if (status == GSL_SUCCESS) {
k_min = params->k;
//Make sure everything is set up correctly for the normal run
params->kva = params->k*params->va;
free(grid->r);
free(grid->x);
free(grid->r2inv);
free(grid);
grid = gridgen(params);
//Now do a normal run with the chosen k
arpack_params->sigma = find_sigma(matrix, params, grid, rotation,
arpack_params);
results = eigensolve(matrix, params, grid, rotation, arpack_params);
//Set the new file name, and write the output
get_sparam("basefilename", input_file_name, output_control->basefilename);
strcat(output_control->basefilename, "_kmin");
wnetcdf(params, grid, rotation, output_control, arpack_params, results);
free(results->lambda);
free(results->z);
free(results->residual);
free(results);
}
*/
/* Now move on to solving for k_max. */
Troot = gsl_root_fsolver_brent;
sroot = gsl_root_fsolver_alloc(Troot);
//Set the initial bounds for the search. We're searching for k_max,
//so search from k_peak to a large number
k_low = k_peak;
k_high = 10000;
status = gsl_root_fsolver_set(sroot, &F, k_low, k_high);
//Make sure that we didn't thrown an error on initialization
if (status == GSL_SUCCESS) {
//Now iterate!
iter = 0;
do
{
iter++;
status = gsl_root_fsolver_iterate(sroot);
//Make sure that we didn't thrown an error in the iteration routine
if (status != GSL_SUCCESS) {
fprintf(stderr, "Aborted attempt to find k_max.\n");
break;
}
params->k = gsl_root_fsolver_root(sroot);
k_low = gsl_root_fsolver_x_lower(sroot);
k_high = gsl_root_fsolver_x_upper(sroot);
status = gsl_root_test_interval(k_low, k_high, errabs, errrel);
if(status == GSL_SUCCESS && params->VERBOSE) {
printf("Converged with k_max=%g\n", params->k);
}
}
while (status == GSL_CONTINUE && iter < max_iter);
} else {
fprintf(stderr, "Aborted attempt to find k_max.\n");
}
gsl_root_fsolver_free (sroot);
//Check to make sure we converged. If not, don't save the results.
if (status == GSL_SUCCESS) {
k_max = params->k;
//Make sure everything is set up correctly for the normal run
params->kva = params->k*params->va;
free(grid->r);
free(grid->x);
free(grid->r2inv);
free(grid);
grid = gridgen(params);
//Now do a normal run with the chosen k
arpack_params->sigma = find_sigma(matrix, params, grid, rotation,
arpack_params);
results = eigensolve(matrix, params, grid, rotation, arpack_params);
//Set the new file name, and write the output
get_sparam("basefilename", input_file_name, output_control->basefilename);
strcat(output_control->basefilename, "_kmax");
wnetcdf(params, grid, rotation, output_control, arpack_params, results);
free(results->lambda);
free(results->z);
free(results->residual);
free(results);
}
printf("Found k_min = %g, k_peak = %g, k_max = %g\n", k_min, k_peak, k_max);
printf("Peak growth rate: %g\n", gr_peak);
free(matrix->A);
free(matrix->B);
free(matrix->Bb);
free(matrix);
free(params);
free(grid->r);
free(grid->x);
free(grid->r2inv);
free(grid);
free(rotation->omega);
free(rotation);
free(output_control);
return;
}
void find_periodic_solution_one_dim(double vz_init, double th_init, struct_all_ode *sao) {
struct_state_ode sx;
int nb_jump, ij, iq;
double t_max = 2;
double vth_init, vth_min, vth_max, vth_tolabs, deltaf;
int status;
int iter = 0, max_iter = 100;
const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
gsl_function F;
// init the function to minimize
vth_init = 0 * M_PI / 180;
vth_min = -200 * M_PI / 180;
vth_max = 200 * M_PI / 180;
vth_tolabs = 0.001 * M_PI / 180;
// find vth init such as to obtain a periodic solution
if (sao == NULL) {
sao = new_all_ode(sao, NULL);
}
sao->eps_rel = 1e-6;
sao->eps_abs = 1e-6;
sao->h = 1e-6;
sao->hmax = 1;
sao->mode = MODE_FLY;
sao->x[INDX_PH] = 0 * M_PI / 180;
sao->x[INDX_TH] = th_init;
sao->x[INDX_X] = 0;
sao->x[INDX_Z] = 0;
sao->x[INDX_R] = sao->r0;
sao->x[INDX_VPH] = 0 * M_PI / 180;
sao->x[INDX_VTH] = 0;
sao->x[INDX_VR] = 0; // vr for flying model
sao->x[INDX_ROOT] = 0;
// trajectoire periodique => pas de perte d'energie a l'aterrisage,
// vx =-vz.tan(theta) est imposee par vz et theta
// le seul parametre permettant de fixer la periodicite est vtheta
sao->x[INDX_TH] = th_init;
sao->x[INDX_VX] = -vz_init * sin(th_init) / cos(th_init);
sao->x[INDX_VZ] = vz_init;
sx.NX = sao->NX;
memcpy(sx.x, sao->x, sx.NX * sizeof (double));
sx.mode = sao->mode;
sx.time_s = 0;
sx.NX = sao->NX;
sao->initial_state = sx;
F.function = &get_delta_x_on_period_one_dim;
F.params = (void *) sao;
T = gsl_min_fminimizer_brent;
s = gsl_min_fminimizer_alloc(T);
gsl_min_fminimizer_set(s, &F, vth_init, vth_min, vth_max);
do {
iter++;
status = gsl_min_fminimizer_iterate(s);
vth_init = gsl_min_fminimizer_x_minimum(s);
vth_min = gsl_min_fminimizer_x_lower(s);
vth_max = gsl_min_fminimizer_x_upper(s);
status
= gsl_min_test_interval(vth_min, vth_max, vth_tolabs, 0.0);
if (status == GSL_SUCCESS) {
deltaf = get_delta_x_on_period_one_dim(vth_init, sao);
printf("Converged, delta f= %.5e:\n", deltaf);
printf("%5d [%.7f, %.7f] "
"%.7f delta=%.7f\n",
iter, vth_min, vth_max,
vth_init, vth_max - vth_min);
}
} while (status == GSL_CONTINUE && iter < max_iter);
gsl_min_fminimizer_free(s);
memcpy(sao->x, sao->initial_state.x, sao->NX * sizeof (double));
sao->time_second = 0;
sao->mode = sx.mode;
/*
sao->print_values_fly_to_sol = 1;
printf("--- initial state at time t= %e ----------\n", sao->final_state.time_s);
print_state(sao->initial_state.x, NULL);
print_energies(sao, sao->initial_state.x);
printf("--- final state at time t= %e ----------\n", sao->final_state.time_s);
print_state(sao->final_state.x, NULL);
print_energies(sao, sao->final_state.x);*/
}
/*
-------------------------------------------------------------------------
* This function minimises fContact defined above using the
* Brent method. It returns the minima with a negative sign (which then
* becomes the maxima of the actual contact function. This can be compared
* to 1 to check if two ellipsoids indeed overlap.
* ------------------------------------------------------------------------*/
REAL8 XLALCheckOverlapOfEllipsoids (
const gsl_vector *ra,
const gsl_vector *rb,
fContactWorkSpace *workSpace )
{
gsl_function F;
INT4 min_status;
INT4 iter = 0, max_iter = 100;
REAL8 m = 0.6180339887;
REAL8 a = 0.0L, b = 1.0L;
gsl_min_fminimizer *s = workSpace->s;
/* Sanity check on input arguments */
if ( !ra || !rb || !workSpace )
XLAL_ERROR_REAL8( XLAL_EFAULT );
if ( ra->size != rb->size || ra->size != workSpace->n )
XLAL_ERROR_REAL8( XLAL_EBADLEN);
/* Set r_AB to be rb - ra */
XLAL_CALLGSL( gsl_vector_memcpy( workSpace->r_AB, rb) );
XLAL_CALLGSL( gsl_vector_sub (workSpace->r_AB, ra) );
if ( gsl_vector_isnull( workSpace->r_AB ))
{
XLALPrintWarning("Position vectors ra and rb are identical.\n");
return 0;
}
F.function = &fContact;
F.params = workSpace;
XLAL_CALLGSL( min_status = gsl_min_fminimizer_set (s, &F, m, a, b) );
if ( min_status != GSL_SUCCESS )
XLAL_ERROR_REAL8( XLAL_EFUNC );
do
{
iter++;
XLAL_CALLGSL( min_status = gsl_min_fminimizer_iterate (s) );
if (min_status != GSL_SUCCESS )
{
if (min_status == GSL_EBADFUNC)
XLAL_ERROR_REAL8( XLAL_EFUNC | XLAL_EFPINVAL );
else if (min_status == GSL_EZERODIV)
XLAL_ERROR_REAL8( XLAL_EFUNC | XLAL_EFPDIV0 );
else
XLAL_ERROR_REAL8( XLAL_EFUNC );
}
m = gsl_min_fminimizer_x_minimum (s);
a = gsl_min_fminimizer_x_lower (s);
b = gsl_min_fminimizer_x_upper (s);
XLAL_CALLGSL( min_status = gsl_min_test_interval (a, b, workSpace->convParam, 0.0) );
if (min_status != GSL_CONTINUE && min_status != GSL_SUCCESS )
XLAL_ERROR_REAL8( XLAL_EFUNC );
}
while (min_status == GSL_CONTINUE && iter < max_iter );
/* End of minimization routine */
/* Throw an error if max iterations would have been exceeded */
if ( iter == max_iter && min_status == GSL_CONTINUE )
{
XLAL_ERROR_REAL8( XLAL_EMAXITER );
}
return ( -(s->f_minimum) );
}
// Find correction
double lodo_correct(lodo_t *self)
{
int i, ni, erc;
vector2_t q;
double r;
double interval;
double offset, err;
double best_offset, best_err, best_outliers;
gsl_function func;
// Pre-compute some values used in the test function
for (i = 0; i < self->num_ranges; i++)
{
// Get range relative to laser
r = self->scan.ranges[i];
if (r > self->range_max)
{
self->scan_points[i].ni = -1;
continue;
}
// Compute cartesian points relative to robot
q.x = r * cos(i * self->range_step + self->range_start);
q.y = r * sin(i * self->range_step + self->range_start); //printf("ni=%d\n",ni);
q = pose2_add_pos(q, self->laser_pose);
// Compute range relative to robot
r = vector2_mag(q);
ni = LODO_LTOR(r);
//printf ("ni = %d\tself->pef_num_ranges = %d\n", ni, self->pef_num_ranges);
if (ni < 0 || ni > self->pef_num_ranges)
ni = -1;
self->scan_points[i].ni = ni;
}
best_offset = 0.0;
best_err = DBL_MAX;
// Initialize the minimizer
func.function = (double (*) (double, void*)) lodo_correct_func;
func.params = self;
erc = gsl_min_fminimizer_set(self->mini, &func,
0.0, -self->fit_interval, +self->fit_interval);
// If the minimizer failes, revert to exhaustive search
if (erc != GSL_SUCCESS)
{
//printf("brute force\n\n");
best_err = DBL_MAX;
for (i = -100; i <= 100; i++)
{
offset = i * self->fit_interval / 100.0;
err = lodo_test_offset(self, offset, NULL);
//printf("%d %f %f\n", i, offset, err);
if (err < best_err)
{
best_err = err;
best_offset = offset;
}
}
//printf("\n\n");
}
else
{
for (i = 0; i < 10; i++) // HACK
{
erc = gsl_min_fminimizer_iterate(self->mini);
if (erc == GSL_EBADFUNC)
assert(0);
if (erc == GSL_FAILURE)
break;
// Test for convergence
interval = gsl_min_fminimizer_x_upper(self->mini);
interval -= gsl_min_fminimizer_x_lower(self->mini);
if (interval < 0.01 * M_PI / 180) // HACK
break;
}
best_offset = gsl_min_fminimizer_x_minimum(self->mini);
best_err = gsl_min_fminimizer_f_minimum(self->mini);
}
// Re-do the test to get the outlier count
lodo_test_offset(self, best_offset, &best_outliers);
// Check it his is a good fit.
if (best_err > self->fit_err_thresh)
{
self->fit_valid = 0;
self->fit_add = 0;
self->fit_correct = 0;
return 0;
}
self->fit_valid = 1;
// See if should add this scan to the map. The logic is this: if we
// have a good fit, but there are lots of points that are "outliers"
// (i.e., large-ish error value), then these points should be added
// to the map. Works for both turning in place and "bursting"
// through doorways.
if (fabs(best_offset) < 5 * M_PI / 180) // HACK
{
if (best_outliers > self->fit_outlier_frac)
self->fit_add = 1;
else
self->fit_add = 0;
}
// Correct the scan pose
self->fit_correct = best_offset;
self->scan.cpose.rot = pose2_add_rot(self->fit_correct, self->scan.cpose);
return 0;
}