Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
Arquivo: libsim.c Projeto: psas/libsim
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;
}