char * get_digits(int n, size_t* len)
{
mpz_ui_pow_ui(pows, 10, n + 20);
actan(t5, 5, pows);
mpz_mul_ui(t5, t5, 16);
actan(t239, 239, pows);
mpz_mul_ui(t239, t239, 4);
mpz_sub(t5, t5, t239);
mpz_ui_pow_ui(pows, 10, 20);
mpz_tdiv_q(t5, t5, pows);
*len = mpz_sizeinbase(t5, 10);
return mpz_get_str(0, 0, t5);
}
/* ENTRY DPPER(EM,XINC,OMGASM,XNODES,XLL) */
void
dpper(SatData *sat, double *EM, double *XINC, double *OMGASM,
double *XNODES, double *XLL, double T)
{
double SINIS, COSIS, ZM, ZF, SINZF, F2, F3, SES, SIS, SLS, SEL, SIL, SLL, PGH, PH, SINOK, COSOK, ALFDP, BETDP, DALF, DBET, XLS, DLS;
#if 0
SINIS = COSIS = ZM = ZF = SINZF = F2 = F3 = SES = SIS = signaling_nan();
SLS = SEL = SIL = SLL = PGH = signaling_nan();
PH = SINOK = COSOK = ALFDP = BETDP = DALF = DBET = XLS = signaling_nan();
DLS = signaling_nan();;
#endif
SINIS = sin(*XINC);
COSIS = cos(*XINC);
/* IF (DABS(SAVTSN-T).LT.(30.D0)) GO TO 210 */
if(fabs(SAVTSN - T) >= (30.0)) {
SAVTSN = T;
ZM = ZMOS + ZNS * T;
/* 205 ZF = ZM + 2.0 * ZES * sin(ZM) */
ZF = ZM + 2.0 * ZES * sin(ZM);
SINZF = sin(ZF);
F2 = .5 * SINZF * SINZF - .25;
F3 = -.5 * SINZF * cos(ZF);
SES = SE2 * F2 + SE3 * F3;
SIS = SI2 * F2 + SI3 * F3;
SLS = SL2 * F2 + SL3 * F3 + SL4 * SINZF;
SGHS = SGH2 * F2 + SGH3 * F3 + SGH4 * SINZF;
SHS = SH2 * F2 + SH3 * F3;
ZM = ZMOL + ZNL * T;
ZF = ZM + 2.0 * ZEL * sin(ZM);
SINZF = sin(ZF);
F2 = .5 * SINZF * SINZF -.25;
F3 = -.5 * SINZF * cos(ZF);
SEL = EE2 * F2 + E3 * F3;
SIL = XI2 * F2 + XI3 * F3;
SLL = XL2 * F2 + XL3 * F3 + XL4 * SINZF;
SGHL = XGH2 * F2 + XGH3 * F3 + XGH4 * SINZF;
SHL = XH2 * F2 + XH3 * F3;
PE = SES + SEL;
PINC = SIS + SIL;
PL = SLS + SLL;
}
/* 210 PGH=SGHS+SGHL */
PGH = SGHS + SGHL;
PH = SHS + SHL;
*XINC = *XINC + PINC;
*EM = *EM + PE;
/* IF(XQNCL.LT.(.2)) GO TO 220 */
if(XQNCL >= (.2)) {
/* GO TO 218 */
/* C */
/* C APPLY PERIODICS DIRECTLY */
/* C */
/* 218 PH=PH/SINIQ */
PH = PH / s_SINIQ;
PGH = PGH - s_COSIQ * PH;
*OMGASM = *OMGASM + PGH;
*XNODES = *XNODES + PH;
*XLL = *XLL + PL;
/* GO TO 230 */
} else {
/* C */
/* C APPLY PERIODICS WITH LYDDANE MODIFICATION */
/* C */
/* 220 SINOK=sin(XNODES) */
SINOK = sin(*XNODES);
COSOK = cos(*XNODES);
ALFDP = SINIS * SINOK;
BETDP = SINIS * COSOK;
DALF = PH * COSOK + PINC * COSIS * SINOK;
DBET = -PH * SINOK + PINC * COSIS * COSOK;
ALFDP = ALFDP + DALF;
BETDP = BETDP + DBET;
XLS = *XLL + *OMGASM + COSIS * *XNODES;
DLS = PL + PGH - PINC * *XNODES * SINIS;
XLS = XLS + DLS;
*XNODES = actan(ALFDP, BETDP);
*XLL = *XLL + PL;
*OMGASM = XLS - *XLL - cos(*XINC) * *XNODES;
}
/* 230 CONTINUE */
/* RETURN */
}
void
dpinit(SatData *sat, double EQSQ, double SINIQ, double COSIQ,
double RTEQSQ, double AO, double COSQ2, double SINOMO,
double COSOMO, double BSQ, double XLLDOT, double OMGDT,
double XNODOT, double XNODP)
{
double A1, A10, A2, A3, A4, A5, A6, A7, A8, A9, AINV2, AQNV, BFACT,
C, CC, COSQ, CTEM, DAY, DS50, EOC, EQ, F220, F221, F311, F321, F322,
F330, F441, F442, F522, F523, F542, F543, G200, G201, G211, G300,
G310, G322, G410, G422, G520, G521, G532, G533, GAM, PREEP, S1, S2,
S3, S4, S5, S6, S7, SE, SGH, SH, SI, SINI2, SINQ, SL, STEM, TEMP,
TEMP1, X1, X2, X3, X4, X5, X6, X7, X8, XMAO, XNO2, XNODCE, XNOI,
XPIDOT, Z1, Z11, Z12, Z13, Z2, Z21, Z22, Z23, Z3, Z31, Z32, Z33,
ZCOSG, ZCOSGL, ZCOSH, ZCOSHL, ZCOSI, ZCOSIL, ZE, ZMO, ZN, ZSING,
ZSINGL, ZSINH, ZSINHL, ZSINI, ZSINIL, ZX, ZY;
int c;
#if 0
A1=A10=A2=A3=A4=A5=A6=A7=A8=A9=AINV2=AQNV=BFACT = signaling_nan();
C=CC=COSQ=CTEM=DAY=DS50=EOC=EQ=F220=F221=F311=F321=F322 = signaling_nan();
F330=F441=F442=F522=F523=F542=F543=G200=G201=G211=G300 = signaling_nan();
G310=G322=G410=G422=G520=G521=G532=G533=GAM=PREEP=S1=S2 = signaling_nan();
S3=S4=S5=S6=S7=SE=SGH=SH=SI=SINI2=SINQ=SL=STEM=TEMP = signaling_nan();
TEMP1=X1=X2=X3=X4=X5=X6=X7=X8=XMAO=XNO2=XNODCE=XNOI = signaling_nan();
XPIDOT=Z1=Z11=Z12=Z13=Z2=Z21=Z22=Z23=Z3=Z31=Z32=Z33 = signaling_nan();
ZCOSG=ZCOSGL=ZCOSH=ZCOSHL=ZCOSI=ZCOSIL=ZE=ZMO=ZN=ZSING = signaling_nan();
ZSINGL=ZSINH=ZSINHL=ZSINI=ZSINIL=ZX=ZY = signaling_nan();
#endif
if(!sat->deep)
sat->deep = (struct deep_data *) malloc(sizeof(struct deep_data));
else
return;
/* init_deep(sat->deep); */
PREEP = 0.0;
ZCOSGL = ZCOSHL = ZCOSIL = ZSINGL = ZSINHL = ZSINIL = 0.0;
/* Save some of the arguments, for use by dpsec() and dpper() */
s_SINIQ = SINIQ;
s_COSIQ = COSIQ;
s_OMGDT = OMGDT;
THGR = thetag(EPOCH, &DS50);
EQ = EO;
XNQ = XNODP;
AQNV = 1.0/AO;
XQNCL = XINCL;
XMAO = XMO;
XPIDOT = OMGDT + XNODOT;
SINQ = sin(XNODEO);
COSQ = cos(XNODEO);
OMEGAQ = OMEGAO;
/* INITIALIZE LUNAR SOLAR TERMS */
DAY = DS50 + 18261.5;
if(DAY != PREEP) {
PREEP = DAY;
XNODCE = 4.5236020 - 9.2422029E-4 * DAY;
STEM = sin(XNODCE);
CTEM = cos(XNODCE);
ZCOSIL = .91375164 - .03568096 * CTEM;
ZSINIL = sqrt(1.0 - ZCOSIL * ZCOSIL);
ZSINHL = .089683511 * STEM / ZSINIL;
ZCOSHL = sqrt(1.0 - ZSINHL * ZSINHL);
C = 4.7199672 + .22997150 * DAY;
GAM = 5.8351514 + .0019443680 * DAY;
ZMOL = fmod(C-GAM, TWOPI);
ZX = .39785416 * STEM / ZSINIL;
ZY = ZCOSHL * CTEM + 0.91744867 * ZSINHL * STEM;
ZX = actan(ZX, ZY);
ZX = GAM + ZX - XNODCE;
ZCOSGL = cos(ZX);
ZSINGL = sin(ZX);
ZMOS = 6.2565837 + .017201977 * DAY;
ZMOS = fmod(ZMOS, TWOPI);
}
/* DO SOLAR TERMS */
SAVTSN = 1.0E20;
ZCOSG = ZCOSGS;
ZSING = ZSINGS;
ZCOSI = ZCOSIS;
ZSINI = ZSINIS;
ZCOSH = COSQ;
ZSINH = SINQ;
CC = C1SS;
ZN = ZNS;
ZE = ZES;
ZMO = ZMOS;
XNOI = 1.0 / XNQ;
for(c = 0; c < 2; c++) {
A1 = ZCOSG * ZCOSH + ZSING * ZCOSI * ZSINH;
A3 = -ZSING * ZCOSH + ZCOSG * ZCOSI * ZSINH;
A7 = -ZCOSG * ZSINH + ZSING * ZCOSI * ZCOSH;
A8 = ZSING * ZSINI;
A9 = ZSING * ZSINH + ZCOSG * ZCOSI * ZCOSH;
A10 = ZCOSG * ZSINI;
A2 = COSIQ * A7 + SINIQ * A8;
A4 = COSIQ * A9 + SINIQ * A10;
A5 = - SINIQ * A7 + COSIQ * A8;
A6 = - SINIQ * A9 + COSIQ * A10;
X1 = A1 * COSOMO + A2 * SINOMO;
X2 = A3 * COSOMO + A4 * SINOMO;
X3 = - A1 * SINOMO + A2 * COSOMO;
X4 = - A3 * SINOMO + A4 * COSOMO;
X5 = A5 * SINOMO;
X6 = A6 * SINOMO;
X7 = A5 * COSOMO;
X8 = A6 * COSOMO;
Z31 = 12.0 * X1 * X1 -3.0 * X3 * X3;
Z32 = 24.0 * X1 * X2 -6.0 * X3 * X4;
Z33 = 12.0 * X2 * X2 -3.0 * X4 * X4;
Z1 = 3.0 * (A1 * A1 + A2 * A2) + Z31 * EQSQ;
Z2 = 6.0 * (A1 * A3 + A2 * A4) + Z32 * EQSQ;
Z3 = 3.0 * (A3 * A3 + A4 * A4) + Z33 * EQSQ;
Z11 = -6.0 * A1 * A5 + EQSQ * (-24.0 * X1 * X7 - 6.0 * X3 * X5);
Z12 = -6.0 * (A1 * A6 + A3 * A5) +
EQSQ * (-24.0 * (X2 * X7 + X1 * X8) - 6.0 * (X3 * X6 + X4 * X5));
Z13 = -6.0 * A3 * A6 + EQSQ * (-24.0 * X2 * X8 - 6.0 * X4 * X6);
Z21 = 6.0 * A2 * A5 + EQSQ * (24.0 * X1 * X5 - 6.0 * X3 * X7);
Z22 = 6.0 * (A4 * A5 + A2 * A6) +
EQSQ * (24.0 * (X2 * X5 + X1 * X6) - 6.0 * (X4 * X7 + X3 * X8));
Z23 = 6.0 * A4 * A6 + EQSQ * (24.0 * X2 * X6 - 6.0 * X4 * X8);
Z1 = Z1 + Z1 + BSQ * Z31;
Z2 = Z2 + Z2 + BSQ * Z32;
Z3 = Z3 + Z3 + BSQ * Z33;
S3 = CC * XNOI;
S2 = -.5 * S3 / RTEQSQ;
S4 = S3 * RTEQSQ;
S1 = -15.0 * EQ * S4;
S5 = X1 * X3 + X2 * X4;
S6 = X2 * X3 + X1 * X4;
S7 = X2 * X4 - X1 * X3;
SE = S1 * ZN * S5;
SI = S2 * ZN * (Z11 + Z13);
SL = -ZN * S3 * (Z1 + Z3 - 14.0 - 6.0 * EQSQ);
SGH = S4 * ZN * (Z31 + Z33 - 6.0);
SH = -ZN * S2 * (Z21 + Z23);
if(XQNCL < 5.2359877E-2)
SH = 0.0;
EE2 = 2.0 * S1 * S6;
E3 = 2.0 * S1 * S7;
XI2 = 2.0 * S2 * Z12;
XI3 = 2.0 * S2 * (Z13 - Z11);
XL2 = -2.0 * S3 * Z2;
XL3 = -2.0 * S3 * (Z3 - Z1);
XL4 = -2.0 * S3 * (-21.0 - 9.0 * EQSQ) * ZE;
XGH2 = 2.0 * S4 * Z32;
XGH3 = 2.0 * S4 * (Z33 - Z31);
XGH4 = -18.0 * S4 * ZE;
XH2 = -2.0 * S2 * Z22;
XH3 = -2.0 * S2 * (Z23 - Z21);
if(c == 0) {
/* DO LUNAR TERMS */
SSE = SE;
SSI = SI;
SSL = SL;
SSH = SH / SINIQ;
SSG = SGH - COSIQ * SSH;
SE2 = EE2;
SI2 = XI2;
SL2 = XL2;
SGH2 = XGH2;
SH2 = XH2;
SE3 = E3;
SI3 = XI3;
SL3 = XL3;
SGH3 = XGH3;
SH3 = XH3;
SL4 = XL4;
SGH4 = XGH4;
ZCOSG = ZCOSGL;
ZSING = ZSINGL;
ZCOSI = ZCOSIL;
ZSINI = ZSINIL;
ZCOSH = ZCOSHL * COSQ + ZSINHL * SINQ;
ZSINH = SINQ * ZCOSHL - COSQ * ZSINHL;
ZN = ZNL;
CC = C1L;
ZE = ZEL;
ZMO = ZMOL;
}
}
SSE = SSE + SE;
SSI = SSI + SI;
SSL = SSL + SL;
SSG = SSG + SGH - COSIQ / SINIQ * SH;
SSH = SSH + SH / SINIQ;
/* GEOPOTENTIAL RESONANCE INITIALIZATION FOR 12 HOUR ORBITS */
IRESFL = 0;
ISYNFL = 0;
if(XNQ <= .0034906585 || XNQ >= .0052359877) {
if(XNQ < (8.26E-3) || XNQ > (9.24E-3))
return;
if(EQ < 0.5)
return;
IRESFL = 1;
EOC = EQ * EQSQ;
G201 = -.306 - (EQ - .64) * .440;
if(EQ <= (.65)) {
G211 = 3.616 - 13.247 * EQ + 16.290 * EQSQ;
G310 = -19.302 + 117.390 * EQ - 228.419 * EQSQ + 156.591 * EOC;
G322 = -18.9068 + 109.7927 * EQ - 214.6334 * EQSQ + 146.5816 * EOC;
G410 = -41.122 + 242.694 * EQ - 471.094 * EQSQ + 313.953 * EOC;
G422 = -146.407 + 841.880 * EQ - 1629.014 * EQSQ + 1083.435 * EOC;
G520 = -532.114 + 3017.977 * EQ - 5740 * EQSQ + 3708.276 * EOC;
} else {
G211 = -72.099 + 331.819 * EQ - 508.738 * EQSQ + 266.724 * EOC;
G310 = -346.844 + 1582.851 * EQ - 2415.925 * EQSQ + 1246.113 * EOC;
G322 = -342.585 + 1554.908 * EQ - 2366.899 * EQSQ + 1215.972 * EOC;
G410 = -1052.797 + 4758.686 * EQ - 7193.992 * EQSQ +
3651.957 * EOC;
G422 = -3581.69 + 16178.11 * EQ - 24462.77 * EQSQ + 12422.52 * EOC;
if(EQ > (.715))
G520 = -5149.66 + 29936.92 * EQ - 54087.36 * EQSQ +
31324.56 * EOC;
G520 = 1464.74 - 4664.75 * EQ + 3763.64 * EQSQ;
}
if(EQ < (.7)) {
G533 = -919.2277 + 4988.61 * EQ - 9064.77 * EQSQ + 5542.21 * EOC;
G521 = -822.71072 + 4568.6173 * EQ - 8491.4146 * EQSQ +
5337.524 * EOC;
G532 = -853.666 + 4690.25 * EQ - 8624.77 * EQSQ + 5341.4 * EOC;
} else {
G533 = -37995.78 + 161616.52 * EQ - 229838.2 * EQSQ +
109377.94 * EOC;
G521 = -51752.104 + 218913.95 * EQ - 309468.16 * EQSQ +
146349.42 * EOC;
G532 = -40023.88 + 170470.89 * EQ - 242699.48 * EQSQ +
115605.82 * EOC;
}
SINI2 = SINIQ * SINIQ;
F220 = .75 * (1.0 + 2.0 * COSIQ + COSQ2);
F221 = 1.5 * SINI2;
F321 = 1.875 * SINIQ * (1.0 - 2.0 * COSIQ - 3.0 * COSQ2);
F322 = -1.875 * SINIQ * (1.0 + 2.0 * COSIQ - 3.0 * COSQ2);
F441 = 35.0 * SINI2 * F220;
F442 = 39.3750 * SINI2 * SINI2;
F522 = 9.84375 * SINIQ * (SINI2 * (1.0 - 2.0 * COSIQ - 5.0 * COSQ2) +
.33333333 * (-2.0 + 4.0 * COSIQ +
6.0 * COSQ2));
F523 = SINIQ * (4.92187512 * SINI2 * (-2.0 - 4.0 * COSIQ +
10.0 * COSQ2) +
6.56250012 * (1.0 +
2.0 * COSIQ -
3.0 * COSQ2));
F542 = 29.53125 * SINIQ * (2.0 - 8.0 * COSIQ +
COSQ2 * (-12.0 + 8.0 * COSIQ +
10.0 * COSQ2));
F543 = 29.53125 * SINIQ * (-2.0 - 8.0 * COSIQ +
COSQ2 * (12.0 + 8.0 * COSIQ -
10.0 * COSQ2));
XNO2 = XNQ * XNQ;
AINV2 = AQNV * AQNV;
TEMP1 = 3.0 * XNO2 * AINV2;
TEMP = TEMP1 * ROOT22;
D2201 = TEMP * F220 * G201;
D2211 = TEMP * F221 * G211;
TEMP1 = TEMP1 * AQNV;
TEMP = TEMP1 * ROOT32;
D3210 = TEMP * F321 * G310;
D3222 = TEMP * F322 * G322;
TEMP1 = TEMP1 * AQNV;
TEMP = 2.0 * TEMP1 * ROOT44;
D4410 = TEMP * F441 * G410;
D4422 = TEMP * F442 * G422;
TEMP1 = TEMP1 * AQNV;
TEMP = TEMP1 * ROOT52;
D5220 = TEMP * F522 * G520;
D5232 = TEMP * F523 * G532;
TEMP = 2.0 * TEMP1 * ROOT54;
D5421 = TEMP * F542* G521;
D5433 = TEMP * F543* G533;
XLAMO = XMAO + XNODEO + XNODEO - THGR - THGR;
BFACT = XLLDOT + XNODOT + XNODOT - THDT - THDT;
BFACT = BFACT + SSL + SSH + SSH;
} else {
/* SYNCHRONOUS RESONANCE TERMS INITIALIZATION */
IRESFL = 1;
ISYNFL = 1;
G200 = 1.0 + EQSQ * (-2.5 + .8125 * EQSQ);
G310 = 1.0 + 2.0 * EQSQ;
G300 = 1.0 + EQSQ * (-6.0 + 6.60937 * EQSQ);
F220 = .75 * (1.0 + COSIQ) * (1.0 + COSIQ);
F311 = .9375 * SINIQ * SINIQ * (1.0 + 3.0 * COSIQ) -
.75 * (1.0 + COSIQ);
F330 = 1.0 + COSIQ;
F330 = 1.875 * F330 * F330 * F330;
DEL1 = 3.0 * XNQ * XNQ * AQNV * AQNV;
DEL2 = 2.0 * DEL1 * F220 * G200 * Q22;
DEL3 = 3.0 * DEL1 * F330 * G300 * Q33 * AQNV;
DEL1 = DEL1 * F311 * G310 * Q31 * AQNV;
FASX2 = .13130908;
FASX4 = 2.8843198;
FASX6 = .37448087;
XLAMO = XMAO + XNODEO + OMEGAO - THGR;
BFACT = XLLDOT + XPIDOT - THDT;
BFACT = BFACT + SSL + SSG + SSH;
}
XFACT = BFACT - XNQ;
XLI = XLAMO;
XNI = XNQ;
ATIME = 0.0;
STEPP = 720.0;
STEPN = -720.0;
STEP2 = 259200.0;
}
int orbit::sgp(double* position, double* velocity, const double& t)
{
// The NORAD mean element sets can be used for prediction with SGP.
// All symbols not defined below are defined in the list of symbols in Section Twelve.
// Predictions are made by first calculating the constants.
double a1,d1,a0,p0,q0,L0;
double cosio,sinio;
double dW_dt,dw_dt;
cosio = cos(i0);
sinio = sin(i0);
a1 = pow(ke/n0, 2./3.);
d1 = 1.5*CK2/(a1*a1)*(3.*cosio*cosio-1.)/pow(1.- e0*e0,3./2.);
a0 = a1*(1. - (1./3.)*d1 - d1*d1 - (134./81.)*d1*d1*d1);
p0 = a0 * (1. - e0*e0);
q0 = a0 * (1. - e0);
L0 = fmod2p(M0 + w0 + W0);
#if 0
printf("a1 = %e\n",a1);
printf("d1 = %e\n",d1);
printf("a0 = %e\n",a0);
printf("p0 = %e\n",p0);
printf("q0 = %e\n",q0);
printf("L0 = %e\n",L0);
#endif
dW_dt = -3.*CK2/(p0*p0)*n0*cosio;
dw_dt = 1.5*CK2/(p0*p0)*n0*(5.*cosio*cosio-1.);
#if 0
printf("dW/dt = %e\n",dW_dt);
printf("dw/dt = %e\n",dw_dt);
#endif
// The secular effects of atmospheric drag and gravitation are included through the equations
double a,e,p;
double Ws0,ws0,Ls;
a = a0 * pow(n0/(n0 + 2.*(dn0/2.)*t + 3.*(ddn0/6.)*pow(t,2)),2./3.);
e = (a > q0)? 1-(q0/a): 1.E-6;
p = a*(1 - e*e);
#if 0
printf("a = %e\n",a);
printf("e = %e\n",e);
printf("p = %e\n",p);
#endif
Ws0 = fmod2p(W0 + dW_dt*t);
ws0 = fmod2p(w0 + dw_dt*t);
Ls = fmod2p(L0 + (n0+dw_dt+dW_dt)*t + (dn0/2.)*t*t + (ddn0/6.)*t*t*t);
#if 0
printf("Ws0 = %e\n",Ws0);
printf("ws0 = %e\n",ws0);
printf("Ls = %e\n",Ls);
#endif
// Long-period periodics are included through the equations
double L,axNSL,ayNSL;
axNSL = e*cos(ws0);
ayNSL = e*sin(ws0) - (1./2.)*(J3/J2)*(aE/p)*sinio;
#if 0
printf("axNSL = %e\n",axNSL);
printf("ayNSL = %e\n",ayNSL);
#endif
L = fmod2p(Ls - (1./4.)*(J3/J2)*(aE/p)*axNSL*sinio*((3.+5.*cosio)/(1.+cosio)));
#if 0
printf("L = %e\n",L);
#endif
// solve Kepler's equation
double U,Ew,dEw;
double cosEw,sinEw;
U = fmod2p(L - Ws0);
Ew = U;
dEw = 1.;
while(std::abs(dEw) >= E6A) {
cosEw = cos(Ew);
sinEw = sin(Ew);
dEw = (U - ayNSL*cosEw + axNSL*sinEw - Ew)/(-ayNSL*sinEw - axNSL*cosEw + 1.);
Ew += dEw;
}
#if 0
printf("U = %e\n",U);
printf("Ew = %e\n",Ew);
printf("dEw = %e\n",dEw);
#endif
// Then calculate the intermediate (partially osculating) quantities
double ecosE,esinE;
double eL2,pL,pL2,r,dr,rdv;
double u,sinu,cosu;
ecosE = axNSL*cos(Ew) + ayNSL*sin(Ew);
esinE = axNSL*sin(Ew) - ayNSL*cos(Ew);
#if 0
printf("e.cosE = %e\n",ecosE);
printf("e.sinE = %e\n",esinE);
#endif
eL2 = pow(axNSL,2) + pow(ayNSL,2);
pL = a*(1. - eL2);
pL2 = pL * pL;
r = a*(1. - ecosE);
dr = ke*(sqrt(a)/r)*esinE;
rdv = ke*(sqrt(pL)/r);
#if 0
printf("eL2 = %e\n",eL2);
printf("pL = %e\n",pL);
printf("pL2 = %e\n",pL2);
printf("r = %e\n",r);
printf("dr = %e\n",dr);
printf("rdv = %e\n",rdv);
#endif
sinu = (a/r)*(sin(Ew) - ayNSL - axNSL*esinE/(1.+sqrt(1.-eL2)));
cosu = (a/r)*(cos(Ew) - axNSL + ayNSL*esinE/(1.+sqrt(1.-eL2)));
#if 0
printf("cos u = %e\n",cosu);
printf("sin u = %e\n",sinu);
#endif
u = actan(sinu,cosu);
#if 0
printf("s/c = %e\n",sinu/cosu);
printf("u = %e\n",u);
#endif
// Short-period perturbations are now included by
double rk,uk,Wk,ik;
double sin2u,cos2u;
sin2u = sin(2.*u);
cos2u = cos(2.*u);
rk = r + .50 * CK2/pL * sinio*sinio*cos2u ;
uk = u - .25 * CK2/pL2 * (7.*cosio*cosio - 1.)*sin2u;
Wk = Ws0 + 1.5 * CK2/pL2 * cosio*sin2u;
ik = i0 + 1.5 * CK2/pL2 * sinio*cosio*cos2u;
#if 0
printf("rk = %e\n",rk);
printf("uk = %e\n",uk);
printf("Wk = %e\n",Wk);
printf("ik = %e\n",ik);
#endif
// Then unit orientation vectors are calculated by
double Mx,My,Mz;
double Nx,Ny,Nz;
double Ux,Uy,Uz;
double Vx,Vy,Vz;
Mx = -sin(Wk)*cos(ik);
My = cos(Wk)*cos(ik);
Mz = sin(ik);
Nx = cos(Wk);
Ny = sin(Wk);
Nz = 0.;
Ux = Mx*sin(uk) + Nx*cos(uk);
Uy = My*sin(uk) + Ny*cos(uk);
Uz = Mz*sin(uk) + Nz*cos(uk);
Vx = Mx*cos(uk) - Nx*sin(uk);
Vy = My*cos(uk) - Ny*sin(uk);
Vz = Mz*cos(uk) - Nz*sin(uk);
#if 0
printf("Ux = %e\n",Ux);
printf("Uy = %e\n",Uy);
printf("Uz = %e\n",Uz);
printf("Vx = %e\n",Vx);
printf("Vy = %e\n",Vy);
printf("Vz = %e\n",Vz);
#endif
// Then position and velocity are given by
position[0] = (rk*Ux) * XKMPER/AE;
position[1] = (rk*Uy) * XKMPER/AE;
position[2] = (rk*Uz) * XKMPER/AE;
velocity[0] = (dr*Ux + rdv*Vx) * XKMPER/AE * XMNPDA/86400;
velocity[1] = (dr*Uy + rdv*Vy) * XKMPER/AE * XMNPDA/86400;
velocity[2] = (dr*Uz + rdv*Vz) * XKMPER/AE * XMNPDA/86400;
#if 0
printf("X = %e\n",position[0]);
printf("Y = %e\n",position[1]);
printf("Z = %e\n",position[2]);
printf("XDOT = %e\n",velocity[0]);
printf("YDOT = %e\n",velocity[1]);
printf("ZDOT = %e\n",velocity[2]);
#endif
return 0;
}
