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);
}
Sample Input/Output
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.