void grdrag(double px0, double py0, double pz0, double vx0, double
vy0, double vz0, double m, double M, double interval, double *pxfin,
double *pyfin, double *pzfin, double *vxfin, double *vyfin, double *vzfin,
int *merge, double *apocenter, double *pericenter, double subcode_r2, double *t_final, int returnr_p)
{
int i,j;
double r0,r,phi,lx,ly,lz,u_phi,uphi,u_t,ut,ur,vr,vphi;
double t,dt,mgrav,T;
double p0mag,px0unit,py0unit,pz0unit,lxunit,lyunit,lzunit; /* This next group is used for coordinate transformations */
double oxunit,oyunit,ozunit, phixunit,phiyunit,phizunit;
double eps,hdid,hnext,*y,*dydt,*yscal;
double *yold, *dydtold, *yscalold; /* These are to make the particle exit at *exactly* the right radius */
double urold,ur0;
double Enewt,f,v2,v02;
double cubicQ, cubicR, cubicEnergy, theta, cubicA, cubicB, cubicC, root1, root2, root3; /* These are all for the pericenter calculation */
y=calloc(7,sizeof(double));
yold=calloc(7,sizeof(double));
dydt=calloc(7,sizeof(double));
dydtold=calloc(7,sizeof(double));
yscal=calloc(7,sizeof(double));
yscalold=calloc(7,sizeof(double));
eps=1.0e-6;
/* First step: convert from pkdgrav units (AU, 2pi AU/yr, masses
in solar masses, times in yr/2pi) to geometrized units in which
distances and times are in units of M=GM/c^2 and speeds are in
units of c. */
mgrav=M*MSUN*G/(C*C); /* Mass in cm */
T=interval*YR/(2*PI)/(mgrav/C); /* Time in M */
px0*=AU;
py0*=AU;
pz0*=AU; /* Now distances are in cm. */
vx0*=2.0*PI*AU/YR;
vy0*=2.0*PI*AU/YR;
vz0*=2.0*PI*AU/YR; /* Now speeds are in cm/s. */
px0/=mgrav;
py0/=mgrav;
pz0/=mgrav; /* Now distances are in units of M. */
vx0/=C;
vy0/=C;
vz0/=C; /* Speeds are now in units of c. */
r0=sqrt(px0*px0+py0*py0+pz0*pz0); /* Distance from massive object in M*/
Enewt=1.0+0.5*(vx0*vx0+vy0*vy0+vz0*vz0)-1.0/(r0-3);
lx=py0*vz0-pz0*vy0;
ly=pz0*vx0-px0*vz0;
lz=px0*vy0-py0*vx0;
u_phi=sqrt(lx*lx+ly*ly+lz*lz); /* Specific angular momentum, units of M. */
/* Now construct two unit vectors, one along r and the other perpendicular
to r but in the orbital plane, for later use in translating the
angle traveled to Cartesian coordinates. */
p0mag=sqrt(px0*px0+py0*py0+pz0*pz0);
px0unit=px0/p0mag;
py0unit=py0/p0mag;
pz0unit=pz0/p0mag;
lxunit=lx/u_phi;
lyunit=ly/u_phi;
lzunit=lz/u_phi;
oxunit=pz0unit*lyunit-py0unit*lzunit;
oyunit=px0unit*lzunit-pz0unit*lxunit;
ozunit=py0unit*lxunit-px0unit*lyunit;
ur=(px0*vx0+py0*vy0+pz0*vz0)/r0; /* Radial speed, units of c. */
f=Enewt*Enewt/(1.0-2.0/r0)-1.0;
f/=(u_phi*u_phi/(r0*r0)+ur*ur/(1.0-2.0/r0));
f=sqrt(f);
u_phi*=f;
ur*=f;
u_t=-Enewt; /* Specific energy relative to mc^2 */
y[0]=r0;
y[1]=0.0;
y[2]=ur;
y[3]=u_phi;
y[4]=u_t;
y[5]=m*M/(m+M);
y[6]=m+M;
derivs(y, dydt);
yscal[0]=y[0];
yscal[1]=1.0;
yscal[2]=1.0e-4;
yscal[3]=1.0;
yscal[4]=1.0;
yscal[5]=1.0;
yscal[6]=1.0;
dt=0.0001*T;
t=0.0;
*merge=0;
*apocenter=0;
*t_final=0.0;
urold=ur;
r=y[0];
/* If we get the signal from the GRICP function that this is the first entry, we calculate and return the pericenter */
if (returnr_p) {
cubicEnergy = (u_t*u_t - 1.0)/2.0; /* Script E in Hartle's GR book */
cubicA = 1.0/cubicEnergy; /* Values of the constants in the cubic equation */
cubicB = -0.5*u_phi*u_phi/cubicEnergy;
cubicC = u_phi*u_phi/cubicEnergy;
cubicQ = (cubicA*cubicA - 3*cubicB)/9.0;
cubicR = (2.0*cubicA*cubicA*cubicA - 9.0*cubicA*cubicB + 27.0*cubicC)/54.0;
/* We check to see if we have 3 real roots */
theta = acos(cubicR/sqrt(cubicQ*cubicQ*cubicQ));
root1 = -2.0*sqrt(cubicQ)*cos(theta/3.0) - cubicA/3.0;
root2 = -2.0*sqrt(cubicQ)*cos((theta+2.0*PI)/3.0) - cubicA/3.0;
root3 = -2.0*sqrt(cubicQ)*cos((theta-2.0*PI)/3.0) - cubicA/3.0;
/* Now we choose the pericenter (the smallest positive root) */
assert((root1 > 0) || (root2 > 0) || (root3 > 0));
*pericenter = pickpericenter(root1, root2, root3)*mgrav/AU;
}
for (; t<T; )
{
for (i = 0 ; i < 7 ; ++i) {
yold[i] = y[i];
dydtold[i] = dydt[i];
yscalold[i] = yscal[i];
}
rkqc(y,dydt,7,t,dt,eps,yscal,&hdid,&hnext);
r=y[0];
ur0=y[2];
ut=-y[4]/(1-2.0/y[0]);
ur=(1.0-2.0/r)*(-1.0-ut*y[4]-u_phi*u_phi/(r*r));
if (ur<0.0)
{
ur=1.0e-9;
if (r>r0) ur=-1.0e-9;
}
else
{
ur=sqrt(ur);
if (ur0<0.0) ur*=-1.0;
}
y[2]=ur;
ur=y[2];
if (r*mgrav/AU > sqrt(subcode_r2)) /* If we've gone back outside our entry radius we need to find out the time at which we get to exactly the entry radius */
{
for (j = 0 ; j < 4 ; ++j) {
dt *= (sqrt(subcode_r2) - yold[0]*mgrav/AU)/(y[0]*mgrav/AU-yold[0]*mgrav/AU);
for (i = 0 ; i < 7 ; ++i) {
y[i] = yold[i];
dydt[i] = dydtold[i];
yscal[i] = yscalold[i];
}
rkqc(y, dydt, 7, t, dt, eps, yscal, &hdid, &hnext);
}
y[0] = sqrt(subcode_r2)*AU/mgrav;
t+=dt*ut;
*t_final = t*mgrav*2.0*PI/(YR*C);
break;
}
/* Checks to see if we're getting apocenter inside the subcode radius - this shouldn't even be possible without the energy loss terms on,
and it should be extremely unlikely even at that. */
if ((urold > 0) && (ur < 0))
{
*apocenter=y[0];
*apocenter*=mgrav/AU; /* Converts it back to AU */
}
urold=ur;
if ((r<3.0) || (*apocenter > 0.0))
{
*merge=1;
*pxfin=0.0;
*pyfin=0.0;
*pzfin=0.0;
*vxfin=0.0;
*vyfin=0.0;
*vzfin=0.0;
break;
}
t+=dt*ut;
dt=hnext;
if (t+dt>T)
dt=T-t;
phi=y[1];
}
u_phi=y[3];
u_t=y[4];
r=y[0];
phi=y[1];
ur=y[2];
ut=-y[4]/sqrt(1-2.0/r);
uphi=y[3]/(r*r);
vr=ur;
vphi=uphi;
/* Now we convert from theta-phi space back to x-y-z coordinates */
*pxfin=(px0unit*cos(phi)+oxunit*sin(phi));
*pyfin=(py0unit*cos(phi)+oyunit*sin(phi));
*pzfin=(pz0unit*cos(phi)+ozunit*sin(phi));
phixunit=*pzfin*lyunit-*pyfin*lzunit;
phiyunit=*pxfin*lzunit-*pzfin*lxunit;
phizunit=*pyfin*lxunit-*pxfin*lyunit;
*vxfin=vr*(*pxfin)+r*vphi*phixunit;
*vyfin=vr*(*pyfin)+r*vphi*phiyunit;
*vzfin=vr*(*pzfin)+r*vphi*phizunit;
v2=2.0*(-u_t-1.0+1.0/(r-3));
v02=*vxfin*(*vxfin)+*vyfin*(*vyfin)+*vzfin*(*vzfin);
*vxfin*=sqrt(v2/v02);
*vyfin*=sqrt(v2/v02);
*vzfin*=sqrt(v2/v02);
/* Now we convert back to code units */
*vxfin=*vxfin*C*YR/(2*PI*AU);
*vyfin=*vyfin*C*YR/(2*PI*AU);
*vzfin=*vzfin*C*YR/(2*PI*AU);
*pxfin*=r*mgrav/AU;
*pyfin*=r*mgrav/AU;
*pzfin*=r*mgrav/AU;
free(y);
free(dydt);
free(yscal);
free(dydtold);
free(yold);
}
int odeint(double ystart0, double ystart1, double x1, double x2, double den, int *kount, double *xp, double **yp, int M, int KMAX) {
double HMIN = 0.0; //Minimum step size
double H1 = 0.0001; //Size of first step
int MAXSTP = 100000; //Maximum number of steps for integration
double TINY = 1.0E-30; //To avoid certain numbers get zero
double DXSAV = 0.0001; //Output step size for integration
double TOL = 1.0E-12; //Tolerance of integration
double x;
double h;
int i,j;
double y[2];
double hdid, hnext;
double xsav;
double dydx[2], yscal[2];
x = x1; //King's W parameter
if (x2-x1 >= 0.0) {
h = sqrt(pow(H1,2));
} else {
h = - sqrt(pow(H1,2));
} //step size
y[0] = ystart0; //z^2
y[1] = ystart1; //2*z*dz/dW where z is scaled radius
xsav = x-DXSAV*2.0;
for (i=0;i<MAXSTP;i++) { //limit integration to MAXSTP steps
derivs(x,y,dydx,den); //find derivative
for (j=0;j<2;j++) {
yscal[j] = sqrt(pow(y[j],2))+sqrt(h*dydx[j])+TINY; //advance y1 and y2
}
if (sqrt(pow(x-xsav,2)) > sqrt(pow(DXSAV,2))) {
if (*kount < KMAX-1) {
xp[*kount] = x;
for (j=0;j<2;j++) {
yp[*kount][j] = y[j];
}
*kount = *kount + 1;
xsav = x;
}
} //store x, y1 and y2 if the difference in x is smaller as the desired output step size DXSAV
if (((x+h-x2)*(x+h-x1)) > 0.0) h = x2-x;
rkqc(y,dydx,&x,&h,den,yscal, &hdid, &hnext, TOL); //do a Runge-Kutta step
if ((x-x2)*(x2-x1) >= 0.0) {
ystart0 = y[0];
ystart1 = y[1];
xp[*kount] = x;
for (j=0;j<2;j++) {
yp[*kount][j] = y[j];
}
return 0;
*kount = *kount +1;
}
if (sqrt(pow(hnext,2)) < HMIN) {
printf("Stepsize smaller than minimum.\n");
return 0;
}
h = hnext;
}
return 0;
}
static void integrate(state y0, state *yp, double *xp, double x1, double x2, int *steps)
{
int i;
int stepnum = 0; // Track number of steps the integrator has run
double x = x1; // Current time, begin at x1
state s; // Current state
// Integrator memory, each position in an array is a DOF of the system
double y[n]; // array of integrator outputs, y = integral(y' dx)
double dydx[n]; // array of RHS, dy/dx
double yscale[n]; // array of yscale factors (integraion error tolorence)
double h; // timestep
double hdid; // stores actual timestep taken by RK45
double hnext; // guess for next timestep
// stop the integrator
double time_to_stop = x2;
// First guess for timestep
h = x2 - x1;
// Inital conditions
y[0] = y0.x.v.i;
y[1] = y0.v.v.i;
y[2] = y0.x.v.j;
y[3] = y0.v.v.j;
y[4] = y0.x.v.k;
y[5] = y0.v.v.k;
dydx[1] = y0.a.v.i;
dydx[3] = y0.a.v.j;
dydx[5] = y0.a.v.k;
y[6] = y0.m;
while (stepnum <= MAXSTEPS)
{
// First RHS call
deriv(y, dydx, x);
s = rk2state(y, dydx);
// Store current state
xp[stepnum] = x;
yp[stepnum] = s;
// Y-scaling. Holds down fractional errors
for (i=0;i<n;i++) {
yscale[i] = 0.000000001;//dydx[i]+TINY;
}
// Check for stepsize overshoot
if ((x + h) > time_to_stop)
h = time_to_stop - x;
// One quality controled integrator step
rkqc(y, dydx, &x, h, eps, yscale, &hdid, &hnext, n, deriv);
s = rk2state(y, dydx);
// Are we finished?
// hit ground
if (underground(s))
{
deriv(y, dydx, x);
s = rk2state(y, dydx);
xp[stepnum] = x;
yp[stepnum] = s;
stepnum++;
break;
}
// Passed requested integration time
if ( (time_to_stop - x) <= 0.0001 )
{
deriv(y, dydx, x);
s = rk2state(y, dydx);
xp[stepnum] = x;
yp[stepnum] = s;
stepnum++;
break;
}
// set timestep for next go around
h = hnext;
// Incement step counter
stepnum++;
}
(*steps) = stepnum;
return;
}