home *** CD-ROM | disk | FTP | other *** search
/ The C Users' Group Library 1994 August / wc-cdrom-cusersgrouplibrary-1994-08.iso / listings / v_09_05 / 9n05105a < prev    next >
Text File  |  1991-03-20  |  9KB  |  285 lines
  1.  
  2. struct input {
  3.   double s0[6];         // s0[6]: Pos/vel at time t0
  4.   double tau;           // Time interval from t0 to t
  5.   double mu;            // Mu from diff. eq. of motion
  6.   double psi;           // Initial approx for sol. of
  7.                         //  Kepler's equation.
  8. } ;
  9.  
  10. struct output {
  11.   double s[6];            // s[6]: Pos/vel at time t.
  12. } ;
  13.  
  14. twobdy(struct input *in, struct output *out)
  15.  
  16. //  twobdy:  A function to compute the position and
  17. //           velocity of an orbiting body under
  18. //           two-body motion, given the position and
  19. //           velocity at time t0, the elapsed time tau
  20. //           at which to compute the solution, the value
  21. //           for mu for the two bodies, and an initial
  22. //           approximation psi of the solution to
  23. //           the generalized Kepler's equation.
  24. //
  25. //  References:
  26. //    (1)  Goodyear, W.H.,"A General Method for the
  27. //         Computation of Cartesian Coordinates and
  28. //         Partial Derivatives of the Two-Body Problem",
  29. //         NASA Contractor Report NASA CR 522.
  30. //    (2)  Bate, Mueller, White, Fundamentals of
  31. //         Astrodynamics. 
  32.  
  33. {
  34.  
  35. #define SQ(x) (x*x)
  36. #define LARGENUMBER 1.e+300
  37.  
  38.  
  39. // Additional variables
  40.    int    its;                      // Iteration counter
  41.    double a, ap;            // Argument in series
  42.                                     //   expansion.
  43.    double alpha, sig0, u;           // Temporary quantities
  44.    double c0, c1, c2, c3, c4, c5x3, // Series coefficents
  45.           s1, s2, s3;             
  46.    double psin, psip;               // Acceptance bounds for
  47.                                     //   sol. psi to Kep. eq.    
  48.    double dtau,dtaun, dtaup;        // Residuals in Newton
  49.                                     //  method for solving
  50.                     //  Kep. eq. 
  51.    double fm1, g;                   // f,g &
  52.    double fd, gdm1;                //   fdot, gdot express.
  53.  
  54.    double mu     ,                  // Local copies of i/o
  55.         psi    ,                  //   variables
  56.           r      , r0      ,
  57.           s[6]   , s0[6]   ,
  58.           tau;          
  59.  
  60.    int loopexit;                    // Flag to exit main
  61.                                     //   program loop.
  62.  
  63.    int i,j,m;
  64.  
  65. // Copy the input pos, vel, tau, psi into
  66. //  local variables.
  67.    for (i=0; i<6 ; i++)
  68.      s0[i] = in->s0[i];
  69.     mu  = in->mu;
  70.    tau = in->tau;
  71.    psi = in->psi;
  72.  
  73.   
  74. //       Compute initial orbit radius
  75.     r0    = sqrt(SQ(s0[0]) + SQ(s0[1]) + SQ(s0[2]));
  76. //       Compute other intermediate quantities.
  77.     sig0  = s0[0]*s0[3] + s0[1]*s0[4] + s0[2]*s0[5];
  78.     alpha = SQ(s0[3]) + SQ(s0[4]) + SQ(s0[5]) - 2*mu/r0;
  79. //       Initialize series mod count m to zero
  80.     m = 0;
  81.  
  82.  
  83. //---Establish initial psi and initial acceptance bounds
  84. //---for psi.                                             
  85.     if (tau == 0) {              // If input tau = 0, set
  86.       psi = 0;                      //  output psi to 0; else
  87.     } else {                      //  initialize acceptance
  88.       if (tau < 0) {             //  bounds for psi, and
  89.           psin  = -LARGENUMBER;      //  initialize Newton
  90.              psip  = 0;    //  method iteration 
  91.           dtaun = psin;              //  residuals.
  92.           dtaup = -tau;
  93.         } else {                  //tau > 0
  94.           psin = 0;
  95.           psip = LARGENUMBER;
  96.           dtaun = -tau;
  97.           dtaup = psip;
  98.         }
  99.  
  100.         // If the input psi lies between bounds
  101.         //  psin and psip, use it as a first approx.
  102.         //  to a solution of Kepler's equation.
  103.         // If not, we adjust the input psi before using
  104.         //  it to solve Kepler.
  105.  
  106.         if (!( (psi > psin) && (psi < psip)) ) {
  107.           // Try Newton's method for initial psi set equal to zero  
  108.           psi = tau/r0;
  109.           // Set psi = tau if Newton's method fails
  110.           if (psi <= psin || psi >= psip)
  111.             psi = tau;
  112.         }
  113.      }
  114. //---
  115.     loopexit = 0;
  116.     its      = 0;
  117. //-----  BEGINNING OF LOOP FOR SOLVING GENERALIZED KEP. EQ.
  118.     do {
  119.       its++;
  120.       a = alpha * psi * psi;
  121.       if (fabs(a) > 1) {
  122.         ap = a;              // Save original value of a
  123.         // Iterate until abs(a) <= 1.
  124.         while (fabs(a) > 1) {
  125.           m++;              // Keep track of number of
  126.                             //  times reduce a.
  127.           a *= 0.25;
  128.         }
  129.       }
  130.      // Now compute "C series" terms:
  131.      //   Note that they are functions of psi.
  132.      c5x3 = (1+(1+(1+(1+(1+(1+(1+a/342)*a/272)*a/210)*a/156)
  133.             *a/110)*a/72)*a/42)/40;
  134.      c4   = (1+(1+(1+(1+(1+(1+(1+a/306)*a/240)*a/182)*a/132)
  135.             *a/90)*a/56)*a/30)/24;
  136.      c3 = (.5 + a*c5x3)/3;
  137.      c2 = (.5 + a*c4);
  138.      c1 = 1 + a*c3;
  139.      c0 = 1 + a*c2;
  140.  
  141.      //   If we reduced "a" above, we have to adjust the
  142.      //     C terms just computed.
  143.      if (m>0) {
  144.        while (m>0) {
  145.          c1 = c1*c0;
  146.          c0 = 2*c0*c0 - 1;
  147.          m--;
  148.         }
  149.  
  150.        c2 = (c0 -  1)/ap;
  151.        c3 = (c1 -  1)/ap;
  152.        c4 = (c2 - .5)/ap;
  153.        c5x3 = (3 * c3-.5)/ap;
  154.      }
  155.  
  156.      // Now use the C terms to compute the "S series" terms.
  157.      s1 = c1 * psi;
  158.      s2 = c2 * psi * psi;
  159.      s3 = c3 * psi * psi * psi;
  160.  
  161.      g    = r0*s1 + sig0*s2;   // Compute g; used later to
  162.                                //   get position.
  163.  
  164.      // Compute dtau, the function to be zeroed by the
  165.      //   Newton method.
  166.      dtau = (g + mu*s3) - tau;
  167.      // r is the derivative of dtau with respect to psi.
  168.      r    = fabs(r0*c0 + (sig0*s1 + mu*s2));
  169.  
  170.      // Check for convergence of iteration and
  171.      //  reset bounds for psi.
  172.       if (dtau == 0) {
  173.         loopexit = 1;
  174.         continue;            // Get out of loop if dtau==0. 
  175.       } 
  176.       else if (dtau < 0) {
  177.         psin  = psi;
  178.         dtaun = dtau;
  179.       }
  180.       else {                  // dtau > 0
  181.         psip = psi;
  182.         dtaup = dtau;
  183.       }
  184.       
  185.       // This is the Newton step:
  186.       //   x(n+1) = x(n) - y(n)/(dy/dx).
  187.       psi = psi - dtau/r;
  188.  
  189.  
  190.       // Accept psi for further iterations if it is
  191.       // between bounds psin and psip.
  192.       if ( (psi > psin) && (psi < psip) ) continue;
  193.  
  194. //--  -- -- Begin modifications to Newton method.
  195.       // Try incrementing bound with dtau nearest zero by
  196.       // the ratio 4*dtau/tau.
  197.       if ( fabs(dtaun) < fabs(dtaup) )
  198.         psi = psin * (1 - (4*dtaun)/tau);
  199.       if ( fabs(dtaup) < fabs(dtaun) )
  200.         psi = psip * (1 - (4*dtaup)/tau);
  201.       if ( (psi > psin) && (psi < psip) ) continue;
  202.  
  203.       // Try doubling bound closest to zero.
  204.       if (tau > 0)
  205.         psi = psin + psin; 
  206.         if (tau < 0)
  207.         psi = psip + psip;
  208.       if ( (psi > psin) && (psi < psip) ) continue;
  209.  
  210.       // Try interpolation between bounds.
  211.       psi = psin + (psip - psin) * (-dtaun/(dtaup - dtaun));
  212.       if ( (psi > psin) && (psi < psip) ) continue;
  213.  
  214.       // Try halving between bounds.
  215.       psi = psin + (psip - psin) * .5;
  216.       if ( (psi > psin) && (psi < psip) ) continue;
  217. //-- -- --
  218.       // If we still are not between bounds, we've improved
  219.       //  the estimate of psi as much as possible.
  220.       loopexit = 1;
  221.       
  222.     } while ( loopexit == 0 && its <= 10);
  223. //-----  END OF LOOP FOR SOLVING GENERALIZED KEPLER EQ.
  224.  
  225.     // Compute remaining three of four functions fm1, g, fd,
  226.     //   gdm1
  227.     fm1  = -mu * s2 / r0;
  228.     fd   = -mu * s1 / ( r0 * r);
  229.     gdm1 = -mu * s2 / r;
  230.  
  231.     // Compute output solution for time t = t0 + tau
  232.     for (i=0;i<3;i++) {
  233.       // Position solution    at time t
  234.       out->s[i]  = s[i] = s0[i] + (fm1 * s0[i] + g * s0[i+3]);
  235.       // Velocity solution at time t
  236.       out->s[i+3] = s[i+3] = (fd * s0[i] + gdm1 * s0[i+3])
  237.                                               + s0[i+3];
  238.       // Generalized eccentric anomaly for time t
  239.       in->psi = psi;
  240.     }
  241.  
  242.    return(0);
  243. } 
  244.  
  245.  
  246.      SAMPLE INPUT/OUTPUT
  247.   The same pos/and vel was input for four
  248.   different values of tau.
  249. Input:
  250. X   6640.32 Y  0.00    Z  0.00      
  251. XD     0.00 YD 7.03    ZD 4.06
  252.        
  253. tau      = 1576.78 sec = 1/4 orbit period 
  254.             
  255. Output: 
  256. X  -1470.76 Y  6326.18 Z  3652.42 
  257. XD    -7.24 YD   -0.62 ZD   -0.35
  258.  
  259. tau      = 3153.56 sec = 1/2 orbit period
  260.  
  261. Output: 
  262. X  -8115.95 Y     0.00 Z     0.00 
  263. XD     0.00 YD   -5.75 ZD   -3.32 
  264.  
  265. tau     = 4730.34 sec = 3/4 orbit period
  266.  
  267. Output: 
  268. X  -1470.77 Y -6326.17 Z -3652.42 
  269. XD  7.24    YD   -0.62 ZD   -0.35 
  270.  
  271. tau     = 6307.12 = orbit period 
  272.  
  273. Output:
  274. X   6640.32 Y     0.00 Z     0.00 
  275. XD     0.00 YD    7.03 ZD    4.06 
  276.  
  277. At the end of one period, the original state repeats,
  278. as expected.
  279.  
  280.  
  281.  
  282.  
  283.  
  284.  
  285.