double RK4_adaptive_step( // returns adapted time step
Matrix<double,1>& x, // solution vector
double dt, // initial time step
Matrix<double,1> flow(Matrix<double,1>&), // derivative vector
double accuracy) // desired accuracy
{
// from Numerical Recipes
const double SAFETY = 0.9, PGROW = -0.2, PSHRINK = -0.25,
ERRCON = 1.89E-4, TINY = 1.0E-30;
int n = x.size();
Matrix<double,1> scale = flow(x), x_half(n), Delta(n);
for (int i = 0; i < n; i++)
scale[i] = abs(x[i]) + abs(scale[i] * dt) + TINY;
double error = 0;
while (true) {
dt /= 2;
x_half = x;
RK4_step(x_half, dt, flow);
RK4_step(x_half, dt, flow);
dt *= 2;
Matrix<double,1> x_full = x;
RK4_step(x_full, dt, flow);
for (int i = 0; i < n; i++)
Delta[i] = x_half[i] - x_full[i];
error = 0;
for (int i = 0; i < n; i++)
error = max(abs(Delta[i] / scale[i]), error);
error /= accuracy;
if (error <= 1)
break;
double dt_temp = SAFETY * dt * pow(error, PSHRINK);
if (dt >= 0)
dt = max(dt_temp, 0.1 * dt);
else
dt = min(dt_temp, 0.1 * dt);
if (abs(dt) == 0.0) {
cerr << " step size underflow" << endl;
exit(1);
}
}
if (error > ERRCON)
dt *= SAFETY * pow(error, PGROW);
else
dt *= 5.0;
for (int i = 0; i < n; i++)
x[i] = x_half[i] + Delta[i] / 15.0;
return dt;
}
int main(){
double x_RK4, x_LF;
double v_RK4, v_LF;
double t;
double T=1E5;
double delta_t=1E-1;
int n_step = (int)(T/delta_t);
double x_step = 0.0;
double v_step = 0.0;
int i;
t=0.0;
x_RK4=1.0;
v_RK4=0.0;
x_LF=1.0;
v_LF=0.0;
for(i=0;i<n_step;i++){
printf("%f %.15e %.15e %.15e %.15e \n", t, x_RK4, v_RK4, x_LF, v_LF);
RK4_step(delta_t, t, &x_RK4, &v_RK4);
leapfrog_step(delta_t, t, &x_LF, &v_LF);
t += delta_t;
}
return 0;
}
/* RK4 step with adaptive step size. Adjusts the step size to reach
an approximate error equal to `tolerance'. */
void RK4_qc_step(Real3Vect *xn, Real3Vect *xnp1,
double h_try, double *h_did, double *h_next,
double tolerance, int dir)
{
double h, err;
Real3Vect x_coarse;
int i;
h = h_try;
i = 0;
while (1==1){
/* take two half-steps. save in xnp1; use x_coarse as a scratch buffer */
RK4_step(xn, &x_coarse, 0.5*h, dir);
RK4_step(&x_coarse, xnp1, 0.5*h, dir);
/* take a full step. save in x_coarse */
RK4_step(xn, &x_coarse, h, dir);
/* estimate the error */
err = dist(&x_coarse, xnp1) / tolerance;
err = MAX(fabs(err), TINY_NUMBER);
/* if the error is small enough, increase the next time-step and exit... */
if (err <= 1.0) {
*h_did = h;
*h_next = 0.9*exp(-0.20*log(err)) * h; /* err ~ h^5 */
*h_next = MIN(*h_next, 4.0*h); /* limit growth to a factor of 4 */
break;
} else if (i > 15) {
*h_did = h;
*h_next = h_try;
printf("hit %d iterations, h hit %f. giving up.\n", i, h);
break;
}
i+=1;
/* ...otherwise, shrink time step and try again. */
h = 0.9 * exp(-0.25*log(err)) * h; /* err ~ h^4 */
}
return;
}
void Physics::RK4(real_t dt) {
//reset states and derivatives
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->state.dx = Vector3::Zero();
spheres[i]->state.dv = Vector3::Zero();
spheres[i]->state.dax = Vector3::Zero();
spheres[i]->state.dav = Vector3::Zero();
}
RK4_step(dt, 0.5, 1.0/6.0);
RK4_step(dt, 0.5, 1.0/3.0);
RK4_step(dt, 1.0, 1.0/3.0);
RK4_step(dt, 0.0, 1.0/6.0); //dummy value
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->position =
spheres[i]->initial_position + (spheres[i]->state.dx);
spheres[i]->velocity =
spheres[i]->initial_velocity + (spheres[i]->state.dv);
Vector3 dax = spheres[i]->state.dax;
real_t x_radians = dax.x; //rotation around x axis
real_t y_radians = dax.y; //rotation around y axis
real_t z_radians = dax.z; //rotation around z axis
Quaternion qx = Quaternion(Vector3::UnitX(), x_radians);
Quaternion qy = Quaternion(Vector3::UnitY(), y_radians);
Quaternion qz = Quaternion(Vector3::UnitZ(), z_radians);
spheres[i]->orientation =
normalize(spheres[i]->initial_orientation * qz); //roll
spheres[i]->orientation =
normalize(spheres[i]->orientation * qx); //pitch
spheres[i]->orientation =
normalize(spheres[i]->orientation * qy); //yaw
spheres[i]->angular_velocity =
spheres[i]->initial_angular_velocity +
(spheres[i]->state.dav);
}
}
int main(){
double x_RK4, x_LF, x3_RK4,x3_LF;
double v_RK4, v_LF, v3_RK4,v3_LF;
double t;
double x3_CI;
double T=2800;
double delta_t=6E-3;
int n_step = (int)(T/delta_t);
double x_step = 0.0, e=1;
double v_step = 0.0;
int i,j;
file1 = fopen("salida3LF.dat", "w");
file2 = fopen("salida3RK4.dat", "w");
t=0.0;
x_RK4=0.425;
v_RK4=0.0;
x3_RK4=0.0;
v3_RK4=0.0;
x3_LF=0.0;
v_LF=0.0;
x_LF=0.46;
v3_LF=0.0;
x3_CI=-0.2;
for(j=0;j<100;j++){
x3_LF=x3_CI;
v_LF=0.0;
x_LF=0.46;
v3_LF=0.0;
x3_RK4=x3_CI;
v_RK4=0.0;
x_RK4=0.425;
v3_RK4=0.0;
for(i=0;i<n_step;i++){
if(fabs(v_LF)<0.0005)
fprintf(file1,"%f %.15e %.15e %.15e %.15e \n", t, x_LF, v_LF, x3_LF, v3_LF);
if(fabs(v_RK4)<0.0005)
fprintf(file2,"%f %.15e %.15e %.15e %.15e \n", t, x_LF, v_LF, x3_RK4, v3_RK4);
RK4_step(delta_t,e, t, &x_RK4, &v_RK4, &x3_RK4, &v3_RK4);
leapfrog_step(delta_t,e, t, &x_LF, &v_LF,&x3_LF,&v3_LF);
t += delta_t;
}
x3_CI+=0.03;
}
fclose(file1);
fclose(file2);
return 0;
}
double RK4_integrate( // returns adapted time step
Matrix<double,1>& x, // solution vector
double dt, // initial time step
Matrix<double,1> flow(Matrix<double,1>&), // derivative vector
double Delta_t, // finite step in time x[0]
double accuracy) // desired accuracy
{
if (dt * Delta_t <= 0) {
cerr << "dt * Delta_t <= 0" << endl;
exit(1);
}
if (abs(dt) > abs(Delta_t))
dt = Delta_t / 5.0;
double t0 = x[0];
while (abs(x[0] - t0) < abs(Delta_t))
dt = RK4_adaptive_step(x, dt, flow, accuracy);
RK4_step(x, t0 + Delta_t - x[0], flow);
return dt;
}
int main(void)
{
double y_RK2;
double y_RK4;
double t;
int i;
double T=2.0;
double step=1E-2;
int n_step = (int)(T/step);
t=0.0;
y_RK2=1.0;
y_RK4=1.0;
for(i=0; i<n_step; i++) {
printf("%f %.15e %.15e %.15e\n", t, exact_sol_y(t), y_RK2, y_RK4);
y_RK2 += RK2_step(step, t, y_RK2, func_y);
y_RK4 += RK4_step(step, t, y_RK4, func_y);
t += step;
}
return 0;
}
