deep.cpp

卫星计算码

      deep_arg->resonance_flag = deep_arg->synchronous_flag = 1;
      /* Synchronous resonance terms initialization */
      deep_arg->del1 = 3*deep_arg->xnq*deep_arg->xnq*aqnv*aqnv;
      deep_arg->del2 = 2*deep_arg->del1*f220*g200*q22;
      deep_arg->del3 = 3*deep_arg->del1*f330*g300*q33*aqnv;
      deep_arg->del1 = deep_arg->del1*f311*g310*q31*aqnv;
      deep_arg->xlamo = tle->xmo+tle->xnodeo+tle->omegao-deep_arg->thgr;
      bfact = deep_arg->xmdot + deep_arg->omgdot + deep_arg->xnodot - thdt;
      bfact = bfact+deep_arg->ssl+deep_arg->ssg+deep_arg->ssh;
      } /* End of geosych case */
   else              /* it's neither a high-e 12-hr orbit nor a geosynch: */
      deep_arg->resonance_flag = deep_arg->synchronous_flag = 0;

   if( deep_arg->resonance_flag)
      {
      deep_arg->xfact = bfact-deep_arg->xnq;

      /* Initialize integrator */
      deep_arg->xli = deep_arg->xlamo;
      deep_arg->xni = deep_arg->xnq;
      deep_arg->atime = 0;
      }
   /* End case dpinit: */
}

void Deep_dpsec( const tle_t *tle, deep_arg_t *deep_arg)
{
   double delt, temp;
   int n_steps;

   deep_arg->xll += deep_arg->ssl*deep_arg->t;
   deep_arg->omgadf += deep_arg->ssg*deep_arg->t;
   deep_arg->xnode += deep_arg->ssh*deep_arg->t;
   deep_arg->em = tle->eo+deep_arg->sse*deep_arg->t;
   deep_arg->xinc = tle->xincl+deep_arg->ssi*deep_arg->t;
   if (deep_arg->xinc < 0)       /* Begin April 1983 errata correction: */
      {
      deep_arg->xinc = -deep_arg->xinc;
      deep_arg->xnode = deep_arg->xnode + pi;
      deep_arg->omgadf = deep_arg->omgadf-pi;
      }                          /* End April 1983 errata correction. */
   if( !deep_arg->resonance_flag ) return;

            /* If we're closer to t=0 than to the currently-stored data
               from the previous call to this function,  then we're
               better off "restarting",  going back to the initial data */
   if( fabs( deep_arg->t) < fabs( deep_arg->t - deep_arg->atime))
      {                                    /* Epoch restart */
      deep_arg->atime = 0;
      deep_arg->xni = deep_arg->xnq;
      deep_arg->xli = deep_arg->xlamo;
      }

               /* How many integration steps does it take to get   */
               /* get from our starting time, deep_arg->atime,     */
               /* to the desired time,  deep_arg->t?:              */
   n_steps = (int)ceil( fabs( deep_arg->t - deep_arg->atime) / INTEGRATION_STEP);
   if( n_steps)
      delt = (deep_arg->t - deep_arg->atime) / (double)n_steps;
   else
      delt = 0.;

   while( n_steps--)
      {
      const double sin_li = sin( deep_arg->xli);
      const double cos_li = cos( deep_arg->xli);
      const double sin_2li = 2. * sin_li * cos_li;
      const double cos_2li = 2. * cos_li * cos_li - 1.;
      double xldot, xnddt, xndot;

                /* Dot terms calculated,  using a lot of trig add/subtract */
                /* identities to reduce the computational load... at the   */
                /* cost of making the code somewhat hard to follow:        */
      if( deep_arg->synchronous_flag )
         {
/*       const double fasx2 = 0.13130908;       */
/*       const double fasx4 = 2.8843198;        */
/*       const double fasx6 = 0.37448087;       */
         const double c_fasx2 =  0.99139134268488593;
         const double s_fasx2 =  0.13093206501640101;
         const double c_2fasx4 =  0.87051638752972937;
         const double s_2fasx4 = -0.49213943048915526;
         const double c_3fasx6 =  0.43258117585763334;
         const double s_3fasx6 =  0.90159499016666422;
         const double sin_3li = sin_2li * cos_li + cos_2li * sin_li;
         const double cos_3li = cos_2li * cos_li - sin_2li * sin_li;

         xndot = deep_arg->del1 * (sin_li  * c_fasx2  - cos_li  * s_fasx2)
               + deep_arg->del2 * (sin_2li * c_2fasx4 - cos_2li * s_2fasx4)
               + deep_arg->del3 * (sin_3li * c_3fasx6 - cos_3li * s_3fasx6);
         xnddt = deep_arg->del1 * (cos_li  * c_fasx2  + sin_li  * s_fasx2)
           + 2 * deep_arg->del2 * (cos_2li * c_2fasx4 + sin_2li * s_2fasx4)
           + 3 * deep_arg->del3 * (cos_3li * c_3fasx6 + sin_3li * s_3fasx6);
         }        /* end of geosynch case */
      else
         {        /* orbit is a 12-hour resonant one: */
/*       const double g22    =  5.7686396;      */
/*       const double g32    =  0.95240898;     */
/*       const double g44    =  1.8014998;      */
/*       const double g52    =  1.0508330;      */
/*       const double g54    =  4.4108898;      */
         const double c_g22 =  0.87051638752972937;
         const double s_g22 = -0.49213943048915526;
         const double c_g32 =  0.57972190187001149;
         const double s_g32 =  0.81481440616389245;
         const double c_g44 = -0.22866241528815548;
         const double s_g44 =  0.97350577801807991;
         const double c_g52 =  0.49684831179884198;
         const double s_g52 =  0.86783740128127729;
         const double c_g54 = -0.29695209575316894;
         const double s_g54 = -0.95489237761529999;
         const double xomi =
                   deep_arg->omegaq + deep_arg->omgdot * deep_arg->atime;
         const double sin_omi = sin( xomi), cos_omi = cos( xomi);
         const double sin_li_m_omi = sin_li * cos_omi - sin_omi * cos_li;
         const double sin_li_p_omi = sin_li * cos_omi + sin_omi * cos_li;
         const double cos_li_m_omi = cos_li * cos_omi + sin_omi * sin_li;
         const double cos_li_p_omi = cos_li * cos_omi - sin_omi * sin_li;
         const double sin_2omi = 2. * sin_omi * cos_omi;
         const double cos_2omi = 2. * cos_omi * cos_omi - 1.;
         const double sin_2li_m_omi = sin_2li * cos_omi - sin_omi * cos_2li;
         const double sin_2li_p_omi = sin_2li * cos_omi + sin_omi * cos_2li;
         const double cos_2li_m_omi = cos_2li * cos_omi + sin_omi * sin_2li;
         const double cos_2li_p_omi = cos_2li * cos_omi - sin_omi * sin_2li;
         const double sin_2li_p_2omi = sin_2li * cos_2omi + sin_2omi * cos_2li;
         const double cos_2li_p_2omi = cos_2li * cos_2omi - sin_2omi * sin_2li;
         const double sin_2omi_p_li = sin_li * cos_2omi + sin_2omi * cos_li;
         const double cos_2omi_p_li = cos_li * cos_2omi - sin_2omi * sin_li;

         xndot = deep_arg->d2201 * (sin_2omi_p_li*c_g22 - cos_2omi_p_li*s_g22)
               + deep_arg->d2211 * (sin_li * c_g22 - cos_li * s_g22)
               + deep_arg->d3210 * (sin_li_p_omi*c_g32 - cos_li_p_omi*s_g32)
               + deep_arg->d3222 * (sin_li_m_omi*c_g32 - cos_li_m_omi*s_g32)
               + deep_arg->d4410 * (sin_2li_p_2omi*c_g44 - cos_2li_p_2omi*s_g44)
               + deep_arg->d4422 * (sin_2li * c_g44 - cos_2li * s_g44)
               + deep_arg->d5220 * (sin_li_p_omi*c_g52 - cos_li_p_omi*s_g52)
               + deep_arg->d5232 * (sin_li_m_omi*c_g52 - cos_li_m_omi*s_g52)
               + deep_arg->d5421 * (sin_2li_p_omi*c_g54 - cos_2li_p_omi*s_g54)
               + deep_arg->d5433 * (sin_2li_m_omi*c_g54 - cos_2li_m_omi*s_g54);
         xnddt = deep_arg->d2201 * (cos_2omi_p_li*c_g22 + sin_2omi_p_li*s_g22)
               + deep_arg->d2211 * (cos_li * c_g22 + sin_li * s_g22)
               + deep_arg->d3210 * (cos_li_p_omi*c_g32 + sin_li_p_omi*s_g32)
               + deep_arg->d3222 * (cos_li_m_omi*c_g32 + sin_li_m_omi*s_g32)
               + deep_arg->d5220 * (cos_li_p_omi*c_g52 + sin_li_p_omi*s_g52)
               + deep_arg->d5232 * (cos_li_m_omi*c_g52 + sin_li_m_omi*s_g52)
            + 2*(deep_arg->d4410 * (cos_2li_p_2omi*c_g44 + sin_2li_p_2omi*s_g44)
               + deep_arg->d4422 * (cos_2li * c_g44 + sin_2li * s_g44)
               + deep_arg->d5421 * (cos_2li_p_omi*c_g54 + sin_2li_p_omi*s_g54)
               + deep_arg->d5433 * (cos_2li_m_omi*c_g54 + sin_2li_m_omi*s_g54));
         } /* End of 12-hr resonant case */

      xldot = deep_arg->xni+deep_arg->xfact;
      xnddt *= xldot;

      deep_arg->xli += delt * (xldot + xndot * delt / 2.);
      deep_arg->xni += delt * (xndot + xnddt * delt / 2.);
      deep_arg->atime += delt;
      }

   deep_arg->xn = deep_arg->xni;

   temp = -deep_arg->xnode + deep_arg->thgr + deep_arg->t * thdt;

   deep_arg->xll = deep_arg->xli + temp
         + (deep_arg->synchronous_flag ? -deep_arg->omgadf : temp);
   /*End case dpsec: */
}

void Deep_dpper( deep_arg_t *deep_arg)
{
               /* If the time didn't change by more than 30 minutes,     */
               /* there's no good reason to recompute the perturbations; */
               /* they don't change enough over so short a time span.    */
      if (fabs(deep_arg->savtsn-deep_arg->t) >= 30.)
         {
         double zf, zm, sinzf, ses, sis, sil, sel, sll, sls;
         double f2, f3, sghl, sghs, shs, sh1;

         deep_arg->savtsn = deep_arg->t;

               /* Update solar perturbations for time T: */
         zm = deep_arg->zmos+zns*deep_arg->t;
         zf = zm+2*zes*sin(zm);
         sinzf = sin(zf);
         f2 = 0.5*sinzf*sinzf-0.25;
         f3 = -0.5*sinzf*cos(zf);
         ses = deep_arg->se2*f2+deep_arg->se3*f3;
         sis = deep_arg->si2*f2+deep_arg->si3*f3;
         sls = deep_arg->sl2*f2+deep_arg->sl3*f3+deep_arg->sl4*sinzf;
         sghs = deep_arg->sgh2*f2+deep_arg->sgh3*f3+deep_arg->sgh4*sinzf;
         shs = deep_arg->sh2*f2+deep_arg->sh3*f3;

               /* Update lunar perturbations for time T: */
         zm = deep_arg->zmol+znl*deep_arg->t;
         zf = zm+2*zel*sin(zm);
         sinzf = sin(zf);
         f2 = 0.5*sinzf*sinzf-0.25;
         f3 = -0.5*sinzf*cos(zf);
         sel = deep_arg->ee2*f2+deep_arg->e3*f3;
         sil = deep_arg->xi2*f2+deep_arg->xi3*f3;
         sll = deep_arg->xl2*f2+deep_arg->xl3*f3+deep_arg->xl4*sinzf;
         sghl = deep_arg->xgh2*f2+deep_arg->xgh3*f3+deep_arg->xgh4*sinzf;
         sh1 = deep_arg->xh2*f2+deep_arg->xh3*f3;

               /* Sum the solar and lunar contributions: */
         deep_arg->pe = ses+sel;
         deep_arg->pinc = sis+sil;
         deep_arg->pl = sls+sll;
         deep_arg->pgh = sghs+sghl;
         deep_arg->ph = shs+sh1;
#ifdef RETAIN_PERTURBATION_VALUES_AT_EPOCH
         if( deep_arg->solar_lunar_init_flag)
            {
            deep_arg->pe0 = deep_arg->pe;
            deep_arg->pinc0 = deep_arg->pinc;
            deep_arg->pl0 = deep_arg->pl;
            deep_arg->pgh0 = deep_arg->pgh;
            deep_arg->ph0 = deep_arg->ph;
            }
         deep_arg->pe  -= deep_arg->pe0;
         deep_arg->pinc -= deep_arg->pinc0;
         deep_arg->pl  -= deep_arg->pl0;
         deep_arg->pgh -= deep_arg->pgh0;
         deep_arg->ph  -= deep_arg->ph0;
         if( deep_arg->solar_lunar_init_flag)
            return;        /* done all we really need to do here... */
#endif
         }

               /* In Spacetrack 3, sinis & cosis were initialized       */
               /* _before_ perturbations were added to xinc.  In        */
               /* Spacetrack 6,  it's the other way around (see below). */
#ifdef SPACETRACK_3
      const double sinis = sin( deep_arg->xinc);
      const double cosis = cos( deep_arg->xinc);
#endif
         /* Add solar/lunar perturbation correction to inclination: */
      deep_arg->xinc += deep_arg->pinc;

         /* Add solar/lunar perturbation correction to eccentricity: */
      deep_arg->em += deep_arg->pe;

      if (deep_arg->xqncl >= 0.2)
         {
                /* Apply periodics directly */
         const double temp_val = deep_arg->ph / deep_arg->sinio;

         deep_arg->omgadf += deep_arg->pgh - deep_arg->cosio * temp_val;
         deep_arg->xnode += temp_val;
         deep_arg->xll += deep_arg->pl;
         }
      else
         {
           /* Apply periodics with Lyddane modification */
         const double sinok = sin(deep_arg->xnode);
         const double cosok = cos(deep_arg->xnode);
               /* Correction from Spacetrack Report #3 to #6:      */
               /* used to be sinis & cosis were computed _before_  */
               /* adding perturbations to XINC.  Now it's _after_: */
#ifndef SPACETRACK_3
         const double sinis = sin(deep_arg->xinc);
         const double cosis = cos(deep_arg->xinc);
#endif
         const double alfdp = deep_arg->ph * cosok
                    + (deep_arg->pinc * cosis + sinis) * sinok;
         const double betdp = - deep_arg->ph * sinok
                    + (deep_arg->pinc * cosis + sinis) * cosok;
         double xls, xnoh;

         deep_arg->xnode = FMod2p(deep_arg->xnode);
         xls = deep_arg->xll + deep_arg->omgadf + cosis * deep_arg->xnode;
         xls += deep_arg->pl + deep_arg->pgh
                            - deep_arg->pinc * deep_arg->xnode * sinis;
         xnoh = deep_arg->xnode;
         deep_arg->xnode = atan2(alfdp,betdp);

          /* This is a patch to Lyddane modification suggested */
          /* by Rob Matson, streamlined very slightly by BJG, to */
          /* keep 'xnode' & 'xnoh' within 180 degrees of each other. */

         if( deep_arg->xnode < xnoh - pi)
            deep_arg->xnode += twopi;
         else if( deep_arg->xnode > xnoh + pi)
            deep_arg->xnode -= twopi;

         deep_arg->xll += deep_arg->pl;
         deep_arg->omgadf = xls - deep_arg->xll - cos(deep_arg->xinc)*
                             deep_arg->xnode;
         } /* End case dpper: */
}

/*------------------------------------------------------------------*/

/* THETAG */
static double ThetaG( double jd)
{
  /* Reference:  The 1992 Astronomical Almanac, page B6. */
  const double omega_E = 1.00273790934;
                   /* Earth rotations per sidereal day (non-constant) */
  const double UT = fmod( jd + .5, 1.);
  double t_cen, GMST, rval;

  t_cen = (jd - UT - 2451545.0) / 36525.;
  GMST = 24110.54841 + t_cen * (8640184.812866 + t_cen *
                           (0.093104 - t_cen * 6.2E-6));
  GMST = fmod( GMST + secday * omega_E * UT, secday);
  if( GMST < 0.)
     GMST += secday;
  rval = twopi * GMST / secday;

  return( rval);
} /*Function thetag*/