void ode_free(odesolver_t* par)
{
gsl_odeiv_evolve_free(par->e);
gsl_odeiv_control_free(par->c);
gsl_odeiv_step_free(par->s);
}
// The actual (user defined) qoi routine
void qoiRoutine(const QUESO::GslVector& paramValues,
const QUESO::GslVector* paramDirection,
const void* functionDataPtr,
QUESO::GslVector& qoiValues,
QUESO::DistArray<QUESO::GslVector*>* gradVectors,
QUESO::DistArray<QUESO::GslMatrix*>* hessianMatrices,
QUESO::DistArray<QUESO::GslVector*>* hessianEffects)
{
if (paramDirection &&
functionDataPtr &&
gradVectors &&
hessianMatrices &&
hessianEffects) {
// Just to eliminate INTEL compiler warnings
}
double A = paramValues[0];
double E = paramValues[1];
double beta = ((qoiRoutine_Data *) functionDataPtr)->m_beta;
double criticalMass = ((qoiRoutine_Data *) functionDataPtr)->m_criticalMass;
double criticalTime = ((qoiRoutine_Data *) functionDataPtr)->m_criticalTime;
double params[]={A,E,beta};
// integration
const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1);
gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1);
gsl_odeiv_system sys = {func, NULL, 1, (void *)params};
double temperature = 0.1;
double h = 1e-3;
double Mass[1];
Mass[0]=1.;
double temperature_old = 0.;
double M_old[1];
M_old[0]=1.;
double crossingTemperature = 0.;
//unsigned int loopSize = 0;
while ((temperature < criticalTime*beta) &&
(Mass[0] > criticalMass )) {
int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &temperature, criticalTime*beta, &h, Mass);
UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS),
paramValues.env().fullRank(),
"qoiRoutine()",
"gsl_odeiv_evolve_apply() failed");
//printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]);
//loopSize++;
if (Mass[0] <= criticalMass) {
crossingTemperature = temperature_old + (temperature - temperature_old) * (M_old[0]-criticalMass)/(M_old[0]-Mass[0]);
}
temperature_old=temperature;
M_old[0]=Mass[0];
}
if (criticalMass > 0.) qoiValues[0] = crossingTemperature/beta; // QoI = time to achieve critical mass
if (criticalTime > 0.) qoiValues[0] = Mass[0]; // QoI = mass fraction remaining at critical time
//printf("loopSize = %d\n",loopSize);
if ((paramValues.env().displayVerbosity() >= 3) && (paramValues.env().fullRank() == 0)) {
printf("In qoiRoutine(), A = %g, E = %g, beta = %.3lf, criticalTime = %.3lf, criticalMass = %.3lf: qoi = %lf.\n",A,E,beta,criticalTime,criticalMass,qoiValues[0]);
}
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free (s);
return;
}
GslInternal::~GslInternal(){
gsl_odeiv_evolve_free (evolve_ptr);
gsl_odeiv_control_free (control_ptr);
gsl_odeiv_step_free (step_ptr);
}
void pert_ode_free (pert_ode * po) {
gsl_odeiv_evolve_free (po->ee);
gsl_odeiv_control_free (po->ce);
gsl_odeiv_step_free (po->se);
free(po);
}
int main()
{
double E2=0.;
double A,E,beta,te[11],Me[11],Mt[11];
int num_data;
FILE *inp, *outp;
inp = fopen("global.dat","r");
fscanf(inp,"%lf %lf %lf",&A,&E,&beta); /* read kinetic parameters */
beta/=60.; /* Convert heating rate to K/s */
double params[]={A,E,beta};
// read experimental data
int i=0;
int status;
while (1){
status = fscanf(inp,"%lf %lf",&te[i],&Me[i]);
if (status == EOF) break;
i++;
}
num_data = i;
fclose(inp);
// integration
const gsl_odeiv_step_type * T = gsl_odeiv_step_gear1;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1);
gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1);
gsl_odeiv_system sys = {func, NULL, 1, (void *)params};
double t = 0.1, t1 = 800.;
double h = 1e-3;
double M[1];
M[0]=1.;
outp = fopen("global.out","w");
fprintf(outp,"Temp (K) M\n------------------\n");
i=0;
double t_old=0., M_old[1];
M_old[0]=1.;
while (t < t1){
int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t1, &h, M);
if (status != GSL_SUCCESS) break;
fprintf(outp,"%6.1lf %10.4lf\n",t,M[0]);
if ( (t >= te[i]) && (t_old <= te[i]) ) {
Mt[i] = (te[i]-t_old)*(M[0]-M_old[0])/(t-t_old) + M_old[0];
E2+=(Me[i]-Mt[i])*(Me[i]-Mt[i]);
// fprintf(outp,"%i %lf %lf %lf %lf\n",i,te[i],Me[i],Mt[i],E2);
i++;
}
t_old=t;
M_old[0]=M[0];
}
fprintf(outp,"For A = %g, E = %g, and beta = %.3lf\n",A,E,beta);
fprintf(outp,"the sum of squared errors is %lf.",E2);
gsl_odeiv_evolve_free(e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free(s);
fclose(outp);
return 0;
}
int
main (void)
{
const gsl_odeiv_step_type * T
= gsl_odeiv_step_rk8pd;
gsl_odeiv_step * s
= gsl_odeiv_step_alloc (T, 4);
gsl_odeiv_control * c
= gsl_odeiv_control_y_new (1e-6, 0.0);
gsl_odeiv_evolve * e
= gsl_odeiv_evolve_alloc (4);
double d;
gsl_odeiv_system sys = {func, jac, 4, &d};
int ii=1, num = 100; // minimum number of points to have
double t = 0.0, t1 =.140, ti;
double h = 1e-6, velx, vely, vtot, vangle;
// initialization of all the shooter physical parameters.
pi = 4.0*atan(1.0);
m = 1.4; //mass of the ball, Kg
r = .3; //radius of the ball, meters
Iball = 2.0*m/5.0*r*r; // I of ball
l0 = .45; // distance between pivot and ball rest, m
gamm = .5; // angle of the wrist. radians
Iarm = (1.75)*l0*l0/3.0; // moment of inertia of the catapult ar,
k = 70; // torsional spring constant of the catapult spring. j/rad/rad
marmg_gam = 13.7; // m_catapult_arm * g * cm distance to pivot. For the potential energy in the arm lift;
theta0 = 90*2*pi/360; // the zero of the force is at theta0. For this simulation, it is combined wiht k to get the starting torque and how fast that decreases with angle.
g = 9.8; // gravity , m/s^2
A =m*r*r+Iball;
B = Iball+Iarm+m*(l0*l0+r*r)-2*l0*r*sin(gamm); // ignoring the non-linear terms here in the coef...not too systematic!!!
C = -Iball + m*r*(l0*sin(gamm)-r);
det = A*B-C*C;
// initial conditions in y = phi, phidot, theta, thetadot
double y[4] = { 0.0, 0.0, -0.40, 0.0};// initial consditions (phi, phidot, theta, thetadot)
while (ii < num)
{
ti = ii*t1/num;
ii++;
while (t < ti)
{
int status = gsl_odeiv_evolve_apply (e, c, s,
&sys,
&t, ti,
&h, y);
if (status != GSL_SUCCESS)
break;
// have to use the release co-ordinates to determine the final velocity vector of the ball.
velx = -l0*sin(y[2])*y[3]+r*y[1]*cos(y[2]+gamm)-r*y[0]*y[3]*sin(y[2]+gamm)-r*y[3]*cos(y[2]+gamm);
vely = l0*y[3]*cos(y[2])+r*y[0]*y[3]*cos(y[2]+gamm)+r*y[1]*sin(y[2]+gamm)-r*y[3]*sin(y[2]+gamm);
// printf("%.5e %.5e %.5e %.5e %.5e\n", t, y[0], y[1], y[2], y[3]);
vtot = pow(velx*velx+vely*vely,.5);
vangle = -180*atan(vely/velx)/pi;
printf("%.5e %.5e %.5e %.5e %.5e %.5e\n", t, y[0], 180*y[2]/pi, vtot, vangle, y[1]-y[3]);
}
}
gsl_odeiv_evolve_free(e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free(s);
return 0;
}
double
Likelihood<V, M>::lnValue(const QUESO::GslVector & paramValues) const
{
double resultValue = 0.;
m_env.subComm().Barrier();
//env.syncPrintDebugMsg("Entering likelihoodRoutine()",1,env.fullComm());
// Compute likelihood for scenario 1
double betaTest = m_beta1;
if (betaTest) {
double A = paramValues[0];
double E = paramValues[1];
double beta = m_beta1;
double variance = m_variance1;
const std::vector<double>& Te = m_Te1;
const std::vector<double>& Me = m_Me1;
std::vector<double> Mt(Me.size(),0.);
double params[]={A,E,beta};
// integration
const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1);
gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1);
gsl_odeiv_system sys = {func, NULL, 1, (void *)params};
double t = 0.1, t_final = 1900.;
double h = 1e-3;
double Mass[1];
Mass[0]=1.;
unsigned int i = 0;
double t_old = 0.;
double M_old[1];
M_old[0]=1.;
double misfit=0.;
//unsigned int loopSize = 0;
while ((t < t_final) && (i < Me.size())) {
int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t_final, &h, Mass);
UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS),
paramValues.env().fullRank(),
"likelihoodRoutine()",
"gsl_odeiv_evolve_apply() failed");
//printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]);
//loopSize++;
while ( (i < Me.size()) && (t_old <= Te[i]) && (Te[i] <= t) ) {
Mt[i] = (Te[i]-t_old)*(Mass[0]-M_old[0])/(t-t_old) + M_old[0];
misfit += (Me[i]-Mt[i])*(Me[i]-Mt[i]);
//printf("%i %lf %lf %lf %lf\n",i,Te[i],Me[i],Mt[i],misfit);
i++;
}
t_old=t;
M_old[0]=Mass[0];
}
resultValue += misfit/variance;
//printf("loopSize = %d\n",loopSize);
if ((paramValues.env().displayVerbosity() >= 10) && (paramValues.env().fullRank() == 0)) {
printf("In likelihoodRoutine(), A = %g, E = %g, beta = %.3lf: misfit = %lf, likelihood = %lf.\n",A,E,beta,misfit,resultValue);
}
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free (s);
}
// Compute likelihood for scenario 2
betaTest = m_beta2;
if (betaTest > 0.) {
double A = paramValues[0];
double E = paramValues[1];
double beta = m_beta2;
double variance = m_variance2;
const std::vector<double>& Te = m_Te2;
const std::vector<double>& Me = m_Me2;
std::vector<double> Mt(Me.size(),0.);
double params[]={A,E,beta};
// integration
const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1);
gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1);
gsl_odeiv_system sys = {func, NULL, 1, (void *)params};
double t = 0.1, t_final = 1900.;
double h = 1e-3;
double Mass[1];
Mass[0]=1.;
unsigned int i = 0;
double t_old = 0.;
double M_old[1];
M_old[0]=1.;
double misfit=0.;
//unsigned int loopSize = 0;
while ((t < t_final) && (i < Me.size())) {
int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t_final, &h, Mass);
UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS),
paramValues.env().fullRank(),
"likelihoodRoutine()",
"gsl_odeiv_evolve_apply() failed");
//printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]);
//loopSize++;
while ( (i < Me.size()) && (t_old <= Te[i]) && (Te[i] <= t) ) {
Mt[i] = (Te[i]-t_old)*(Mass[0]-M_old[0])/(t-t_old) + M_old[0];
misfit += (Me[i]-Mt[i])*(Me[i]-Mt[i]);
//printf("%i %lf %lf %lf %lf\n",i,Te[i],Me[i],Mt[i],misfit);
i++;
}
t_old=t;
M_old[0]=Mass[0];
}
resultValue += misfit/variance;
//printf("loopSize = %d\n",loopSize);
if ((paramValues.env().displayVerbosity() >= 10) && (paramValues.env().fullRank() == 0)) {
printf("In likelihoodRoutine(), A = %g, E = %g, beta = %.3lf: misfit = %lf, likelihood = %lf.\n",A,E,beta,misfit,resultValue);
}
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free (s);
}
// Compute likelihood for scenario 3
betaTest = m_beta3;
if (betaTest > 0.) {
double A = paramValues[0];
double E = paramValues[1];
double beta = m_beta3;
double variance = m_variance3;
const std::vector<double>& Te = m_Te3;
const std::vector<double>& Me = m_Me3;
std::vector<double> Mt(Me.size(),0.);
double params[]={A,E,beta};
// integration
const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1);
gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1);
gsl_odeiv_system sys = {func, NULL, 1, (void *)params};
double t = 0.1, t_final = 1900.;
double h = 1e-3;
double Mass[1];
Mass[0]=1.;
unsigned int i = 0;
double t_old = 0.;
double M_old[1];
M_old[0]=1.;
double misfit=0.;
//unsigned int loopSize = 0;
while ((t < t_final) && (i < Me.size())) {
int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t_final, &h, Mass);
UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS),
paramValues.env().fullRank(),
"likelihoodRoutine()",
"gsl_odeiv_evolve_apply() failed");
//printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]);
//loopSize++;
while ( (i < Me.size()) && (t_old <= Te[i]) && (Te[i] <= t) ) {
Mt[i] = (Te[i]-t_old)*(Mass[0]-M_old[0])/(t-t_old) + M_old[0];
misfit += (Me[i]-Mt[i])*(Me[i]-Mt[i]);
//printf("%i %lf %lf %lf %lf\n",i,Te[i],Me[i],Mt[i],misfit);
i++;
}
t_old=t;
M_old[0]=Mass[0];
}
resultValue += misfit/variance;
//printf("loopSize = %d\n",loopSize);
if ((paramValues.env().displayVerbosity() >= 10) && (paramValues.env().fullRank() == 0)) {
printf("In likelihoodRoutine(), A = %g, E = %g, beta = %.3lf: misfit = %lf, likelihood = %lf.\n",A,E,beta,misfit,resultValue);
}
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free (s);
}
m_env.subComm().Barrier();
//env.syncPrintDebugMsg("Leaving likelihoodRoutine()",1,env.fullComm());
return -.5*resultValue;
}
MATRICEd *lsoda_oscillatore1(MATRICEd *ris, LISTA *params, VETTOREd *X0, VETTOREd *times, const char *metodo, double atol, double rtol, double stat_thr, double stat_width)
{
gsl_odeiv_step_type *T =NULL;
int i, j;
MATRICEd *tmp = NULL;
Params p;
_Intestazione("\n*** lsoda_oscillatore1 ***\n");
// M<-parms[[1]]
CtrlLlst(params, 1);
p.m = ACCEDIv_d(ACCEDIlst(params, 1, vd), 1);
// R<-parms[[2]]
// da parametro
// N<-parms[[3]]
CtrlLlst(params, 2);
p.b = ACCEDIv_d(ACCEDIlst(params, 2, vd), 1);
// k<-parms[[4]]
CtrlLlst(params, 3);
p.k = ACCEDIv_d(ACCEDIlst(params, 3, vd), 1);
// alpha<-parms[[5]]
CtrlLlst(params, 4);
p.F = ACCEDIv_d(ACCEDIlst(params, 4, vd), 1);
// theta<-parms[[6]]
CtrlLlst(params, 5);
p.o = ACCEDIv_d(ACCEDIlst(params, 5, vd), 1);
if (!strncmp(metodo, "rkf45", 5))
T = gsl_odeiv_step_rkf45;
else if (!strncmp(metodo, "rkck", 4))
T = gsl_odeiv_step_rkck;
else if (!strncmp(metodo, "rk8pd", 5))
T = gsl_odeiv_step_rk8pd;
else if (!strncmp(metodo, "rk2imp", 6))
T = gsl_odeiv_step_rk2imp;
else if (!strncmp(metodo, "rk4imp", 6))
T = gsl_odeiv_step_rk4imp;
else if (!strncmp(metodo, "gear1", 5))
T = gsl_odeiv_step_gear1;
else if (!strncmp(metodo, "gear2", 5))
T = gsl_odeiv_step_gear2;
else
error("The requested stepping function does not exist!\n");
gsl_odeiv_step *s = gsl_odeiv_step_alloc (T, 2);
gsl_odeiv_control *c = gsl_odeiv_control_y_new (atol, rtol);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc (2);
gsl_odeiv_system sys = {funzione, NULL, 2, &p};
double t = 0.0;
double h = 1e-6;
double y[2] = { ACCEDIv_d(X0, 1), ACCEDIv_d(X0, 2) };
CREAm_d(tmp, LENGTHv_d(times), 3);
int st = 0;
double y1 = 0.0;
double max_stat;
if (stat_width == 0.0)
max_stat = LENGTHv_d(times);
else
max_stat = (double) stat_width * LENGTHv_d(times);
for (i = 1; (!stat_width || st < max_stat) && i <= LENGTHv_d(times); i++) {
double ti = ACCEDIv_d(times, i);
while (t < ti) {
gsl_odeiv_evolve_apply (e, c, s,
&sys,
&t, ti, &h,
y);
}
if (fabs(y[0] - y1) < stat_thr)
st++;
ASSEGNAm_d(tmp, i, 1, t);
ASSEGNAm_d(tmp, i, 2, y[0]);
ASSEGNAm_d(tmp, i, 3, y[1]);
y1 = y[0];
}
CREAm_d(ris, i - 1, 3);
for (j = 1; j < i; j++)
copia1_m_riga_d(ris, j, tmp, j);
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free (c);
gsl_odeiv_step_free (s);
StrBilanciam();
return ris;
}
int
main(int argc, char **argv)
{
/* DEFAULT VALUES */
par.potentialonly = 0;
par.dt = 1e-4;
par.poincare = 1;
par.nbpmax = 500;
par.t1 = 1e2;
par.r0 = 0.0;
par.L0 = 0.0;
par.E0 = 0.0;
par.V_ex = 0.0; /* excitation */
par.omega_ex = 0.0;
par.rmax = 1.0; /* cutoff */
/* PARSE */
{
int c;
extern char *optarg;
extern int optind;
while((c = getopt(argc, argv, "Pd:n:t:V:W:")) != -1) {
switch(c) {
case 'P':
par.potentialonly = 1;
break;
case 'd':
par.dt = atof(optarg);
break;
case 'n':
par.nbpmax = atoi(optarg);
par.poincare = 1;
par.t1 = 1e99;
break;
case 't':
par.t1 = atof(optarg);
par.poincare = 0;
break;
case 'V':
par.V_ex = atof(optarg);
break;
case 'W':
par.omega_ex = atof(optarg);
break;
case ':':
case '?':
usage(argv);
default:
fprintf(stderr, "Unknown error\n");
}
}
if (optind + (par.potentialonly?5:8) > argc)
usage(argv);
V1 = atof(argv[optind++]);
V2 = atof(argv[optind++]);
V3 = atof(argv[optind++]);
V4 = atof(argv[optind++]);
V5 = atof(argv[optind++]);
if (!par.potentialonly) {
par.E0 = atof(argv[optind++]);
par.L0 = atof(argv[optind++]);
par.r0 = atof(argv[optind++]);
}
}
/* INIT TRAP */
{
size_t k;
for(k=0; k<NPOINTS; k++) {
zi[k] += 0.1525;
}
trap = trap_alloc(zi, NPOINTS);
trap->n = 11;
trap->R = 0.008;
trap->Rz = 0.013;
trap_init(trap);
Vi[0] = 0.0;
Vi[1] = Vi[2] = V1;
Vi[3] = Vi[4] = V2;
Vi[5] = Vi[6] = V3;
Vi[7] = Vi[8] = V4;
Vi[9] = Vi[10] = 0.0;
Vi[11] = Vi[12] = V5;
Vi[13] = 0.0;
assert(NPOINTS == 14);
trap_set_pot(trap, Vi);
excit = trap_alloc(zi, NPOINTS);
excit->n = trap->n;
excit->R = trap->R;
excit->Rz = trap->Rz;
trap_init(excit);
{
double Vex[NPOINTS];
for(k=0; k<NPOINTS; k++) {
Vex[k] = 0.0;
}
Vex[9] = Vex[10] = par.V_ex;
trap_set_pot(excit, Vex);
}
}
/* the -P switch */
if (par.potentialonly) {
double z;
printf("# static potential\n");
for(z=0.0; z<0.2111; z+=1e-4) {
printf("%e %e %e %e %e\n", z,
trap_get_pot(trap, z),
trap_get_pot1(trap, z),
trap_get_pot2(trap, z),
trap_get_pot3(trap, z)
);
}
printf("\n");
printf("# excitation potential\n");
for(z=0.0; z<0.2111; z+=1e-4) {
printf("%e %e %e %e %e\n", z,
trap_get_pot(excit, z),
trap_get_pot1(excit, z),
trap_get_pot2(excit, z),
trap_get_pot3(excit, z)
);
}
exit(0);
}
/* rkck is good for energy conservation */
#define GSLODEIVTYPE gsl_odeiv_step_rkck
#define GSLODEIVEPSREL 1e-8
gsl_odeiv_step *s = gsl_odeiv_step_alloc(GSLODEIVTYPE, DIM);
gsl_odeiv_system sys = {&func, NULL, DIM, trap};
gsl_odeiv_control *c;
gsl_odeiv_evolve *e;
double t = 0.0, tprev, dt = par.dt;
double y[DIM], yprev[DIM];
int status;
size_t iter = 0, npp = 0;
y[0] = par.r0; // x_i
y[1] = 0.0; // y_i
y[2] = 0.0; // z_i
y[3] = 0.0; // dot x_i
y[4] = (par.L0 == 0.0) ? 0.0 : par.L0/par.r0; // dot y_i
y[5] = sqrt(2*par.E0 - y[3]*y[3] - y[4]*y[4]); // dot z_i
printf("\n");
printf("#V[i]: %g %g %g %g %g\n", V1, V2, V3, V4, V5);
printf("#excitation: %g %g\n", par.V_ex, par.omega_ex);
printf("#E0,L0,r0: %g %g %g\n", par.E0, par.L0, par.r0);
printf("#y0: %e %e %e %e\n", y[0], y[1], y[2], y[3]);
printf("# 1:t 2:x 3:y 4:z 5:px 6:py 7:pz\n");
c = gsl_odeiv_control_y_new(GSLODEIVEPSREL, 0.0);
e = gsl_odeiv_evolve_alloc(DIM);
while (t < par.t1)
{
tprev = t;
memcpy(yprev, y, sizeof(y));
/* ONE STEP */
status = gsl_odeiv_evolve_apply (e, c, s, &sys, &t, par.t1, &dt, y);
if (status != GSL_SUCCESS) {
fprintf(stderr, "GSL failure. Exiting\n");
break;
}
if (fabs(y[1]) >= 0.013 || fabs(y[2]) >= 0.24 || gsl_isnan(y[2])) {
fprintf(stderr, "Trajectory goes out of bound. Exiting.\n");
break;
}
if (dt > par.dt) dt = par.dt;
/* OUTPUT */
if (par.poincare) {
if (yprev[2]*y[2] <= 0.0) {
double h0 = -yprev[2] / (y[2] - yprev[2]);
printf("%e %e %e %e %e %e %e\n",
tprev + h0*dt,
yprev[0] + h0*(y[0]-yprev[0]),
yprev[1] + h0*(y[1]-yprev[1]),
yprev[2] + h0*(y[2]-yprev[2]),
yprev[3] + h0*(y[3]-yprev[3]),
yprev[4] + h0*(y[4]-yprev[4]),
yprev[5] + h0*(y[5]-yprev[5])
);
fflush(stdout);
npp++;
if (npp % 10 == 0) {
fprintf(stderr, "npp:%zu t:%.3e iter:%zuk dt:%.2e\n",
npp, t, iter/1000, dt);
}
if (npp >= par.nbpmax) {
fprintf(stderr, "Enough points on section. Exiting.\n");
break;
}
}
}
else {
printf("%e %e %e %e %e %e %e\n",
t, y[0], y[1], y[2], y[3], y[4], y[5]);
}
if (iter % 10000 == 0) {
if (par.V_ex == 0.0) {
double dE = (energy(y) - par.E0) / par.E0;
fprintf(stderr, "t:%e iter:%zuk dt:%.2e dL0:%+.2e dE0:%+.2e\n",
t, iter/1000, dt, kinmom(y)-par.L0, dE);
if (fabs(dE) > 1e-3) {
fprintf(stderr, "dE is now too high. Exiting.\n");
break;
}
}
if (isnan(y[0]) || isnan(y[1]) || hypot(y[0],y[1]) > par.rmax) {
fprintf(stderr, "Diverging (x:%g, y:%g). Exiting.\n", y[0], y[1]);
break;
}
}
iter++;
}
fprintf(stderr, "END t:%e (%zu iters) r:%e z:%e\n",
t, iter, hypot(y[0],y[1]), y[2]);
/* */
gsl_odeiv_evolve_free(e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free(s);
trap_free(trap);
return 0;
}
void
test_evolve_system (const gsl_odeiv_step_type * T,
const gsl_odeiv_system * sys,
double t0, double t1, double hstart,
double y[], double yfin[],
double err_target, const char *desc)
{
/* Tests system sys with stepper T. Step length is controlled by
error estimation from the stepper.
*/
int steps = 0;
size_t i;
double t = t0;
double h = hstart;
/* Tolerance factor in testing errors */
const double factor = 10;
gsl_odeiv_step * step = gsl_odeiv_step_alloc (T, sys->dimension);
gsl_odeiv_control *c =
gsl_odeiv_control_standard_new (err_target, err_target, 1.0, 0.0);
gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc (sys->dimension);
while (t < t1)
{
int s = gsl_odeiv_evolve_apply (e, c, step, sys, &t, t1, &h, y);
if (s != GSL_SUCCESS && sys != &rhs_func_xsin)
{
gsl_test(s, "%s evolve_apply returned %d",
gsl_odeiv_step_name (step), s);
break;
}
if (steps > 100000)
{
gsl_test(GSL_EFAILED,
"%s evolve_apply reached maxiter",
gsl_odeiv_step_name (step));
break;
}
steps++;
}
/* err_target is target error of one step. Test if stepper has made
larger error than (tolerance factor times) the number of steps
times the err_target */
for (i = 0; i < sys->dimension; i++)
{
gsl_test_abs (y[i], yfin[i], factor * e->count * err_target,
"%s %s evolve(%d)",
gsl_odeiv_step_name (step), desc, i);
}
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free (c);
gsl_odeiv_step_free (step);
}
int ode(int method, double h, double eps_abs, double eps_rel,
int f(double, int, const double*, int, double*),
int jac(double, int, const double*, int, int, double*),
KRVEC(xi), KRVEC(ts), RMAT(sol)) {
const gsl_odeiv_step_type * T;
switch(method) {
case 0 : {T = gsl_odeiv_step_rk2; break; }
case 1 : {T = gsl_odeiv_step_rk4; break; }
case 2 : {T = gsl_odeiv_step_rkf45; break; }
case 3 : {T = gsl_odeiv_step_rkck; break; }
case 4 : {T = gsl_odeiv_step_rk8pd; break; }
case 5 : {T = gsl_odeiv_step_rk2imp; break; }
case 6 : {T = gsl_odeiv_step_rk4imp; break; }
case 7 : {T = gsl_odeiv_step_bsimp; break; }
case 8 : { printf("Sorry: ODE rk1imp not available in this GSL version\n"); exit(0); }
case 9 : { printf("Sorry: ODE msadams not available in this GSL version\n"); exit(0); }
case 10: { printf("Sorry: ODE msbdf not available in this GSL version\n"); exit(0); }
default: ERROR(BAD_CODE);
}
gsl_odeiv_step * s = gsl_odeiv_step_alloc (T, xin);
gsl_odeiv_control * c = gsl_odeiv_control_y_new (eps_abs, eps_rel);
gsl_odeiv_evolve * e = gsl_odeiv_evolve_alloc (xin);
Tode P;
P.f = f;
P.j = jac;
P.n = xin;
gsl_odeiv_system sys = {odefunc, odejac, xin, &P};
double t = tsp[0];
double* y = (double*)calloc(xin,sizeof(double));
int i,j;
for(i=0; i< xin; i++) {
y[i] = xip[i];
solp[i] = xip[i];
}
for (i = 1; i < tsn ; i++)
{
double ti = tsp[i];
while (t < ti)
{
gsl_odeiv_evolve_apply (e, c, s,
&sys,
&t, ti, &h,
y);
// if (h < hmin) h = hmin;
}
for(j=0; j<xin; j++) {
solp[i*xin + j] = y[j];
}
}
free(y);
gsl_odeiv_evolve_free (e);
gsl_odeiv_control_free (c);
gsl_odeiv_step_free (s);
return 0;
}
/* combination EPS_ABS = 1e-12, EPS_REL=0.0, method = 1 = RK Cash-Karp
is believed to be predictable and accurate; returns GSL_SUCCESS=0 if success */
int exactM_old(double r,
double *alpha, double *Drr,
const double EPS_ABS, const double EPS_REL, const int ode_method) {
double ug = 100.0 / SperA; /* velocity across grounding line is 100 m/a */
double DrrRg, xx, xA, nu, aa, rr, myalf, step;
const gsl_odeiv_step_type* T;
int status = NOT_DONE;
gsl_odeiv_step* s;
gsl_odeiv_control* c;
gsl_odeiv_evolve* e;
gsl_odeiv_system sys = {funcM_ode_G, NULL, 1, NULL}; /* Jac-free method and no params */
if (r < 0) {
return NEGATIVE_R; /* only nonnegative radial coord allowed */
} else if (r <= Rg/4.0) {
*alpha = 0.0; /* zero velocity near center */
*Drr = 0.0;
return GSL_SUCCESS;
} else if (r <= Rg) {
/* power law from alpha=0 to alpha=ug in Rg/4 < r <= Rg;
f(r) w: f(Rg/4)=f'(Rg/4)=0 and f(Rg)=ug and f(Rg) = DrrRg */
funcM_ode_G(Rg, &ug, &DrrRg, NULL); /* first get Drr = alpha' at Rg where alpha=ug */
/* printf("DrrRg=%e (1/a)\n",DrrRg*SperA); */
xx = r - 0.25 * Rg;
xA = 0.75 * Rg;
nu = DrrRg * xA / ug;
aa = ug / pow(xA, nu);
/* printf("power nu=%e\n",nu); */
*alpha = aa * pow(xx, nu);
*Drr = aa * nu * pow(xx, nu - 1);
return GSL_SUCCESS;
} else if (r >= Rc + 1.0) {
*alpha = 0.0; /* zero velocity beyond calving front */
*Drr = 0.0;
return GSL_SUCCESS;
}
/* need to solve ODE to find alpha, so setup for GSL ODE solver */
switch (ode_method) {
case 1:
T = gsl_odeiv_step_rkck; /* RK Cash-Karp */
break;
case 2:
T = gsl_odeiv_step_rk2;
break;
case 3:
T = gsl_odeiv_step_rk4;
break;
case 4:
T = gsl_odeiv_step_rk8pd;
break;
default:
printf("INVALID ode_method in exactM(): must be 1,2,3,4\n");
return INVALID_METHOD;
}
s = gsl_odeiv_step_alloc(T, (size_t)1); /* one scalar ode */
c = gsl_odeiv_control_y_new(EPS_ABS,EPS_REL);
e = gsl_odeiv_evolve_alloc((size_t)1); /* one scalar ode */
/* initial conditions: (r,alf) = (Rg,ug); r increases */
rr = Rg;
myalf = ug;
/* printf (" r (km) alpha (m/a)\n");
printf (" %11.5e %11.5e\n", rr/1000.0, myalf * SperA); */
while (rr < r) {
/* step = r - rr; try to get to solution in one step; trust stepping algorithm */
step = MIN(r-rr,20.0e3);
status = gsl_odeiv_evolve_apply(e, c, s, &sys, &rr, r, &step, &myalf);
if (status != GSL_SUCCESS) break;
/* printf (" %11.5e %11.5e\n", rr/1000.0, myalf * SperA); */
}
gsl_odeiv_evolve_free(e);
gsl_odeiv_control_free(c);
gsl_odeiv_step_free(s);
*alpha = myalf;
funcM_ode_G(r, alpha, Drr, NULL);
return status;
}
struct_all_ode *new_all_ode_fly(struct_all_ode *old, double *x_init) {
int i;
struct_all_ode * s;
double deltaLs;
if (old == NULL) {
s = malloc(sizeof (struct_all_ode));
s->NQ = 5;
s->NX = 1 + 2 * s->NQ;
s->x = malloc(s->NX * sizeof (double));
} else {
s = old;
}
s->mode = MODE_FLY;
s->eps_rel = 1e-6;
s->eps_abs = 1e-6;
s->h = 1e-6;
s->hmax = 1;
// double deltaLs;
struct_inputs_fly_dynamics *sf = &(s->inputs_fly_dynamics);
struct_inputs_sol_dynamics *ss = &(s->inputs_sol_dynamics);
struct_inputs_extremity_fly *se = &(s->inputs_extremity_fly);
struct_inputs_delta_vq_contact *sv = &(s->inputs_delta_vq_contact);
struct_inputs_spring_force *ssf = &(s->inputs_spring_force);
struct_inputs_fly_trajectory *sift = &s->inputs_fly_trajectory;
struct_outputs_fly_trajectory *soft = &s->outputs_fly_trajectory;
struct_inputs_get_energies *sige = &s->inputs_get_energies;
struct_outputs_get_energies *soge = &s->outputs_get_energies;
if (old == NULL) {
init_physical_params(s);
// initial state, must respect the following order:
// x[0..2*Nq-1] =[ q[1..NQ; vq[1..NQ] ]
i = 0;
s->x[i++] = 0 * M_PI / 180;
s->x[i++] = -20 * M_PI / 180;
s->x[i++] = 0;
s->x[i++] = 1.01 * s->eps_abs;
s->x[i++] = sf->r;
s->x[i++] = 0 * M_PI / 180;
s->x[i++] = 40 * M_PI / 180;
s->x[i++] = -5 * sin(sf->th);
s->x[i++] = 5 * cos(sf->th);
s->x[i++] = 0; // vr for flying model
}
if (x_init != NULL) {
for (i = 0; i < s->NX; i++) {
s->x[i] = x_init[i];
}
}
i = 0;
sift->ph = ssf->ph = sf->ph = s->x[i++];
sift->th = ssf->th = se->th = sf->th = s->x[i++];
sift->x = ssf->x = se->x = sf->x = s->x[i++];
sift->z = ssf->z = se->z = sf->z = s->x[i++];
sift->r = ssf->r = se->r = sf->r = s->x[i++];
sift->vph = sf->vph = s->x[i++];
sift->vth = sf->vth = s->x[i++];
sift->vx = sf->vx = s->x[i++];
sift->vz = sf->vz = s->x[i++];
sf->vr = s->x[i++]; // vr for flying model
//-- for zero crossing --
s->x[i++] = 0;
// cf claude samson: le ressort est precontraint a [MB+M3].G pour r0 => -[MB+M3].G = Ks. [ r0-LS0 ]
s->T = gsl_odeiv_step_rkck;
if (old != NULL) {
gsl_odeiv_evolve_free(s->e);
gsl_odeiv_control_free(s->c);
gsl_odeiv_step_free(s->s);
}
s->s = gsl_odeiv_step_alloc(s->T, s->NX);
s->c = gsl_odeiv_control_y_new(s->eps_abs, s->eps_rel);
s->e = gsl_odeiv_evolve_alloc(s->NX);
s->sys.dimension = s->NX;
s->sys.function = func_all_ode;
s->sys.jacobian = NULL;
s->sys.params = s;
s->time_second = 0.0;
s->nb_step = 0;
if (old == NULL) {
s->print_values = 0;
s->print_values_sol_to_fly = 1;
s->print_values_fly_to_sol = 0;
}
}
/*This is the main function of the integration program that calls the gsl Runge-Kutta integrator:*/
void integrator(function F, int D, void *params, double x[], double dxdt[], double x0[], double t[], int iters, double s_noise, double abstol, double reltol) {
/*Temporary variables*/
double *y = (double*) malloc( sizeof(double)*D ); /*state variable at time t-1 (input) and then at time t(output)*/
double *dydt_in = (double*) malloc( sizeof(double)*D ); /*rate of change at time point t-1*/
double *dydt_out= (double*) malloc( sizeof(double)*D ); /*rate of change at time point t*/
double *yerr = (double*) malloc( sizeof(double)*D );/*error*/
double t0,tf,tc,dt=(t[1]-t[0])/2,noise;/*initial time point, final time point, current time point, current time step, noise*/
int j,ii;/*State variable and iteration indexes*/
int status;/*integrator success flag*/
/*Definitions and initializations of gsl integrator necessary inputs and parameters:*/
/*Prepare noise generator*/
const gsl_rng_type *Q;
gsl_rng *r;
gsl_rng_env_setup();
Q = gsl_rng_default;
r = gsl_rng_alloc(Q);
/*Create a stepping function*/
const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45;
gsl_odeiv_step *s = gsl_odeiv_step_alloc(T, D);
/*Create an adaptive control function*/
gsl_odeiv_control *c = gsl_odeiv_control_y_new(abstol, reltol);
/*Create the system to be integrated (with NULL jacobian)*/
gsl_odeiv_system sys = {F, NULL, D, params};
/*The integration loop:*/
/*Initialize*/
/*Calculate dx/dt for x0*/
tc=t[0];
/* initialise dydt_in from system parameters */
/*GSL_ODEIV_FN_EVAL(&sys, t, y, dydt_in);*/
GSL_ODEIV_FN_EVAL(&sys, tc, x0, dydt_in);
for (j=0;j<D;j++) {
y[j] = x[j] = x0[j];
}
/*Integration*/
for (ii=1; ii<iters; ii++) {
/*Call the integrator*/
/*int gsl_odeiv_step_apply(gsl_odeiv_step * s, double t, double h, double y[], double yerr[], const double dydt_in[], double dydt_out[], const gsl_odeiv_system * dydt)*/
t0=t[ii-1];
tf=t[ii];
tc=t0;
while (tc<tf) {
/*Constraint time step h such as that tc+h<=tf*/
if (tc+dt>tf) dt=tf-tc;
/*Advance a h time step*/
status=gsl_odeiv_step_apply(s, tc, dt, y, yerr, dydt_in, dydt_out, &sys);
if (status != GSL_SUCCESS) break;
/*Modify time sep*/
gsl_odeiv_control_hadjust(c,s,y,yerr,dydt_in,&dt);
/*Increase current time*/
tc += dt;
/*Add noise*/
for (j=0;j<D;j++) {
noise=gsl_ran_gaussian_ziggurat(r, s_noise);
y[j] += sqrt(dt)*noise;
//dydt_in[j]+=noise;
}
}
/*Unpack and store result for this time point*/
if (status != GSL_SUCCESS) break;
for (j=0;j<D;j++) {
x[ii*D+j] = y[j];
dxdt[(ii-1)*D+j] = dydt_in[j];
/*Prepare next step*/
dydt_in[j] = dydt_out[j];
}
}
/*Get dxdt for the last time point*/
for (j=0;j<D;j++) dxdt[(iters-1)*D+j] = dydt_out[j];
/*Free dynamically allocated memory*/
gsl_odeiv_control_free(c);
printf("c freed\n");
gsl_odeiv_step_free(s);
printf("s freed\n");
gsl_rng_free(r);
printf("rng freed\n");
free(yerr);
printf("yerr freed\n");
free(dydt_out);
printf("dydt_out freed\n");
free(dydt_in);
printf("dydt_in freed\n");
free(y);
printf("y freed\n");
}
