Listing 4
struct input { double s0[6]; // s0[6]: Pos/vel at time t0 double tau; // Time interval from t0 to t double mu; // Mu from diff. eq. of motion double psi; // Initial approx for sol. of // Kepler's equation. } ; struct output { double s[6]; // s[6]: Pos/vel at time t. }; twobdy(struct input *in, struct output *out) // twobdy: A function to compute the position and // velocity of an orbiting body under // two-body motion, given the position and // velocity at time t0, the elapsed time tau // at which to compute the solution, the value // for mu for the two bodies, and an initial // approximation psi of the solution to // the generalized Kepler's equation. // // References: // (1) Goodyear, W.H., "A General Method for the // Computation of Cartesian Coordinates and // Partial Derivatives of the Two-Body Problem", // NASA Contractor Report NASA CR 522. // (2) Bate, Mueller, White, Fundamentals of // Astrodynamics. { #define SQ(x) (x*x) #define LARGENUMBER 1.e+300 // Additional variables int its; // Iteration counter double a, ap; // Argument in series // expansion. double alpha, sig0, u; // Temporary quantities double c0, c1, c2, c3, c4, c5x 3, // Series coefficents s1, s2, s3; double pain, psip; // Acceptance bounds for // sol. psi to Kep. eq. double dtau, dtaun, dtaup; // Residuals in Newton // method for solving // Kep. eq. double fm1, g; // f, g & double fd, gdm1; // fdot, gdot express. double mu , // Local copies of i/o psi , // variables r , r0 , s[6], s0[6] , tau; int loopexit; // Flag to exit main // program loop. int i,j,m; // Copy the input pos, vel, tau, psi into // local variables. for (i=0; i<6 ; i++) s0[i] = in->s0[i]; mu = in->mu; tau = in->tau; psi = in->psi; // Compute initial orbit radius r0 = sqrt(SQ(s0[0]) + SQ(s0[1]) + SQ(s0[2])); // Compute other intermediate quantities; sig0 = s0[0]*s0[3] + s0[1]*s0[4] + s0[2]*s0[5]; alpha = SQ(s0[3]) + SQ(s0[4]) + SQ(s0[5]) - 2*mu/r0; // Initialize series mod count m to zero m = 0; //------Establish initial psi and initial acceptance bounds //------for psi. if (tau == 0) { // If input tau = 0, set psi = 0; // output psi to 0; else } else { // initialize acceptance if (tau < 0) { // bounds for psi, and psin = -LARGENUMBER; // initialize Newton psip = 0; // method iteration dtaun = psin; // residuals. dtaup = -tau; } else { // tau > 0 psin = 0; psip = LARGENUMBER; dtaun = -tau; dtaup = psip; } // If the input psi lies between bounds // psin and psip, use it as a first approx. // to a solution of Kepler's equation. // If not, we adjust the input psi before using // it to solve Kepler. if (!( (psi > pain) && (psi < psip)) ) { // Try Newton's method for initial psi set equal to zero psi = tau/r0; // Set psi = tau if Newton's method fails if (psi <= psin || psi >= psip) psi = tau; } } //------ loopexit = 0; its = 0; //---------- BEGINNING OF LOOP FOR SOLVING GENERALIZED KEP. EQ. do { its++; a = alpha * psi * psi; if (fabs(a) > 1) { ap = a; // Save original value of a // Iterate until abs(a) <= 1. while (fabs(a) > 1) { m++; // Keep track of number of // times reduce a. a *= 0.25; } } // Now compute "C series" terms: // Note that they are functions of psi. c5x3 = (1+(1+(1+(1+(1+(1+(1+a/342)*a/272)*a/210)*a/156) *a/110)*a/72)*a/42)/40; c4 = (1+(1+(1+(1+(1+(1+(1+a/306)*a/240)*a/182)*a/132) *a/90)*a/56)*a/30)/24; c3 = (.5 + a*c5x3)/3; c2 = (.5 + a*c4); c1 = 1 + a*c3; c0 = 1 + a*c2; // If we reduced "a" above, we have to adjust the // C terms just computed. if (m>0) { while (m>0) { c1 = c1*c0; c0 = 2*c0*c0 - 1; m--; } c2 = (c0 - 1)/ap; c3 = (c1 - 1)/ap; c4 = (c2 - .5)/ap; c5x3 = (3 * c3-.5)/ap; } // Now use the C terms to compute the "S series" terms. s1 = c1 * psi; s2 = c2 * psi * psi; s3 = c3 * psi * psi * psi; g = r0*s1 + sig0*s2; // Compute g; used later to // get position. // Compute dtau, the function to be zeroed by the // Newton method. dtau = (g + mu*s3) - tau; // r is the derivative of dtau with respect to psi. r = fabs(r0*c0 + (sig0*s1 + mu*s2)); // Check for convergence of iteration and // reset bounds for psi. if (dtau == 0) { loopexit = 1; continue; // Get out of loop if dtau==0. } else if (dtau < 0) { psin = psi; dtaun = dtau; } else { // dtau > 0 psip = psi; dtaup = dtau; } // This is the Newton step: // x(n+1) = x(n) - y(n)/(dy/dx). psi = psi - dtau/r; // Accept psi for further iterations if it is // between bounds psin and psip. if ( (psi > psin) && (psi < psip) ) continue; //---- ---- ---- Begin modifications to Newton method. // Try incrementing bound with dtau nearest zero by // the ratio 4*dtau/tau. if (fabs(dtaun) < fabs(dtaup) ) psi = psin * (1 - (4*dtaun)/tau); if (fabs(dtaup) < fabs(dtaun) ) psi = psip * (1 - (4*dtaup)/tau); if ( (psi > psin) && (psi < psip) ) continue; // Try doubling bound closest to zero. if (tau > 0) psi = psin + psin; if (tau< 0) psi = psip + psip; if ( (psi > psin) && (psi < psip) ) continue; // Try interpolation between bounds. psi = psin + (psip - psin) * (-dtaun/(dtaup - dtaun)); if ( (psi > psin) && (psi < psip) ) continue; // Try halving between bounds. psi = psin + (psip - psin) * .5; if ( (psi > psin) && (psi < psip) ) continue; //---- ---- ---- // If we still are not between bounds, we've improved // the estimate of psi as much as possible. loopexit = 1; } while ( loopexit == 0 && its <= 10); //---------- END OF LOOP FOR SOLVING GENERALIZED KEPLER EQ. // Compute remaining three of four functions fm1, g, fd, // gdm1 fm1 = -mu * s2 / r0; fd= -mu * s1 / ( r0 * r); gdm1= -mu * s2 / r; // Compute output solution for time t = t0 + tau for (i=0;i<3;i++) { // Position solution at time t out->s[i] = s[i] = s0[i] + (fm1 * s0[i] + g * s0[i+3]); // Velocity solution at time t out->s[i+3] = s[i+3] = (fd * s0[i] + gdm1 * s0[i+3]) + s0[i+3]; // Generalized eccentric anomaly for time t in->psi = psi; } return(0); } <B>Sample Input/Output</B> The same pos/and vel was input for four different values of tau. Input: X 6640.32 Y 0.00 Z 0.00 XD 0.00 YD 7.03 ZD 4.06 tau = 1576.78 sec = 1/4 orbit period Output: X -1470.76 Y 6326.18 Z 3652.42 XD -7.24 YD -0.62 ZD -0.35 tau = 3153.56 sec = 1/2 orbit period Output: X -8115.95 Y 0.00 Z 0.00 XD 0.00 YD -5.75 ZD -3.32 tau = 4730.34 sec = 3/4 orbit period Output: X -1470.77 Y -6326.17 Z -3652.42 XD 7.24 YD -0.62 ZD -0.35 tau = 6307.12 = orbit period Output: X 6640.32 Y 0.00 Z 0.00 XD 0.00 YD 7.03 ZD 4.06 At the end of one period, the original state repeats, as expected.