1 #define _XOPEN_SOURCE 600
2 #include <math.h>
3 #include <stdbool.h>
4 #include "sdp4.h"
5 #include "defs.h"
6 #include "unsorted.h"
7
8 /// Entry points of deep()
9 #define DPSecular 1
10 #define DPPeriodic 2
11
12 /**
13 * Initialize the fixed part of deep_arg.
14 *
15 * \param tle Orbital elements
16 * \param m Initialized sdp4 model
17 * \param deep_arg Fixed part of deep_arg
18 * \copyright GPLv2+
19 **/
20 void sdp4_deep_initialize(const predict_orbital_elements_t *tle, struct _sdp4 *m, deep_arg_fixed_t *deep_arg);
21
22 /**
23 * Initialize the dynamic part of deep_arg.
24 *
25 * \param m Initialized sdp4 model
26 * \param deep_dyn Dynamic part of deep_arg
27 * \copyright GPLv2+
28 **/
29 void deep_arg_dynamic_init(const struct _sdp4 *m, deep_arg_dynamic_t *deep_dyn);
30
sdp4_init(const predict_orbital_elements_t * tle,struct _sdp4 * m)31 void sdp4_init(const predict_orbital_elements_t *tle, struct _sdp4 *m)
32 {
33 m->lunarTermsDone = 0;
34 m->resonanceFlag = 0;
35 m->synchronousFlag = 0;
36
37 //Calculate old TLE field values as used in the original sdp4
38 double temp_tle = TWO_PI/MINUTES_PER_DAY/MINUTES_PER_DAY;
39 m->xnodeo = tle->right_ascension * M_PI / 180.0;
40 m->omegao = tle->argument_of_perigee * M_PI / 180.0;
41 m->xmo = tle->mean_anomaly * M_PI / 180.0;
42 m->xincl = tle->inclination * M_PI / 180.0;
43 m->eo = tle->eccentricity;
44 m->xno = tle->mean_motion*temp_tle*MINUTES_PER_DAY;
45 m->bstar = tle->bstar_drag_term / AE;
46 m->epoch = 1000.0*tle->epoch_year + tle->epoch_day;
47
48 /* Recover original mean motion (xnodp) and */
49 /* semimajor axis (aodp) from input elements. */
50 double temp1, temp2, temp3, theta4, a1, a3ovk2, ao, c2, coef, coef1, x1m5th, xhdot1, del1, delo, eeta, eta, etasq, perigee, psisq, tsi, qoms24, s4, pinvsq;
51
52 a1=pow(XKE/m->xno,TWO_THIRD);
53 m->deep_arg.cosio=cos(m->xincl);
54 m->deep_arg.theta2=m->deep_arg.cosio*m->deep_arg.cosio;
55 m->x3thm1=3*m->deep_arg.theta2-1;
56 m->deep_arg.eosq=m->eo*m->eo;
57 m->deep_arg.betao2=1-m->deep_arg.eosq;
58 m->deep_arg.betao=sqrt(m->deep_arg.betao2);
59 del1=1.5*CK2*m->x3thm1/(a1*a1*m->deep_arg.betao*m->deep_arg.betao2);
60 ao=a1*(1-del1*(0.5*TWO_THIRD+del1*(1+134/81*del1)));
61 delo=1.5*CK2*m->x3thm1/(ao*ao*m->deep_arg.betao*m->deep_arg.betao2);
62 m->deep_arg.xnodp=m->xno/(1+delo);
63 m->deep_arg.aodp=ao/(1-delo);
64
65 /* For perigee below 156 km, the values */
66 /* of s and qoms2t are altered. */
67
68 s4=S_DENSITY_PARAM;
69 qoms24=QOMS2T;
70 perigee=(m->deep_arg.aodp*(1-m->eo)-AE)*EARTH_RADIUS_KM_WGS84;
71
72 if (perigee<156.0)
73 {
74 if (perigee<=98.0)
75 s4=20.0;
76 else
77 s4=perigee-78.0;
78
79 qoms24=pow((120-s4)*AE/EARTH_RADIUS_KM_WGS84,4);
80 s4=s4/EARTH_RADIUS_KM_WGS84+AE;
81 }
82
83 pinvsq=1/(m->deep_arg.aodp*m->deep_arg.aodp*m->deep_arg.betao2*m->deep_arg.betao2);
84 m->deep_arg.sing=sin(m->omegao);
85 m->deep_arg.cosg=cos(m->omegao);
86 tsi=1/(m->deep_arg.aodp-s4);
87 eta=m->deep_arg.aodp*m->eo*tsi;
88 etasq=eta*eta;
89 eeta=m->eo*eta;
90 psisq=fabs(1-etasq);
91 coef=qoms24*pow(tsi,4);
92 coef1=coef/pow(psisq,3.5);
93 c2=coef1*m->deep_arg.xnodp*(m->deep_arg.aodp*(1+1.5*etasq+eeta*(4+etasq))+0.75*CK2*tsi/psisq*m->x3thm1*(8+3*etasq*(8+etasq)));
94 m->c1=m->bstar*c2;
95 m->deep_arg.sinio=sin(m->xincl);
96 a3ovk2=-J3_HARMONIC_WGS72/CK2*pow(AE,3);
97 m->x1mth2=1-m->deep_arg.theta2;
98 m->c4=2*m->deep_arg.xnodp*coef1*m->deep_arg.aodp*m->deep_arg.betao2*(eta*(2+0.5*etasq)+m->eo*(0.5+2*etasq)-2*CK2*tsi/(m->deep_arg.aodp*psisq)*(-3*m->x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*m->x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*m->omegao)));
99 theta4=m->deep_arg.theta2*m->deep_arg.theta2;
100 temp1=3*CK2*pinvsq*m->deep_arg.xnodp;
101 temp2=temp1*CK2*pinvsq;
102 temp3=1.25*CK4*pinvsq*pinvsq*m->deep_arg.xnodp;
103 m->deep_arg.xmdot=m->deep_arg.xnodp+0.5*temp1*m->deep_arg.betao*m->x3thm1+0.0625*temp2*m->deep_arg.betao*(13-78*m->deep_arg.theta2+137*theta4);
104 x1m5th=1-5*m->deep_arg.theta2;
105 m->deep_arg.omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*m->deep_arg.theta2+395*theta4)+temp3*(3-36*m->deep_arg.theta2+49*theta4);
106 xhdot1=-temp1*m->deep_arg.cosio;
107 m->deep_arg.xnodot=xhdot1+(0.5*temp2*(4-19*m->deep_arg.theta2)+2*temp3*(3-7*m->deep_arg.theta2))*m->deep_arg.cosio;
108 m->xnodcf=3.5*m->deep_arg.betao2*xhdot1*m->c1;
109 m->t2cof=1.5*m->c1;
110 m->xlcof=0.125*a3ovk2*m->deep_arg.sinio*(3+5*m->deep_arg.cosio)/(1+m->deep_arg.cosio);
111 m->aycof=0.25*a3ovk2*m->deep_arg.sinio;
112 m->x7thm1=7*m->deep_arg.theta2-1;
113
114 /* initialize Deep() */
115 sdp4_deep_initialize(tle, m, &(m->deep_arg));
116 }
117
sdp4_predict(const struct _sdp4 * m,double tsince,struct model_output * output)118 void sdp4_predict(const struct _sdp4 *m, double tsince, struct model_output *output)
119 {
120
121 int i;
122 double a, axn, ayn, aynl, beta, betal, capu, cos2u, cosepw, cosik,
123 cosnok, cosu, cosuk, ecose, elsq, epw, esine, pl,
124 rdot,
125 rdotk, rfdot, rfdotk, rk, sin2u, sinepw, sinik, sinnok, sinu,
126 sinuk, tempe, templ, tsq, u, uk, ux, uy, uz, vx, vy, vz, xl,
127 xlt, xmam, xmdf, xmx, xmy, xnoddf, xll,
128 r,
129 temp, tempa, temp1,
130 temp2, temp3, temp4, temp5, temp6;
131 double xnodek, xinck;
132
133 /* Initialize dynamic part of deep_arg */
134 deep_arg_dynamic_t deep_dyn;
135 deep_arg_dynamic_init(m, &deep_dyn);
136
137 /* Update for secular gravity and atmospheric drag */
138 xmdf=m->xmo+m->deep_arg.xmdot*tsince;
139 deep_dyn.omgadf=m->omegao+m->deep_arg.omgdot*tsince;
140 xnoddf=m->xnodeo+m->deep_arg.xnodot*tsince;
141 tsq=tsince*tsince;
142 deep_dyn.xnode=xnoddf+m->xnodcf*tsq;
143 tempa=1-m->c1*tsince;
144 tempe=m->bstar*m->c4*tsince;
145 templ=m->t2cof*tsq;
146 deep_dyn.xn=m->deep_arg.xnodp;
147
148 /* Update for deep-space secular effects */
149 deep_dyn.xll=xmdf;
150 deep_dyn.t=tsince;
151
152 sdp4_deep(m, DPSecular, &m->deep_arg, &deep_dyn);
153
154 xmdf=deep_dyn.xll;
155 a=pow(XKE/deep_dyn.xn,TWO_THIRD)*tempa*tempa;
156 deep_dyn.em=deep_dyn.em-tempe;
157 xmam=xmdf+m->deep_arg.xnodp*templ;
158
159 /* Update for deep-space periodic effects */
160 deep_dyn.xll=xmam;
161
162 sdp4_deep(m, DPPeriodic,&m->deep_arg, &deep_dyn);
163
164 xmam=deep_dyn.xll;
165 xl=xmam+deep_dyn.omgadf+deep_dyn.xnode;
166 beta=sqrt(1-deep_dyn.em*deep_dyn.em);
167 deep_dyn.xn=XKE/pow(a,1.5);
168
169 /* Long period periodics */
170 axn=deep_dyn.em*cos(deep_dyn.omgadf);
171 temp=1/(a*beta*beta);
172 xll=temp*m->xlcof*axn;
173 aynl=temp*m->aycof;
174 xlt=xl+xll;
175 ayn=deep_dyn.em*sin(deep_dyn.omgadf)+aynl;
176
177 /* Solve Kepler's Equation */
178 capu=FMod2p(xlt-deep_dyn.xnode);
179 temp2=capu;
180 i=0;
181
182 do
183 {
184 sinepw=sin(temp2);
185 cosepw=cos(temp2);
186 temp3=axn*sinepw;
187 temp4=ayn*cosepw;
188 temp5=axn*cosepw;
189 temp6=ayn*sinepw;
190 epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
191
192 if (fabs(epw-temp2)<=E6A)
193 break;
194
195 temp2=epw;
196
197 } while (i++<10);
198
199 /* Short period preliminary quantities */
200 ecose=temp5+temp6;
201 esine=temp3-temp4;
202 elsq=axn*axn+ayn*ayn;
203 temp=1-elsq;
204 pl=a*temp;
205 r=a*(1-ecose);
206 temp1=1/r;
207 rdot=XKE*sqrt(a)*esine*temp1;
208 rfdot=XKE*sqrt(pl)*temp1;
209 temp2=a*temp1;
210 betal=sqrt(temp);
211 temp3=1/(1+betal);
212 cosu=temp2*(cosepw-axn+ayn*esine*temp3);
213 sinu=temp2*(sinepw-ayn-axn*esine*temp3);
214 u=atan2(sinu,cosu);
215 sin2u=2*sinu*cosu;
216 cos2u=2*cosu*cosu-1;
217 temp=1/pl;
218 temp1=CK2*temp;
219 temp2=temp1*temp;
220
221 /* Update for short periodics */
222 rk=r*(1-1.5*temp2*betal*m->x3thm1)+0.5*temp1*m->x1mth2*cos2u;
223 uk=u-0.25*temp2*m->x7thm1*sin2u;
224 xnodek=deep_dyn.xnode+1.5*temp2*m->deep_arg.cosio*sin2u;
225 xinck=deep_dyn.xinc+1.5*temp2*m->deep_arg.cosio*m->deep_arg.sinio*cos2u;
226 rdotk=rdot-deep_dyn.xn*temp1*m->x1mth2*sin2u;
227 rfdotk=rfdot+deep_dyn.xn*temp1*(m->x1mth2*cos2u+1.5*m->x3thm1);
228
229 /* Orientation vectors */
230 sinuk=sin(uk);
231 cosuk=cos(uk);
232 sinik=sin(xinck);
233 cosik=cos(xinck);
234 sinnok=sin(xnodek);
235 cosnok=cos(xnodek);
236 xmx=-sinnok*cosik;
237 xmy=cosnok*cosik;
238 ux=xmx*sinuk+cosnok*cosuk;
239 uy=xmy*sinuk+sinnok*cosuk;
240 uz=sinik*sinuk;
241 vx=xmx*cosuk-cosnok*sinuk;
242 vy=xmy*cosuk-sinnok*sinuk;
243 vz=sinik*cosuk;
244
245 /* Position and velocity */
246 output->pos[0] = rk*ux;
247 output->pos[1] = rk*uy;
248 output->pos[2] = rk*uz;
249 output->vel[0] = rdotk*ux+rfdotk*vx;
250 output->vel[1] = rdotk*uy+rfdotk*vy;
251 output->vel[2] = rdotk*uz+rfdotk*vz;
252
253 /* Phase in radians */
254 double phase=xlt-deep_dyn.xnode-deep_dyn.omgadf+TWO_PI;
255
256 if (phase<0.0)
257 phase+=TWO_PI;
258
259 phase=FMod2p(phase);
260 output->phase = phase;
261
262 output->omgadf = deep_dyn.omgadf;
263 output->xnodek = xnodek;
264 output->xinck = xinck;
265 }
266
267 /**
268 * Calculates the Greenwich Mean Sidereal Time
269 * for an epoch specified in the format used in the NORAD two-line
270 * element sets.
271 * It has been adapted for dates beyond the year 1999.
272 * Reference: The 1992 Astronomical Almanac, page B6.
273 * Modification to support Y2K. Valid 1957 through 2056.
274 *
275 * \param epoch TLE epoch
276 * \param deep_arg Deep arg
277 * \copyright GPLv2+
278 **/
ThetaG(double epoch,deep_arg_fixed_t * deep_arg)279 double ThetaG(double epoch, deep_arg_fixed_t *deep_arg)
280 {
281 double year, day, UT, jd, TU, GMST, ThetaG;
282
283 /* Modification to support Y2K */
284 /* Valid 1957 through 2056 */
285
286 day=modf(epoch*1E-3,&year)*1E3;
287
288 if (year<57)
289 year+=2000;
290 else
291 year+=1900;
292
293 UT=modf(day,&day);
294 jd=Julian_Date_of_Year(year)+day;
295 TU=(jd-2451545.0)/36525;
296 GMST=24110.54841+TU*(8640184.812866+TU*(0.093104-TU*6.2E-6));
297 GMST=fmod(GMST+SECONDS_PER_DAY*EARTH_ROTATIONS_PER_SIDERIAL_DAY*UT,SECONDS_PER_DAY);
298 ThetaG = 2*M_PI*GMST/SECONDS_PER_DAY;
299 deep_arg->ds50=jd-2433281.5+UT;
300 ThetaG=FMod2p(6.3003880987*deep_arg->ds50+1.72944494);
301
302 return ThetaG;
303 }
304
sdp4_deep_initialize(const predict_orbital_elements_t * tle,struct _sdp4 * m,deep_arg_fixed_t * deep_arg)305 void sdp4_deep_initialize(const predict_orbital_elements_t *tle, struct _sdp4 *m, deep_arg_fixed_t *deep_arg)
306 {
307 double a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, ainv2, aqnv,
308 sgh, sini2, sh, si, day, bfact, c,
309 cc, cosq, ctem, f322, zx, zy, eoc, eq,
310 f220, f221, f311, f321, f330, f441, f442, f522, f523,
311 f542, f543, g200, g201, g211, s1, s2, s3, s4, s5, s6, s7,
312 se, g300, g310, g322, g410, g422, g520, g521, g532,
313 g533, gam, sinq, sl, stem, temp, temp1, x1,
314 x2, x3, x4, x5, x6, x7, x8, xmao,
315 xno2, xnodce, xnoi, xpidot, z1, z11, z12, z13, z2,
316 z21, z22, z23, z3, z31, z32, z33, ze, zn, zsing,
317 zsinh, zsini, zcosg, zcosh, zcosi;
318
319 /* Entrance for deep space initialization */
320 m->thgr=ThetaG(m->epoch,deep_arg);
321 eq=m->eo;
322 m->xnq=deep_arg->xnodp;
323 aqnv=1/deep_arg->aodp;
324 m->xqncl=m->xincl;
325 xmao=m->xmo;
326 xpidot=deep_arg->omgdot+deep_arg->xnodot;
327 sinq=sin(m->xnodeo);
328 cosq=cos(m->xnodeo);
329 m->omegaq=m->omegao;
330
331 /* Initialize lunar solar terms */
332 day=deep_arg->ds50+18261.5; /* Days since 1900 Jan 0.5 */
333
334 m->preep=day;
335 xnodce=4.5236020-9.2422029E-4*day;
336 stem=sin(xnodce);
337 ctem=cos(xnodce);
338 m->zcosil=0.91375164-0.03568096*ctem;
339 m->zsinil=sqrt(1-m->zcosil*m->zcosil);
340 m->zsinhl=0.089683511*stem/m->zsinil;
341 m->zcoshl=sqrt(1-m->zsinhl*m->zsinhl);
342 c=4.7199672+0.22997150*day;
343 gam=5.8351514+0.0019443680*day;
344 m->zmol=FMod2p(c-gam);
345 zx=0.39785416*stem/m->zsinil;
346 zy=m->zcoshl*ctem+0.91744867*m->zsinhl*stem;
347 zx=atan2(zx,zy);
348 zx=gam+zx-xnodce;
349 m->zcosgl=cos(zx);
350 m->zsingl=sin(zx);
351 m->zmos=6.2565837+0.017201977*day;
352 m->zmos=FMod2p(m->zmos);
353
354 /* Do solar terms */
355 zcosg=ZCOSGS;
356 zsing=ZSINGS;
357 zcosi=ZCOSIS;
358 zsini=ZSINIS;
359 zcosh=cosq;
360 zsinh= sinq;
361 cc=C1SS;
362 zn=ZNS;
363 ze=ZES;
364 /* zmo=m->zmos; */
365 xnoi=1/m->xnq;
366
367 /* Loop breaks when Solar terms are done a second */
368 /* time, after Lunar terms are initialized */
369
370 for (;;)
371 {
372 /* Solar terms done again after Lunar terms are done */
373 a1=zcosg*zcosh+zsing*zcosi*zsinh;
374 a3=-zsing*zcosh+zcosg*zcosi*zsinh;
375 a7=-zcosg*zsinh+zsing*zcosi*zcosh;
376 a8=zsing*zsini;
377 a9=zsing*zsinh+zcosg*zcosi*zcosh;
378 a10=zcosg*zsini;
379 a2=deep_arg->cosio*a7+deep_arg->sinio*a8;
380 a4=deep_arg->cosio*a9+deep_arg->sinio*a10;
381 a5=-deep_arg->sinio*a7+deep_arg->cosio*a8;
382 a6=-deep_arg->sinio*a9+deep_arg->cosio*a10;
383 x1=a1*deep_arg->cosg+a2*deep_arg->sing;
384 x2=a3*deep_arg->cosg+a4*deep_arg->sing;
385 x3=-a1*deep_arg->sing+a2*deep_arg->cosg;
386 x4=-a3*deep_arg->sing+a4*deep_arg->cosg;
387 x5=a5*deep_arg->sing;
388 x6=a6*deep_arg->sing;
389 x7=a5*deep_arg->cosg;
390 x8=a6*deep_arg->cosg;
391 z31=12*x1*x1-3*x3*x3;
392 z32=24*x1*x2-6*x3*x4;
393 z33=12*x2*x2-3*x4*x4;
394 z1=3*(a1*a1+a2*a2)+z31*deep_arg->eosq;
395 z2=6*(a1*a3+a2*a4)+z32*deep_arg->eosq;
396 z3=3*(a3*a3+a4*a4)+z33*deep_arg->eosq;
397 z11=-6*a1*a5+deep_arg->eosq*(-24*x1*x7-6*x3*x5);
398 z12=-6*(a1*a6+a3*a5)+deep_arg->eosq*(-24*(x2*x7+x1*x8)-6*(x3*x6+x4*x5));
399 z13=-6*a3*a6+deep_arg->eosq*(-24*x2*x8-6*x4*x6);
400 z21=6*a2*a5+deep_arg->eosq*(24*x1*x5-6*x3*x7);
401 z22=6*(a4*a5+a2*a6)+deep_arg->eosq*(24*(x2*x5+x1*x6)-6*(x4*x7+x3*x8));
402 z23=6*a4*a6+deep_arg->eosq*(24*x2*x6-6*x4*x8);
403 z1=z1+z1+deep_arg->betao2*z31;
404 z2=z2+z2+deep_arg->betao2*z32;
405 z3=z3+z3+deep_arg->betao2*z33;
406 s3=cc*xnoi;
407 s2=-0.5*s3/deep_arg->betao;
408 s4=s3*deep_arg->betao;
409 s1=-15*eq*s4;
410 s5=x1*x3+x2*x4;
411 s6=x2*x3+x1*x4;
412 s7=x2*x4-x1*x3;
413 se=s1*zn*s5;
414 si=s2*zn*(z11+z13);
415 sl=-zn*s3*(z1+z3-14-6*deep_arg->eosq);
416 sgh=s4*zn*(z31+z33-6);
417 sh=-zn*s2*(z21+z23);
418
419 if (m->xqncl<5.2359877E-2)
420 sh=0;
421
422 m->ee2=2*s1*s6;
423 m->e3=2*s1*s7;
424 m->xi2=2*s2*z12;
425 m->xi3=2*s2*(z13-z11);
426 m->xl2=-2*s3*z2;
427 m->xl3=-2*s3*(z3-z1);
428 m->xl4=-2*s3*(-21-9*deep_arg->eosq)*ze;
429 m->xgh2=2*s4*z32;
430 m->xgh3=2*s4*(z33-z31);
431 m->xgh4=-18*s4*ze;
432 m->xh2=-2*s2*z22;
433 m->xh3=-2*s2*(z23-z21);
434
435 //Skip lunar terms?
436 if (m->lunarTermsDone) {
437 break;
438 }
439
440 /* Do lunar terms */
441 m->sse=se;
442 m->ssi=si;
443 m->ssl=sl;
444 m->ssh=sh/deep_arg->sinio;
445 m->ssg=sgh-deep_arg->cosio*m->ssh;
446 m->se2=m->ee2;
447 m->si2=m->xi2;
448 m->sl2=m->xl2;
449 m->sgh2=m->xgh2;
450 m->sh2=m->xh2;
451 m->se3=m->e3;
452 m->si3=m->xi3;
453 m->sl3=m->xl3;
454 m->sgh3=m->xgh3;
455 m->sh3=m->xh3;
456 m->sl4=m->xl4;
457 m->sgh4=m->xgh4;
458 zcosg=m->zcosgl;
459 zsing=m->zsingl;
460 zcosi=m->zcosil;
461 zsini=m->zsinil;
462 zcosh=m->zcoshl*cosq+m->zsinhl*sinq;
463 zsinh=sinq*m->zcoshl-cosq*m->zsinhl;
464 zn=ZNL;
465 cc=C1L;
466 ze=ZEL;
467 /* zmo=m->zmol; */
468 //Set lunarTermsDone flag:
469 m->lunarTermsDone = true;
470 }
471
472 m->sse=m->sse+se;
473 m->ssi=m->ssi+si;
474 m->ssl=m->ssl+sl;
475 m->ssg=m->ssg+sgh-deep_arg->cosio/deep_arg->sinio*sh;
476 m->ssh=m->ssh+sh/deep_arg->sinio;
477
478 /* Geopotential resonance initialization for 12 hour orbits */
479 m->resonanceFlag = 0;
480 m->synchronousFlag = 0;
481
482 if (!((m->xnq<0.0052359877) && (m->xnq>0.0034906585)))
483 {
484 if ((m->xnq<0.00826) || (m->xnq>0.00924))
485 return;
486
487 if (eq<0.5)
488 return;
489
490 m->resonanceFlag = 1;
491 eoc=eq*deep_arg->eosq;
492 g201=-0.306-(eq-0.64)*0.440;
493
494 if (eq<=0.65)
495 {
496 g211=3.616-13.247*eq+16.290*deep_arg->eosq;
497 g310=-19.302+117.390*eq-228.419*deep_arg->eosq+156.591*eoc;
498 g322=-18.9068+109.7927*eq-214.6334*deep_arg->eosq+146.5816*eoc;
499 g410=-41.122+242.694*eq-471.094*deep_arg->eosq+313.953*eoc;
500 g422=-146.407+841.880*eq-1629.014*deep_arg->eosq+1083.435 * eoc;
501 g520=-532.114+3017.977*eq-5740*deep_arg->eosq+3708.276*eoc;
502 }
503
504 else
505 {
506 g211=-72.099+331.819*eq-508.738*deep_arg->eosq+266.724*eoc;
507 g310=-346.844+1582.851*eq-2415.925*deep_arg->eosq+1246.113*eoc;
508 g322=-342.585+1554.908*eq-2366.899*deep_arg->eosq+1215.972*eoc;
509 g410=-1052.797+4758.686*eq-7193.992*deep_arg->eosq+3651.957*eoc;
510 g422=-3581.69+16178.11*eq-24462.77*deep_arg->eosq+12422.52*eoc;
511
512 if (eq<=0.715)
513 g520=1464.74-4664.75*eq+3763.64*deep_arg->eosq;
514
515 else
516 g520=-5149.66+29936.92*eq-54087.36*deep_arg->eosq+31324.56*eoc;
517 }
518
519 if (eq<0.7)
520 {
521 g533=-919.2277+4988.61*eq-9064.77*deep_arg->eosq+5542.21*eoc;
522 g521=-822.71072+4568.6173*eq-8491.4146*deep_arg->eosq+5337.524*eoc;
523 g532=-853.666+4690.25*eq-8624.77*deep_arg->eosq+5341.4*eoc;
524 }
525
526 else
527 {
528 g533=-37995.78+161616.52*eq-229838.2*deep_arg->eosq+109377.94*eoc;
529 g521 =-51752.104+218913.95*eq-309468.16*deep_arg->eosq+146349.42*eoc;
530 g532 =-40023.88+170470.89*eq-242699.48*deep_arg->eosq+115605.82*eoc;
531 }
532
533 sini2=deep_arg->sinio*deep_arg->sinio;
534 f220=0.75*(1+2*deep_arg->cosio+deep_arg->theta2);
535 f221=1.5*sini2;
536 f321=1.875*deep_arg->sinio*(1-2*deep_arg->cosio-3*deep_arg->theta2);
537 f322=-1.875*deep_arg->sinio*(1+2*deep_arg->cosio-3*deep_arg->theta2);
538 f441=35*sini2*f220;
539 f442=39.3750*sini2*sini2;
540 f522=9.84375*deep_arg->sinio*(sini2*(1-2*deep_arg->cosio-5*deep_arg->theta2)+0.33333333*(-2+4*deep_arg->cosio+6*deep_arg->theta2));
541 f523=deep_arg->sinio*(4.92187512*sini2*(-2-4*deep_arg->cosio+10*deep_arg->theta2)+6.56250012*(1+2*deep_arg->cosio-3*deep_arg->theta2));
542 f542=29.53125*deep_arg->sinio*(2-8*deep_arg->cosio+deep_arg->theta2*(-12+8*deep_arg->cosio+10*deep_arg->theta2));
543 f543=29.53125*deep_arg->sinio*(-2-8*deep_arg->cosio+deep_arg->theta2*(12+8*deep_arg->cosio-10*deep_arg->theta2));
544 xno2=m->xnq*m->xnq;
545 ainv2=aqnv*aqnv;
546 temp1=3*xno2*ainv2;
547 temp=temp1*ROOT22;
548 m->d2201=temp*f220*g201;
549 m->d2211=temp*f221*g211;
550 temp1=temp1*aqnv;
551 temp=temp1*ROOT32;
552 m->d3210=temp*f321*g310;
553 m->d3222=temp*f322*g322;
554 temp1=temp1*aqnv;
555 temp=2*temp1*ROOT44;
556 m->d4410=temp*f441*g410;
557 m->d4422=temp*f442*g422;
558 temp1=temp1*aqnv;
559 temp=temp1*ROOT52;
560 m->d5220=temp*f522*g520;
561 m->d5232=temp*f523*g532;
562 temp=2*temp1*ROOT54;
563 m->d5421=temp*f542*g521;
564 m->d5433=temp*f543*g533;
565 m->xlamo=xmao+m->xnodeo+m->xnodeo-m->thgr-m->thgr;
566 bfact=deep_arg->xmdot+deep_arg->xnodot+deep_arg->xnodot-THDT-THDT;
567 bfact=bfact+m->ssl+m->ssh+m->ssh;
568 }
569
570 else
571 {
572 m->resonanceFlag = 1;
573 m->synchronousFlag = 1;
574
575 /* Synchronous resonance terms initialization */
576 g200=1+deep_arg->eosq*(-2.5+0.8125*deep_arg->eosq);
577 g310=1+2*deep_arg->eosq;
578 g300=1+deep_arg->eosq*(-6+6.60937*deep_arg->eosq);
579 f220=0.75*(1+deep_arg->cosio)*(1+deep_arg->cosio);
580 f311=0.9375*deep_arg->sinio*deep_arg->sinio*(1+3*deep_arg->cosio)-0.75*(1+deep_arg->cosio);
581 f330=1+deep_arg->cosio;
582 f330=1.875*f330*f330*f330;
583 m->del1=3*m->xnq*m->xnq*aqnv*aqnv;
584 m->del2=2*m->del1*f220*g200*Q22;
585 m->del3=3*m->del1*f330*g300*Q33*aqnv;
586 m->del1=m->del1*f311*g310*Q31*aqnv;
587 m->fasx2=0.13130908;
588 m->fasx4=2.8843198;
589 m->fasx6=0.37448087;
590 m->xlamo=xmao+m->xnodeo+m->omegao-m->thgr;
591 bfact=deep_arg->xmdot+xpidot-THDT;
592 bfact=bfact+m->ssl+m->ssg+m->ssh;
593 }
594
595 m->xfact=bfact-m->xnq;
596
597 /* Initialize integrator */
598 m->stepp=720;
599 m->stepn=-720;
600 m->step2=259200;
601
602 return;
603 }
604
deep_arg_dynamic_init(const struct _sdp4 * m,deep_arg_dynamic_t * deep_dyn)605 void deep_arg_dynamic_init(const struct _sdp4 *m, deep_arg_dynamic_t *deep_dyn){
606 deep_dyn->savtsn=1E20;
607 deep_dyn->loopFlag = 0;
608 deep_dyn->epochRestartFlag = 0;
609 deep_dyn->xli=m->xlamo;
610 deep_dyn->xni=m->xnq;
611 deep_dyn->atime=0;
612 }
613
sdp4_deep(const struct _sdp4 * m,int ientry,const deep_arg_fixed_t * deep_arg,deep_arg_dynamic_t * deep_dyn)614 void sdp4_deep(const struct _sdp4 *m, int ientry, const deep_arg_fixed_t * deep_arg, deep_arg_dynamic_t *deep_dyn)
615 {
616 /* This function is used by SDP4 to add lunar and solar */
617 /* perturbation effects to deep-space orbit objects. */
618
619 double alfdp,
620 sinis, sinok, sil, betdp, dalf, cosis, cosok, dbet, dls, f2,
621 f3, xnoh, pgh, ph, sel, ses, xls, sinzf, sis, sll, sls, temp,
622 x2li, x2omi, xl, xldot, xnddt,
623 xndot, xomi, zf, zm,
624 delt=0, ft=0;
625
626
627 switch (ientry)
628 {
629
630 case DPSecular: /* Entrance for deep space secular effects */
631
632 deep_dyn->xll=deep_dyn->xll+m->ssl*deep_dyn->t;
633 deep_dyn->omgadf=deep_dyn->omgadf+m->ssg*deep_dyn->t;
634 deep_dyn->xnode=deep_dyn->xnode+m->ssh*deep_dyn->t;
635 deep_dyn->em=m->eo+m->sse*deep_dyn->t;
636 deep_dyn->xinc=m->xincl+m->ssi*deep_dyn->t;
637
638 if (deep_dyn->xinc<0)
639 {
640 deep_dyn->xinc=-deep_dyn->xinc;
641 deep_dyn->xnode=deep_dyn->xnode+PI;
642 deep_dyn->omgadf=deep_dyn->omgadf-PI;
643 }
644
645 if (!m->resonanceFlag) {
646 return;
647 }
648
649 do
650 {
651 if ((deep_dyn->atime==0) || ((deep_dyn->t>=0) && (deep_dyn->atime<0)) || ((deep_dyn->t<0) && (deep_dyn->atime>=0)))
652 {
653 /* Epoch restart */
654
655 if (deep_dyn->t>=0)
656 delt=m->stepp;
657 else
658 delt=m->stepn;
659
660 deep_dyn->atime=0;
661 deep_dyn->xni=m->xnq;
662 deep_dyn->xli=m->xlamo;
663 }
664
665 else
666 {
667 if (fabs(deep_dyn->t)>=fabs(deep_dyn->atime))
668 {
669 if (deep_dyn->t>0)
670 delt=m->stepp;
671 else
672 delt=m->stepn;
673 }
674 }
675
676 do
677 {
678 if (fabs(deep_dyn->t-deep_dyn->atime)>=m->stepp)
679 {
680 deep_dyn->loopFlag = 1;
681 deep_dyn->epochRestartFlag = 0;
682 }
683
684 else
685 {
686 ft=deep_dyn->t-deep_dyn->atime;
687 deep_dyn->loopFlag = 0;
688 }
689
690 if (fabs(deep_dyn->t)<fabs(deep_dyn->atime))
691 {
692 if (deep_dyn->t>=0)
693 delt=m->stepn;
694 else
695 delt=m->stepp;
696
697 deep_dyn->loopFlag = 1;
698 deep_dyn->epochRestartFlag = 1;
699 }
700
701 /* Dot terms calculated */
702 if (m->synchronousFlag) {
703 xndot=m->del1*sin(deep_dyn->xli-m->fasx2)+m->del2*sin(2*(deep_dyn->xli-m->fasx4))+m->del3*sin(3*(deep_dyn->xli-m->fasx6));
704 xnddt=m->del1*cos(deep_dyn->xli-m->fasx2)+2*m->del2*cos(2*(deep_dyn->xli-m->fasx4))+3*m->del3*cos(3*(deep_dyn->xli-m->fasx6));
705 }
706
707 else
708 {
709 xomi=m->omegaq+deep_arg->omgdot*deep_dyn->atime;
710 x2omi=xomi+xomi;
711 x2li=deep_dyn->xli+deep_dyn->xli;
712 xndot=m->d2201*sin(x2omi+deep_dyn->xli-G22)+m->d2211*sin(deep_dyn->xli-G22)+m->d3210*sin(xomi+deep_dyn->xli-G32)+m->d3222*sin(-xomi+deep_dyn->xli-G32)+m->d4410*sin(x2omi+x2li-G44)+m->d4422*sin(x2li-G44)+m->d5220*sin(xomi+deep_dyn->xli-G52)+m->d5232*sin(-xomi+deep_dyn->xli-G52)+m->d5421*sin(xomi+x2li-G54)+m->d5433*sin(-xomi+x2li-G54);
713 xnddt=m->d2201*cos(x2omi+deep_dyn->xli-G22)+m->d2211*cos(deep_dyn->xli-G22)+m->d3210*cos(xomi+deep_dyn->xli-G32)+m->d3222*cos(-xomi+deep_dyn->xli-G32)+m->d5220*cos(xomi+deep_dyn->xli-G52)+m->d5232*cos(-xomi+deep_dyn->xli-G52)+2*(m->d4410*cos(x2omi+x2li-G44)+m->d4422*cos(x2li-G44)+m->d5421*cos(xomi+x2li-G54)+m->d5433*cos(-xomi+x2li-G54));
714 }
715
716 xldot=deep_dyn->xni+m->xfact;
717 xnddt=xnddt*xldot;
718
719 if (deep_dyn->loopFlag) {
720 deep_dyn->xli=deep_dyn->xli+xldot*delt+xndot*m->step2;
721 deep_dyn->xni=deep_dyn->xni+xndot*delt+xnddt*m->step2;
722 deep_dyn->atime=deep_dyn->atime+delt;
723 }
724 } while (deep_dyn->loopFlag && !deep_dyn->epochRestartFlag);
725 } while (deep_dyn->loopFlag && deep_dyn->epochRestartFlag);
726
727 deep_dyn->xn=deep_dyn->xni+xndot*ft+xnddt*ft*ft*0.5;
728 xl=deep_dyn->xli+xldot*ft+xndot*ft*ft*0.5;
729 temp=-deep_dyn->xnode+m->thgr+deep_dyn->t*THDT;
730
731 if (!m->synchronousFlag) {
732 deep_dyn->xll=xl+temp+temp;
733 }else{
734 deep_dyn->xll=xl-deep_dyn->omgadf+temp;
735 }
736
737 return;
738
739 case DPPeriodic: /* Entrance for lunar-solar periodics */
740 sinis=sin(deep_dyn->xinc);
741 cosis=cos(deep_dyn->xinc);
742
743 if (fabs(deep_dyn->savtsn-deep_dyn->t)>=30)
744 {
745 deep_dyn->savtsn=deep_dyn->t;
746 zm=m->zmos+ZNS*deep_dyn->t;
747 zf=zm+2*ZES*sin(zm);
748 sinzf=sin(zf);
749 f2=0.5*sinzf*sinzf-0.25;
750 f3=-0.5*sinzf*cos(zf);
751 ses=m->se2*f2+m->se3*f3;
752 sis=m->si2*f2+m->si3*f3;
753 sls=m->sl2*f2+m->sl3*f3+m->sl4*sinzf;
754 deep_dyn->sghs=m->sgh2*f2+m->sgh3*f3+m->sgh4*sinzf;
755 deep_dyn->shs=m->sh2*f2+m->sh3*f3;
756 zm=m->zmol+ZNL*deep_dyn->t;
757 zf=zm+2*ZEL*sin(zm);
758 sinzf=sin(zf);
759 f2=0.5*sinzf*sinzf-0.25;
760 f3=-0.5*sinzf*cos(zf);
761 sel=m->ee2*f2+m->e3*f3;
762 sil=m->xi2*f2+m->xi3*f3;
763 sll=m->xl2*f2+m->xl3*f3+m->xl4*sinzf;
764 deep_dyn->sghl=m->xgh2*f2+m->xgh3*f3+m->xgh4*sinzf;
765 deep_dyn->sh1=m->xh2*f2+m->xh3*f3;
766 deep_dyn->pe=ses+sel;
767 deep_dyn->pinc=sis+sil;
768 deep_dyn->pl=sls+sll;
769 }
770
771 pgh=deep_dyn->sghs+deep_dyn->sghl;
772 ph=deep_dyn->shs+deep_dyn->sh1;
773 deep_dyn->xinc=deep_dyn->xinc+deep_dyn->pinc;
774 deep_dyn->em=deep_dyn->em+deep_dyn->pe;
775
776 if (m->xqncl>=0.2)
777 {
778 /* Apply periodics directly */
779 ph=ph/deep_arg->sinio;
780 pgh=pgh-deep_arg->cosio*ph;
781 deep_dyn->omgadf=deep_dyn->omgadf+pgh;
782 deep_dyn->xnode=deep_dyn->xnode+ph;
783 deep_dyn->xll=deep_dyn->xll+deep_dyn->pl;
784 }
785
786 else
787 {
788 /* Apply periodics with Lyddane modification */
789 sinok=sin(deep_dyn->xnode);
790 cosok=cos(deep_dyn->xnode);
791 alfdp=sinis*sinok;
792 betdp=sinis*cosok;
793 dalf=ph*cosok+deep_dyn->pinc*cosis*sinok;
794 dbet=-ph*sinok+deep_dyn->pinc*cosis*cosok;
795 alfdp=alfdp+dalf;
796 betdp=betdp+dbet;
797 deep_dyn->xnode=FMod2p(deep_dyn->xnode);
798 xls=deep_dyn->xll+deep_dyn->omgadf+cosis*deep_dyn->xnode;
799 dls=deep_dyn->pl+pgh-deep_dyn->pinc*deep_dyn->xnode*sinis;
800 xls=xls+dls;
801 xnoh=deep_dyn->xnode;
802 deep_dyn->xnode=atan2(alfdp,betdp);
803
804 /* This is a patch to Lyddane modification */
805 /* suggested by Rob Matson. */
806
807 if (fabs(xnoh-deep_dyn->xnode)>PI)
808 {
809 if (deep_dyn->xnode<xnoh)
810 deep_dyn->xnode+=TWO_PI;
811 else
812 deep_dyn->xnode-=TWO_PI;
813 }
814
815 deep_dyn->xll=deep_dyn->xll+deep_dyn->pl;
816 deep_dyn->omgadf=xls-deep_dyn->xll-cos(deep_dyn->xinc)*deep_dyn->xnode;
817 }
818 return;
819 }
820 }
821
822