sgp4sdp4.c

00001 /*
00002  *  Unit SGP4SDP4
00003  *           Author:  Dr TS Kelso 
00004  * Original Version:  1991 Oct 30
00005  * Current Revision:  1992 Sep 03
00006  *          Version:  1.50 
00007  *        Copyright:  1991-1992, All Rights Reserved 
00008  *
00009  *   Ported to C by:  Neoklis Kyriazis  April 10  2001
00010  */
00011 
00012 #include "sgp4sdp4.h"
00013 
00014 /* SGP4 */
00015 /* This function is used to calculate the position and velocity */
00016 /* of near-earth (period < 225 minutes) satellites. tsince is   */
00017 /* time since epoch in minutes, tle is a pointer to a tle_t     */
00018 /* structure with Keplerian orbital elements and pos and vel    */
00019 /* are vector_t structures returning ECI satellite position and */
00020 /* velocity. Use Convert_Sat_State() to convert to km and km/s.*/
00021 void
00022 SGP4(double tsince, tle_t *tle, vector_t *pos, vector_t *vel, double* phase)
00023 {
00024   static double
00025     aodp,aycof,c1,c4,c5,cosio,d2,d3,d4,delmo,omgcof,
00026     eta,omgdot,sinio,xnodp,sinmo,t2cof,t3cof,t4cof,t5cof,
00027     x1mth2,x3thm1,x7thm1,xmcof,xmdot,xnodcf,xnodot,xlcof;
00028  
00029   double
00030     cosuk,sinuk,rfdotk,vx,vy,vz,ux,uy,uz,xmy,xmx,
00031     cosnok,sinnok,cosik,sinik,rdotk,xinck,xnodek,uk,
00032     rk,cos2u,sin2u,u,sinu,cosu,betal,rfdot,rdot,r,pl,
00033     elsq,esine,ecose,epw,cosepw,x1m5th,xhdot1,tfour,
00034     sinepw,capu,ayn,xlt,aynl,xll,axn,xn,beta,xl,e,a,
00035     tcube,delm,delomg,templ,tempe,tempa,xnode,tsq,xmp,
00036     omega,xnoddf,omgadf,xmdf,a1,a3ovk2,ao,betao,betao2,
00037     c1sq,c2,c3,coef,coef1,del1,delo,eeta,eosq,etasq,
00038     perige,pinvsq,psisq,qoms24,s4,temp,temp1,temp2,
00039     temp3,temp4,temp5,temp6,theta2,theta4,tsi;
00040 
00041   int i;  
00042 
00043   /* Initialization */
00044   if (isFlagClear(SGP4_INITIALIZED_FLAG))
00045     {
00046       SetFlag(SGP4_INITIALIZED_FLAG);
00047 
00048       /* Recover original mean motion (xnodp) and   */
00049       /* semimajor axis (aodp) from input elements. */
00050       a1 = pow(xke/tle->xno,tothrd);
00051       cosio = cos(tle->xincl);
00052       theta2 = cosio*cosio;
00053       x3thm1 = 3*theta2-1.0;
00054       eosq = tle->eo*tle->eo;
00055       betao2 = 1-eosq;
00056       betao = sqrt(betao2);
00057       del1 = 1.5*ck2*x3thm1/(a1*a1*betao*betao2);
00058       ao = a1*(1-del1*(0.5*tothrd+del1*(1+134/81*del1)));
00059       delo = 1.5*ck2*x3thm1/(ao*ao*betao*betao2);
00060       xnodp = tle->xno/(1+delo);
00061       aodp = ao/(1-delo);
00062 
00063       /* For perigee less than 220 kilometers, the "simple" flag is set */
00064       /* and the equations are truncated to linear variation in sqrt a  */
00065       /* and quadratic variation in mean anomaly.  Also, the c3 term,   */
00066       /* the delta omega term, and the delta m term are dropped.        */
00067       if((aodp*(1-tle->eo)/ae) < (220/xkmper+ae))
00068     SetFlag(SIMPLE_FLAG);
00069       else
00070     ClearFlag(SIMPLE_FLAG);
00071 
00072       /* For perigee below 156 km, the       */ 
00073       /* values of s and qoms2t are altered. */
00074       s4 = __s__;
00075       qoms24 = qoms2t;
00076       perige = (aodp*(1-tle->eo)-ae)*xkmper;
00077       if(perige < 156)
00078     {
00079              if(perige <= 98)
00080         s4 = 20;
00081           else
00082         s4 = perige-78;
00083       qoms24 = pow((120-s4)*ae/xkmper,4);
00084       s4 = s4/xkmper+ae;
00085     }; /* End of if(perige <= 98) */
00086 
00087       pinvsq = 1/(aodp*aodp*betao2*betao2);
00088       tsi = 1/(aodp-s4);
00089       eta = aodp*tle->eo*tsi;
00090       etasq = eta*eta;
00091       eeta = tle->eo*eta;
00092       psisq = fabs(1-etasq);
00093       coef = qoms24*pow(tsi,4);
00094       coef1 = coef/pow(psisq,3.5);
00095       c2 = coef1*xnodp*(aodp*(1+1.5*etasq+eeta*(4+etasq))+
00096        0.75*ck2*tsi/psisq*x3thm1*(8+3*etasq*(8+etasq)));
00097       c1 = tle->bstar*c2;
00098       sinio = sin(tle->xincl);
00099       a3ovk2 = -xj3/ck2*pow(ae,3);
00100       c3 = coef*tsi*a3ovk2*xnodp*ae*sinio/tle->eo;
00101       x1mth2 = 1-theta2;
00102       c4 = 2*xnodp*coef1*aodp*betao2*(eta*(2+0.5*etasq)+
00103        tle->eo*(0.5+2*etasq)-2*ck2*tsi/(aodp*psisq)*
00104        (-3*x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*
00105        x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*tle->omegao)));
00106       c5 = 2*coef1*aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
00107       theta4 = theta2*theta2;
00108       temp1 = 3*ck2*pinvsq*xnodp;
00109       temp2 = temp1*ck2*pinvsq;
00110       temp3 = 1.25*ck4*pinvsq*pinvsq*xnodp;
00111       xmdot = xnodp+0.5*temp1*betao*x3thm1+
00112           0.0625*temp2*betao*(13-78*theta2+137*theta4);
00113       x1m5th = 1-5*theta2;
00114       omgdot = -0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+
00115            395*theta4)+temp3*(3-36*theta2+49*theta4);
00116       xhdot1 = -temp1*cosio;
00117       xnodot = xhdot1+(0.5*temp2*(4-19*theta2)+
00118            2*temp3*(3-7*theta2))*cosio;
00119       omgcof = tle->bstar*c3*cos(tle->omegao);
00120       xmcof = -tothrd*coef*tle->bstar*ae/eeta;
00121       xnodcf = 3.5*betao2*xhdot1*c1;
00122       t2cof = 1.5*c1;
00123       xlcof = 0.125*a3ovk2*sinio*(3+5*cosio)/(1+cosio);
00124       aycof = 0.25*a3ovk2*sinio;
00125       delmo = pow(1+eta*cos(tle->xmo),3);
00126       sinmo = sin(tle->xmo);
00127       x7thm1 = 7*theta2-1;
00128       if (isFlagClear(SIMPLE_FLAG))
00129     {
00130       c1sq = c1*c1;
00131       d2 = 4*aodp*tsi*c1sq;
00132       temp = d2*tsi*c1/3;
00133       d3 = (17*aodp+s4)*temp;
00134       d4 = 0.5*temp*aodp*tsi*(221*aodp+31*s4)*c1;
00135       t3cof = d2+2*c1sq;
00136       t4cof = 0.25*(3*d3+c1*(12*d2+10*c1sq));
00137       t5cof = 0.2*(3*d4+12*c1*d3+6*d2*d2+15*c1sq*(2*d2+c1sq));
00138     }; /* End of if (isFlagClear(SIMPLE_FLAG)) */
00139     }; /* End of SGP4() initialization */
00140 
00141   /* Update for secular gravity and atmospheric drag. */
00142   xmdf = tle->xmo+xmdot*tsince;
00143   omgadf = tle->omegao+omgdot*tsince;
00144   xnoddf = tle->xnodeo+xnodot*tsince;
00145   omega = omgadf;
00146   xmp = xmdf;
00147   tsq = tsince*tsince;
00148   xnode = xnoddf+xnodcf*tsq;
00149   tempa = 1-c1*tsince;
00150   tempe = tle->bstar*c4*tsince;
00151   templ = t2cof*tsq;
00152   if (isFlagClear(SIMPLE_FLAG))
00153     {
00154       delomg = omgcof*tsince;
00155       delm = xmcof*(pow(1+eta*cos(xmdf),3)-delmo);
00156       temp = delomg+delm;
00157       xmp = xmdf+temp;
00158       omega = omgadf-temp;
00159       tcube = tsq*tsince;
00160       tfour = tsince*tcube;
00161       tempa = tempa-d2*tsq-d3*tcube-d4*tfour;
00162       tempe = tempe+tle->bstar*c5*(sin(xmp)-sinmo);
00163       templ = templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof);
00164     }; /* End of if (isFlagClear(SIMPLE_FLAG)) */
00165 
00166   a = aodp*pow(tempa,2);
00167   e = tle->eo-tempe;
00168   xl = xmp+omega+xnode+xnodp*templ;
00169   beta = sqrt(1-e*e);
00170   xn = xke/pow(a,1.5);
00171 
00172   /* Long period periodics */
00173   axn = e*cos(omega);
00174   temp = 1/(a*beta*beta);
00175   xll = temp*xlcof*axn;
00176   aynl = temp*aycof;
00177   xlt = xl+xll;
00178   ayn = e*sin(omega)+aynl;
00179 
00180   /* Solve Kepler's' Equation */
00181   capu = FMod2p(xlt-xnode);
00182   temp2 = capu;
00183 
00184   i = 0;
00185   do
00186     {
00187       sinepw = sin(temp2);
00188       cosepw = cos(temp2);
00189       temp3 = axn*sinepw;
00190       temp4 = ayn*cosepw;
00191       temp5 = axn*cosepw;
00192       temp6 = ayn*sinepw;
00193       epw = (capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
00194       if(fabs(epw-temp2) <= e6a) break;
00195       temp2 = epw;
00196     }
00197   while( i++ < 10 );
00198 
00199   /* Short period preliminary quantities */
00200   ecose = temp5+temp6;
00201   esine = temp3-temp4;
00202   elsq = axn*axn+ayn*ayn;
00203   temp = 1-elsq;
00204   pl = a*temp;
00205   r = a*(1-ecose);
00206   temp1 = 1/r;
00207   rdot = xke*sqrt(a)*esine*temp1;
00208   rfdot = xke*sqrt(pl)*temp1;
00209   temp2 = a*temp1;
00210   betal = sqrt(temp);
00211   temp3 = 1/(1+betal);
00212   cosu = temp2*(cosepw-axn+ayn*esine*temp3);
00213   sinu = temp2*(sinepw-ayn-axn*esine*temp3);
00214   u = AcTan(sinu, cosu);
00215   sin2u = 2*sinu*cosu;
00216   cos2u = 2*cosu*cosu-1;
00217   temp = 1/pl;
00218   temp1 = ck2*temp;
00219   temp2 = temp1*temp;
00220 
00221   /* Update for short periodics */
00222   rk = r*(1-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u;
00223   uk = u-0.25*temp2*x7thm1*sin2u;
00224   xnodek = xnode+1.5*temp2*cosio*sin2u;
00225   xinck = tle->xincl+1.5*temp2*cosio*sinio*cos2u;
00226   rdotk = rdot-xn*temp1*x1mth2*sin2u;
00227   rfdotk = rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1);
00228 
00229   /* Orientation vectors */
00230   sinuk = sin(uk);
00231   cosuk = cos(uk);
00232   sinik = sin(xinck);
00233   cosik = cos(xinck);
00234   sinnok = sin(xnodek);
00235   cosnok = cos(xnodek);
00236   xmx = -sinnok*cosik;
00237   xmy = cosnok*cosik;
00238   ux = xmx*sinuk+cosnok*cosuk;
00239   uy = xmy*sinuk+sinnok*cosuk;
00240   uz = sinik*sinuk;
00241   vx = xmx*cosuk-cosnok*sinuk;
00242   vy = xmy*cosuk-sinnok*sinuk;
00243   vz = sinik*cosuk;
00244 
00245   /* Position and velocity */
00246   pos->x = rk*ux;
00247   pos->y = rk*uy;
00248   pos->z = rk*uz;
00249   vel->x = rdotk*ux+rfdotk*vx;
00250   vel->y = rdotk*uy+rfdotk*vy;
00251   vel->z = rdotk*uz+rfdotk*vz;
00252 
00253   *phase = xlt-xnode-omgadf+twopi;
00254   if(*phase < 0) *phase += twopi;
00255   *phase = FMod2p(*phase);
00256 
00257   tle->omegao1=omega;
00258   tle->xincl1=xinck;
00259   tle->xnodeo1=xnodek;
00260 
00261 } /*SGP4*/
00262 
00263 /*------------------------------------------------------------------*/
00264 
00265 /* SDP4 */
00266 /* This function is used to calculate the position and velocity */
00267 /* of deep-space (period > 225 minutes) satellites. tsince is   */
00268 /* time since epoch in minutes, tle is a pointer to a tle_t     */
00269 /* structure with Keplerian orbital elements and pos and vel    */
00270 /* are vector_t structures returning ECI satellite position and */
00271 /* velocity. Use Convert_Sat_State() to convert to km and km/s. */
00272 void 
00273 SDP4(double tsince, tle_t *tle, vector_t *pos, vector_t *vel, double* phase)
00274 {
00275   int i;
00276 
00277   static double
00278     x3thm1,c1,x1mth2,c4,xnodcf,t2cof,xlcof,aycof,x7thm1;
00279 
00280   double
00281     a,axn,ayn,aynl,beta,betal,capu,cos2u,cosepw,cosik,
00282     cosnok,cosu,cosuk,ecose,elsq,epw,esine,pl,theta4,
00283     rdot,rdotk,rfdot,rfdotk,rk,sin2u,sinepw,sinik,
00284     sinnok,sinu,sinuk,tempe,templ,tsq,u,uk,ux,uy,uz,
00285     vx,vy,vz,xinck,xl,xlt,xmam,xmdf,xmx,xmy,xnoddf,
00286     xnodek,xll,a1,a3ovk2,ao,c2,coef,coef1,x1m5th,
00287     xhdot1,del1,r,delo,eeta,eta,etasq,perige,
00288     psisq,tsi,qoms24,s4,pinvsq,temp,tempa,temp1,
00289     temp2,temp3,temp4,temp5,temp6;
00290 
00291   static deep_arg_t deep_arg;
00292 
00293   /* Initialization */
00294   if (isFlagClear(SDP4_INITIALIZED_FLAG))
00295     {
00296       SetFlag(SDP4_INITIALIZED_FLAG);
00297 
00298       /* Recover original mean motion (xnodp) and   */
00299       /* semimajor axis (aodp) from input elements. */
00300       a1 = pow(xke/tle->xno,tothrd);
00301       deep_arg.cosio = cos(tle->xincl);
00302       deep_arg.theta2 = deep_arg.cosio*deep_arg.cosio;
00303       x3thm1 = 3*deep_arg.theta2-1;
00304       deep_arg.eosq = tle->eo*tle->eo;
00305       deep_arg.betao2 = 1-deep_arg.eosq;
00306       deep_arg.betao = sqrt(deep_arg.betao2);
00307       del1 = 1.5*ck2*x3thm1/(a1*a1*deep_arg.betao*deep_arg.betao2);
00308       ao = a1*(1-del1*(0.5*tothrd+del1*(1+134/81*del1)));
00309       delo = 1.5*ck2*x3thm1/(ao*ao*deep_arg.betao*deep_arg.betao2);
00310       deep_arg.xnodp = tle->xno/(1+delo);
00311       deep_arg.aodp = ao/(1-delo);
00312 
00313       /* For perigee below 156 km, the values */
00314       /* of s and qoms2t are altered.         */
00315       s4 = __s__;
00316       qoms24 = qoms2t;
00317       perige = (deep_arg.aodp*(1-tle->eo)-ae)*xkmper;
00318       if(perige < 156)
00319     {
00320              if(perige <= 98)
00321         s4 = 20;
00322           else
00323         s4 = perige-78;
00324       qoms24 = pow((120-s4)*ae/xkmper,4);
00325       s4 = s4/xkmper+ae;
00326     }
00327       pinvsq = 1/(deep_arg.aodp*deep_arg.aodp*
00328                deep_arg.betao2*deep_arg.betao2);
00329       deep_arg.sing = sin(tle->omegao);
00330       deep_arg.cosg = cos(tle->omegao);
00331       tsi = 1/(deep_arg.aodp-s4);
00332       eta = deep_arg.aodp*tle->eo*tsi;
00333       etasq = eta*eta;
00334       eeta = tle->eo*eta;
00335       psisq = fabs(1-etasq);
00336       coef = qoms24*pow(tsi,4);
00337       coef1 = coef/pow(psisq,3.5);
00338       c2 = coef1*deep_arg.xnodp*(deep_arg.aodp*(1+1.5*etasq+eeta*
00339        (4+etasq))+0.75*ck2*tsi/psisq*x3thm1*(8+3*etasq*(8+etasq)));
00340       c1 = tle->bstar*c2;
00341       deep_arg.sinio = sin(tle->xincl);
00342       a3ovk2 = -xj3/ck2*pow(ae,3);
00343       x1mth2 = 1-deep_arg.theta2;
00344       c4 = 2*deep_arg.xnodp*coef1*deep_arg.aodp*deep_arg.betao2*
00345            (eta*(2+0.5*etasq)+tle->eo*(0.5+2*etasq)-2*ck2*tsi/
00346            (deep_arg.aodp*psisq)*(-3*x3thm1*(1-2*eeta+etasq*
00347            (1.5-0.5*eeta))+0.75*x1mth2*(2*etasq-eeta*(1+etasq))*
00348            cos(2*tle->omegao)));
00349       theta4 = deep_arg.theta2*deep_arg.theta2;
00350       temp1 = 3*ck2*pinvsq*deep_arg.xnodp;
00351       temp2 = temp1*ck2*pinvsq;
00352       temp3 = 1.25*ck4*pinvsq*pinvsq*deep_arg.xnodp;
00353       deep_arg.xmdot = deep_arg.xnodp+0.5*temp1*deep_arg.betao*
00354                    x3thm1+0.0625*temp2*deep_arg.betao*
00355                        (13-78*deep_arg.theta2+137*theta4);
00356       x1m5th = 1-5*deep_arg.theta2;
00357       deep_arg.omgdot = -0.5*temp1*x1m5th+0.0625*temp2*
00358                         (7-114*deep_arg.theta2+395*theta4)+
00359                     temp3*(3-36*deep_arg.theta2+49*theta4);
00360       xhdot1 = -temp1*deep_arg.cosio;
00361       deep_arg.xnodot = xhdot1+(0.5*temp2*(4-19*deep_arg.theta2)+
00362                 2*temp3*(3-7*deep_arg.theta2))*deep_arg.cosio;
00363       xnodcf = 3.5*deep_arg.betao2*xhdot1*c1;
00364       t2cof = 1.5*c1;
00365       xlcof = 0.125*a3ovk2*deep_arg.sinio*(3+5*deep_arg.cosio)/
00366               (1+deep_arg.cosio);
00367       aycof = 0.25*a3ovk2*deep_arg.sinio;
00368       x7thm1 = 7*deep_arg.theta2-1;
00369 
00370       /* initialize Deep() */
00371       Deep(dpinit, tle, &deep_arg);
00372     }; /*End of SDP4() initialization */
00373 
00374   /* Update for secular gravity and atmospheric drag */
00375   xmdf = tle->xmo+deep_arg.xmdot*tsince;
00376   deep_arg.omgadf = tle->omegao+deep_arg.omgdot*tsince;
00377   xnoddf = tle->xnodeo+deep_arg.xnodot*tsince;
00378   tsq = tsince*tsince;
00379   deep_arg.xnode = xnoddf+xnodcf*tsq;
00380   tempa = 1-c1*tsince;
00381   tempe = tle->bstar*c4*tsince;
00382   templ = t2cof*tsq;
00383   deep_arg.xn = deep_arg.xnodp;
00384 
00385   /* Update for deep-space secular effects */
00386   deep_arg.xll = xmdf;
00387   deep_arg.t = tsince;
00388 
00389   Deep(dpsec, tle, &deep_arg);
00390 
00391   xmdf = deep_arg.xll;
00392   a = pow(xke/deep_arg.xn,tothrd)*tempa*tempa;
00393   deep_arg.em = deep_arg.em-tempe;
00394   xmam = xmdf+deep_arg.xnodp*templ;
00395 
00396   /* Update for deep-space periodic effects */
00397   deep_arg.xll = xmam;
00398 
00399   Deep(dpper, tle, &deep_arg);
00400 
00401   xmam = deep_arg.xll;
00402   xl = xmam+deep_arg.omgadf+deep_arg.xnode;
00403   beta = sqrt(1-deep_arg.em*deep_arg.em);
00404   deep_arg.xn = xke/pow(a,1.5);
00405 
00406   /* Long period periodics */
00407   axn = deep_arg.em*cos(deep_arg.omgadf);
00408   temp = 1/(a*beta*beta);
00409   xll = temp*xlcof*axn;
00410   aynl = temp*aycof;
00411   xlt = xl+xll;
00412   ayn = deep_arg.em*sin(deep_arg.omgadf)+aynl;
00413 
00414   /* Solve Kepler's Equation */
00415   capu = FMod2p(xlt-deep_arg.xnode);
00416   temp2 = capu;
00417 
00418   i = 0;
00419   do
00420     {
00421       sinepw = sin(temp2);
00422       cosepw = cos(temp2);
00423       temp3 = axn*sinepw;
00424       temp4 = ayn*cosepw;
00425       temp5 = axn*cosepw;
00426       temp6 = ayn*sinepw;
00427       epw = (capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
00428       if(fabs(epw-temp2) <= e6a) break;
00429       temp2 = epw;
00430     }
00431   while( i++ < 10 );
00432 
00433   /* Short period preliminary quantities */
00434   ecose = temp5+temp6;
00435   esine = temp3-temp4;
00436   elsq = axn*axn+ayn*ayn;
00437   temp = 1-elsq;
00438   pl = a*temp;
00439   r = a*(1-ecose);
00440   temp1 = 1/r;
00441   rdot = xke*sqrt(a)*esine*temp1;
00442   rfdot = xke*sqrt(pl)*temp1;
00443   temp2 = a*temp1;
00444   betal = sqrt(temp);
00445   temp3 = 1/(1+betal);
00446   cosu = temp2*(cosepw-axn+ayn*esine*temp3);
00447   sinu = temp2*(sinepw-ayn-axn*esine*temp3);
00448   u = AcTan(sinu,cosu);
00449   sin2u = 2*sinu*cosu;
00450   cos2u = 2*cosu*cosu-1;
00451   temp = 1/pl;
00452   temp1 = ck2*temp;
00453   temp2 = temp1*temp;
00454 
00455   /* Update for short periodics */
00456   rk = r*(1-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u;
00457   uk = u-0.25*temp2*x7thm1*sin2u;
00458   xnodek = deep_arg.xnode+1.5*temp2*deep_arg.cosio*sin2u;
00459   xinck = deep_arg.xinc+1.5*temp2*deep_arg.cosio*deep_arg.sinio*cos2u;
00460   rdotk = rdot-deep_arg.xn*temp1*x1mth2*sin2u;
00461   rfdotk = rfdot+deep_arg.xn*temp1*(x1mth2*cos2u+1.5*x3thm1);
00462 
00463   /* Orientation vectors */
00464   sinuk = sin(uk);
00465   cosuk = cos(uk);
00466   sinik = sin(xinck);
00467   cosik = cos(xinck);
00468   sinnok = sin(xnodek);
00469   cosnok = cos(xnodek);
00470   xmx = -sinnok*cosik;
00471   xmy = cosnok*cosik;
00472   ux = xmx*sinuk+cosnok*cosuk;
00473   uy = xmy*sinuk+sinnok*cosuk;
00474   uz = sinik*sinuk;
00475   vx = xmx*cosuk-cosnok*sinuk;
00476   vy = xmy*cosuk-sinnok*sinuk;
00477   vz = sinik*cosuk;
00478 
00479   /* Position and velocity */
00480   pos->x = rk*ux;
00481   pos->y = rk*uy;
00482   pos->z = rk*uz;
00483   vel->x = rdotk*ux+rfdotk*vx;
00484   vel->y = rdotk*uy+rfdotk*vy;
00485   vel->z = rdotk*uz+rfdotk*vz;
00486 
00487  /* Phase in rads */
00488   *phase = xlt-deep_arg.xnode-deep_arg.omgadf+twopi;
00489   if(*phase < 0) *phase += twopi;
00490   *phase = FMod2p(*phase);
00491 
00492   tle->omegao1=deep_arg.omgadf;
00493   tle->xincl1=deep_arg.xinc;
00494   tle->xnodeo1=deep_arg.xnode;
00495 } /* SDP4 */
00496 
00497 /*------------------------------------------------------------------*/
00498 
00499 /* DEEP */
00500 /* This function is used by SDP4 to add lunar and solar */
00501 /* perturbation effects to deep-space orbit objects.    */
00502 void
00503 Deep(int ientry, tle_t *tle, deep_arg_t *deep_arg)
00504 {
00505   static double
00506     thgr,xnq,xqncl,omegaq,zmol,zmos,savtsn,ee2,e3,xi2,
00507     xl2,xl3,xl4,xgh2,xgh3,xgh4,xh2,xh3,sse,ssi,ssg,xi3,
00508     se2,si2,sl2,sgh2,sh2,se3,si3,sl3,sgh3,sh3,sl4,sgh4,
00509     ssl,ssh,d3210,d3222,d4410,d4422,d5220,d5232,d5421,
00510     d5433,del1,del2,del3,fasx2,fasx4,fasx6,xlamo,xfact,
00511     xni,atime,stepp,stepn,step2,preep,pl,sghs,xli,
00512     d2201,d2211,sghl,sh1,pinc,pe,shs,zsingl,zcosgl,
00513     zsinhl,zcoshl,zsinil,zcosil;
00514 
00515   double
00516     a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,ainv2,alfdp,aqnv,
00517     sgh,sini2,sinis,sinok,sh,si,sil,day,betdp,dalf,
00518     bfact,c,cc,cosis,cosok,cosq,ctem,f322,zx,zy,
00519     dbet,dls,eoc,eq,f2,f220,f221,f3,f311,f321,xnoh,
00520     f330,f441,f442,f522,f523,f542,f543,g200,g201,
00521     g211,pgh,ph,s1,s2,s3,s4,s5,s6,s7,se,sel,ses,xls,
00522     g300,g310,g322,g410,g422,g520,g521,g532,g533,gam,
00523     sinq,sinzf,sis,sl,sll,sls,stem,temp,temp1,x1,x2,
00524     x2li,x2omi,x3,x4,x5,x6,x7,x8,xl,xldot,xmao,xnddt,
00525     xndot,xno2,xnodce,xnoi,xomi,xpidot,z1,z11,z12,z13,
00526     z2,z21,z22,z23,z3,z31,z32,z33,ze,zf,zm,/* zmo, (see below) */zn,
00527     zsing,zsinh,zsini,zcosg,zcosh,zcosi,delt=0,ft=0;
00528 
00529   /* Compiler complains defined but not used. I never like to
00530      edit other peoples libraries but I also dislike compiler 
00531      warnings. AjK, 7th Sep 2010. */
00532   double zmo __attribute__((unused));
00533   
00534   switch(ientry)
00535     {
00536     case dpinit : /* Entrance for deep space initialization */
00537       thgr = ThetaG(tle->epoch, deep_arg);
00538       eq = tle->eo;
00539       xnq = deep_arg->xnodp;
00540       aqnv = 1/deep_arg->aodp;
00541       xqncl = tle->xincl;
00542       xmao = tle->xmo;
00543       xpidot = deep_arg->omgdot+deep_arg->xnodot;
00544       sinq = sin(tle->xnodeo);
00545       cosq = cos(tle->xnodeo);
00546       omegaq = tle->omegao;
00547 
00548       /* Initialize lunar solar terms */
00549       day = deep_arg->ds50+18261.5;  /*Days since 1900 Jan 0.5*/
00550       if (day != preep)
00551     {
00552       preep = day;
00553       xnodce = 4.5236020-9.2422029E-4*day;
00554       stem = sin(xnodce);
00555       ctem = cos(xnodce);
00556       zcosil = 0.91375164-0.03568096*ctem;
00557       zsinil = sqrt(1-zcosil*zcosil);
00558       zsinhl = 0.089683511*stem/zsinil;
00559       zcoshl = sqrt(1-zsinhl*zsinhl);
00560       c = 4.7199672+0.22997150*day;
00561       gam = 5.8351514+0.0019443680*day;
00562       zmol = FMod2p(c-gam);
00563       zx = 0.39785416*stem/zsinil;
00564       zy = zcoshl*ctem+0.91744867*zsinhl*stem;
00565       zx = AcTan(zx,zy);
00566       zx = gam+zx-xnodce;
00567       zcosgl = cos(zx);
00568       zsingl = sin(zx);
00569       zmos = 6.2565837+0.017201977*day;
00570       zmos = FMod2p(zmos);
00571     } /* End if(day != preep) */
00572 
00573       /* Do solar terms */
00574       savtsn = 1E20;
00575       zcosg = zcosgs;
00576       zsing = zsings;
00577       zcosi = zcosis;
00578       zsini = zsinis;
00579       zcosh = cosq;
00580       zsinh = sinq;
00581       cc = c1ss;
00582       zn = zns;
00583       ze = zes;
00584       zmo = zmos;
00585       xnoi = 1/xnq;
00586 
00587       /* Loop breaks when Solar terms are done a second */
00588       /* time, after Lunar terms are initialized        */
00589       for(;;) 
00590     {
00591       /* Solar terms done again after Lunar terms are done */
00592       a1 = zcosg*zcosh+zsing*zcosi*zsinh;
00593       a3 = -zsing*zcosh+zcosg*zcosi*zsinh;
00594       a7 = -zcosg*zsinh+zsing*zcosi*zcosh;
00595       a8 = zsing*zsini;
00596       a9 = zsing*zsinh+zcosg*zcosi*zcosh;
00597       a10 = zcosg*zsini;
00598       a2 = deep_arg->cosio*a7+ deep_arg->sinio*a8;
00599       a4 = deep_arg->cosio*a9+ deep_arg->sinio*a10;
00600       a5 = -deep_arg->sinio*a7+ deep_arg->cosio*a8;
00601       a6 = -deep_arg->sinio*a9+ deep_arg->cosio*a10;
00602       x1 = a1*deep_arg->cosg+a2*deep_arg->sing;
00603       x2 = a3*deep_arg->cosg+a4*deep_arg->sing;
00604       x3 = -a1*deep_arg->sing+a2*deep_arg->cosg;
00605       x4 = -a3*deep_arg->sing+a4*deep_arg->cosg;
00606       x5 = a5*deep_arg->sing;
00607       x6 = a6*deep_arg->sing;
00608       x7 = a5*deep_arg->cosg;
00609       x8 = a6*deep_arg->cosg;
00610       z31 = 12*x1*x1-3*x3*x3;
00611       z32 = 24*x1*x2-6*x3*x4;
00612       z33 = 12*x2*x2-3*x4*x4;
00613       z1 = 3*(a1*a1+a2*a2)+z31*deep_arg->eosq;
00614       z2 = 6*(a1*a3+a2*a4)+z32*deep_arg->eosq;
00615       z3 = 3*(a3*a3+a4*a4)+z33*deep_arg->eosq;
00616       z11 = -6*a1*a5+deep_arg->eosq*(-24*x1*x7-6*x3*x5);
00617       z12 = -6*(a1*a6+a3*a5)+ deep_arg->eosq*
00618                 (-24*(x2*x7+x1*x8)-6*(x3*x6+x4*x5));
00619       z13 = -6*a3*a6+deep_arg->eosq*(-24*x2*x8-6*x4*x6);
00620       z21 = 6*a2*a5+deep_arg->eosq*(24*x1*x5-6*x3*x7);
00621       z22 = 6*(a4*a5+a2*a6)+ deep_arg->eosq*
00622                 (24*(x2*x5+x1*x6)-6*(x4*x7+x3*x8));
00623       z23 = 6*a4*a6+deep_arg->eosq*(24*x2*x6-6*x4*x8);
00624       z1 = z1+z1+deep_arg->betao2*z31;
00625       z2 = z2+z2+deep_arg->betao2*z32;
00626       z3 = z3+z3+deep_arg->betao2*z33;
00627       s3 = cc*xnoi;
00628       s2 = -0.5*s3/deep_arg->betao;
00629       s4 = s3*deep_arg->betao;
00630       s1 = -15*eq*s4;
00631       s5 = x1*x3+x2*x4;
00632       s6 = x2*x3+x1*x4;
00633       s7 = x2*x4-x1*x3;
00634       se = s1*zn*s5;
00635       si = s2*zn*(z11+z13);
00636       sl = -zn*s3*(z1+z3-14-6*deep_arg->eosq);
00637       sgh = s4*zn*(z31+z33-6);
00638       sh = -zn*s2*(z21+z23);
00639       if (xqncl < 5.2359877E-2) sh = 0;
00640       ee2 = 2*s1*s6;
00641       e3 = 2*s1*s7;
00642       xi2 = 2*s2*z12;
00643       xi3 = 2*s2*(z13-z11);
00644       xl2 = -2*s3*z2;
00645       xl3 = -2*s3*(z3-z1);
00646       xl4 = -2*s3*(-21-9*deep_arg->eosq)*ze;
00647       xgh2 = 2*s4*z32;
00648       xgh3 = 2*s4*(z33-z31);
00649       xgh4 = -18*s4*ze;
00650       xh2 = -2*s2*z22;
00651       xh3 = -2*s2*(z23-z21);
00652 
00653       if(isFlagSet(LUNAR_TERMS_DONE_FLAG)) break;
00654 
00655       /* Do lunar terms */
00656       sse = se;
00657       ssi = si;
00658       ssl = sl;
00659       ssh = sh/deep_arg->sinio;
00660       ssg = sgh-deep_arg->cosio*ssh;
00661       se2 = ee2;
00662       si2 = xi2;
00663       sl2 = xl2;
00664       sgh2 = xgh2;
00665       sh2 = xh2;
00666       se3 = e3;
00667       si3 = xi3;
00668       sl3 = xl3;
00669       sgh3 = xgh3;
00670       sh3 = xh3;
00671       sl4 = xl4;
00672       sgh4 = xgh4;
00673       zcosg = zcosgl;
00674       zsing = zsingl;
00675       zcosi = zcosil;
00676       zsini = zsinil;
00677       zcosh = zcoshl*cosq+zsinhl*sinq;
00678       zsinh = sinq*zcoshl-cosq*zsinhl;
00679       zn = znl;
00680       cc = c1l;
00681       ze = zel;
00682       zmo = zmol;
00683       SetFlag(LUNAR_TERMS_DONE_FLAG);
00684     } /* End of for(;;) */
00685 
00686       sse = sse+se;
00687       ssi = ssi+si;
00688       ssl = ssl+sl;
00689       ssg = ssg+sgh-deep_arg->cosio/deep_arg->sinio*sh;
00690       ssh = ssh+sh/deep_arg->sinio;
00691 
00692       /* Geopotential resonance initialization for 12 hour orbits */
00693       ClearFlag(RESONANCE_FLAG);
00694       ClearFlag(SYNCHRONOUS_FLAG);
00695 
00696       if( !((xnq < 0.0052359877) && (xnq > 0.0034906585)) )
00697     {
00698       if( (xnq < 0.00826) || (xnq > 0.00924) ) return;
00699       if (eq < 0.5) return;
00700       SetFlag(RESONANCE_FLAG);
00701       eoc = eq*deep_arg->eosq;
00702       g201 = -0.306-(eq-0.64)*0.440;
00703       if (eq <= 0.65)
00704         {
00705           g211 = 3.616-13.247*eq+16.290*deep_arg->eosq;
00706           g310 = -19.302+117.390*eq-228.419*
00707                       deep_arg->eosq+156.591*eoc;
00708           g322 = -18.9068+109.7927*eq-214.6334*
00709                      deep_arg->eosq+146.5816*eoc;
00710           g410 = -41.122+242.694*eq-471.094*
00711                      deep_arg->eosq+313.953*eoc;
00712           g422 = -146.407+841.880*eq-1629.014*
00713                      deep_arg->eosq+1083.435*eoc;
00714           g520 = -532.114+3017.977*eq-5740*
00715                      deep_arg->eosq+3708.276*eoc;
00716         }
00717       else
00718         {
00719           g211 = -72.099+331.819*eq-508.738*
00720                      deep_arg->eosq+266.724*eoc;
00721           g310 = -346.844+1582.851*eq-2415.925*
00722                      deep_arg->eosq+1246.113*eoc;
00723           g322 = -342.585+1554.908*eq-2366.899*
00724                      deep_arg->eosq+1215.972*eoc;
00725           g410 = -1052.797+4758.686*eq-7193.992*
00726                      deep_arg->eosq+3651.957*eoc;
00727           g422 = -3581.69+16178.11*eq-24462.77*
00728                      deep_arg->eosq+ 12422.52*eoc;
00729           if (eq <= 0.715)
00730         g520 = 1464.74-4664.75*eq+3763.64*deep_arg->eosq;
00731           else
00732         g520 = -5149.66+29936.92*eq-54087.36*
00733                        deep_arg->eosq+31324.56*eoc;
00734         } /* End if (eq <= 0.65) */
00735 
00736       if (eq < 0.7)
00737         {
00738           g533 = -919.2277+4988.61*eq-9064.77*
00739                      deep_arg->eosq+5542.21*eoc;
00740           g521 = -822.71072+4568.6173*eq-8491.4146*
00741                      deep_arg->eosq+5337.524*eoc;
00742           g532 = -853.666+4690.25*eq-8624.77*
00743                      deep_arg->eosq+ 5341.4*eoc;
00744         }
00745       else
00746         {
00747           g533 = -37995.78+161616.52*eq-229838.2*
00748                      deep_arg->eosq+109377.94*eoc;
00749           g521 = -51752.104+218913.95*eq-309468.16*
00750                      deep_arg->eosq+146349.42*eoc;
00751          g532 = -40023.88+170470.89*eq-242699.48*
00752                     deep_arg->eosq+115605.82*eoc;
00753         } /* End if (eq <= 0.7) */
00754 
00755       sini2 = deep_arg->sinio*deep_arg->sinio;
00756       f220 = 0.75*(1+2*deep_arg->cosio+deep_arg->theta2);
00757       f221 = 1.5*sini2;
00758       f321 = 1.875*deep_arg->sinio*(1-2*\
00759                  deep_arg->cosio-3*deep_arg->theta2);
00760       f322 = -1.875*deep_arg->sinio*(1+2*
00761                  deep_arg->cosio-3*deep_arg->theta2);
00762       f441 = 35*sini2*f220;
00763       f442 = 39.3750*sini2*sini2;
00764       f522 = 9.84375*deep_arg->sinio*(sini2*(1-2*deep_arg->cosio-5*
00765          deep_arg->theta2)+0.33333333*(-2+4*deep_arg->cosio+
00766          6*deep_arg->theta2));
00767       f523 = deep_arg->sinio*(4.92187512*sini2*(-2-4*
00768          deep_arg->cosio+10*deep_arg->theta2)+6.56250012
00769                  *(1+2*deep_arg->cosio-3*deep_arg->theta2));
00770       f542 = 29.53125*deep_arg->sinio*(2-8*
00771                  deep_arg->cosio+deep_arg->theta2*
00772          (-12+8*deep_arg->cosio+10*deep_arg->theta2));
00773       f543 = 29.53125*deep_arg->sinio*(-2-8*deep_arg->cosio+
00774          deep_arg->theta2*(12+8*deep_arg->cosio-10*
00775                  deep_arg->theta2));
00776       xno2 = xnq*xnq;
00777       ainv2 = aqnv*aqnv;
00778       temp1 = 3*xno2*ainv2;
00779       temp = temp1*root22;
00780       d2201 = temp*f220*g201;
00781       d2211 = temp*f221*g211;
00782       temp1 = temp1*aqnv;
00783       temp = temp1*root32;
00784       d3210 = temp*f321*g310;
00785       d3222 = temp*f322*g322;
00786       temp1 = temp1*aqnv;
00787       temp = 2*temp1*root44;
00788       d4410 = temp*f441*g410;
00789       d4422 = temp*f442*g422;
00790       temp1 = temp1*aqnv;
00791       temp = temp1*root52;
00792       d5220 = temp*f522*g520;
00793       d5232 = temp*f523*g532;
00794       temp = 2*temp1*root54;
00795       d5421 = temp*f542*g521;
00796       d5433 = temp*f543*g533;
00797       xlamo = xmao+tle->xnodeo+tle->xnodeo-thgr-thgr;
00798       bfact = deep_arg->xmdot+deep_arg->xnodot+
00799                   deep_arg->xnodot-thdt-thdt;
00800       bfact = bfact+ssl+ssh+ssh;
00801     } /* if( !(xnq < 0.0052359877) && (xnq > 0.0034906585) ) */
00802       else
00803     {
00804       SetFlag(RESONANCE_FLAG);
00805       SetFlag(SYNCHRONOUS_FLAG);
00806       /* Synchronous resonance terms initialization */
00807       g200 = 1+deep_arg->eosq*(-2.5+0.8125*deep_arg->eosq);
00808       g310 = 1+2*deep_arg->eosq;
00809       g300 = 1+deep_arg->eosq*(-6+6.60937*deep_arg->eosq);
00810       f220 = 0.75*(1+deep_arg->cosio)*(1+deep_arg->cosio);
00811       f311 = 0.9375*deep_arg->sinio*deep_arg->sinio*
00812              (1+3*deep_arg->cosio)-0.75*(1+deep_arg->cosio);
00813       f330 = 1+deep_arg->cosio;
00814       f330 = 1.875*f330*f330*f330;
00815       del1 = 3*xnq*xnq*aqnv*aqnv;
00816       del2 = 2*del1*f220*g200*q22;
00817       del3 = 3*del1*f330*g300*q33*aqnv;
00818       del1 = del1*f311*g310*q31*aqnv;
00819       fasx2 = 0.13130908;
00820       fasx4 = 2.8843198;
00821       fasx6 = 0.37448087;
00822       xlamo = xmao+tle->xnodeo+tle->omegao-thgr;
00823       bfact = deep_arg->xmdot+xpidot-thdt;
00824       bfact = bfact+ssl+ssg+ssh;
00825     } /* End if( !(xnq < 0.0052359877) && (xnq > 0.0034906585) ) */
00826 
00827       xfact = bfact-xnq;
00828 
00829       /* Initialize integrator */
00830       xli = xlamo;
00831       xni = xnq;
00832       atime = 0;
00833       stepp = 720;
00834       stepn = -720;
00835       step2 = 259200;
00836       /* End case dpinit: */
00837       return;
00838 
00839     case dpsec: /* Entrance for deep space secular effects */
00840       deep_arg->xll = deep_arg->xll+ssl*deep_arg->t;
00841       deep_arg->omgadf = deep_arg->omgadf+ssg*deep_arg->t;
00842       deep_arg->xnode = deep_arg->xnode+ssh*deep_arg->t;
00843       deep_arg->em = tle->eo+sse*deep_arg->t;
00844       deep_arg->xinc = tle->xincl+ssi*deep_arg->t;
00845       if (deep_arg->xinc < 0)
00846     {
00847       deep_arg->xinc = -deep_arg->xinc;
00848       deep_arg->xnode = deep_arg->xnode + pi;
00849       deep_arg->omgadf = deep_arg->omgadf-pi;
00850     }
00851       if( isFlagClear(RESONANCE_FLAG) ) return;
00852 
00853       do
00854     {
00855           if( (atime == 0) ||
00856          ((deep_arg->t >= 0) && (atime < 0)) || 
00857          ((deep_arg->t < 0) && (atime >= 0)) )
00858         {
00859           /* Epoch restart */
00860           if( deep_arg->t >= 0 )
00861         delt = stepp;
00862           else
00863         delt = stepn;
00864 
00865           atime = 0;
00866           xni = xnq;
00867           xli = xlamo;
00868         }
00869       else
00870         {      
00871               if( fabs(deep_arg->t) >= fabs(atime) )
00872         {
00873           if ( deep_arg->t > 0 )
00874             delt = stepp;
00875           else
00876             delt = stepn;
00877         }
00878         }
00879 
00880           do 
00881         {
00882           if ( fabs(deep_arg->t-atime) >= stepp )
00883         {
00884           SetFlag(DO_LOOP_FLAG);
00885           ClearFlag(EPOCH_RESTART_FLAG);
00886         }
00887           else
00888         {
00889           ft = deep_arg->t-atime;
00890           ClearFlag(DO_LOOP_FLAG);
00891         }
00892 
00893           if( fabs(deep_arg->t) < fabs(atime) )
00894         {
00895           if (deep_arg->t >= 0)
00896             delt = stepn;
00897           else
00898             delt = stepp;
00899           SetFlag(DO_LOOP_FLAG | EPOCH_RESTART_FLAG);
00900         }
00901 
00902           /* Dot terms calculated */
00903               if( isFlagSet(SYNCHRONOUS_FLAG) )
00904         {
00905           xndot = del1*sin(xli-fasx2)+del2*sin(2*(xli-fasx4))
00906                   +del3*sin(3*(xli-fasx6));
00907           xnddt = del1*cos(xli-fasx2)+2*del2*cos(2*(xli-fasx4))
00908                   +3*del3*cos(3*(xli-fasx6));
00909         }
00910           else
00911         {
00912           xomi = omegaq+deep_arg->omgdot*atime;
00913           x2omi = xomi+xomi;
00914           x2li = xli+xli;
00915           xndot = d2201*sin(x2omi+xli-g22)
00916                   +d2211*sin(xli-g22)
00917                   +d3210*sin(xomi+xli-g32)
00918                   +d3222*sin(-xomi+xli-g32)
00919                   +d4410*sin(x2omi+x2li-g44)
00920                   +d4422*sin(x2li-g44)
00921                   +d5220*sin(xomi+xli-g52)
00922                   +d5232*sin(-xomi+xli-g52)
00923                   +d5421*sin(xomi+x2li-g54)
00924                   +d5433*sin(-xomi+x2li-g54);
00925           xnddt = d2201*cos(x2omi+xli-g22)
00926                   +d2211*cos(xli-g22)
00927                   +d3210*cos(xomi+xli-g32)
00928                   +d3222*cos(-xomi+xli-g32)
00929                   +d5220*cos(xomi+xli-g52)
00930                   +d5232*cos(-xomi+xli-g52)
00931                   +2*(d4410*cos(x2omi+x2li-g44)
00932                   +d4422*cos(x2li-g44)
00933                   +d5421*cos(xomi+x2li-g54)
00934                   +d5433*cos(-xomi+x2li-g54));
00935         } /* End of if (isFlagSet(SYNCHRONOUS_FLAG)) */
00936 
00937           xldot = xni+xfact;
00938           xnddt = xnddt*xldot;
00939 
00940           if(isFlagSet(DO_LOOP_FLAG))
00941         {
00942           xli = xli+xldot*delt+xndot*step2;
00943           xni = xni+xndot*delt+xnddt*step2;
00944           atime = atime+delt;
00945         }
00946         }
00947       while(isFlagSet(DO_LOOP_FLAG) && isFlagClear(EPOCH_RESTART_FLAG));
00948     }
00949       while(isFlagSet(DO_LOOP_FLAG) && isFlagSet(EPOCH_RESTART_FLAG));
00950 
00951       deep_arg->xn = xni+xndot*ft+xnddt*ft*ft*0.5;
00952       xl = xli+xldot*ft+xndot*ft*ft*0.5;
00953       temp = -deep_arg->xnode+thgr+deep_arg->t*thdt;
00954 
00955       if (isFlagClear(SYNCHRONOUS_FLAG))
00956     deep_arg->xll = xl+temp+temp;
00957       else
00958     deep_arg->xll = xl-deep_arg->omgadf+temp;
00959 
00960       return;
00961       /*End case dpsec: */
00962 
00963     case dpper: /* Entrance for lunar-solar periodics */
00964       sinis = sin(deep_arg->xinc);
00965       cosis = cos(deep_arg->xinc);
00966       if (fabs(savtsn-deep_arg->t) >= 30)
00967     {
00968       savtsn = deep_arg->t;
00969       zm = zmos+zns*deep_arg->t;
00970       zf = zm+2*zes*sin(zm);
00971       sinzf = sin(zf);
00972       f2 = 0.5*sinzf*sinzf-0.25;
00973       f3 = -0.5*sinzf*cos(zf);
00974       ses = se2*f2+se3*f3;
00975       sis = si2*f2+si3*f3;
00976       sls = sl2*f2+sl3*f3+sl4*sinzf;
00977       sghs = sgh2*f2+sgh3*f3+sgh4*sinzf;
00978       shs = sh2*f2+sh3*f3;
00979       zm = zmol+znl*deep_arg->t;
00980       zf = zm+2*zel*sin(zm);
00981       sinzf = sin(zf);
00982       f2 = 0.5*sinzf*sinzf-0.25;
00983       f3 = -0.5*sinzf*cos(zf);
00984       sel = ee2*f2+e3*f3;
00985       sil = xi2*f2+xi3*f3;
00986       sll = xl2*f2+xl3*f3+xl4*sinzf;
00987       sghl = xgh2*f2+xgh3*f3+xgh4*sinzf;
00988       sh1 = xh2*f2+xh3*f3;
00989       pe = ses+sel;
00990       pinc = sis+sil;
00991       pl = sls+sll;
00992     }
00993 
00994       pgh = sghs+sghl;
00995       ph = shs+sh1;
00996       deep_arg->xinc = deep_arg->xinc+pinc;
00997       deep_arg->em = deep_arg->em+pe;
00998 
00999       if (xqncl >= 0.2)
01000     {
01001       /* Apply periodics directly */
01002       ph = ph/deep_arg->sinio;
01003       pgh = pgh-deep_arg->cosio*ph;
01004       deep_arg->omgadf = deep_arg->omgadf+pgh;
01005       deep_arg->xnode = deep_arg->xnode+ph;
01006       deep_arg->xll = deep_arg->xll+pl;
01007     }
01008       else
01009         {
01010       /* Apply periodics with Lyddane modification */
01011       sinok = sin(deep_arg->xnode);
01012       cosok = cos(deep_arg->xnode);
01013       alfdp = sinis*sinok;
01014       betdp = sinis*cosok;
01015       dalf = ph*cosok+pinc*cosis*sinok;
01016       dbet = -ph*sinok+pinc*cosis*cosok;
01017       alfdp = alfdp+dalf;
01018       betdp = betdp+dbet;
01019       deep_arg->xnode = FMod2p(deep_arg->xnode);
01020       xls = deep_arg->xll+deep_arg->omgadf+cosis*deep_arg->xnode;
01021       dls = pl+pgh-pinc*deep_arg->xnode*sinis;
01022       xls = xls+dls;
01023       xnoh = deep_arg->xnode;
01024       deep_arg->xnode = AcTan(alfdp,betdp);
01025 
01026           /* This is a patch to Lyddane modification */
01027           /* suggested by Rob Matson. */
01028       if(fabs(xnoh-deep_arg->xnode) > pi)
01029         {
01030           if(deep_arg->xnode < xnoh)
01031         deep_arg->xnode +=twopi;
01032           else
01033         deep_arg->xnode -=twopi;
01034         }
01035 
01036       deep_arg->xll = deep_arg->xll+pl;
01037       deep_arg->omgadf = xls-deep_arg->xll-cos(deep_arg->xinc)*
01038                              deep_arg->xnode;
01039     } /* End case dpper: */
01040       return;
01041 
01042     } /* End switch(ientry) */
01043 
01044 } /* End of Deep() */
01045 
01046 /*------------------------------------------------------------------*/
01047 
01048 /* Functions for testing and setting/clearing flags */
01049 
01050 /* An int variable holding the single-bit flags */
01051 static int Flags = 0;
01052 
01053 int
01054 isFlagSet(int flag)
01055 {
01056   return (Flags & flag);
01057 }
01058 
01059 int
01060 isFlagClear(int flag)
01061 {
01062   return (~Flags & flag);
01063 }
01064 
01065 void
01066 SetFlag(int flag)
01067 {
01068   Flags |= flag;
01069 }
01070 
01071 void
01072 ClearFlag(int flag)
01073 {
01074   Flags &= ~flag;
01075 }
01076 
01077 /*------------------------------------------------------------------*/