
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, c5x3, // Series coefficents
          s1, s2, s3;             
   double psin, 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 > psin) && (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.
 





