Exemplo n.º 1
0
CAMLprim value ml_gsl_multiroot_test_residual_fdf(value S, value epsabs)
{
  int status;
  status = gsl_multiroot_test_residual(GSLMULTIROOTFDFSOLVER_VAL(S)->f, 
				       Double_val(epsabs));
  return Val_negbool(status);
}
Exemplo n.º 2
0
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);
  */
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
static void cv(gsl_vector * lean, gsl_vector * pitch,
               const gsl_vector * steer, Whipple * bike)
{
  int i, N = lean->size, iter, iter_max = ITER_MAX, status;
  double ftol = FTOL;
  gsl_vector * x = gsl_vector_alloc(2);         // vector to store the solution
  const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
  gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
  gsl_multiroot_function_fdf f = {&cv_f, &cv_df, &cv_fdf, 2, bike};

  gsl_vector_set(x, 0, bike->q1);
  gsl_vector_set(x, 1, bike->q2);
  bike->q3 = gsl_vector_get(steer, 0);
  gsl_multiroot_fdfsolver_set(s, &f, x);
  gsl_vector_set(lean, 0, gsl_vector_get(s->x, 0));
  gsl_vector_set(pitch, 0, gsl_vector_get(s->x, 1));

  // for loop to loop over all values of steer
  for (i = 1; i < N - 1; ++i) {
    bike->q3 = gsl_vector_get(steer, i);  // steer as a parameter
    iter = 0;
    do
    {
      status = gsl_multiroot_fdfsolver_iterate(s);
      if (status)
        iterateError(status, "cv()", gsl_vector_get(steer, i));
      status = gsl_multiroot_test_residual(s->f, ftol);
    } while (status == GSL_CONTINUE && ++iter < iter_max);

    // Increase the tolerance by an order of magnitude to improve convergence
    //if (iter == iter_max) {
    //  gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
    //  gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
    //  gsl_multiroot_fdfsolver_set(s, &f, x);
    //  increaseftol(&ftol, &i, iter_max, "cv()", bike->q3);
    //  continue;
    //} // if

    // Store the lean into the lean vector
    gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
    gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
    //  cout << gsl_vector_get(lean, i) << ", " 
    //       << gsl_vector_get(pitch, i) << ", " 
    //       << gsl_vector_get(steer, i) << '\n';
    //ftol = FTOL;
  } // for
  bike->q1 = 0.0; bike->q3 = M_PI; bike->calcPitch();
  gsl_vector_set(lean, i, 0.0);
  gsl_vector_set(pitch, i, bike->q2);

  // Free dynamically allocated variables
  gsl_multiroot_fdfsolver_free(s);
  gsl_vector_free(x);
} // cv()
Exemplo n.º 5
0
static PyObject* 
PyGSL_multiroot_fdfsolver_test_residual(PyGSL_solver *self, PyObject *args)
{
     int flag;
     double epsabs;     
     gsl_multiroot_fdfsolver *s = self->solver;     
     if(!PyArg_ParseTuple(args, "d", &epsabs))
	  return NULL;
     flag = gsl_multiroot_test_residual(s->f, epsabs);
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}
Exemplo n.º 6
0
void
fastSI_GSL_MULTIROOT_wrap (struct BD_imp *b,
			   const double *q0,
			   double *q)
{
  int np3 = 3 * b->BD->sys->np;

  // set the initial connector in b->x0[]
  fastSI_set_Q0 (b, q0);

  /**
   * set the initial guess
   */
  int i;
  for (i = 0; i < np3; i ++)
    {
      gsl_vector_set (b->guess, i, q0[i]);
    }
  gsl_multiroot_fsolver_set (b->S, b->F, b->guess);

  int status;
  int iter = 0;
  do
    {
      iter++;
      status = gsl_multiroot_fsolver_iterate (b->S);

      if (status)   /* check if solver is stuck */
	break;

      status = gsl_multiroot_test_residual (b->S->f, b->eps);
    }
  while (status == GSL_CONTINUE && iter < b->itmax);

  if (status != GSL_SUCCESS)
    {
      fprintf (stdout, "status = %s\n", gsl_strerror (status));
    }

  /**
   * retreive the solution
   */
  gsl_vector *root = gsl_multiroot_fsolver_root (b->S);
  for (i = 0; i < np3; i ++)
    {
      q[i] = gsl_vector_get (root, i);
    }
}
int main(void) {
	const gsl_multiroot_fdfsolver_type *T;
	gsl_multiroot_fdfsolver *s;

	int status;
	size_t i, iter = 0;

	const size_t n = 2;
	struct rparams p = {1.0, 10.0};
	gsl_multiroot_function_fdf f = {&rosenbrock_f,
	&rosenbrock_df,
	&rosenbrock_fdf,
	n, &p};

	double x_init[2] = {-10.0, -5.0};
	gsl_vector *x = gsl_vector_alloc (n);

	gsl_vector_set (x, 0, x_init[0]);
	gsl_vector_set (x, 1, x_init[1]);

	T = gsl_multiroot_fdfsolver_gnewton;
	s = gsl_multiroot_fdfsolver_alloc (T, n);
	gsl_multiroot_fdfsolver_set (s, &f, x);

	print_state (iter, s);

	do {
		iter++;

		status = gsl_multiroot_fdfsolver_iterate (s);

		print_state (iter, s);

		if (status)
		break;

		status = gsl_multiroot_test_residual (s->f, 1e-7);
	} while (status == GSL_CONTINUE && iter < 1000);

	printf ("status = %s\n", gsl_strerror (status));

	gsl_multiroot_fdfsolver_free (s);
	gsl_vector_free (x);

	return EXIT_SUCCESS;
}
Exemplo n.º 8
0
SteamState freesteam_solver2_region1(FREESTEAM_CHAR A, FREESTEAM_CHAR B, double atarget, double btarget, SteamState guess, int *retstatus){
	const gsl_multiroot_fdfsolver_type *T;
	gsl_multiroot_fdfsolver *s;
	int status;
	size_t iter = 0;
	const size_t n = 2;


	//fprintf(stderr,"region 1 solver...\n");
	Solver2Data D = {A,B,solver2_region1_propfn(A), solver2_region1_propfn(B), atarget,btarget};

	gsl_multiroot_function_fdf f = {&region1_f, &region1_df, &region1_fdf, n, &D};

	gsl_vector *x = gsl_vector_alloc(n);
	gsl_vector_set(x, 0, freesteam_rho(guess));
	gsl_vector_set(x, 1, freesteam_T(guess));
	T = gsl_multiroot_fdfsolver_gnewton;
	s = gsl_multiroot_fdfsolver_alloc(T, n);
	gsl_multiroot_fdfsolver_set(s, &f, x);
	//region1_print_state(iter, s);

	do{
		iter++;
		status = gsl_multiroot_fdfsolver_iterate(s);
		//region1_print_state(iter, s);
		if(status){
			/* check if solver is stuck */
			break;
		}
		status = gsl_multiroot_test_residual(s->f, 1e-6);
	} while(status == GSL_CONTINUE && iter < 20);

	SteamState S = freesteam_region1_set_pT(gsl_vector_get(s->x,0), gsl_vector_get(s->x,1));
	gsl_multiroot_fdfsolver_free(s);

	gsl_vector_free(x);
	*retstatus = status;
	if(status){
		fprintf(stderr,"%s (%s:%d): %s: ",__func__,__FILE__,__LINE__,gsl_strerror(status));
		freesteam_fprint(stderr,S);
	}
	return S;
}
static void 
intersect_polish_root (Curve const &A, double &s,
                       Curve const &B, double &t) {
    int status;
    size_t iter = 0;
     
    const size_t n = 2;
    struct rparams p = {A, B};
    gsl_multiroot_function f = {&intersect_polish_f, n, &p};
     
    double x_init[2] = {s, t};
    gsl_vector *x = gsl_vector_alloc (n);
     
    gsl_vector_set (x, 0, x_init[0]);
    gsl_vector_set (x, 1, x_init[1]);
     
    const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrids;
    gsl_multiroot_fsolver *sol = gsl_multiroot_fsolver_alloc (T, 2);
    gsl_multiroot_fsolver_set (sol, &f, x);
     
    do
    {
        iter++;
        status = gsl_multiroot_fsolver_iterate (sol);
     
        if (status)   /* check if solver is stuck */
            break;
     
        status =
            gsl_multiroot_test_residual (sol->f, 1e-12);
    }
    while (status == GSL_CONTINUE && iter < 1000);
    
    s = gsl_vector_get (sol->x, 0);
    t = gsl_vector_get (sol->x, 1);
    
    gsl_multiroot_fsolver_free (sol);
    gsl_vector_free (x);
}
Exemplo n.º 10
0
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;
}
Exemplo n.º 11
0
int
main (void)
{
  const SOLVER_TYPE *T;
  SOLVER *s;

  int status;
  size_t i, iter = 0;

  const size_t n = 2;
  struct rparams p = {1.0, 10.0};
#ifdef DERIV
  gsl_multiroot_function_fdf f = {&rosenbrock_f, &rosenbrock_df, &rosenbrock_fdf, n, &p};
#else
  gsl_multiroot_function f = {&rosenbrock_f, n, &p};
#endif

  double x_init[2] = {-10.0, -5.0};
  gsl_vector *x = gsl_vector_alloc (n);

  gsl_vector_set (x, 0, x_init[0]);
  gsl_vector_set (x, 1, x_init[1]);

#ifdef DERIV
  T = gsl_multiroot_fdfsolver_gnewton;
  s = gsl_multiroot_fdfsolver_alloc (T, &f, x);
#else
  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc (T, &f, x);
#endif

  print_state (iter, s);

  do
    {
      iter++;
#ifdef DERIV
      status = gsl_multiroot_fdfsolver_iterate (s);
#else
      status = gsl_multiroot_fsolver_iterate (s);
#endif

      print_state (iter, s);

      if (status)
        break;

      status = gsl_multiroot_test_residual (s->f, 0.0000001);
    }
  while (status == GSL_CONTINUE && iter < 1000);

  printf ("status = %s\n", gsl_strerror (status));

#ifdef DERIV
  gsl_multiroot_fdfsolver_free (s);
#else
  gsl_multiroot_fsolver_free (s);
#endif

  gsl_vector_free (x);
}
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
0
/** *************************************************************************************
*****************************************************************************************
*****************************************************************************************/          
double g_inner_gaus( gsl_vector *beta, const datamatrix *designdata, int groupid, double epsabs, int maxiters, int verbose){
 
   /** this function perform a Laplace approx on a single data group given fixed beta, so only integrate over single term epsilon **/
 
/* const gsl_multiroot_fdfsolver_type *T;
 gsl_multiroot_fdfsolver *s;
 gsl_multiroot_function_fdf FDF;*/
 struct fnparams gparams;/** for passing to the gsl zero finding functions */
 /*double epsilon=0;*//** the variable we want to find the root of **/
 gsl_vector *epsilon = gsl_vector_alloc (1);
 gsl_vector *dgvalues = gsl_vector_alloc (1);
 gsl_matrix *hessgvalue = gsl_matrix_alloc (1,1);
 /*int iter=0;*/
 /*int status;*/
 /*double epsabs=1e-5;
 int maxiters=100;*/
 /*int verbose=1;*/

 gsl_vector *vectmp1 = gsl_vector_alloc (designdata->numparams+1);/** scratch space same length as number of params inc precision **/
 gsl_vector *vectmp1long = gsl_vector_alloc ( ((designdata->array_of_Y)[groupid])->size);/** scratch space same length as number of obs in group j**/
 gsl_vector *vectmp2long = gsl_vector_alloc ( ((designdata->array_of_Y)[groupid])->size);
 double logscore;
 double gvalue;int n,m;
 
 /*for(i=0;i<beta->size;i++){Rprintf("g_inner_gaus=%f\n",gsl_vector_get(beta,i));}*/
 
 /*Rprintf("I HAVE epsabs_inner=%f maxiters_inner=%d verbose=%d\n",epsabs,maxiters,verbose);*/
 
 /*
 FDF.f = &rv_dg_inner_gaus;
 FDF.df = &rv_hessg_inner_gaus;
 FDF.fdf = &wrapper_rv_fdf_inner_gaus;
 FDF.n = 1;
 FDF.params = &gparams;
 */
 gparams.Y=designdata->array_of_Y[groupid];
 gparams.X=designdata->array_of_designs[groupid];
 gparams.beta=beta;/** inc group and residual precision **/
 /*Rprintf("tau in g_inner=%f\n",gsl_vector_get(beta,beta->size-1));
 if(gsl_vector_get(beta,beta->size-1)<0.0){Rprintf("got negative tau!!=%f\n",gsl_vector_get(beta,beta->size-1));error("");}*/
 gparams.vectmp1=vectmp1;/** same length as beta but used as scratch space */
 gparams.vectmp1long=vectmp1long;
 gparams.vectmp2long=vectmp2long;
  
  /** ******************** FIRST TRY for a root using hybridsj  *******************************************************/
 #ifdef NO
  iter=0; 
    /*T = gsl_root_fdfsolver_newton;
    s = gsl_root_fdfsolver_alloc (T);*/ 
    T = gsl_multiroot_fdfsolver_hybridsj;
    s = gsl_multiroot_fdfsolver_alloc (T, 1);
    status=GSL_FAILURE;/** just set it to something not equal to GSL_SUCCESS */
    /*status_inits=generate_inits_rv_n(x,&gparams);*/
    gsl_vector_set(epsilon,0,0.0);/** initial guess */
    /*gsl_root_fdfsolver_set (s, &FDF, epsilon);*/
   gsl_multiroot_fdfsolver_set (s, &FDF, epsilon);
    
    /*Rprintf ("using %s method\n", 
               gsl_root_fdfsolver_name (s));
     
       Rprintf ("%-5s %10s %10s %10s\n",
               "iter", "root", "err", "err(est)");
   */ 
   
   /*print_state (iter, s);*/
  
    iter=0; 
       do
         {
           iter++;
       
           status = gsl_multiroot_fdfsolver_iterate (s);
           
           /*print_state (iter, s);*/
          
          if (status)
             break;
     
           status = gsl_multiroot_test_residual (s->f, epsabs);
         }
       while (status == GSL_CONTINUE && iter < maxiters);
       if( status != GSL_SUCCESS){Rprintf ("Zero finding warning: internal--- epsilon status = %s\n", gsl_strerror (status));
                                  /*for(i=0;i<s->x->size;i++){Rprintf("0epsilon=%f ",gsl_vector_get(s->x,i));}Rprintf("\n");*/}
       gsl_vector_memcpy(epsilon,s->x);
       Rprintf("modes: %f\n",gsl_vector_get(epsilon,0));
     gsl_multiroot_fdfsolver_free(s);
    /*Rprintf("x=%5.10f f=%5.10f\n",gsl_root_fdfsolver_root(s),rv_dg_inner(gsl_root_fdfsolver_root(s),&gparams));*/  
   
  /* if(status != GSL_SUCCESS){*//*error("no root\n");*//*Rprintf("binary no root at node %d\n",groupid+1);*//*logscore= DBL_MAX;*/ /** root finding failed so discard model by setting fit to worst possible */
  /*} else {*/
    /*gsl_vector_set(epsilon,0,0.3);*/
 #endif  

  rv_dg_inner_gaus(epsilon,&gparams, dgvalues);/** value is returned in dgvalues - first entry **/
  gsl_vector_memcpy(epsilon,dgvalues);/** copy value dgvalues into epsilon */
  /*Rprintf("mode for epsilon=%f\n",gsl_vector_get(epsilon,0));*/
  
  rv_g_inner_gaus(epsilon,&gparams, &gvalue);/*Rprintf("==>g()=%e %f tau=%f\n",gvalue,gsl_vector_get(epsilon,0),gsl_vector_get(beta,2));*/
  /*if(status != GSL_SUCCESS){Rprintf("1epsilon=%f %f\n",gsl_vector_get(epsilon,0), gvalue);}*/
  rv_hessg_inner_gaus(epsilon,&gparams, hessgvalue); 
  
                  /* Rprintf("node=%d hessian at g\n",nodeid+1);
		   for(j=0;j<myBeta->size;j++){Rprintf("%f ",gsl_vector_get(myBeta,j));}Rprintf("\n");      
                   for(j=0;j<hessgvalue->size1;j++){
                   for(k=0;k<hessgvalue->size2;k++){Rprintf("%f ",gsl_matrix_get(hessgvalue,j,k));} Rprintf("\n");}*/
   /*Rprintf("epsilon=%f\n",epsilon);*/ 
   n=((designdata->array_of_designs)[groupid])->size1;/** number of obs in group */
   m=1;/** number of params */
   /*Rprintf("gvalue in g_inner=|%f| n=|%d| |%f|\n",gvalue,n,-n*gvalue);*/
   /*if(status != GSL_SUCCESS){Rprintf("2epsilon=%f %f\n",gsl_vector_get(epsilon,0), gvalue);}*/
   logscore= -n*gvalue-0.5*log(gsl_matrix_get(hessgvalue,0,0))+(m/2.0)*log((2.0*M_PI)/n); /** this is the final value */
   if(gsl_isnan(logscore)){error("nan in g_inner hessmat=%f epsilon=%f gvalue=%f\n",gsl_matrix_get(hessgvalue,0,0),gsl_vector_get(epsilon,0),gvalue);}       
     /*}*/
    
   /*Rprintf("group=%d logscore=%f\n",groupid+1,logscore);*/
    

      gsl_vector_free(dgvalues);
      gsl_vector_free(epsilon);
      gsl_matrix_free(hessgvalue);
      gsl_vector_free(vectmp1);
      gsl_vector_free(vectmp1long);
      gsl_vector_free(vectmp2long);
     
      return(logscore);
      
  
  
} 
  std::vector<double> operator() (FUNCTION func, const std::vector<double>& xg, 
      const int nFunc) {

    gsl_set_error_handler_off();
    // Build the GSL type functions from the passed function
    // Similar to stack overflow 13289311 
    gsl_multiroot_function F;
    F.n = nFunc; 
    F.f = [] (const gsl_vector * x, void * p, gsl_vector * f)->int { 
      std::vector<double> xin; 
      for (unsigned int i=0; i < x->size; ++i) {
        double xt = gsl_vector_get(x, i);
        if (xt != xt) return GSL_EDOM; 
        xin.push_back(xt);
      }
      std::vector<double> ff = (*static_cast<FUNCTION*>(p))(xin);
      
      for (unsigned int i=0; i < x->size; ++i) {
        if (ff[i] != ff[i]) return GSL_ERANGE;
        gsl_vector_set(f, i, ff[i]);
      }
      return GSL_SUCCESS;
    };
    F.params = &func;
   
    gsl_vector *x = gsl_vector_alloc(nFunc); 
    for (unsigned int i=0; i<nFunc; ++i) {
      gsl_vector_set(x, i, xg[i]); 
    }

    //const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrids; 
    //const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrid; 
    const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_dnewton; 
    gsl_multiroot_fsolver *s = gsl_multiroot_fsolver_alloc(T, nFunc);
    int status = gsl_multiroot_fsolver_set(s, &F, x);
     
    //if (status) printf(" GSL Error: %s\n", gsl_strerror(status)); 
    
    int iter = 0; 
    do { 
      iter++; 
      status = gsl_multiroot_fsolver_iterate(s);
      if (status) break; // Solver is stuck  
      status = gsl_multiroot_test_residual(s->f, mTol);  
    } while (status == GSL_CONTINUE && iter < mMaxIter);
    
    gsl_set_error_handler(NULL);
    
    if (iter >= mMaxIter || status) {
        std::vector<double> ferr, xx;  
        for (int i=0; i<nFunc; i++) {
          ferr.push_back(gsl_vector_get(s->f, i));
          xx.push_back(gsl_vector_get(s->x, i));
        }
        throw MultiDRootException("Multi root find did not converge", iter,
            status, xx, ferr);
    }
    
    std::vector<double> out; 
    for (unsigned int i=0; i<nFunc; ++i) {
      out.push_back(gsl_vector_get(s->x, i)); 
    }
    
    return out;
  }
Exemplo n.º 15
0
static void cfglim(gsl_vector * lean_max, gsl_vector * pitch_max,
                gsl_vector * lean_min, gsl_vector * pitch_min,
                const gsl_vector * steer, Whipple * bike)
{
  int i, N = steer->size, iter = 0, iter_max = ITER_MAX, status;
  double ftol = FTOL;
  gsl_vector * x = gsl_vector_alloc(2);         // vector to store the solution
  gsl_vector * lean, * pitch;
  const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
  gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
  gsl_multiroot_function_fdf f = {&cfglim_f, &cfglim_df,
                                  &cfglim_fdf, 2, bike};

  // Maximum lean initial guess
  gsl_vector_set(x, 0, M_PI/3.0);
  gsl_vector_set(x, 1, M_PI/2.0);
  lean = lean_max;            // set lean to point at max lean vector
  pitch = pitch_max;          // set pitch to point at max pitch vector
  for (int c = 0; c < 2; gsl_vector_set(x, 0, -M_PI/3.0), // min lean i.g.
                         gsl_vector_set(x, 1, M_PI/2.0),
                         lean = lean_min,     // point at min lean vector
                         pitch = pitch_min,   // point at min pitch vector
                         ++c) {
    gsl_multiroot_fdfsolver_set(s, &f, x);
    for (i = N / 2; i < N - 1; ++i) {
      bike->q3 = gsl_vector_get(steer, i);    // steer as a parameter
      iter = 0;
      do
      {
        ++iter;
        status = gsl_multiroot_fdfsolver_iterate(s);
        if (status)
          iterateError(status, "cfglim()", gsl_vector_get(steer, i));
        status = gsl_multiroot_test_residual(s->f, ftol);
      } while(status == GSL_CONTINUE && iter < iter_max);
      
      // Increase the tolerance by an order of magnitude to improve convergence
      //if (iter == iter_max) {
      //  gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
      //  gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
      //  gsl_multiroot_fdfsolver_set(s, &f, x);
      //  increaseftol(&ftol, &i, iter_max, "cfglim()", bike->q3);
      //  continue;
      //} // if

      // Store the lean and pitch
      gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
      gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));

      ftol = FTOL;      // reset FTOL
    } // for i (steer from PI/2 to PI)
    gsl_vector_set(lean, i, gsl_vector_get(lean, i-1));
    gsl_vector_set(pitch, i, gsl_vector_get(pitch, i-1));

    gsl_vector_set(x, 0, gsl_vector_get(lean, N/2));
    gsl_vector_set(x, 1, gsl_vector_get(pitch, N/2));
    gsl_multiroot_fdfsolver_set(s, &f, x);
    for (i = N / 2 - 1; i > 0; --i) {
      bike->q3 = gsl_vector_get(steer, i);  // steer as a parameter
      iter = 0;
      do
      {
        ++iter;
        status = gsl_multiroot_fdfsolver_iterate(s);
        if (status)
          iterateError(status, "cfglim()", gsl_vector_get(steer, i));
        status = gsl_multiroot_test_residual(s->f, ftol);
      } while(status == GSL_CONTINUE && iter < iter_max);
      
      // Increase the tolerance by an order of magnitude to improve convergence
      //if (iter == iter_max) {
      //  gsl_vector_set(x, 0, gsl_vector_get(lean, i+1));
      //  gsl_vector_set(x, 1, gsl_vector_get(pitch, i+1));
      //  gsl_multiroot_fdfsolver_set(s, &f, x);
      //  increaseftol(&ftol, &i, iter_max, "cfglim()", bike->q3);
      //  continue;
      //} // if

      // Store the lean and pitch
      gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
      gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
      // Reset ftol in case it had been increased due to convergence issues
      ftol = FTOL;
    } // for i (steer from PI/2 to O)
    gsl_vector_set(lean, 0, gsl_vector_get(lean, 1));
    gsl_vector_set(pitch, 0, gsl_vector_get(pitch, 1));
  } // for c

  // Free dynamically allocated variables
  gsl_multiroot_fdfsolver_free(s);
  gsl_vector_free(x);
} // cfglim()
Exemplo n.º 16
0
// Given a vector of steer values, calculate the lean values associated with
// static equilibrium.  Also, return the indices of the steer/lean vectors
// which most nearly cause the u5^2 coefficient to go to zero.
static int staticEq(gsl_vector * lean, gsl_vector * pitch,
             const gsl_vector * steer, Whipple * bike)
{
  int i, N = lean->size, iter, iter_max = ITER_MAX, status;

  double ftol = FTOL;
  gsl_vector * x = gsl_vector_alloc(2);         // vector to store the solution
  gsl_vector * u5s_coefs = zeros(steer->size);
  const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
  gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
  gsl_multiroot_function_fdf f = {&static_f, &static_df, &static_fdf, 2, bike};
  bike->q1 = bike->q3 = 0.0;
  bike->calcPitch();
  gsl_vector_set(x, 0, bike->q1);
  gsl_vector_set(x, 1, bike->q2);
  gsl_multiroot_fdfsolver_set(s, &f, x);

  // for loop to loop over all values of steer
  for (i = 0; i < N; ++i) {
    bike->q3 = gsl_vector_get(steer, i);  // steer as a parameter
    iter = 0;
    do
    {
      ++iter;
      status = gsl_multiroot_fdfsolver_iterate(s);
      if (status)
        iterateError(status, "staticEq()", gsl_vector_get(steer, i));
      status = gsl_multiroot_test_residual(s->f, ftol);
    } while (status == GSL_CONTINUE && iter < iter_max);

    // Increase the tolerance by an order of magnitude to improve convergence
    if (iter == iter_max) {
      gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
      gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
      gsl_multiroot_fdfsolver_set(s, &f, x);
      increaseftol(&ftol, &i, iter_max, "staticEq()", bike->q3);
      continue;
    } // if

    // Store the lean into the lean vector
    gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
    gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));

    // Store the square of the coefficient of the u5^2 term;
    gsl_vector_set(u5s_coefs, i, bike->F[10] * bike->F[10]);
    ftol = FTOL;  // reset the error tolerance
  } // for

  // Assign a large value to the u5s_coefs vector near steer = 0 and steer = PI
  // This ensure the minimum will be near PI/2 where the two boudary curves
  // cross
  for (i = 0; i < 5; ++i) {
    gsl_vector_set(u5s_coefs, i, 10000.0);
    gsl_vector_set(u5s_coefs, u5s_coefs->size - 1 - i, 10000.0);
  }

  // Free dynamically allocated variables
  gsl_multiroot_fdfsolver_free(s);
  gsl_vector_free(x);
  i = gsl_vector_min_index(u5s_coefs);
  gsl_vector_free(u5s_coefs);
  return i;
} // staticEq()
Exemplo n.º 17
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;

}
Exemplo n.º 18
0
int main(int argc, char** args) {
  int ext = 0,c;
  double ra,dec;
  double sol[2];
  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;
  int status;
  size_t iter=0;
  const size_t n=2;
  gsl_multiroot_function f={&fvec,n,NULL};
  gsl_vector *x = gsl_vector_alloc(n);
  char *wcsfn1=NULL, *wcsfn2=NULL;
  
  while ((c = getopt(argc, args, OPTIONS)) != -1) {
    switch(c) {
    case 'v':
      loglvl++;
      break;
    case 'h':
      print_help(args[0]);
      exit(0);
    case '1':
      wcsfn1 = optarg;
      break;
    case '2':
      wcsfn2 = optarg;
      break;
    }
  }
  log_init(loglvl);
  if (optind != argc) {
    print_help(args[0]);
    exit(-1);
  }
  if (!(wcsfn1) || !(wcsfn2)) {
    print_help(args[0]);
    exit(-1);
  }
  /* open the two wcs systems */
  wcs1 = anwcs_open(wcsfn1, ext);
  if (!wcs1) {
    ERROR("Failed to read WCS file");
    exit(-1);
  }
  logverb("Read WCS:\n");
  if (log_get_level() >= LOG_VERB) {
    anwcs_print(wcs1, log_get_fid());
  }
  wcs2 = anwcs_open(wcsfn2, ext);
  if (!wcs2) {
    ERROR("Failed to read WCS file");
    exit(-1);
  }
  logverb("Read WCS:\n");
  if (log_get_level() >= LOG_VERB) {
    anwcs_print(wcs2, log_get_fid());
  }
  
  /* setup the solver, start in the middle */

  gsl_vector_set(x,0,anwcs_imagew(wcs1)/2.0);
  gsl_vector_set(x,1,anwcs_imageh(wcs1)/2.0);
  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc (T,2);
  gsl_multiroot_fsolver_set(s,&f,x);
  print_state(iter,s);
  do {
    iter++;
    status = gsl_multiroot_fsolver_iterate(s);
    print_state(iter,s);
    if (status) break;
    status = gsl_multiroot_test_residual(s->f,1e-7);
  } while (status == GSL_CONTINUE && iter < 1000);
  sol[0]=gsl_vector_get(s->x,0);
  sol[1]=gsl_vector_get(s->x,1);


  /* write some diagnostics on stderr */
  /* transform to ra/dec */
  anwcs_pixelxy2radec(wcs1, sol[0], sol[1], &ra, &dec);
  if (loglvl > LOG_MSG)
    fprintf(stderr,"Pixel (%.10f, %.10f) -> RA,Dec (%.10f, %.10f)\n", 
	    sol[0], sol[1], ra, dec);
  /* transform to x/y with second wcs 
     center of rotation should stay the same x/y */
  anwcs_radec2pixelxy(wcs2, ra, dec, &sol[0], &sol[1]);
  if (loglvl > LOG_MSG)
    fprintf(stderr,"RA,Dec (%.10f, %.10f) -> Pixel (%.10f, %.10f) \n", 
	    ra, dec, sol[0], sol[1]);

  /* write the solution */
  fprintf(stdout,"%f\n",sol[0]); 
  fprintf(stdout,"%f\n",sol[1]);
  
  return(0);
}
static void intersect_polish_root (D2<SBasis> const &A, double &s,
                                   D2<SBasis> const &B, double &t) {
#ifdef HAVE_GSL
    const gsl_multiroot_fsolver_type *T;
    gsl_multiroot_fsolver *sol;

    int status;
    size_t iter = 0;
#endif
    std::vector<Point> as, bs;
    as = A.valueAndDerivatives(s, 2);
    bs = B.valueAndDerivatives(t, 2);
    Point F = as[0] - bs[0];
    double best = dot(F, F);
    
    for(int i = 0; i < 4; i++) {
        
        /**
           we want to solve
           J*(x1 - x0) = f(x0)
           
           |dA(s)[0]  -dB(t)[0]|  (X1 - X0) = A(s) - B(t)
           |dA(s)[1]  -dB(t)[1]| 
        **/

        // We're using the standard transformation matricies, which is numerically rather poor.  Much better to solve the equation using elimination.

        Affine jack(as[1][0], as[1][1],
                    -bs[1][0], -bs[1][1],
                    0, 0);
        Point soln = (F)*jack.inverse();
        double ns = s - soln[0];
        double nt = t - soln[1];
        
        as = A.valueAndDerivatives(ns, 2);
        bs = B.valueAndDerivatives(nt, 2);
        F = as[0] - bs[0];
        double trial = dot(F, F);
        if (trial > best*0.1) {// we have standards, you know
            // At this point we could do a line search
            break;
        }
        best = trial;
        s = ns;
        t = nt;
    }
    
#ifdef HAVE_GSL
    const size_t n = 2;
    struct rparams p = {A, B};
    gsl_multiroot_function f = {&intersect_polish_f, n, &p};

    double x_init[2] = {s, t};
    gsl_vector *x = gsl_vector_alloc (n);

    gsl_vector_set (x, 0, x_init[0]);
    gsl_vector_set (x, 1, x_init[1]);

    T = gsl_multiroot_fsolver_hybrids;
    sol = gsl_multiroot_fsolver_alloc (T, 2);
    gsl_multiroot_fsolver_set (sol, &f, x);

    do
    {
        iter++;
        status = gsl_multiroot_fsolver_iterate (sol);

        if (status)   /* check if solver is stuck */
            break;

        status =
            gsl_multiroot_test_residual (sol->f, 1e-12);
    }
    while (status == GSL_CONTINUE && iter < 1000);

    s = gsl_vector_get (sol->x, 0);
    t = gsl_vector_get (sol->x, 1);

    gsl_multiroot_fsolver_free (sol);
    gsl_vector_free (x);
#endif
    
    {
    // This code does a neighbourhood search for minor improvements.
    double best_v = L1(A(s) - B(t));
    //std::cout  << "------\n" <<  best_v << std::endl;
    Point best(s,t);
    while (true) {
        Point trial = best;
        double trial_v = best_v;
        for(int nsi = -1; nsi < 2; nsi++) {
        for(int nti = -1; nti < 2; nti++) {
            Point n(EpsilonBy(best[0], nsi),
                    EpsilonBy(best[1], nti));
            double c = L1(A(n[0]) - B(n[1]));
            //std::cout << c << "; ";
            if (c < trial_v) {
                trial = n;
                trial_v = c;
            }
        }
        }
        if(trial == best) {
            //std::cout << "\n" << s << " -> " << s - best[0] << std::endl;
            //std::cout << t << " -> " << t - best[1] << std::endl;
            //std::cout << best_v << std::endl;
            s = best[0];
            t = best[1];
            return;
        } else {
            best = trial;
            best_v = trial_v;
        }
    }
    }
}
static int
XLALSimIMRSpinEOBInitialConditionsPrec(
				   REAL8Vector * initConds,	/**<< OUTPUT, Initial dynamical variables */
				   const REAL8 mass1,	/**<< mass 1 */
				   const REAL8 mass2,	/**<< mass 2 */
				   const REAL8 fMin,	/**<< Initial frequency (given) */
				   const REAL8 inc,	/**<< Inclination */
				   const REAL8 spin1[],	/**<< Initial spin vector 1 */
				   const REAL8 spin2[],	/**<< Initial spin vector 2 */
				   SpinEOBParams * params	/**<< Spin EOB parameters */
)
{

#ifndef LAL_NDEBUG
	if (!initConds) {
		XLAL_ERROR(XLAL_EINVAL);
	}
#endif

	int	debugPK = 0; int printPK = 0;
  FILE* UNUSED out = NULL;

	if (printPK) {
		XLAL_PRINT_INFO("Inside the XLALSimIMRSpinEOBInitialConditionsPrec function!\n");
		XLAL_PRINT_INFO(
    "Inputs: m1 = %.16e, m2 = %.16e, fMin = %.16e, inclination = %.16e\n",
      mass1, mass2, (double)fMin, (double)inc);
		XLAL_PRINT_INFO("Inputs: mSpin1 = {%.16e, %.16e, %.16e}\n",
      spin1[0], spin1[1], spin1[2]);
		XLAL_PRINT_INFO("Inputs: mSpin2 = {%.16e, %.16e, %.16e}\n",
      spin2[0], spin2[1], spin2[2]);
		fflush(NULL);
	}
	static const int UNUSED lMax = 8;

	int		i;

	/* Variable to keep track of whether the user requested the tortoise */
	int		tmpTortoise;

	UINT4		SpinAlignedEOBversion;

	REAL8		mTotal;
	REAL8		eta;
	REAL8		omega   , v0;	/* Initial velocity and angular
					 * frequency */

	REAL8		ham;	/* Hamiltonian */

	REAL8		LnHat    [3];	/* Initial orientation of angular
					 * momentum */
	REAL8		rHat     [3];	/* Initial orientation of radial
					 * vector */
	REAL8		vHat     [3];	/* Initial orientation of velocity
					 * vector */
	REAL8		Lhat     [3];	/* Direction of relativistic ang mom */
	REAL8		qHat     [3];
	REAL8		pHat     [3];

	/* q and p vectors in Cartesian and spherical coords */
	REAL8		qCart    [3], pCart[3];
	REAL8		qSph     [3], pSph[3];

	/* We will need to manipulate the spin vectors */
	/* We will use temporary vectors to do this */
	REAL8		tmpS1    [3];
	REAL8		tmpS2    [3];
	REAL8		tmpS1Norm[3];
	REAL8		tmpS2Norm[3];

	REAL8Vector	qCartVec, pCartVec;
	REAL8Vector	s1Vec, s2Vec, s1VecNorm, s2VecNorm;
	REAL8Vector	sKerr, sStar;
	REAL8		sKerrData[3], sStarData[3];
	REAL8		a = 0.;
	//, chiS, chiA;
	//REAL8 chi1, chi2;

	/*
	 * We will need a full values vector for calculating derivs of
	 * Hamiltonian
	 */
	REAL8		sphValues[12];
	REAL8		cartValues[12];

	/* Matrices for rotating to the new basis set. */
	/* It is more convenient to calculate the ICs in a simpler basis */
	gsl_matrix     *rotMatrix = NULL;
	gsl_matrix     *invMatrix = NULL;
	gsl_matrix     *rotMatrix2 = NULL;
	gsl_matrix     *invMatrix2 = NULL;

	/* Root finding stuff for finding the spherical orbit */
	SEOBRootParams	rootParams;
	const gsl_multiroot_fsolver_type *T = gsl_multiroot_fsolver_hybrid;
	gsl_multiroot_fsolver *rootSolver = NULL;

	gsl_multiroot_function rootFunction;
	gsl_vector     *initValues = NULL;
	gsl_vector     *finalValues = NULL;
	INT4 gslStatus;
        INT4 cntGslNoProgress = 0, MAXcntGslNoProgress = 5;
        //INT4 cntGslNoProgress = 0, MAXcntGslNoProgress = 50;
        REAL8 multFacGslNoProgress = 3./5.;
	//const int	maxIter = 2000;
	const int	maxIter = 10000;

	memset(&rootParams, 0, sizeof(rootParams));

	mTotal = mass1 + mass2;
	eta = mass1 * mass2 / (mTotal * mTotal);
	memcpy(tmpS1, spin1, sizeof(tmpS1));
	memcpy(tmpS2, spin2, sizeof(tmpS2));
	memcpy(tmpS1Norm, spin1, sizeof(tmpS1Norm));
	memcpy(tmpS2Norm, spin2, sizeof(tmpS2Norm));
	for (i = 0; i < 3; i++) {
		tmpS1Norm[i] /= mTotal * mTotal;
		tmpS2Norm[i] /= mTotal * mTotal;
	}
	SpinAlignedEOBversion = params->seobCoeffs->SpinAlignedEOBversion;
	/* We compute the ICs for the non-tortoise p, and convert at the end */
	tmpTortoise = params->tortoise;
	params->tortoise = 0;

	EOBNonQCCoeffs *nqcCoeffs = NULL;
	nqcCoeffs = params->nqcCoeffs;

	/*
	 * STEP 1) Rotate to LNhat0 along z-axis and N0 along x-axis frame,
	 * where LNhat0 and N0 are initial normal to orbital plane and
	 * initial orbital separation;
	 */

	/* Set the initial orbital ang mom direction. Taken from STPN code */
	LnHat[0] = sin(inc);
	LnHat[1] = 0.;
	LnHat[2] = cos(inc);

	/*
	 * Set the radial direction - need to take care to avoid singularity
	 * if L is along z axis
	 */
	if (LnHat[2] > 0.9999) {
		rHat[0] = 1.;
		rHat[1] = rHat[2] = 0.;
	} else {
		REAL8		theta0 = atan(-LnHat[2] / LnHat[0]);	/* theta0 is between 0
									 * and Pi */
		rHat[0] = sin(theta0);
		rHat[1] = 0;
		rHat[2] = cos(theta0);
	}

	/* Now we can complete the triad */
	vHat[0] = CalculateCrossProductPrec(0, LnHat, rHat);
	vHat[1] = CalculateCrossProductPrec(1, LnHat, rHat);
	vHat[2] = CalculateCrossProductPrec(2, LnHat, rHat);

	NormalizeVectorPrec(vHat);

	/* Vectors BEFORE rotation */
	if (printPK) {
		for (i = 0; i < 3; i++)
			XLAL_PRINT_INFO(" LnHat[%d] = %.16e, rHat[%d] = %.16e, vHat[%d] = %.16e\n",
        i, LnHat[i], i, rHat[i], i, vHat[i]);

		XLAL_PRINT_INFO("\n\n");
		for (i = 0; i < 3; i++)
			XLAL_PRINT_INFO(" s1[%d] = %.16e, s2[%d] = %.16e\n", i, tmpS1[i], i, tmpS2[i]);
    fflush(NULL);
	}

	/* Allocate and compute the rotation matrices */
	XLAL_CALLGSL(rotMatrix = gsl_matrix_alloc(3, 3));
	XLAL_CALLGSL(invMatrix = gsl_matrix_alloc(3, 3));
	if (!rotMatrix || !invMatrix) {
		if (rotMatrix)
			gsl_matrix_free(rotMatrix);
		if (invMatrix)
			gsl_matrix_free(invMatrix);
		XLAL_ERROR(XLAL_ENOMEM);
	}
	if (CalculateRotationMatrixPrec(rotMatrix, invMatrix, rHat, vHat, LnHat) == XLAL_FAILURE) {
		gsl_matrix_free(rotMatrix);
		gsl_matrix_free(invMatrix);
		XLAL_ERROR(XLAL_ENOMEM);
	}
	/* Rotate the orbital vectors and spins */
	ApplyRotationMatrixPrec(rotMatrix, rHat);
	ApplyRotationMatrixPrec(rotMatrix, vHat);
	ApplyRotationMatrixPrec(rotMatrix, LnHat);
	ApplyRotationMatrixPrec(rotMatrix, tmpS1);
	ApplyRotationMatrixPrec(rotMatrix, tmpS2);
	ApplyRotationMatrixPrec(rotMatrix, tmpS1Norm);
	ApplyRotationMatrixPrec(rotMatrix, tmpS2Norm);

	/* See if Vectors have been rotated fine */
	if (printPK) {
		XLAL_PRINT_INFO("\nAfter applying rotation matrix:\n\n");
		for (i = 0; i < 3; i++)
			XLAL_PRINT_INFO(" LnHat[%d] = %.16e, rHat[%d] = %.16e, vHat[%d] = %.16e\n",
                i, LnHat[i], i, rHat[i], i, vHat[i]);

		XLAL_PRINT_INFO("\n");
		for (i = 0; i < 3; i++)
			XLAL_PRINT_INFO(" s1[%d] = %.16e, s2[%d] = %.16e\n", i, tmpS1[i], i, tmpS2[i]);

    fflush(NULL);
	}
	/*
	 * STEP 2) After rotation in STEP 1, in spherical coordinates, phi0
	 * and theta0 are given directly in Eq. (4.7), r0, pr0, ptheta0 and
	 * pphi0 are obtained by solving Eqs. (4.8) and (4.9) (using
	 * gsl_multiroot_fsolver). At this step, we find initial conditions
	 * for a spherical orbit without radiation reaction.
	 */

  /* Initialise the gsl stuff */
	XLAL_CALLGSL(rootSolver = gsl_multiroot_fsolver_alloc(T, 3));
	if (!rootSolver) {
		gsl_matrix_free(rotMatrix);
		gsl_matrix_free(invMatrix);
		XLAL_ERROR(XLAL_ENOMEM);
	}
	XLAL_CALLGSL(initValues = gsl_vector_calloc(3));
	if (!initValues) {
		gsl_multiroot_fsolver_free(rootSolver);
		gsl_matrix_free(rotMatrix);
		gsl_matrix_free(invMatrix);
		XLAL_ERROR(XLAL_ENOMEM);
	}

	rootFunction.f = XLALFindSphericalOrbitPrec;
	rootFunction.n = 3;
	rootFunction.params = &rootParams;

	/* Calculate the initial velocity from the given initial frequency */
	omega = LAL_PI * mTotal * LAL_MTSUN_SI * fMin;
	v0 = cbrt(omega);

	/* Given this, we can start to calculate the initial conditions */
	/* for spherical coords in the new basis */
	rootParams.omega = omega;
	rootParams.params = params;

	/* To start with, we will just assign Newtonian-ish ICs to the system */
	rootParams.values[0] = scale1 * 1. / (v0 * v0);	/* Initial r */
	rootParams.values[4] = scale2 * v0;	            /* Initial p */
	rootParams.values[5] = scale3 * 1e-3;
	//PK
  memcpy(rootParams.values + 6, tmpS1, sizeof(tmpS1));
	memcpy(rootParams.values + 9, tmpS2, sizeof(tmpS2));

	if (printPK) {
    XLAL_PRINT_INFO("ICs guess: x = %.16e, py = %.16e, pz = %.16e\n",
      rootParams.values[0]/scale1, rootParams.values[4]/scale2,
      rootParams.values[5]/scale3);
    fflush(NULL);
  }

	gsl_vector_set(initValues, 0, rootParams.values[0]);
	gsl_vector_set(initValues, 1, rootParams.values[4]);
  gsl_vector_set(initValues, 2, rootParams.values[5]);

	gsl_multiroot_fsolver_set(rootSolver, &rootFunction, initValues);

	/* We are now ready to iterate to find the solution */
	i = 0;

  if(debugPK){ out = fopen("ICIterations.dat", "w"); }
	do {
		XLAL_CALLGSL(gslStatus = gsl_multiroot_fsolver_iterate(rootSolver));
		if (debugPK) {
      fprintf( out, "%d\t", i );

      /* Write to file */
      fprintf( out, "%.16e\t%.16e\t%.16e\t",
        rootParams.values[0]/scale1, rootParams.values[4]/scale2,
        rootParams.values[5]/scale3 );

      /* Residual Function values whose roots we are trying to find */
      finalValues = gsl_multiroot_fsolver_f(rootSolver);

      /* Write to file */
      fprintf( out, "%.16e\t%.16e\t%.16e\t",
        gsl_vector_get(finalValues, 0),
        gsl_vector_get(finalValues, 1),
        gsl_vector_get(finalValues, 2) );

      /* Step sizes in each of function variables */
      finalValues = gsl_multiroot_fsolver_dx(rootSolver);

      /* Write to file */
      fprintf( out, "%.16e\t%.16e\t%.16e\t%d\n",
        gsl_vector_get(finalValues, 0)/scale1,
        gsl_vector_get(finalValues, 1)/scale2,
        gsl_vector_get(finalValues, 2)/scale3,
        gslStatus );
		}

    if (gslStatus == GSL_ENOPROG || gslStatus == GSL_ENOPROGJ) {
      XLAL_PRINT_INFO(
        "\n NO PROGRESS being made by Spherical orbit root solver\n");

      /* Print Residual Function values whose roots we are trying to find */
      finalValues = gsl_multiroot_fsolver_f(rootSolver);
      XLAL_PRINT_INFO("Function value here given by the following:\n");
      XLAL_PRINT_INFO(" F1 = %.16e, F2 = %.16e, F3 = %.16e\n",
          gsl_vector_get(finalValues, 0),
		       gsl_vector_get(finalValues, 1), gsl_vector_get(finalValues, 2));

      /* Print Step sizes in each of function variables */
      finalValues = gsl_multiroot_fsolver_dx(rootSolver);
//      XLAL_PRINT_INFO("Stepsizes in each dimension:\n");
//      XLAL_PRINT_INFO(" x = %.16e, py = %.16e, pz = %.16e\n",
//          gsl_vector_get(finalValues, 0)/scale1,
//	       gsl_vector_get(finalValues, 1)/scale2,
//          gsl_vector_get(finalValues, 2)/scale3);

      /* Only allow this flag to be caught MAXcntGslNoProgress no. of times */
      cntGslNoProgress += 1;
      if (cntGslNoProgress >= MAXcntGslNoProgress) {
        cntGslNoProgress = 0;

        if(multFacGslNoProgress < 1.){ multFacGslNoProgress *= 1.02; }
        else{ multFacGslNoProgress /= 1.01; }

      } 
      /* Now that no progress is being made, we need to reset the initial guess
       * for the (r,pPhi, pTheta) and reset the integrator */
      rootParams.values[0] = scale1 * 1. / (v0 * v0);	/* Initial r */
      rootParams.values[4] = scale2 * v0;	            /* Initial p */
      if( cntGslNoProgress % 2 )
        rootParams.values[5] = scale3 * 1e-3 / multFacGslNoProgress;
      else
        rootParams.values[5] = scale3 * 1e-3 * multFacGslNoProgress;
      //PK
      memcpy(rootParams.values + 6, tmpS1, sizeof(tmpS1));
      memcpy(rootParams.values + 9, tmpS2, sizeof(tmpS2));

      if (printPK) {
        XLAL_PRINT_INFO("New ICs guess: x = %.16e, py = %.16e, pz = %.16e\n",
                rootParams.values[0]/scale1, rootParams.values[4]/scale2,
                rootParams.values[5]/scale3);
        fflush(NULL);
      }

      gsl_vector_set(initValues, 0, rootParams.values[0]);
      gsl_vector_set(initValues, 1, rootParams.values[4]);
      gsl_vector_set(initValues, 2, rootParams.values[5]);
      gsl_multiroot_fsolver_set(rootSolver, &rootFunction, initValues);
    }
    else if (gslStatus == GSL_EBADFUNC) {
      XLALPrintError(
      "Inf or Nan encountered in evaluluation of spherical orbit Eqn\n");
			gsl_multiroot_fsolver_free(rootSolver);
			gsl_vector_free(initValues);
			gsl_matrix_free(rotMatrix);
			gsl_matrix_free(invMatrix);
			XLAL_ERROR(XLAL_EDOM);
    }
		else if (gslStatus != GSL_SUCCESS) {
			XLALPrintError("Error in GSL iteration function!\n");
			gsl_multiroot_fsolver_free(rootSolver);
			gsl_vector_free(initValues);
			gsl_matrix_free(rotMatrix);
			gsl_matrix_free(invMatrix);
			XLAL_ERROR(XLAL_EDOM);
		}

    /* different ways to test convergence of the method */
		XLAL_CALLGSL(gslStatus = gsl_multiroot_test_residual(rootSolver->f, 1.0e-8));
    /*XLAL_CALLGSL(gslStatus= gsl_multiroot_test_delta(
          gsl_multiroot_fsolver_dx(rootSolver),
          gsl_multiroot_fsolver_root(rootSolver),
          1.e-8, 1.e-5));*/
		i++;
	}
	while (gslStatus == GSL_CONTINUE && i <= maxIter);

  if(debugPK) { fflush(NULL); fclose(out); }

	if (i > maxIter && gslStatus != GSL_SUCCESS) {
		gsl_multiroot_fsolver_free(rootSolver);
		gsl_vector_free(initValues);
		gsl_matrix_free(rotMatrix);
		gsl_matrix_free(invMatrix);
		//XLAL_ERROR(XLAL_EMAXITER);
		XLAL_ERROR(XLAL_EDOM);
	}
	finalValues = gsl_multiroot_fsolver_root(rootSolver);

	if (printPK) {
		XLAL_PRINT_INFO("Spherical orbit conditions here given by the following:\n");
		XLAL_PRINT_INFO(" x = %.16e, py = %.16e, pz = %.16e\n",
           gsl_vector_get(finalValues, 0)/scale1,
		       gsl_vector_get(finalValues, 1)/scale2,
           gsl_vector_get(finalValues, 2)/scale3);
	}
	memset(qCart, 0, sizeof(qCart));
	memset(pCart, 0, sizeof(pCart));

	qCart[0] = gsl_vector_get(finalValues, 0)/scale1;
	pCart[1] = gsl_vector_get(finalValues, 1)/scale2;
	pCart[2] = gsl_vector_get(finalValues, 2)/scale3;


	/* Free the GSL root finder, since we're done with it */
	gsl_multiroot_fsolver_free(rootSolver);
	gsl_vector_free(initValues);


	/*
	 * STEP 3) Rotate to L0 along z-axis and N0 along x-axis frame, where
	 * L0 is the initial orbital angular momentum and L0 is calculated
	 * using initial position and linear momentum obtained in STEP 2.
	 */

	/* Now we can calculate the relativistic L and rotate to a new basis */
	memcpy(qHat, qCart, sizeof(qCart));
	memcpy(pHat, pCart, sizeof(pCart));

	NormalizeVectorPrec(qHat);
	NormalizeVectorPrec(pHat);

	Lhat[0] = CalculateCrossProductPrec(0, qHat, pHat);
	Lhat[1] = CalculateCrossProductPrec(1, qHat, pHat);
	Lhat[2] = CalculateCrossProductPrec(2, qHat, pHat);

	NormalizeVectorPrec(Lhat);

	XLAL_CALLGSL(rotMatrix2 = gsl_matrix_alloc(3, 3));
	XLAL_CALLGSL(invMatrix2 = gsl_matrix_alloc(3, 3));

	if (CalculateRotationMatrixPrec(rotMatrix2, invMatrix2, qHat, pHat, Lhat) == XLAL_FAILURE) {
		gsl_matrix_free(rotMatrix);
		gsl_matrix_free(invMatrix);
		XLAL_ERROR(XLAL_ENOMEM);
	}
	ApplyRotationMatrixPrec(rotMatrix2, rHat);
	ApplyRotationMatrixPrec(rotMatrix2, vHat);
	ApplyRotationMatrixPrec(rotMatrix2, LnHat);
	ApplyRotationMatrixPrec(rotMatrix2, tmpS1);
	ApplyRotationMatrixPrec(rotMatrix2, tmpS2);
	ApplyRotationMatrixPrec(rotMatrix2, tmpS1Norm);
	ApplyRotationMatrixPrec(rotMatrix2, tmpS2Norm);
	ApplyRotationMatrixPrec(rotMatrix2, qCart);
	ApplyRotationMatrixPrec(rotMatrix2, pCart);

        gsl_matrix_free(rotMatrix);
        gsl_matrix_free(rotMatrix2);

        if (printPK) {
		XLAL_PRINT_INFO("qCart after rotation2 %3.10f %3.10f %3.10f\n", qCart[0], qCart[1], qCart[2]);
		XLAL_PRINT_INFO("pCart after rotation2 %3.10f %3.10f %3.10f\n", pCart[0], pCart[1], pCart[2]);
		XLAL_PRINT_INFO("S1 after rotation2 %3.10f %3.10f %3.10f\n", tmpS1Norm[0], tmpS1Norm[1], tmpS1Norm[2]);
		XLAL_PRINT_INFO("S2 after rotation2 %3.10f %3.10f %3.10f\n", tmpS2Norm[0], tmpS2Norm[1], tmpS2Norm[2]);
	}
	/*
	 * STEP 4) In the L0-N0 frame, we calculate (dE/dr)|sph using Eq.
	 * (4.14), then initial dr/dt using Eq. (4.10), and finally pr0 using
	 * Eq. (4.15).
	 */

	/* Now we can calculate the flux. Change to spherical co-ords */
	CartesianToSphericalPrec(qSph, pSph, qCart, pCart);
	memcpy(sphValues, qSph, sizeof(qSph));
	memcpy(sphValues + 3, pSph, sizeof(pSph));
	memcpy(sphValues + 6, tmpS1, sizeof(tmpS1));
	memcpy(sphValues + 9, tmpS2, sizeof(tmpS2));

	memcpy(cartValues, qCart, sizeof(qCart));
	memcpy(cartValues + 3, pCart, sizeof(pCart));
	memcpy(cartValues + 6, tmpS1, sizeof(tmpS1));
	memcpy(cartValues + 9, tmpS2, sizeof(tmpS2));

	REAL8		dHdpphi , d2Hdr2, d2Hdrdpphi;
	REAL8		rDot    , dHdpr, flux, dEdr;

	d2Hdr2 = XLALCalculateSphHamiltonianDeriv2Prec(0, 0, sphValues, params);
	d2Hdrdpphi = XLALCalculateSphHamiltonianDeriv2Prec(0, 5, sphValues, params);

	if (printPK)
		XLAL_PRINT_INFO("d2Hdr2 = %.16e, d2Hdrdpphi = %.16e\n", d2Hdr2, d2Hdrdpphi);

	/* New code to compute derivatives w.r.t. cartesian variables */

	REAL8		tmpDValues[14];
	int UNUSED	status;
	for (i = 0; i < 3; i++) {
		cartValues[i + 6] /= mTotal * mTotal;
		cartValues[i + 9] /= mTotal * mTotal;
	}
    UINT4 oldignoreflux = params->ignoreflux;
    params->ignoreflux = 1;
	status = XLALSpinPrecHcapNumericalDerivative(0, cartValues, tmpDValues, params);
    params->ignoreflux = oldignoreflux;
	for (i = 0; i < 3; i++) {
		cartValues[i + 6] *= mTotal * mTotal;
		cartValues[i + 9] *= mTotal * mTotal;
	}

	dHdpphi = tmpDValues[1] / sqrt(cartValues[0] * cartValues[0] + cartValues[1] * cartValues[1] + cartValues[2] * cartValues[2]);
	//XLALSpinPrecHcapNumDerivWRTParam(4, cartValues, params) / sphValues[0];

	dEdr = -dHdpphi * d2Hdr2 / d2Hdrdpphi;

	if (printPK)
		XLAL_PRINT_INFO("d2Hdr2 = %.16e d2Hdrdpphi = %.16e dHdpphi = %.16e\n",
            d2Hdr2, d2Hdrdpphi, dHdpphi);

	if (d2Hdr2 != 0.0) {
		/* We will need to calculate the Hamiltonian to get the flux */
		s1Vec.length = s2Vec.length = s1VecNorm.length = s2VecNorm.length = sKerr.length = sStar.length = 3;
		s1Vec.data = tmpS1;
		s2Vec.data = tmpS2;
		s1VecNorm.data = tmpS1Norm;
		s2VecNorm.data = tmpS2Norm;
		sKerr.data = sKerrData;
		sStar.data = sStarData;

		qCartVec.length = pCartVec.length = 3;
		qCartVec.data = qCart;
		pCartVec.data = pCart;

		//chi1 = tmpS1[0] * LnHat[0] + tmpS1[1] * LnHat[1] + tmpS1[2] * LnHat[2];
		//chi2 = tmpS2[0] * LnHat[0] + tmpS2[1] * LnHat[1] + tmpS2[2] * LnHat[2];

		//if (debugPK)
			//XLAL_PRINT_INFO("magS1 = %.16e, magS2 = %.16e\n", chi1, chi2);

		//chiS = 0.5 * (chi1 / (mass1 * mass1) + chi2 / (mass2 * mass2));
		//chiA = 0.5 * (chi1 / (mass1 * mass1) - chi2 / (mass2 * mass2));

		XLALSimIMRSpinEOBCalculateSigmaKerr(&sKerr, mass1, mass2, &s1Vec, &s2Vec);
		XLALSimIMRSpinEOBCalculateSigmaStar(&sStar, mass1, mass2, &s1Vec, &s2Vec);

		/*
		 * The a in the flux has been set to zero, but not in the
		 * Hamiltonian
		 */
		a = sqrt(sKerr.data[0] * sKerr.data[0] + sKerr.data[1] * sKerr.data[1] + sKerr.data[2] * sKerr.data[2]);
		//XLALSimIMREOBCalcSpinPrecFacWaveformCoefficients(params->eobParams->hCoeffs, mass1, mass2, eta, /* a */ 0.0, chiS, chiA);
		//XLALSimIMRCalculateSpinPrecEOBHCoeffs(params->seobCoeffs, eta, a);
		ham = XLALSimIMRSpinPrecEOBHamiltonian(eta, &qCartVec, &pCartVec, &s1VecNorm, &s2VecNorm, &sKerr, &sStar, params->tortoise, params->seobCoeffs);

		if (printPK)
			XLAL_PRINT_INFO("Stas: hamiltonian in ICs at this point is %.16e\n", ham);

		/* And now, finally, the flux */
		REAL8Vector	polarDynamics, cartDynamics;
		REAL8		polarData[4], cartData[12];

		polarDynamics.length = 4;
		polarDynamics.data = polarData;

		polarData[0] = qSph[0];
		polarData[1] = 0.;
		polarData[2] = pSph[0];
		polarData[3] = pSph[2];

		cartDynamics.length = 12;
		cartDynamics.data = cartData;

		memcpy(cartData, qCart, 3 * sizeof(REAL8));
		memcpy(cartData + 3, pCart, 3 * sizeof(REAL8));
		memcpy(cartData + 6, tmpS1Norm, 3 * sizeof(REAL8));
		memcpy(cartData + 9, tmpS2Norm, 3 * sizeof(REAL8));

		//XLAL_PRINT_INFO("Stas: starting FLux calculations\n");

		flux = XLALInspiralPrecSpinFactorizedFlux(&polarDynamics, &cartDynamics, nqcCoeffs, omega, params, ham, lMax, SpinAlignedEOBversion);
		/*
		 * flux  = XLALInspiralSpinFactorizedFlux( &polarDynamics,
		 * nqcCoeffs, omega, params, ham, lMax, SpinAlignedEOBversion
		 * );
		 */
		//XLAL_PRINT_INFO("Stas flux = %.16e \n", flux);
		//exit(0);
		flux = flux / eta;

		rDot = -flux / dEdr;
		if (debugPK) {
			XLAL_PRINT_INFO("Stas here I am 2  \n");
		}
		/*
		 * We now need dHdpr - we take it that it is safely linear up
		 * to a pr of 1.0e-3 PK: Ideally, the pr should be of the
		 * order of other momenta, in order for its contribution to
		 * the Hamiltonian to not get buried in the numerical noise
		 * in the numerically larger momenta components
		 */
		cartValues[3] = 1.0e-3;
		for (i = 0; i < 3; i++) {
			cartValues[i + 6] /= mTotal * mTotal;
			cartValues[i + 9] /= mTotal * mTotal;
		}
        oldignoreflux = params->ignoreflux;
        params->ignoreflux = 1;
        params->seobCoeffs->updateHCoeffs = 1;
		status = XLALSpinPrecHcapNumericalDerivative(0, cartValues, tmpDValues, params);
        params->ignoreflux = oldignoreflux;
		for (i = 0; i < 3; i++) {
			cartValues[i + 6] *= mTotal * mTotal;
			cartValues[i + 9] *= mTotal * mTotal;
		}
        REAL8		csi = sqrt(XLALSimIMRSpinPrecEOBHamiltonianDeltaT(params->seobCoeffs, qSph[0], eta, a)*XLALSimIMRSpinPrecEOBHamiltonianDeltaR(params->seobCoeffs, qSph[0], eta, a)) / (qSph[0] * qSph[0] + a * a);

		dHdpr = csi*tmpDValues[0];
		//XLALSpinPrecHcapNumDerivWRTParam(3, cartValues, params);

		if (debugPK) {
			XLAL_PRINT_INFO("Ingredients going into prDot:\n");
			XLAL_PRINT_INFO("flux = %.16e, dEdr = %.16e, dHdpr = %.16e, dHdpr/pr = %.16e\n", flux, dEdr, dHdpr, dHdpr / cartValues[3]);
		}
		/*
		 * We can now calculate what pr should be taking into account
		 * the flux
		 */
		pSph[0] = rDot / (dHdpr / cartValues[3]);
	} else {
		/*
		 * Since d2Hdr2 has evaluated to zero, we cannot do the
		 * above. Just set pr to zero
		 */
		//XLAL_PRINT_INFO("d2Hdr2 is zero!\n");
		pSph[0] = 0;
	}

	/* Now we are done - convert back to cartesian coordinates ) */
	SphericalToCartesianPrec(qCart, pCart, qSph, pSph);

	/*
	 * STEP 5) Rotate back to the original inertial frame by inverting
	 * the rotation of STEP 3 and then  inverting the rotation of STEP 1.
	 */

	/* Undo rotations to get back to the original basis */
	/* Second rotation */
	ApplyRotationMatrixPrec(invMatrix2, rHat);
	ApplyRotationMatrixPrec(invMatrix2, vHat);
	ApplyRotationMatrixPrec(invMatrix2, LnHat);
	ApplyRotationMatrixPrec(invMatrix2, tmpS1);
	ApplyRotationMatrixPrec(invMatrix2, tmpS2);
	ApplyRotationMatrixPrec(invMatrix2, tmpS1Norm);
	ApplyRotationMatrixPrec(invMatrix2, tmpS2Norm);
	ApplyRotationMatrixPrec(invMatrix2, qCart);
	ApplyRotationMatrixPrec(invMatrix2, pCart);

	/* First rotation */
	ApplyRotationMatrixPrec(invMatrix, rHat);
	ApplyRotationMatrixPrec(invMatrix, vHat);
	ApplyRotationMatrixPrec(invMatrix, LnHat);
	ApplyRotationMatrixPrec(invMatrix, tmpS1);
	ApplyRotationMatrixPrec(invMatrix, tmpS2);
	ApplyRotationMatrixPrec(invMatrix, tmpS1Norm);
	ApplyRotationMatrixPrec(invMatrix, tmpS2Norm);
	ApplyRotationMatrixPrec(invMatrix, qCart);
	ApplyRotationMatrixPrec(invMatrix, pCart);

        gsl_matrix_free(invMatrix);
        gsl_matrix_free(invMatrix2);

        /* If required, apply the tortoise transform */
	if (tmpTortoise) {
		REAL8		r = sqrt(qCart[0] * qCart[0] + qCart[1] * qCart[1] + qCart[2] * qCart[2]);
		REAL8		deltaR = XLALSimIMRSpinPrecEOBHamiltonianDeltaR(params->seobCoeffs, r, eta, a);
		REAL8		deltaT = XLALSimIMRSpinPrecEOBHamiltonianDeltaT(params->seobCoeffs, r, eta, a);
		REAL8		csi = sqrt(deltaT * deltaR) / (r * r + a * a);

		REAL8		pr = (qCart[0] * pCart[0] + qCart[1] * pCart[1] + qCart[2] * pCart[2]) / r;

		params->tortoise = tmpTortoise;

		if (debugPK) {
			XLAL_PRINT_INFO("Applying the tortoise to p (csi = %.26e)\n", csi);
			XLAL_PRINT_INFO("pCart = %3.10f %3.10f %3.10f\n", pCart[0], pCart[1], pCart[2]);
		}
		for (i = 0; i < 3; i++) {
			pCart[i] = pCart[i] + qCart[i] * pr * (csi - 1.) / r;
		}
	}


    /* Now copy the initial conditions back to the return vector */
	memcpy(initConds->data, qCart, sizeof(qCart));
	memcpy(initConds->data + 3, pCart, sizeof(pCart));
	memcpy(initConds->data + 6, tmpS1Norm, sizeof(tmpS1Norm));
	memcpy(initConds->data + 9, tmpS2Norm, sizeof(tmpS2Norm));

    for (i=0; i<12; i++) {
        if (fabs(initConds->data[i]) <=1.0e-15) {
            initConds->data[i] = 0.;
        }
    }

	if (debugPK) {
		XLAL_PRINT_INFO("THE FINAL INITIAL CONDITIONS:\n");
		XLAL_PRINT_INFO(" %.16e %.16e %.16e\n%.16e %.16e %.16e\n%.16e %.16e %.16e\n%.16e %.16e %.16e\n", initConds->data[0], initConds->data[1], initConds->data[2],
		       initConds->data[3], initConds->data[4], initConds->data[5], initConds->data[6], initConds->data[7], initConds->data[8],
		       initConds->data[9], initConds->data[10], initConds->data[11]);
	}
	return XLAL_SUCCESS;
}
Exemplo n.º 21
0
void newton_remap(double xres[], long *success)
{
  
  double *x_init, zero[2], def1i, def2i, fctgrid, dx;
  const gsl_multiroot_fdfsolver_type *T;
  gsl_multiroot_fdfsolver *s;
  int status;
  size_t iter = 0;
  const size_t n = 2;
  gsl_vector * x = gsl_vector_alloc (NDIM_N); 
  gsl_multiroot_function_fdf f = {&lenseq_f_remap,
                                  &lenseq_df_remap,
                                  &lenseq_fdf_remap,
                                  n,NULL}; /* first is the function, then the derivative, then both, then number of simultaneous equations, then params -- Added comment AH 10/29/2015 */


  fctgrid = ((double) lens_grid.nedge-1)/(LX);
  dx = (LX / ((double) lens_grid.nedge-1));
 
  if ((x_init = (double *) malloc((n)*sizeof(double))) == NULL)
    error("main","can't allocate memory for caustic");

  x_init[0] = xres[0];
  x_init[1] = xres[1];
  gsl_vector_set (x, 0, x_init[0]);
  gsl_vector_set (x, 1, x_init[1]);

  T = gsl_multiroot_fdfsolver_newton;
  s = gsl_multiroot_fdfsolver_alloc (T, n);
  gsl_multiroot_fdfsolver_set (s, &f, x);

  do
    {
      iter++;

      status = gsl_multiroot_fdfsolver_iterate (s);


      if (status)
        break;

     status = gsl_multiroot_test_residual (s->f, 0.001);
   }
 while (status == GSL_CONTINUE && iter < 100);

 xres[0] = gsl_vector_get (s->x, 0);
 xres[1] = gsl_vector_get (s->x, 1);
 def1i = interp(lens_grid.def1l, xres[0], xres[1], lens_grid.nedge, lens_grid.nedge, LX, LX); 
 def2i = interp(lens_grid.def2l, xres[0], xres[1], lens_grid.nedge, lens_grid.nedge,  LX, LX);	

 zero[0] = gsl_vector_get (s->f, 0);
 zero[1] = gsl_vector_get (s->f, 1);
 
 gsl_multiroot_fdfsolver_free(s);
 
 zero[0] = xres[0]*fctgrid - def1i - lens_grid.y01;
 zero[1] = xres[1]*fctgrid - def2i - lens_grid.y02;

 
 if (iter > 98 || fabs(zero[0]) > 10.0 || fabs(zero[1]) > 10.0) *success = 0;
 else *success = 1;


 free(x_init);
 return;
}
Exemplo n.º 22
0
static void infspeed(gsl_vector * lean, gsl_vector * pitch,
                    double lean_ig, double pitch_ig, int ig_index,
                    const gsl_vector * steer, Whipple * bike)
{
  int i, N = lean->size, iter, status, iter_max = ITER_MAX;
  double ftol = FTOL;

  gsl_vector * x = gsl_vector_alloc(2);         // vector to store the solution
  const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
  gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
  gsl_multiroot_function_fdf f = {&inf_f, &inf_df, &inf_fdf, 2, bike};

  // Setup the initial conditions
  bike->q1 = lean_ig;
  bike->q2 = pitch_ig;
  bike->q3 = gsl_vector_get(steer, ig_index);
  bike->calcPitch();
  gsl_vector_set(x, 0, lean_ig);
  gsl_vector_set(x, 1, bike->q2);
  gsl_multiroot_fdfsolver_set(s, &f, x);

  for (i = ig_index; i > 0; --i) {
    bike->q3 = gsl_vector_get(steer, i);
    iter = 0;

    do {
      status = gsl_multiroot_fdfsolver_iterate(s);
      if (status)
        iterateError(status, "infspeed()", gsl_vector_get(steer, i));
      status = gsl_multiroot_test_residual(s->f, ftol);
    } while(status == GSL_CONTINUE && ++iter < iter_max);
    // Increase the tolerance by an order of magnitude to improve convergence
    //if (iter == iter_max) {
     // gsl_vector_set(x, 0, gsl_vector_get(lean, i+1));
     // gsl_vector_set(x, 1, gsl_vector_get(pitch, i+1));
    //  gsl_multiroot_fdfsolver_set(s, &f, x);
     // increaseftol(&ftol, &i, iter_max, "infspeed()", bike->q3);
     // continue;
    //} // if

    // Store the lean into the lean vector
    gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
    gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
    ftol = FTOL; // reset ftol
  } // for
  gsl_vector_set(lean, i, gsl_vector_get(lean, 1));
  gsl_vector_set(pitch, i, gsl_vector_get(pitch, 1));

  // Setup the initial conditions
  bike->q1 = lean_ig;
  bike->q2 = pitch_ig;
  bike->q3 = gsl_vector_get(steer, ig_index);
  bike->calcPitch();
  gsl_vector_set(x, 0, lean_ig);
  gsl_vector_set(x, 1, bike->q2);
  gsl_multiroot_fdfsolver_set(s, &f, x);

  for (i = ig_index + 1; i < N - 1; ++i) {
    bike->q3 = gsl_vector_get(steer, i);

    iter = 0;
    do {
      status = gsl_multiroot_fdfsolver_iterate(s);
      if (status)
        iterateError(status, "infspeed()", gsl_vector_get(steer, i));
      status = gsl_multiroot_test_residual(s->f, ftol);
    } while (status == GSL_CONTINUE && ++iter < iter_max);
    // Increase the tolerance by an order of magnitude to improve convergence
    if (iter == iter_max) {
      gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
      gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
      gsl_multiroot_fdfsolver_set(s, &f, x);
      increaseftol(&ftol, &i, iter_max, "infspeed()", bike->q3);
      continue;
    } // if

    // Store the lean into the lean vector
    gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
    gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
    ftol = FTOL; // reset ftol
  } // for
  gsl_vector_set(lean, i, gsl_vector_get(lean, i - 1));
  gsl_vector_set(pitch, i, gsl_vector_get(pitch, i - 1));
  
  gsl_multiroot_fdfsolver_free(s);
  gsl_vector_free(x);
}
Exemplo n.º 23
0
/*
 * 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;
}
Exemplo n.º 24
0
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 = &mp;

        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 = &mp;

        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;
}
Exemplo n.º 25
0
static VALUE MSolver_test_residual(VALUE self, VALUE f, VALUE epsabs) {
  gsl_vector * myf;
  Data_Get_Struct(f, gsl_vector, myf);
  return INT2FIX(gsl_multiroot_test_residual(myf, NUM2DBL(epsabs)));
}
Exemplo n.º 26
0
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;
}
Exemplo n.º 27
0
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
}
Exemplo n.º 28
0
int
test_fdf (const char * desc, gsl_multiroot_function_fdf * function,
          initpt_function initpt, double factor,
          const gsl_multiroot_fdfsolver_type * T)
{
    int status;
    double residual = 0;
    size_t i, n = function->n, iter = 0;

    gsl_vector *x = gsl_vector_alloc (n);
    gsl_matrix *J = gsl_matrix_alloc (n, n);

    gsl_multiroot_fdfsolver *s;

    (*initpt) (x);

    if (factor != 1.0) scale(x, factor);

    s = gsl_multiroot_fdfsolver_alloc (T, n);
    gsl_multiroot_fdfsolver_set (s, function, x);

    do
    {
        iter++;
        status = gsl_multiroot_fdfsolver_iterate (s);

        if (status)
            break ;

        status = gsl_multiroot_test_residual (s->f, 0.0000001);
    }
    while (status == GSL_CONTINUE && iter < 1000);

#ifdef DEBUG
    printf("x ");
    gsl_vector_fprintf (stdout, s->x, "%g");
    printf("\n");
    printf("f ");
    gsl_vector_fprintf (stdout, s->f, "%g");
    printf("\n");
#endif


#ifdef TEST_JACOBIAN
    {
        double r,sum;
        size_t j;

        gsl_multiroot_function f1 ;
        f1.f = function->f ;
        f1.n = function->n ;
        f1.params = function->params ;

        gsl_multiroot_fdjacobian (&f1, s->x, s->f, GSL_SQRT_DBL_EPSILON, J);

        /* compare J and s->J */

        r=0;
        sum=0;
        for (i = 0; i < n; i++)
            for (j = 0; j< n ; j++)
            {
                double u = gsl_matrix_get(J,i,j);
                double su = gsl_matrix_get(s->J, i, j);
                r = fabs(u - su)/(1e-6 + 1e-6 * fabs(u));
                sum+=r;
                if (fabs(u - su) > 1e-6 + 1e-6 * fabs(u))
                    printf("broken jacobian %g\n", r);
            }
        printf("avg r = %g\n", sum/(n*n));
    }
#endif

    for (i = 0; i < n ; i++)
    {
        residual += fabs(gsl_vector_get(s->f, i));
    }

    gsl_multiroot_fdfsolver_free (s);
    gsl_matrix_free(J);
    gsl_vector_free(x);

    gsl_test(status, "%s on %s (%g), %u iterations, residual = %.2g", T->name, desc, factor, iter, residual);

    return status;
}