1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33
|
double kx[4];
double ky[4];
double tempx;
double tempy;
//K0
kx[0] = accx(x,y,vel_x,vel_y);
ky[0] = accy(x,y,vel_x,vel_y);
//K1
tempx = x + dt/2*(vel_x+dt/2*kx[0]);
tempy = y + dt/2*(vel_y+dt/2*ky[0]);
kx[1] = accx(tempx,tempy,vel_x + dt/2*kx[0],vel_y+dt/2*ky[0]);
ky[1] = accy(tempx,tempy,vel_x + dt/2*kx[0],vel_y+dt/2*ky[0]);
//K2
tempx = x + dt/2*(vel_x+dt/2*kx[1]);
tempy = y + dt/2*(vel_y+dt/2*ky[1]);
kx[2] = accx(tempx,tempy,vel_x + dt/2*kx[1],vel_y+dt/2*ky[1]);
ky[2] = accy(tempx,tempy,vel_x + dt/2*kx[1],vel_y+dt/2*ky[1]);
//K4
tempx = x + dt*(vel_x+dt*kx[2]);
tempy = y + dt*(vel_y+dt*ky[2]);
kx[3] = accx(tempx,tempy,vel_x + dt*kx[2] ,vel_y+dt*ky[2]);
ky[3] = accy(tempx,tempy,vel_x + dt*kx[2] ,vel_y+dt*ky[2]);
vel_x += dt/6*(kx[0]+2*kx[1]+2*kx[2]+kx[3]);
vel_y += dt/6*(ky[0]+2*ky[1]+2*ky[2]+ky[3]);
x+=dt*vel_x;
y+=dt*vel_y;
|