1 /*     ----------------------------------------------------------------
2 *
3 *                               sgp4unit.cpp
4 *
5 *    this file contains the sgp4 procedures for analytical propagation
6 *    of a satellite. the code was originally released in the 1980 and 1986
7 *    spacetrack papers. a detailed discussion of the theory and history
8 *    may be found in the 2006 aiaa paper by vallado, crawford, hujsak,
9 *    and kelso.
10 *
11 *                            companion code for
12 *               fundamentals of astrodynamics and applications
13 *                                    2007
14 *                              by david vallado
15 *
16 *       (w) 719-573-2600, email dvallado@agi.com
17 *
18 *    current :
19 *               3 Nov 08  david vallado
20 *                           put returns in for error codes
21 *    changes :
22 *              29 sep 08  david vallado
23 *                           fix atime for faster operation in dspace
24 *                           add operationmode for afspc (a) or improved (i)
25 *                           performance mode
26 *              16 jun 08  david vallado
27 *                           update small eccentricity check
28 *              16 nov 07  david vallado
29 *                           misc fixes for better compliance
30 *              20 apr 07  david vallado
31 *                           misc fixes for constants
32 *              11 aug 06  david vallado
33 *                           chg lyddane choice back to strn3, constants, misc doc
34 *              15 dec 05  david vallado
35 *                           misc fixes
36 *              26 jul 05  david vallado
37 *                           fixes for paper
38 *                           note that each fix is preceded by a
39 *                           comment with "sgp4fix" and an explanation of
40 *                           what was changed
41 *              10 aug 04  david vallado
42 *                           2nd printing baseline working
43 *              14 may 01  david vallado
44 *                           2nd edition baseline
45 *                     80  norad
46 *                           original baseline
47 *       ----------------------------------------------------------------      */
48 
49 #include "sgp4unit.h"
50 #include <cmath>
51 
52 //FILE *dbgfile;
53 
54 
55 /* ----------- local functions - only ever used internally by sgp4 ---------- */
56 static void dpper(double e3,     double ee2,    double peo,     double pgho,   double pho,
57 		  double pinco,  double plo,    double se2,     double se3,    double sgh2,
58 		  double sgh3,   double sgh4,   double sh2,     double sh3,    double si2,
59 		  double si3,    double sl2,    double sl3,     double sl4,    double t,
60 		  double xgh2,   double xgh3,   double xgh4,    double xh2,    double xh3,
61 		  double xi2,    double xi3,    double xl2,     double xl3,    double xl4,
62 		  double zmol,   double zmos,   double inclo,
63 		  char init,
64 		  double& ep,    double& inclp, double& nodep,  double& argpp, double& mp,
65 		  char opsmode);
66 
67 static void dscom(double epoch,  double ep,     double argpp,   double tc,     double inclp,
68 		  double nodep,  double np,
69 		  double& snodm, double& cnodm, double& sinim,  double& cosim, double& sinomm,
70 		  double& cosomm,double& day,   double& e3,     double& ee2,   double& em,
71 		  double& emsq,  double& gam,   double& peo,    double& pgho,  double& pho,
72 		  double& pinco, double& plo,   double& rtemsq, double& se2,   double& se3,
73 		  double& sgh2,  double& sgh3,  double& sgh4,   double& sh2,   double& sh3,
74 		  double& si2,   double& si3,   double& sl2,    double& sl3,   double& sl4,
75 		  double& s1,    double& s2,    double& s3,     double& s4,    double& s5,
76 		  double& s6,    double& s7,    double& ss1,    double& ss2,   double& ss3,
77 		  double& ss4,   double& ss5,   double& ss6,    double& ss7,   double& sz1,
78 		  double& sz2,   double& sz3,   double& sz11,   double& sz12,  double& sz13,
79 		  double& sz21,  double& sz22,  double& sz23,   double& sz31,  double& sz32,
80 		  double& sz33,  double& xgh2,  double& xgh3,   double& xgh4,  double& xh2,
81 		  double& xh3,   double& xi2,   double& xi3,    double& xl2,   double& xl3,
82 		  double& xl4,   double& nm,    double& z1,     double& z2,    double& z3,
83 		  double& z11,   double& z12,   double& z13,    double& z21,   double& z22,
84 		  double& z23,   double& z31,   double& z32,    double& z33,   double& zmol,
85 		  double& zmos);
86 
87 static void dsinit(gravconsttype whichconst,
88 		   double cosim,  double emsq,   double argpo,   double s1,     double s2,
89 		   double s3,     double s4,     double s5,      double sinim,  double ss1,
90 		   double ss2,    double ss3,    double ss4,     double ss5,    double sz1,
91 		   double sz3,    double sz11,   double sz13,    double sz21,   double sz23,
92 		   double sz31,   double sz33,   double t,       double tc,     double gsto,
93 		   double mo,     double mdot,   double no,      double nodeo,  double nodedot,
94 		   double xpidot, double z1,     double z3,      double z11,    double z13,
95 		   double z21,    double z23,    double z31,     double z33,    double ecco,
96 		   double eccsq,  double& em,    double& argpm,  double& inclm, double& mm,
97 		   double& nm,    double& nodem,
98 		   int& irez,
99 		   double& atime, double& d2201, double& d2211,  double& d3210, double& d3222,
100 		   double& d4410, double& d4422, double& d5220,  double& d5232, double& d5421,
101 		   double& d5433, double& dedt,  double& didt,   double& dmdt,  double& dndt,
102 		   double& dnodt, double& domdt, double& del1,   double& del2,  double& del3,
103 		   double& xfact, double& xlamo, double& xli,    double& xni);
104 
105 static void dspace(int irez,
106 		   double d2201,  double d2211,  double d3210,   double d3222,  double d4410,
107 		   double d4422,  double d5220,  double d5232,   double d5421,  double d5433,
108 		   double dedt,   double del1,   double del2,    double del3,   double didt,
109 		   double dmdt,   double dnodt,  double domdt,   double argpo,  double argpdot,
110 		   double t,      double tc,     double gsto,    double xfact,  double xlamo,
111 		   double no,
112 		   double& atime, double& em,    double& argpm,  double& inclm, double& xli,
113 		   double& mm,    double& xni,   double& nodem,  double& dndt,  double& nm);
114 
115 static void initl(int satn,      gravconsttype whichconst,
116 		  double ecco,   double epoch,  double inclo,   double& no,
117 		  char& method,
118 		  double& ainv,  double& ao,    double& con41,  double& con42, double& cosio,
119 		  double& cosio2,double& eccsq, double& omeosq, double& posq,
120 		  double& rp,    double& rteosq,double& sinio , double& gsto, char opsmode);
121 
122 /* -----------------------------------------------------------------------------
123 *
124 *                           procedure dpper
125 *
126 *  this procedure provides deep space long period periodic contributions
127 *    to the mean elements.  by design, these periodics are zero at epoch.
128 *    this used to be dscom which included initialization, but it's really a
129 *    recurring function.
130 *
131 *  author        : david vallado                  719-573-2600   28 jun 2005
132 *
133 *  inputs        :
134 *    e3          -
135 *    ee2         -
136 *    peo         -
137 *    pgho        -
138 *    pho         -
139 *    pinco       -
140 *    plo         -
141 *    se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 -
142 *    t           -
143 *    xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
144 *    zmol        -
145 *    zmos        -
146 *    ep          - eccentricity                           0.0 - 1.0
147 *    inclo       - inclination - needed for lyddane modification
148 *    nodep       - right ascension of ascending node
149 *    argpp       - argument of perigee
150 *    mp          - mean anomaly
151 *
152 *  outputs       :
153 *    ep          - eccentricity                           0.0 - 1.0
154 *    inclp       - inclination
155 *    nodep        - right ascension of ascending node
156 *    argpp       - argument of perigee
157 *    mp          - mean anomaly
158 *
159 *  locals        :
160 *    alfdp       -
161 *    betdp       -
162 *    cosip  , sinip  , cosop  , sinop  ,
163 *    dalf        -
164 *    dbet        -
165 *    dls         -
166 *    f2, f3      -
167 *    pe          -
168 *    pgh         -
169 *    ph          -
170 *    pinc        -
171 *    pl          -
172 *    sel   , ses   , sghl  , sghs  , shl   , shs   , sil   , sinzf , sis   ,
173 *    sll   , sls
174 *    xls         -
175 *    xnoh        -
176 *    zf          -
177 *    zm          -
178 *
179 *  coupling      :
180 *    none.
181 *
182 *  references    :
183 *    hoots, roehrich, norad spacetrack report #3 1980
184 *    hoots, norad spacetrack report #6 1986
185 *    hoots, schumacher and glover 2004
186 *    vallado, crawford, hujsak, kelso  2006
187   ----------------------------------------------------------------------------*/
188 
dpper(double e3,double ee2,double peo,double pgho,double pho,double pinco,double plo,double se2,double se3,double sgh2,double sgh3,double sgh4,double sh2,double sh3,double si2,double si3,double sl2,double sl3,double sl4,double t,double xgh2,double xgh3,double xgh4,double xh2,double xh3,double xi2,double xi3,double xl2,double xl3,double xl4,double zmol,double zmos,double inclo,char init,double & ep,double & inclp,double & nodep,double & argpp,double & mp,char opsmode)189 static void dpper(double e3,     double ee2,    double peo,     double pgho,   double pho,
190 		  double pinco,  double plo,    double se2,     double se3,    double sgh2,
191 		  double sgh3,   double sgh4,   double sh2,     double sh3,    double si2,
192 		  double si3,    double sl2,    double sl3,     double sl4,    double t,
193 		  double xgh2,   double xgh3,   double xgh4,    double xh2,    double xh3,
194 		  double xi2,    double xi3,    double xl2,     double xl3,    double xl4,
195 		  double zmol,   double zmos,   double inclo,
196 		  char init,
197 		  double& ep,    double& inclp, double& nodep,  double& argpp, double& mp,
198 		  char opsmode)
199 {
200 	/* --------------------- local variables ------------------------ */
201 	const double twopi = 2.0 * M_PI;
202 	double alfdp, betdp, cosip, cosop, dalf, dbet, dls,
203 	       f2,    f3,    pe,    pgh,   ph,   pinc, pl ,
204 	       sel,   ses,   sghl,  sghs,  shll, shs,  sil,
205 	       sinip, sinop, sinzf, sis,   sll,  sls,  xls,
206 	       xnoh,  zf,    zm,    zel,   zes,  znl,  zns;
207 
208 	/* kill warning */
209 	inclo = 0.;
210 	static_cast<void>(inclo);
211 
212 	/* ---------------------- constants ----------------------------- */
213 	zns   = 1.19459e-5;
214 	zes   = 0.01675;
215 	znl   = 1.5835218e-4;
216 	zel   = 0.05490;
217 
218 	/* --------------- calculate time varying periodics ----------- */
219 	zm    = zmos + zns * t;
220 	// be sure that the initial call has time set to zero
221 	if (init == 'y')
222 		zm = zmos;
223 	zf    = zm + 2.0 * zes * std::sin(zm);
224 	sinzf = std::sin(zf);
225 	f2    =  0.5 * sinzf * sinzf - 0.25;
226 	f3    = -0.5 * sinzf * std::cos(zf);
227 	ses   = se2* f2 + se3 * f3;
228 	sis   = si2 * f2 + si3 * f3;
229 	sls   = sl2 * f2 + sl3 * f3 + sl4 * sinzf;
230 	sghs  = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf;
231 	shs   = sh2 * f2 + sh3 * f3;
232 	zm    = zmol + znl * t;
233 	if (init == 'y')
234 		zm = zmol;
235 	zf    = zm + 2.0 * zel * std::sin(zm);
236 	sinzf = std::sin(zf);
237 	f2    =  0.5 * sinzf * sinzf - 0.25;
238 	f3    = -0.5 * sinzf * std::cos(zf);
239 	sel   = ee2 * f2 + e3 * f3;
240 	sil   = xi2 * f2 + xi3 * f3;
241 	sll   = xl2 * f2 + xl3 * f3 + xl4 * sinzf;
242 	sghl  = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf;
243 	shll  = xh2 * f2 + xh3 * f3;
244 	pe    = ses + sel;
245 	pinc  = sis + sil;
246 	pl    = sls + sll;
247 	pgh   = sghs + sghl;
248 	ph    = shs + shll;
249 
250 	if (init == 'n')
251 	{
252 		pe    = pe - peo;
253 		pinc  = pinc - pinco;
254 		pl    = pl - plo;
255 		pgh   = pgh - pgho;
256 		ph    = ph - pho;
257 		inclp = inclp + pinc;
258 		ep    = ep + pe;
259 		sinip = std::sin(inclp);
260 		cosip = std::cos(inclp);
261 
262 		/* ----------------- apply periodics directly ------------ */
263 		//  sgp4fix for lyddane choice
264 		//  strn3 used original inclination - this is technically feasible
265 		//  gsfc used perturbed inclination - also technically feasible
266 		//  probably best to readjust the 0.2 limit value and limit discontinuity
267 		//  0.2 rad = 11.45916 deg
268 		//  use next line for original strn3 approach and original inclination
269 		//  if (inclo >= 0.2)
270 		//  use next line for gsfc version and perturbed inclination
271 		if (inclp >= 0.2)
272 		{
273 			ph     = ph / sinip;
274 			pgh    = pgh - cosip * ph;
275 			argpp  = argpp + pgh;
276 			nodep  = nodep + ph;
277 			mp     = mp + pl;
278 		}
279 		else
280 		{
281 			/* ---- apply periodics with lyddane modification ---- */
282 			sinop  = std::sin(nodep);
283 			cosop  = std::cos(nodep);
284 			alfdp  = sinip * sinop;
285 			betdp  = sinip * cosop;
286 			dalf   =  ph * cosop + pinc * cosip * sinop;
287 			dbet   = -ph * sinop + pinc * cosip * cosop;
288 			alfdp  = alfdp + dalf;
289 			betdp  = betdp + dbet;
290 			nodep  = std::fmod(nodep, twopi);
291 			//  sgp4fix for afspc written intrinsic functions
292 			// nodep used without a trigonometric function ahead
293 			if ((nodep < 0.0) && (opsmode == 'a'))
294 				nodep = nodep + twopi;
295 			xls    = mp + argpp + cosip * nodep;
296 			dls    = pl + pgh - pinc * nodep * sinip;
297 			xls    = xls + dls;
298 			xnoh   = nodep;
299 			nodep  = std::atan2(alfdp, betdp);
300 			//  sgp4fix for afspc written intrinsic functions
301 			// nodep used without a trigonometric function ahead
302 			if ((nodep < 0.0) && (opsmode == 'a'))
303 				nodep = nodep + twopi;
304 			if (std::fabs(xnoh - nodep) > M_PI)
305 			{
306 				if (nodep < xnoh)
307 					nodep = nodep + twopi;
308 				else
309 					nodep = nodep - twopi;
310 			}
311 			mp    = mp + pl;
312 			argpp = xls - mp - cosip * nodep;
313 		}
314 	}   // if init == 'n'
315 }  // end dpper
316 
317 /*-----------------------------------------------------------------------------
318 *
319 *                           procedure dscom
320 *
321 *  this procedure provides deep space common items used by both the secular
322 *    and periodics subroutines.  input is provided as shown. this routine
323 *    used to be called dpper, but the functions inside weren't well organized.
324 *
325 *  author        : david vallado                  719-573-2600   28 jun 2005
326 *
327 *  inputs        :
328 *    epoch       -
329 *    ep          - eccentricity
330 *    argpp       - argument of perigee
331 *    tc          -
332 *    inclp       - inclination
333 *    nodep       - right ascension of ascending node
334 *    np          - mean motion
335 *
336 *  outputs       :
337 *    sinim  , cosim  , sinomm , cosomm , snodm  , cnodm
338 *    day         -
339 *    e3          -
340 *    ee2         -
341 *    em          - eccentricity
342 *    emsq        - eccentricity squared
343 *    gam         -
344 *    peo         -
345 *    pgho        -
346 *    pho         -
347 *    pinco       -
348 *    plo         -
349 *    rtemsq      -
350 *    se2, se3         -
351 *    sgh2, sgh3, sgh4        -
352 *    sh2, sh3, si2, si3, sl2, sl3, sl4         -
353 *    s1, s2, s3, s4, s5, s6, s7          -
354 *    ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3         -
355 *    sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33        -
356 *    xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4         -
357 *    nm          - mean motion
358 *    z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33         -
359 *    zmol        -
360 *    zmos        -
361 *
362 *  locals        :
363 *    a1, a2, a3, a4, a5, a6, a7, a8, a9, a10         -
364 *    betasq      -
365 *    cc          -
366 *    ctem, stem        -
367 *    x1, x2, x3, x4, x5, x6, x7, x8          -
368 *    xnodce      -
369 *    xnoi        -
370 *    zcosg  , zsing  , zcosgl , zsingl , zcosh  , zsinh  , zcoshl , zsinhl ,
371 *    zcosi  , zsini  , zcosil , zsinil ,
372 *    zx          -
373 *    zy          -
374 *
375 *  coupling      :
376 *    none.
377 *
378 *  references    :
379 *    hoots, roehrich, norad spacetrack report #3 1980
380 *    hoots, norad spacetrack report #6 1986
381 *    hoots, schumacher and glover 2004
382 *    vallado, crawford, hujsak, kelso  2006
383   ----------------------------------------------------------------------------*/
384 
dscom(double epoch,double ep,double argpp,double tc,double inclp,double nodep,double np,double & snodm,double & cnodm,double & sinim,double & cosim,double & sinomm,double & cosomm,double & day,double & e3,double & ee2,double & em,double & emsq,double & gam,double & peo,double & pgho,double & pho,double & pinco,double & plo,double & rtemsq,double & se2,double & se3,double & sgh2,double & sgh3,double & sgh4,double & sh2,double & sh3,double & si2,double & si3,double & sl2,double & sl3,double & sl4,double & s1,double & s2,double & s3,double & s4,double & s5,double & s6,double & s7,double & ss1,double & ss2,double & ss3,double & ss4,double & ss5,double & ss6,double & ss7,double & sz1,double & sz2,double & sz3,double & sz11,double & sz12,double & sz13,double & sz21,double & sz22,double & sz23,double & sz31,double & sz32,double & sz33,double & xgh2,double & xgh3,double & xgh4,double & xh2,double & xh3,double & xi2,double & xi3,double & xl2,double & xl3,double & xl4,double & nm,double & z1,double & z2,double & z3,double & z11,double & z12,double & z13,double & z21,double & z22,double & z23,double & z31,double & z32,double & z33,double & zmol,double & zmos)385 static void dscom(double epoch,  double ep,     double argpp,   double tc,     double inclp,
386 		  double nodep,  double np,
387 		  double& snodm, double& cnodm, double& sinim,  double& cosim, double& sinomm,
388 		  double& cosomm,double& day,   double& e3,     double& ee2,   double& em,
389 		  double& emsq,  double& gam,   double& peo,    double& pgho,  double& pho,
390 		  double& pinco, double& plo,   double& rtemsq, double& se2,   double& se3,
391 		  double& sgh2,  double& sgh3,  double& sgh4,   double& sh2,   double& sh3,
392 		  double& si2,   double& si3,   double& sl2,    double& sl3,   double& sl4,
393 		  double& s1,    double& s2,    double& s3,     double& s4,    double& s5,
394 		  double& s6,    double& s7,    double& ss1,    double& ss2,   double& ss3,
395 		  double& ss4,   double& ss5,   double& ss6,    double& ss7,   double& sz1,
396 		  double& sz2,   double& sz3,   double& sz11,   double& sz12,  double& sz13,
397 		  double& sz21,  double& sz22,  double& sz23,   double& sz31,  double& sz32,
398 		  double& sz33,  double& xgh2,  double& xgh3,   double& xgh4,  double& xh2,
399 		  double& xh3,   double& xi2,   double& xi3,    double& xl2,   double& xl3,
400 		  double& xl4,   double& nm,    double& z1,     double& z2,    double& z3,
401 		  double& z11,   double& z12,   double& z13,    double& z21,   double& z22,
402 		  double& z23,   double& z31,   double& z32,    double& z33,   double& zmol,
403 		  double& zmos)
404 {
405 	/* -------------------------- constants ------------------------- */
406 	const double zes     =  0.01675;
407 	const double zel     =  0.05490;
408 	const double c1ss    =  2.9864797e-6;
409 	const double c1l     =  4.7968065e-7;
410 	const double zsinis  =  0.39785416;
411 	const double zcosis  =  0.91744867;
412 	const double zcosgs  =  0.1945905;
413 	const double zsings  = -0.98088458;
414 	const double twopi   =  2.0 * M_PI;
415 
416 	/* --------------------- local variables ------------------------ */
417 	int lsflg;
418 	double a1    , a2    , a3    , a4    , a5    , a6    , a7    ,
419 	       a8    , a9    , a10   , betasq, cc    , ctem  , stem  ,
420 	       x1    , x2    , x3    , x4    , x5    , x6    , x7    ,
421 	       x8    , xnodce, xnoi  , zcosg , zcosgl, zcosh , zcoshl,
422 	       zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini ,
423 	       zsinil, zx    , zy;
424 
425 	nm     = np;
426 	em     = ep;
427 	snodm  = std::sin(nodep);
428 	cnodm  = std::cos(nodep);
429 	sinomm = std::sin(argpp);
430 	cosomm = std::cos(argpp);
431 	sinim  = std::sin(inclp);
432 	cosim  = std::cos(inclp);
433 	emsq   = em * em;
434 	betasq = 1.0 - emsq;
435 	rtemsq = std::sqrt(betasq);
436 
437 	/* ----------------- initialize lunar solar terms --------------- */
438 	peo    = 0.0;
439 	pinco  = 0.0;
440 	plo    = 0.0;
441 	pgho   = 0.0;
442 	pho    = 0.0;
443 	day    = epoch + 18261.5 + tc / 1440.0;
444 	xnodce = std::fmod(4.5236020 - 9.2422029e-4 * day, twopi);
445 	stem   = std::sin(xnodce);
446 	ctem   = std::cos(xnodce);
447 	zcosil = 0.91375164 - 0.03568096 * ctem;
448 	zsinil = std::sqrt(1.0 - zcosil * zcosil);
449 	zsinhl = 0.089683511 * stem / zsinil;
450 	zcoshl = std::sqrt(1.0 - zsinhl * zsinhl);
451 	gam    = 5.8351514 + 0.0019443680 * day;
452 	zx     = 0.39785416 * stem / zsinil;
453 	zy     = zcoshl * ctem + 0.91744867 * zsinhl * stem;
454 	zx     = std::atan2(zx, zy);
455 	zx     = gam + zx - xnodce;
456 	zcosgl = std::cos(zx);
457 	zsingl = std::sin(zx);
458 
459 	/* ------------------------- do solar terms --------------------- */
460 	zcosg = zcosgs;
461 	zsing = zsings;
462 	zcosi = zcosis;
463 	zsini = zsinis;
464 	zcosh = cnodm;
465 	zsinh = snodm;
466 	cc    = c1ss;
467 	xnoi  = 1.0 / nm;
468 
469 	for (lsflg = 1; lsflg <= 2; lsflg++)
470 	{
471 		a1  =   zcosg * zcosh + zsing * zcosi * zsinh;
472 		a3  =  -zsing * zcosh + zcosg * zcosi * zsinh;
473 		a7  =  -zcosg * zsinh + zsing * zcosi * zcosh;
474 		a8  =   zsing * zsini;
475 		a9  =   zsing * zsinh + zcosg * zcosi * zcosh;
476 		a10 =   zcosg * zsini;
477 		a2  =   cosim * a7 + sinim * a8;
478 		a4  =   cosim * a9 + sinim * a10;
479 		a5  =  -sinim * a7 + cosim * a8;
480 		a6  =  -sinim * a9 + cosim * a10;
481 
482 		x1  =  a1 * cosomm + a2 * sinomm;
483 		x2  =  a3 * cosomm + a4 * sinomm;
484 		x3  = -a1 * sinomm + a2 * cosomm;
485 		x4  = -a3 * sinomm + a4 * cosomm;
486 		x5  =  a5 * sinomm;
487 		x6  =  a6 * sinomm;
488 		x7  =  a5 * cosomm;
489 		x8  =  a6 * cosomm;
490 
491 		z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
492 		z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
493 		z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4;
494 		z1  =  3.0 *  (a1 * a1 + a2 * a2) + z31 * emsq;
495 		z2  =  6.0 *  (a1 * a3 + a2 * a4) + z32 * emsq;
496 		z3  =  3.0 *  (a3 * a3 + a4 * a4) + z33 * emsq;
497 		z11 = -6.0 * a1 * a5 + emsq *  (-24.0 * x1 * x7-6.0 * x3 * x5);
498 		z12 = -6.0 *  (a1 * a6 + a3 * a5) + emsq * (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5));
499 		z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6);
500 		z21 =  6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7);
501 		z22 =  6.0 *  (a4 * a5 + a2 * a6) + emsq * (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8));
502 		z23 =  6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8);
503 		z1  = z1 + z1 + betasq * z31;
504 		z2  = z2 + z2 + betasq * z32;
505 		z3  = z3 + z3 + betasq * z33;
506 		s3  = cc * xnoi;
507 		s2  = -0.5 * s3 / rtemsq;
508 		s4  = s3 * rtemsq;
509 		s1  = -15.0 * em * s4;
510 		s5  = x1 * x3 + x2 * x4;
511 		s6  = x2 * x3 + x1 * x4;
512 		s7  = x2 * x4 - x1 * x3;
513 
514 		/* ----------------------- do lunar terms ------------------- */
515 		if (lsflg == 1)
516 		{
517 			ss1   = s1;
518 			ss2   = s2;
519 			ss3   = s3;
520 			ss4   = s4;
521 			ss5   = s5;
522 			ss6   = s6;
523 			ss7   = s7;
524 			sz1   = z1;
525 			sz2   = z2;
526 			sz3   = z3;
527 			sz11  = z11;
528 			sz12  = z12;
529 			sz13  = z13;
530 			sz21  = z21;
531 			sz22  = z22;
532 			sz23  = z23;
533 			sz31  = z31;
534 			sz32  = z32;
535 			sz33  = z33;
536 			zcosg = zcosgl;
537 			zsing = zsingl;
538 			zcosi = zcosil;
539 			zsini = zsinil;
540 			zcosh = zcoshl * cnodm + zsinhl * snodm;
541 			zsinh = snodm * zcoshl - cnodm * zsinhl;
542 			cc    = c1l;
543 		}
544 	}
545 
546 	zmol = std::fmod(4.7199672 + 0.22997150  * day - gam, twopi);
547 	zmos = std::fmod(6.2565837 + 0.017201977 * day, twopi);
548 
549 	/* ------------------------ do solar terms ---------------------- */
550 	se2  =   2.0 * ss1 * ss6;
551 	se3  =   2.0 * ss1 * ss7;
552 	si2  =   2.0 * ss2 * sz12;
553 	si3  =   2.0 * ss2 * (sz13 - sz11);
554 	sl2  =  -2.0 * ss3 * sz2;
555 	sl3  =  -2.0 * ss3 * (sz3 - sz1);
556 	sl4  =  -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes;
557 	sgh2 =   2.0 * ss4 * sz32;
558 	sgh3 =   2.0 * ss4 * (sz33 - sz31);
559 	sgh4 = -18.0 * ss4 * zes;
560 	sh2  =  -2.0 * ss2 * sz22;
561 	sh3  =  -2.0 * ss2 * (sz23 - sz21);
562 
563 	/* ------------------------ do lunar terms ---------------------- */
564 	ee2  =   2.0 * s1 * s6;
565 	e3   =   2.0 * s1 * s7;
566 	xi2  =   2.0 * s2 * z12;
567 	xi3  =   2.0 * s2 * (z13 - z11);
568 	xl2  =  -2.0 * s3 * z2;
569 	xl3  =  -2.0 * s3 * (z3 - z1);
570 	xl4  =  -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel;
571 	xgh2 =   2.0 * s4 * z32;
572 	xgh3 =   2.0 * s4 * (z33 - z31);
573 	xgh4 = -18.0 * s4 * zel;
574 	xh2  =  -2.0 * s2 * z22;
575 	xh3  =  -2.0 * s2 * (z23 - z21);
576 }  // end dscom
577 
578 /*-----------------------------------------------------------------------------
579 *
580 *                           procedure dsinit
581 *
582 *  this procedure provides deep space contributions to mean motion dot due
583 *    to geopotential resonance with half day and one day orbits.
584 *
585 *  author        : david vallado                  719-573-2600   28 jun 2005
586 *
587 *  inputs        :
588 *    cosim, sinim-
589 *    emsq        - eccentricity squared
590 *    argpo       - argument of perigee
591 *    s1, s2, s3, s4, s5      -
592 *    ss1, ss2, ss3, ss4, ss5 -
593 *    sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 -
594 *    t           - time
595 *    tc          -
596 *    gsto        - greenwich sidereal time                   rad
597 *    mo          - mean anomaly
598 *    mdot        - mean anomaly dot (rate)
599 *    no          - mean motion
600 *    nodeo       - right ascension of ascending node
601 *    nodedot     - right ascension of ascending node dot (rate)
602 *    xpidot      -
603 *    z1, z3, z11, z13, z21, z23, z31, z33 -
604 *    eccm        - eccentricity
605 *    argpm       - argument of perigee
606 *    inclm       - inclination
607 *    mm          - mean anomaly
608 *    xn          - mean motion
609 *    nodem       - right ascension of ascending node
610 *
611 *  outputs       :
612 *    em          - eccentricity
613 *    argpm       - argument of perigee
614 *    inclm       - inclination
615 *    mm          - mean anomaly
616 *    nm          - mean motion
617 *    nodem       - right ascension of ascending node
618 *    irez        - flag for resonance           0-none, 1-one day, 2-half day
619 *    atime       -
620 *    d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433    -
621 *    dedt        -
622 *    didt        -
623 *    dmdt        -
624 *    dndt        -
625 *    dnodt       -
626 *    domdt       -
627 *    del1, del2, del3        -
628 *    ses  , sghl , sghs , sgs  , shl  , shs  , sis  , sls
629 *    theta       -
630 *    xfact       -
631 *    xlamo       -
632 *    xli         -
633 *    xni
634 *
635 *  locals        :
636 *    ainv2       -
637 *    aonv        -
638 *    cosisq      -
639 *    eoc         -
640 *    f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543  -
641 *    g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533  -
642 *    sini2       -
643 *    temp        -
644 *    temp1       -
645 *    theta       -
646 *    xno2        -
647 *
648 *  coupling      :
649 *    getgravconst
650 *
651 *  references    :
652 *    hoots, roehrich, norad spacetrack report #3 1980
653 *    hoots, norad spacetrack report #6 1986
654 *    hoots, schumacher and glover 2004
655 *    vallado, crawford, hujsak, kelso  2006
656   ----------------------------------------------------------------------------*/
657 
dsinit(gravconsttype whichconst,double cosim,double emsq,double argpo,double s1,double s2,double s3,double s4,double s5,double sinim,double ss1,double ss2,double ss3,double ss4,double ss5,double sz1,double sz3,double sz11,double sz13,double sz21,double sz23,double sz31,double sz33,double t,double tc,double gsto,double mo,double mdot,double no,double nodeo,double nodedot,double xpidot,double z1,double z3,double z11,double z13,double z21,double z23,double z31,double z33,double ecco,double eccsq,double & em,double & argpm,double & inclm,double & mm,double & nm,double & nodem,int & irez,double & atime,double & d2201,double & d2211,double & d3210,double & d3222,double & d4410,double & d4422,double & d5220,double & d5232,double & d5421,double & d5433,double & dedt,double & didt,double & dmdt,double & dndt,double & dnodt,double & domdt,double & del1,double & del2,double & del3,double & xfact,double & xlamo,double & xli,double & xni)658 static void dsinit(gravconsttype whichconst,
659 		   double cosim,  double emsq,   double argpo,   double s1,     double s2,
660 		   double s3,     double s4,     double s5,      double sinim,  double ss1,
661 		   double ss2,    double ss3,    double ss4,     double ss5,    double sz1,
662 		   double sz3,    double sz11,   double sz13,    double sz21,   double sz23,
663 		   double sz31,   double sz33,   double t,       double tc,     double gsto,
664 		   double mo,     double mdot,   double no,      double nodeo,  double nodedot,
665 		   double xpidot, double z1,     double z3,      double z11,    double z13,
666 		   double z21,    double z23,    double z31,     double z33,    double ecco,
667 		   double eccsq,  double& em,    double& argpm,  double& inclm, double& mm,
668 		   double& nm,    double& nodem,
669 		   int& irez,
670 		   double& atime, double& d2201, double& d2211,  double& d3210, double& d3222,
671 		   double& d4410, double& d4422, double& d5220,  double& d5232, double& d5421,
672 		   double& d5433, double& dedt,  double& didt,   double& dmdt,  double& dndt,
673 		   double& dnodt, double& domdt, double& del1,   double& del2,  double& del3,
674 		   double& xfact, double& xlamo, double& xli,    double& xni)
675 {
676 	/* --------------------- local variables ------------------------ */
677 	const double twopi = 2.0 * M_PI;
678 
679 	double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221  , f311  ,
680 	       f321  , f322  , f330  , f441  , f442  , f522  , f523  ,
681 	       f542  , f543  , g200  , g201  , g211  , g300  , g310  ,
682 	       g322  , g410  , g422  , g520  , g521  , g532  , g533  ,
683 	       ses   , sgs   , sghl  , sghs  , shs   , shll  , sis   ,
684 	       sini2 , sls   , temp  , temp1 , theta , xno2  , q22   ,
685 	       q31   , q33   , root22, root44, root54, rptim , root32,
686 	       root52, x2o3  , xke=0   , znl   , emo   , zns   , emsqo,
687 	       tumin, mu, radiusearthkm, j2, j3, j4, j3oj2;
688 
689 	q22    = 1.7891679e-6;
690 	q31    = 2.1460748e-6;
691 	q33    = 2.2123015e-7;
692 	root22 = 1.7891679e-6;
693 	root44 = 7.3636953e-9;
694 	root54 = 2.1765803e-9;
695 	rptim  = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
696 	root32 = 3.7393792e-7;
697 	root52 = 1.1428639e-7;
698 	x2o3   = 2.0 / 3.0;
699 	znl    = 1.5835218e-4;
700 	zns    = 1.19459e-5;
701 
702 	// sgp4fix identify constants and allow alternate values
703 	getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
704 
705 	/* -------------------- deep space initialization ------------ */
706 	irez = 0;
707 	if ((nm < 0.0052359877) && (nm > 0.0034906585))
708 		irez = 1;
709 	if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5))
710 		irez = 2;
711 
712 	/* ------------------------ do solar terms ------------------- */
713 	ses  =  ss1 * zns * ss5;
714 	sis  =  ss2 * zns * (sz11 + sz13);
715 	sls  = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq);
716 	sghs =  ss4 * zns * (sz31 + sz33 - 6.0);
717 	shs  = -zns * ss2 * (sz21 + sz23);
718 	// sgp4fix for 180 deg incl
719 	if ((inclm < 5.2359877e-2) || (inclm > M_PI - 5.2359877e-2))
720 		shs = 0.0;
721 	if (sinim != 0.0)
722 		shs = shs / sinim;
723 	sgs  = sghs - cosim * shs;
724 
725 	/* ------------------------- do lunar terms ------------------ */
726 	dedt = ses + s1 * znl * s5;
727 	didt = sis + s2 * znl * (z11 + z13);
728 	dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq);
729 	sghl = s4 * znl * (z31 + z33 - 6.0);
730 	shll = -znl * s2 * (z21 + z23);
731 	// sgp4fix for 180 deg incl
732 	if ((inclm < 5.2359877e-2) || (inclm > M_PI - 5.2359877e-2))
733 		shll = 0.0;
734 	domdt = sgs + sghl;
735 	dnodt = shs;
736 	if (sinim != 0.0)
737 	{
738 		domdt = domdt - cosim / sinim * shll;
739 		dnodt = dnodt + shll / sinim;
740 	}
741 
742 	/* ----------- calculate deep space resonance effects -------- */
743 	dndt   = 0.0;
744 	theta  = std::fmod(gsto + tc * rptim, twopi);
745 	em     = em + dedt * t;
746 	inclm  = inclm + didt * t;
747 	argpm  = argpm + domdt * t;
748 	nodem  = nodem + dnodt * t;
749 	mm     = mm + dmdt * t;
750 	//   sgp4fix for negative inclinations
751 	//   the following if statement should be commented out
752 	//if (inclm < 0.0)
753 	//  {
754 	//    inclm  = -inclm;
755 	//    argpm  = argpm - M_PI;
756 	//    nodem = nodem + M_PI;
757 	//  }
758 
759 	/* -------------- initialize the resonance terms ------------- */
760 	if (irez != 0)
761 	{
762 		aonv = std::pow(nm / xke, x2o3);
763 
764 		/* ---------- geopotential resonance for 12 hour orbits ------ */
765 		if (irez == 2)
766 		{
767 			cosisq = cosim * cosim;
768 			emo    = em;
769 			em     = ecco;
770 			emsqo  = emsq;
771 			emsq   = eccsq;
772 			eoc    = em * emsq;
773 			g201   = -0.306 - (em - 0.64) * 0.440;
774 
775 			if (em <= 0.65)
776 			{
777 				g211 =    3.616  -  13.2470 * em +  16.2900 * emsq;
778 				g310 =  -19.302  + 117.3900 * em - 228.4190 * emsq +  156.5910 * eoc;
779 				g322 =  -18.9068 + 109.7927 * em - 214.6334 * emsq +  146.5816 * eoc;
780 				g410 =  -41.122  + 242.6940 * em - 471.0940 * emsq +  313.9530 * eoc;
781 				g422 = -146.407  + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc;
782 				g520 = -532.114  + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc;
783 			}
784 			else
785 			{
786 				g211 =   -72.099 +   331.819 * em -   508.738 * emsq +   266.724 * eoc;
787 				g310 =  -346.844 +  1582.851 * em -  2415.925 * emsq +  1246.113 * eoc;
788 				g322 =  -342.585 +  1554.908 * em -  2366.899 * emsq +  1215.972 * eoc;
789 				g410 = -1052.797 +  4758.686 * em -  7193.992 * emsq +  3651.957 * eoc;
790 				g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc;
791 				if (em > 0.715)
792 					g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc;
793 				else
794 					g520 = 1464.74 -  4664.75 * em +  3763.64 * emsq;
795 			}
796 
797 			if (em < 0.7)
798 			{
799 				g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21  * eoc;
800 				g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc;
801 				g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4  * eoc;
802 			}
803 			else
804 			{
805 				g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc;
806 				g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc;
807 				g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc;
808 			}
809 
810 			sini2=  sinim * sinim;
811 			f220 =  0.75 * (1.0 + 2.0 * cosim+cosisq);
812 			f221 =  1.5 * sini2;
813 			f321 =  1.875 * sinim  *  (1.0 - 2.0 * cosim - 3.0 * cosisq);
814 			f322 = -1.875 * sinim  *  (1.0 + 2.0 * cosim - 3.0 * cosisq);
815 			f441 = 35.0 * sini2 * f220;
816 			f442 = 39.3750 * sini2 * sini2;
817 			f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) +
818 			       0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) );
819 			f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim +
820 			       10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq));
821 			f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq *
822 			       (-12.0 + 8.0 * cosim + 10.0 * cosisq));
823 			f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq *
824 			       (12.0 + 8.0 * cosim - 10.0 * cosisq));
825 			xno2  =  nm * nm;
826 			ainv2 =  aonv * aonv;
827 			temp1 =  3.0 * xno2 * ainv2;
828 			temp  =  temp1 * root22;
829 			d2201 =  temp * f220 * g201;
830 			d2211 =  temp * f221 * g211;
831 			temp1 =  temp1 * aonv;
832 			temp  =  temp1 * root32;
833 			d3210 =  temp * f321 * g310;
834 			d3222 =  temp * f322 * g322;
835 			temp1 =  temp1 * aonv;
836 			temp  =  2.0 * temp1 * root44;
837 			d4410 =  temp * f441 * g410;
838 			d4422 =  temp * f442 * g422;
839 			temp1 =  temp1 * aonv;
840 			temp  =  temp1 * root52;
841 			d5220 =  temp * f522 * g520;
842 			d5232 =  temp * f523 * g532;
843 			temp  =  2.0 * temp1 * root54;
844 			d5421 =  temp * f542 * g521;
845 			d5433 =  temp * f543 * g533;
846 			xlamo =  std::fmod(mo + nodeo + nodeo-theta - theta, twopi);
847 			xfact =  mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no;
848 			em    = emo;
849 			emsq  = emsqo;
850 		}
851 
852 		/* ---------------- synchronous resonance terms -------------- */
853 		if (irez == 1)
854 		{
855 			g200  = 1.0 + emsq * (-2.5 + 0.8125 * emsq);
856 			g310  = 1.0 + 2.0 * emsq;
857 			g300  = 1.0 + emsq * (-6.0 + 6.60937 * emsq);
858 			f220  = 0.75 * (1.0 + cosim) * (1.0 + cosim);
859 			f311  = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim);
860 			f330  = 1.0 + cosim;
861 			f330  = 1.875 * f330 * f330 * f330;
862 			del1  = 3.0 * nm * nm * aonv * aonv;
863 			del2  = 2.0 * del1 * f220 * g200 * q22;
864 			del3  = 3.0 * del1 * f330 * g300 * q33 * aonv;
865 			del1  = del1 * f311 * g310 * q31 * aonv;
866 			xlamo = std::fmod(mo + nodeo + argpo - theta, twopi);
867 			xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no;
868 		}
869 
870 		/* ------------ for sgp4, initialize the integrator ---------- */
871 		xli   = xlamo;
872 		xni   = no;
873 		atime = 0.0;
874 		nm    = no + dndt;
875 	}
876 }  // end dsinit
877 
878 /*-----------------------------------------------------------------------------
879 *
880 *                           procedure dspace
881 *
882 *  this procedure provides deep space contributions to mean elements for
883 *    perturbing third body.  these effects have been averaged over one
884 *    revolution of the sun and moon.  for earth resonance effects, the
885 *    effects have been averaged over no revolutions of the satellite.
886 *    (mean motion)
887 *
888 *  author        : david vallado                  719-573-2600   28 jun 2005
889 *
890 *  inputs        :
891 *    d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
892 *    dedt        -
893 *    del1, del2, del3  -
894 *    didt        -
895 *    dmdt        -
896 *    dnodt       -
897 *    domdt       -
898 *    irez        - flag for resonance           0-none, 1-one day, 2-half day
899 *    argpo       - argument of perigee
900 *    argpdot     - argument of perigee dot (rate)
901 *    t           - time
902 *    tc          -
903 *    gsto        - gst
904 *    xfact       -
905 *    xlamo       -
906 *    no          - mean motion
907 *    atime       -
908 *    em          - eccentricity
909 *    ft          -
910 *    argpm       - argument of perigee
911 *    inclm       - inclination
912 *    xli         -
913 *    mm          - mean anomaly
914 *    xni         - mean motion
915 *    nodem       - right ascension of ascending node
916 *
917 *  outputs       :
918 *    atime       -
919 *    em          - eccentricity
920 *    argpm       - argument of perigee
921 *    inclm       - inclination
922 *    xli         -
923 *    mm          - mean anomaly
924 *    xni         -
925 *    nodem       - right ascension of ascending node
926 *    dndt        -
927 *    nm          - mean motion
928 *
929 *  locals        :
930 *    delt        -
931 *    ft          -
932 *    theta       -
933 *    x2li        -
934 *    x2omi       -
935 *    xl          -
936 *    xldot       -
937 *    xnddt       -
938 *    xndt        -
939 *    xomi        -
940 *
941 *  coupling      :
942 *    none        -
943 *
944 *  references    :
945 *    hoots, roehrich, norad spacetrack report #3 1980
946 *    hoots, norad spacetrack report #6 1986
947 *    hoots, schumacher and glover 2004
948 *    vallado, crawford, hujsak, kelso  2006
949   ----------------------------------------------------------------------------*/
950 
dspace(int irez,double d2201,double d2211,double d3210,double d3222,double d4410,double d4422,double d5220,double d5232,double d5421,double d5433,double dedt,double del1,double del2,double del3,double didt,double dmdt,double dnodt,double domdt,double argpo,double argpdot,double t,double tc,double gsto,double xfact,double xlamo,double no,double & atime,double & em,double & argpm,double & inclm,double & xli,double & mm,double & xni,double & nodem,double & dndt,double & nm)951 static void dspace(int irez,
952 		   double d2201,  double d2211,  double d3210,   double d3222,  double d4410,
953 		   double d4422,  double d5220,  double d5232,   double d5421,  double d5433,
954 		   double dedt,   double del1,   double del2,    double del3,   double didt,
955 		   double dmdt,   double dnodt,  double domdt,   double argpo,  double argpdot,
956 		   double t,      double tc,     double gsto,    double xfact,  double xlamo,
957 		   double no,
958 		   double& atime, double& em,    double& argpm,  double& inclm, double& xli,
959 		   double& mm,    double& xni,   double& nodem,  double& dndt,  double& nm)
960 {
961 	const double twopi = 2.0 * M_PI;
962 	int iretn , iret;
963 	double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32,
964 		g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp;
965 
966 	xldot = 0.0;
967 	xnddt = 0.0;
968 	xndt  = 0.0;
969 	fasx2 = 0.13130908;
970 	fasx4 = 2.8843198;
971 	fasx6 = 0.37448087;
972 	g22   = 5.7686396;
973 	g32   = 0.95240898;
974 	g44   = 1.8014998;
975 	g52   = 1.0508330;
976 	g54   = 4.4108898;
977 	rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
978 	stepp =    720.0;
979 	stepn =   -720.0;
980 	step2 = 259200.0;
981 
982 	/* ----------- calculate deep space resonance effects ----------- */
983 	dndt   = 0.0;
984 	theta  = std::fmod(gsto + tc * rptim, twopi);
985 	em     = em + dedt * t;
986 
987 	inclm  = inclm + didt * t;
988 	argpm  = argpm + domdt * t;
989 	nodem  = nodem + dnodt * t;
990 	mm     = mm + dmdt * t;
991 
992 	//   sgp4fix for negative inclinations
993 	//   the following if statement should be commented out
994 	//  if (inclm < 0.0)
995 	// {
996 	//    inclm = -inclm;
997 	//    argpm = argpm - pi;
998 	//    nodem = nodem + pi;
999 	//  }
1000 
1001 	/* - update resonances : numerical (euler-maclaurin) integration - */
1002 	/* ------------------------- epoch restart ----------------------  */
1003 	//   sgp4fix for propagator problems
1004 	//   the following integration works for negative time steps and periods
1005 	//   the specific changes are unknown because the original code was so convoluted
1006 
1007 	// sgp4fix take out atime = 0.0 and fix for faster operation
1008 	ft    = 0.0;
1009 	if (irez != 0)
1010 	{
1011 		// sgp4fix streamline check
1012 		if ((atime == 0.0) || (t * atime <= 0.0) || (std::fabs(t) < std::fabs(atime)) )
1013 		{
1014 			atime  = 0.0;
1015 			xni    = no;
1016 			xli    = xlamo;
1017 		}
1018 		// sgp4fix move check outside loop
1019 		if (t > 0.0)
1020 			delt = stepp;
1021 		else
1022 			delt = stepn;
1023 
1024 		iretn = 381; // added for do loop
1025 		iret  =   0; // added for loop
1026 		static_cast<void>(iret);
1027 		while (iretn == 381)
1028 		{
1029 			/* ------------------- dot terms calculated ------------- */
1030 			/* ----------- near - synchronous resonance terms ------- */
1031 			if (irez != 2)
1032 			{
1033 				xndt  = del1 * std::sin(xli - fasx2) + del2 * std::sin(2.0 * (xli - fasx4)) +
1034 					del3 * std::sin(3.0 * (xli - fasx6));
1035 				xldot = xni + xfact;
1036 				xnddt = del1 * std::cos(xli - fasx2) +
1037 					2.0 * del2 * std::cos(2.0 * (xli - fasx4)) +
1038 					3.0 * del3 * std::cos(3.0 * (xli - fasx6));
1039 				xnddt = xnddt * xldot;
1040 			}
1041 			else
1042 			{
1043 				/* --------- near - half-day resonance terms -------- */
1044 				xomi  = argpo + argpdot * atime;
1045 				x2omi = xomi + xomi;
1046 				x2li  = xli + xli;
1047 				xndt  = d2201 * std::sin(x2omi + xli - g22) + d2211 * std::sin(xli - g22) +
1048 					d3210 * std::sin(xomi + xli - g32)  + d3222 * std::sin(-xomi + xli - g32)+
1049 					d4410 * std::sin(x2omi + x2li - g44)+ d4422 * std::sin(x2li - g44) +
1050 					d5220 * std::sin(xomi + xli - g52)  + d5232 * std::sin(-xomi + xli - g52)+
1051 					d5421 * std::sin(xomi + x2li - g54) + d5433 * std::sin(-xomi + x2li - g54);
1052 				xldot = xni + xfact;
1053 				xnddt = d2201 * std::cos(x2omi + xli - g22) + d2211 * std::cos(xli - g22) +
1054 					d3210 * std::cos(xomi + xli - g32)  + d3222 * std::cos(-xomi + xli - g32) +
1055 					d5220 * std::cos(xomi + xli - g52)  + d5232 * std::cos(-xomi + xli - g52) +
1056 					2.0 * (d4410 * std::cos(x2omi + x2li - g44) +
1057 					d4422 * std::cos(x2li - g44) + d5421 * std::cos(xomi + x2li - g54) +
1058 					d5433 * std::cos(-xomi + x2li - g54));
1059 				xnddt = xnddt * xldot;
1060 			}
1061 
1062 			/* ----------------------- integrator ------------------- */
1063 			// sgp4fix move end checks to end of routine
1064 			if (std::fabs(t - atime) >= stepp)
1065 			{
1066 				// NOTE: Never read!
1067 				// iret  = 0;
1068 				iretn = 381;
1069 			}
1070 			else // exit here
1071 			{
1072 				ft    = t - atime;
1073 				iretn = 0;
1074 			}
1075 
1076 			if (iretn == 381)
1077 			{
1078 				xli   = xli + xldot * delt + xndt * step2;
1079 				xni   = xni + xndt * delt + xnddt * step2;
1080 				atime = atime + delt;
1081 			}
1082 		}  // while iretn = 381
1083 
1084 		nm = xni + xndt * ft + xnddt * ft * ft * 0.5;
1085 		xl = xli + xldot * ft + xndt * ft * ft * 0.5;
1086 		if (irez != 1)
1087 		{
1088 			mm   = xl - 2.0 * nodem + 2.0 * theta;
1089 			dndt = nm - no;
1090 		}
1091 		else
1092 		{
1093 			mm   = xl - nodem - argpm + theta;
1094 			dndt = nm - no;
1095 		}
1096 		nm = no + dndt;
1097 	}
1098 }  // end dsspace
1099 
1100 /*-----------------------------------------------------------------------------
1101 *
1102 *                           procedure initl
1103 *
1104 *  this procedure initializes the spg4 propagator. all the initialization is
1105 *    consolidated here instead of having multiple loops inside other routines.
1106 *
1107 *  author        : david vallado                  719-573-2600   28 jun 2005
1108 *
1109 *  inputs        :
1110 *    ecco        - eccentricity                           0.0 - 1.0
1111 *    epoch       - epoch time in days from jan 0, 1950. 0 hr
1112 *    inclo       - inclination of satellite
1113 *    no          - mean motion of satellite
1114 *    satn        - satellite number
1115 *
1116 *  outputs       :
1117 *    ainv        - 1.0 / a
1118 *    ao          - semi major axis
1119 *    con41       -
1120 *    con42       - 1.0 - 5.0 cos(i)
1121 *    cosio       - cosine of inclination
1122 *    cosio2      - cosio squared
1123 *    eccsq       - eccentricity squared
1124 *    method      - flag for deep space                    'd', 'n'
1125 *    omeosq      - 1.0 - ecco * ecco
1126 *    posq        - semi-parameter squared
1127 *    rp          - radius of perigee
1128 *    rteosq      - square root of (1.0 - ecco*ecco)
1129 *    sinio       - sine of inclination
1130 *    gsto        - gst at time of observation               rad
1131 *    no          - mean motion of satellite
1132 *
1133 *  locals        :
1134 *    ak          -
1135 *    d1          -
1136 *    del         -
1137 *    adel        -
1138 *    po          -
1139 *
1140 *  coupling      :
1141 *    getgravconst
1142 *    gstime      - find greenwich sidereal time from the julian date
1143 *
1144 *  references    :
1145 *    hoots, roehrich, norad spacetrack report #3 1980
1146 *    hoots, norad spacetrack report #6 1986
1147 *    hoots, schumacher and glover 2004
1148 *    vallado, crawford, hujsak, kelso  2006
1149   ----------------------------------------------------------------------------*/
1150 
initl(int satn,gravconsttype whichconst,double ecco,double epoch,double inclo,double & no,char & method,double & ainv,double & ao,double & con41,double & con42,double & cosio,double & cosio2,double & eccsq,double & omeosq,double & posq,double & rp,double & rteosq,double & sinio,double & gsto,char opsmode)1151 static void initl(int satn,      gravconsttype whichconst,
1152 		  double ecco,   double epoch,  double inclo,   double& no,
1153 		  char& method,
1154 		  double& ainv,  double& ao,    double& con41,  double& con42, double& cosio,
1155 		  double& cosio2,double& eccsq, double& omeosq, double& posq,
1156 		  double& rp,    double& rteosq,double& sinio , double& gsto,
1157 		  char opsmode)
1158 {
1159 	/* --------------------- local variables ------------------------ */
1160 	double ak, d1, del, adel, po, x2o3, j2=0, xke=0,
1161 	       tumin, mu, radiusearthkm, j3, j4, j3oj2;
1162 
1163 	/* kill warning */
1164 	satn = 0;
1165 	static_cast<void>(satn);
1166 
1167 	// sgp4fix use old way of finding gst
1168 	double ds70;
1169 	double ts70, tfrac, c1, thgr70, fk5r, c1p2p;
1170 	const double twopi = 2.0 * M_PI;
1171 
1172 	/* ----------------------- earth constants ---------------------- */
1173 	// sgp4fix identify constants and allow alternate values
1174 	getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
1175 	x2o3   = 2.0 / 3.0;
1176 
1177 	/* ------------- calculate auxillary epoch quantities ---------- */
1178 	eccsq  = ecco * ecco;
1179 	omeosq = 1.0 - eccsq;
1180 	rteosq = std::sqrt(omeosq);
1181 	cosio  = std::cos(inclo);
1182 	cosio2 = cosio * cosio;
1183 
1184 	/* ------------------ un-kozai the mean motion ----------------- */
1185 	ak    = std::pow(xke / no, x2o3);
1186 	d1    = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq);
1187 	del   = d1 / (ak * ak);
1188 	adel  = ak * (1.0 - del * del - del * (1.0 / 3.0 + 134.0 * del * del / 81.0));
1189 	del   = d1/(adel * adel);
1190 	no    = no / (1.0 + del);
1191 
1192 	ao    = std::pow(xke / no, x2o3);
1193 	sinio = std::sin(inclo);
1194 	po    = ao * omeosq;
1195 	con42 = 1.0 - 5.0 * cosio2;
1196 	con41 = -con42-cosio2-cosio2;
1197 	ainv  = 1.0 / ao;
1198 	posq  = po * po;
1199 	rp    = ao * (1.0 - ecco);
1200 	method = 'n';
1201 
1202 	// sgp4fix modern approach to finding sidereal time
1203 	if (opsmode == 'a')
1204 	{
1205 		// sgp4fix use old way of finding gst
1206 		// count integer number of days from 0 jan 1970
1207 		ts70  = epoch - 7305.0;
1208 		ds70 = std::floor(ts70 + 1.0e-8);
1209 		tfrac = ts70 - ds70;
1210 		// find greenwich location at epoch
1211 		c1    = 1.72027916940703639e-2;
1212 		thgr70= 1.7321343856509374;
1213 		fk5r  = 5.07551419432269442e-15;
1214 		c1p2p = c1 + twopi;
1215 		gsto  = std::fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi);
1216 		if ( gsto < 0.0 )
1217 			gsto = gsto + twopi;
1218 	}
1219 	else
1220 		gsto = gstime(epoch + 2433281.5);
1221 }  // end initl
1222 
1223 /*-----------------------------------------------------------------------------
1224 *
1225 *                             procedure sgp4init
1226 *
1227 *  this procedure initializes variables for sgp4.
1228 *
1229 *  author        : david vallado                  719-573-2600   28 jun 2005
1230 *
1231 *  inputs        :
1232 *    opsmode     - mode of operation afspc or improved 'a', 'i'
1233 *    whichconst  - which set of constants to use  72, 84
1234 *    satn        - satellite number
1235 *    bstar       - sgp4 type drag coefficient              kg/m2er
1236 *    ecco        - eccentricity
1237 *    epoch       - epoch time in days from jan 0, 1950. 0 hr
1238 *    argpo       - argument of perigee (output if ds)
1239 *    inclo       - inclination
1240 *    mo          - mean anomaly (output if ds)
1241 *    no          - mean motion
1242 *    nodeo       - right ascension of ascending node
1243 *
1244 *  outputs       :
1245 *    satrec      - common values for subsequent calls
1246 *    return code - non-zero on error.
1247 *                   1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
1248 *                   2 - mean motion less than 0.0
1249 *                   3 - pert elements, ecc < 0.0  or  ecc > 1.0
1250 *                   4 - semi-latus rectum < 0.0
1251 *                   5 - epoch elements are sub-orbital
1252 *                   6 - satellite has decayed
1253 *
1254 *  locals        :
1255 *    cnodm  , snodm  , cosim  , sinim  , cosomm , sinomm
1256 *    cc1sq  , cc2    , cc3
1257 *    coef   , coef1
1258 *    cosio4      -
1259 *    day         -
1260 *    dndt        -
1261 *    em          - eccentricity
1262 *    emsq        - eccentricity squared
1263 *    eeta        -
1264 *    etasq       -
1265 *    gam         -
1266 *    argpm       - argument of perigee
1267 *    nodem       -
1268 *    inclm       - inclination
1269 *    mm          - mean anomaly
1270 *    nm          - mean motion
1271 *    perige      - perigee
1272 *    pinvsq      -
1273 *    psisq       -
1274 *    qzms24      -
1275 *    rtemsq      -
1276 *    s1, s2, s3, s4, s5, s6, s7          -
1277 *    sfour       -
1278 *    ss1, ss2, ss3, ss4, ss5, ss6, ss7         -
1279 *    sz1, sz2, sz3
1280 *    sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33        -
1281 *    tc          -
1282 *    temp        -
1283 *    temp1, temp2, temp3       -
1284 *    tsi         -
1285 *    xpidot      -
1286 *    xhdot1      -
1287 *    z1, z2, z3          -
1288 *    z11, z12, z13, z21, z22, z23, z31, z32, z33         -
1289 *
1290 *  coupling      :
1291 *    getgravconst-
1292 *    initl       -
1293 *    dscom       -
1294 *    dpper       -
1295 *    dsinit      -
1296 *    sgp4        -
1297 *
1298 *  references    :
1299 *    hoots, roehrich, norad spacetrack report #3 1980
1300 *    hoots, norad spacetrack report #6 1986
1301 *    hoots, schumacher and glover 2004
1302 *    vallado, crawford, hujsak, kelso  2006
1303   ----------------------------------------------------------------------------*/
1304 
sgp4init(gravconsttype whichconst,char opsmode,const int satn,const double epoch,const double xbstar,const double xecco,const double xargpo,const double xinclo,const double xmo,const double xno,const double xnodeo,elsetrec & satrec)1305 bool sgp4init(gravconsttype whichconst, char opsmode,   const int satn,     const double epoch,
1306 	      const double xbstar,  const double xecco, const double xargpo,
1307 	      const double xinclo,  const double xmo,   const double xno,
1308 	      const double xnodeo,  elsetrec& satrec)
1309 {
1310 	/* --------------------- local variables ------------------------ */
1311 	double ao    , ainv  , con42 , cosio , sinio , cosio2, eccsq ,
1312 	       omeosq, posq  , rp    , rteosq,
1313 	       cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq ,
1314 	       cc2   , cc3   , coef  , coef1 , cosio4, day   , dndt  ,
1315 	       em    , emsq  , eeta  , etasq , gam   , argpm , nodem ,
1316 	       inclm , mm    , nm    , perige, pinvsq, psisq , qzms24,
1317 	       rtemsq, s1    , s2    , s3    , s4    , s5    , s6    ,
1318 	       s7    , sfour , ss1   , ss2   , ss3   , ss4   , ss5   ,
1319 	       ss6   , ss7   , sz1   , sz2   , sz3   , sz11  , sz12  ,
1320 	       sz13  , sz21  , sz22  , sz23  , sz31  , sz32  , sz33  ,
1321 	       tc    , temp  , temp1 , temp2 , temp3 , tsi   , xpidot,
1322 	       xhdot1, z1    , z2    , z3    , z11   , z12   , z13   ,
1323 	       z21   , z22   , z23   , z31   , z32   , z33   , qzms2t,
1324 	       ss    , j2    , j3oj2 , j4    , x2o3  , r[3]  , v[3]  ,
1325 	       tumin , mu    , radiusearthkm, xke, j3;
1326 
1327 	/* ------------------------ initialization --------------------- */
1328 	// sgp4fix divisor for divide by zero check on inclination
1329 	// the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
1330 	// 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
1331 	const double temp4    =   1.5e-12;
1332 
1333 	ss1  = 0.0;	ss2  = 0.0;	ss3  = 0.0;
1334 	ss4  = 0.0;	ss5  = 0.0;	ss6  = 0.0;
1335 	ss6  = 0.0;
1336 	sz1  = 0.0;	sz2  = 0.0;	sz3  = 0.0;
1337 	sz11 = 0.0;	sz12 = 0.0;	sz13 = 0.0;
1338 	sz21 = 0.0;	sz22 = 0.0;	sz23 = 0.0;
1339 	sz31 = 0.0;	sz32 = 0.0;	sz33 = 0.0;
1340 	j2   = 0;      j3oj2 = 0;       j4   = 0;
1341 	radiusearthkm = 0;
1342 
1343 	/* ----------- set all near earth variables to zero ------------ */
1344 	satrec.isimp   = 0;   satrec.method = 'n'; satrec.aycof    = 0.0;
1345 	satrec.con41   = 0.0; satrec.cc1    = 0.0; satrec.cc4      = 0.0;
1346 	satrec.cc5     = 0.0; satrec.d2     = 0.0; satrec.d3       = 0.0;
1347 	satrec.d4      = 0.0; satrec.delmo  = 0.0; satrec.eta      = 0.0;
1348 	satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao   = 0.0;
1349 	satrec.t       = 0.0; satrec.t2cof  = 0.0; satrec.t3cof    = 0.0;
1350 	satrec.t4cof   = 0.0; satrec.t5cof  = 0.0; satrec.x1mth2   = 0.0;
1351 	satrec.x7thm1  = 0.0; satrec.mdot   = 0.0; satrec.nodedot  = 0.0;
1352 	satrec.xlcof   = 0.0; satrec.xmcof  = 0.0; satrec.nodecf   = 0.0;
1353 
1354 	/* ----------- set all deep space variables to zero ------------ */
1355 	satrec.irez  = 0;   satrec.d2201 = 0.0; satrec.d2211 = 0.0;
1356 	satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0;
1357 	satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0;
1358 	satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt  = 0.0;
1359 	satrec.del1  = 0.0; satrec.del2  = 0.0; satrec.del3  = 0.0;
1360 	satrec.didt  = 0.0; satrec.dmdt  = 0.0; satrec.dnodt = 0.0;
1361 	satrec.domdt = 0.0; satrec.e3    = 0.0; satrec.ee2   = 0.0;
1362 	satrec.peo   = 0.0; satrec.pgho  = 0.0; satrec.pho   = 0.0;
1363 	satrec.pinco = 0.0; satrec.plo   = 0.0; satrec.se2   = 0.0;
1364 	satrec.se3   = 0.0; satrec.sgh2  = 0.0; satrec.sgh3  = 0.0;
1365 	satrec.sgh4  = 0.0; satrec.sh2   = 0.0; satrec.sh3   = 0.0;
1366 	satrec.si2   = 0.0; satrec.si3   = 0.0; satrec.sl2   = 0.0;
1367 	satrec.sl3   = 0.0; satrec.sl4   = 0.0; satrec.gsto  = 0.0;
1368 	satrec.xfact = 0.0; satrec.xgh2  = 0.0; satrec.xgh3  = 0.0;
1369 	satrec.xgh4  = 0.0; satrec.xh2   = 0.0; satrec.xh3   = 0.0;
1370 	satrec.xi2   = 0.0; satrec.xi3   = 0.0; satrec.xl2   = 0.0;
1371 	satrec.xl3   = 0.0; satrec.xl4   = 0.0; satrec.xlamo = 0.0;
1372 	satrec.zmol  = 0.0; satrec.zmos  = 0.0; satrec.atime = 0.0;
1373 	satrec.xli   = 0.0; satrec.xni   = 0.0;
1374 
1375 	// sgp4fix - note the following variables are also passed directly via satrec.
1376 	// it is possible to streamline the sgp4init call by deleting the "x"
1377 	// variables, but the user would need to set the satrec.* values first. we
1378 	// include the additional assignments in case twoline2rv is not used.
1379 	satrec.bstar   = xbstar;
1380 	satrec.ecco    = xecco;
1381 	satrec.argpo   = xargpo;
1382 	satrec.inclo   = xinclo;
1383 	satrec.mo	    = xmo;
1384 	satrec.no	    = xno;
1385 	satrec.nodeo   = xnodeo;
1386 
1387 	// sgp4fix add opsmode
1388 	satrec.operationmode = opsmode;
1389 
1390 	/* ------------------------ earth constants ----------------------- */
1391 	// sgp4fix identify constants and allow alternate values
1392 	getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
1393 	ss     = 78.0 / radiusearthkm + 1.0;
1394 	qzms2t = std::pow(((120.0 - 78.0) / radiusearthkm), 4);
1395 	x2o3   =  2.0 / 3.0;
1396 
1397 	satrec.init = 'y';
1398 	satrec.t	 = 0.0;
1399 
1400 	initl(satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method,
1401 	      ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq,
1402 	      posq, rp, rteosq, sinio, satrec.gsto, satrec.operationmode);
1403 	satrec.error = 0;
1404 
1405 	// sgp4fix remove this check as it is unnecessary
1406 	// the mrt check in sgp4 handles decaying satellite cases even if the starting
1407 	// condition is below the surface of te earth
1408 	//     if (rp < 1.0)
1409 	//       {
1410 	//         printf("# *** satn%d epoch elts sub-orbital ***\n", satn);
1411 	//         satrec.error = 5;
1412 	//       }
1413 
1414 	if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0))
1415 	{
1416 		satrec.isimp = 0;
1417 		if (rp < (220.0 / radiusearthkm + 1.0))
1418 			satrec.isimp = 1;
1419 		sfour  = ss;
1420 		qzms24 = qzms2t;
1421 		perige = (rp - 1.0) * radiusearthkm;
1422 
1423 		/* - for perigees below 156 km, s and qoms2t are altered - */
1424 		if (perige < 156.0)
1425 		{
1426 			sfour = perige - 78.0;
1427 			if (perige < 98.0)
1428 				sfour = 20.0;
1429 			qzms24 = std::pow(((120.0 - sfour) / radiusearthkm), 4.0);
1430 			sfour  = sfour / radiusearthkm + 1.0;
1431 		}
1432 		pinvsq = 1.0 / posq;
1433 
1434 		tsi  = 1.0 / (ao - sfour);
1435 		satrec.eta  = ao * satrec.ecco * tsi;
1436 		etasq = satrec.eta * satrec.eta;
1437 		eeta  = satrec.ecco * satrec.eta;
1438 		psisq = std::fabs(1.0 - etasq);
1439 		coef  = qzms24 * std::pow(tsi, 4.0);
1440 		coef1 = coef / std::pow(psisq, 3.5);
1441 		cc2   = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta *
1442 			(4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 *
1443 			(8.0 + 3.0 * etasq * (8.0 + etasq)));
1444 		satrec.cc1   = satrec.bstar * cc2;
1445 		cc3   = 0.0;
1446 		if (satrec.ecco > 1.0e-4)
1447 			cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco;
1448 		satrec.x1mth2 = 1.0 - cosio2;
1449 		satrec.cc4    = 2.0* satrec.no * coef1 * ao * omeosq *
1450 				(satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco *
1451 				(0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) *
1452 				(-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq *
1453 				(1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 *
1454 				(2.0 * etasq - eeta * (1.0 + etasq)) * std::cos(2.0 * satrec.argpo)));
1455 		satrec.cc5    = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 *
1456 				(etasq + eeta) + eeta * etasq);
1457 
1458 		cosio4 = cosio2 * cosio2;
1459 		temp1  = 1.5 * j2 * pinvsq * satrec.no;
1460 		temp2  = 0.5 * temp1 * j2 * pinvsq;
1461 		temp3  = -0.46875 * j4 * pinvsq * pinvsq * satrec.no;
1462 		satrec.mdot     = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 *
1463 				  temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4);
1464 		satrec.argpdot  = -0.5 * temp1 * con42 + 0.0625 * temp2 *
1465 				  (7.0 - 114.0 * cosio2 + 395.0 * cosio4) +
1466 				  temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4);
1467 		xhdot1          = -temp1 * cosio;
1468 		satrec.nodedot  = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) +
1469 				  2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio;
1470 		xpidot          =  satrec.argpdot+ satrec.nodedot;
1471 		satrec.omgcof   = satrec.bstar * cc3 * std::cos(satrec.argpo);
1472 		satrec.xmcof    = 0.0;
1473 		if (satrec.ecco > 1.0e-4)
1474 			satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta;
1475 		satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1;
1476 		satrec.t2cof  = 1.5 * satrec.cc1;
1477 		// sgp4fix for divide by zero with xinco = 180 deg
1478 		if (std::fabs(cosio+1.0) > 1.5e-12)
1479 			satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio);
1480 		else
1481 			satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4;
1482 		satrec.aycof   = -0.5 * j3oj2 * sinio;
1483 		satrec.delmo   = std::pow((1.0 + satrec.eta * std::cos(satrec.mo)), 3);
1484 		satrec.sinmao  = std::sin(satrec.mo);
1485 		satrec.x7thm1  = 7.0 * cosio2 - 1.0;
1486 
1487 		/* --------------- deep space initialization ------------- */
1488 		if ((2*M_PI / satrec.no) >= 225.0)
1489 		{
1490 			satrec.method = 'd';
1491 			satrec.isimp  = 1;
1492 			tc    =  0.0;
1493 			inclm = satrec.inclo;
1494 
1495 			dscom(epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo,
1496 			      satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm,
1497 			      day, satrec.e3, satrec.ee2, em, emsq, gam,
1498 			      satrec.peo,  satrec.pgho,   satrec.pho, satrec.pinco,
1499 			      satrec.plo,  rtemsq,        satrec.se2, satrec.se3,
1500 			      satrec.sgh2, satrec.sgh3,   satrec.sgh4,
1501 			      satrec.sh2,  satrec.sh3,    satrec.si2, satrec.si3,
1502 			      satrec.sl2,  satrec.sl3,    satrec.sl4, s1, s2, s3, s4, s5,
1503 			      s6,   s7,   ss1,  ss2,  ss3,  ss4,  ss5,  ss6,  ss7, sz1, sz2, sz3,
1504 			      sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33,
1505 			      satrec.xgh2, satrec.xgh3,   satrec.xgh4, satrec.xh2,
1506 			      satrec.xh3,  satrec.xi2,    satrec.xi3,  satrec.xl2,
1507 			      satrec.xl3,  satrec.xl4,    nm, z1, z2, z3, z11,
1508 			      z12, z13, z21, z22, z23, z31, z32, z33,
1509 			      satrec.zmol, satrec.zmos);
1510 			dpper(satrec.e3, satrec.ee2, satrec.peo, satrec.pgho,
1511 			      satrec.pho, satrec.pinco, satrec.plo, satrec.se2,
1512 			      satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4,
1513 			      satrec.sh2, satrec.sh3, satrec.si2,  satrec.si3,
1514 			      satrec.sl2, satrec.sl3, satrec.sl4,  satrec.t,
1515 			      satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2,
1516 			      satrec.xh3, satrec.xi2, satrec.xi3,  satrec.xl2,
1517 			      satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init,
1518 			      satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo,
1519 			      satrec.operationmode);
1520 
1521 			argpm  = 0.0;
1522 			nodem  = 0.0;
1523 			mm     = 0.0;
1524 
1525 			dsinit(whichconst,
1526 			       cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4,
1527 			       ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc,
1528 			       satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo,
1529 			       satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33,
1530 			       satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem,
1531 			       satrec.irez,  satrec.atime,
1532 			       satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 ,
1533 			       satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232,
1534 			       satrec.d5421, satrec.d5433, satrec.dedt,  satrec.didt,
1535 			       satrec.dmdt,  dndt,         satrec.dnodt, satrec.domdt ,
1536 			       satrec.del1,  satrec.del2,  satrec.del3,  satrec.xfact,
1537 			       satrec.xlamo, satrec.xli,   satrec.xni);
1538 		}
1539 
1540 		/* ----------- set variables if not deep space ----------- */
1541 		if (satrec.isimp != 1)
1542 		{
1543 			cc1sq        = satrec.cc1 * satrec.cc1;
1544 			satrec.d2    = 4.0 * ao * tsi * cc1sq;
1545 			temp         = satrec.d2 * tsi * satrec.cc1 / 3.0;
1546 			satrec.d3    = (17.0 * ao + sfour) * temp;
1547 			satrec.d4    = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * satrec.cc1;
1548 			satrec.t3cof = satrec.d2 + 2.0 * cc1sq;
1549 			satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * (12.0 * satrec.d2 + 10.0 * cc1sq));
1550 			satrec.t5cof = 0.2 * (3.0 * satrec.d4 + 12.0 * satrec.cc1 * satrec.d3 +
1551 				       6.0 * satrec.d2 * satrec.d2 + 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq));
1552 		}
1553 	} // if omeosq = 0 ...
1554 
1555 	/* finally propogate to zero epoch to initialize all others. */
1556 	// sgp4fix take out check to let satellites process until they are actually below earth surface
1557 	//       if(satrec.error == 0)
1558 	sgp4(whichconst, satrec, 0.0, r, v);
1559 
1560 	satrec.init = 'n';
1561 
1562 	//sgp4fix return boolean. satrec.error contains any error codes
1563 	return true;
1564 }  // end sgp4init
1565 
1566 /*-----------------------------------------------------------------------------
1567 *
1568 *                             procedure sgp4
1569 *
1570 *  this procedure is the sgp4 prediction model from space command. this is an
1571 *    updated and combined version of sgp4 and sdp4, which were originally
1572 *    published separately in spacetrack report #3. this version follows the
1573 *    methodology from the aiaa paper (2006) describing the history and
1574 *    development of the code.
1575 *
1576 *  author        : david vallado                  719-573-2600   28 jun 2005
1577 *
1578 *  inputs        :
1579 *    satrec	 - initialised structure from sgp4init() call.
1580 *    tsince	 - time eince epoch (minutes)
1581 *
1582 *  outputs       :
1583 *    r           - position vector                     km
1584 *    v           - velocity                            km/sec
1585 *  return code - non-zero on error.
1586 *                   1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
1587 *                   2 - mean motion less than 0.0
1588 *                   3 - pert elements, ecc < 0.0  or  ecc > 1.0
1589 *                   4 - semi-latus rectum < 0.0
1590 *                   5 - epoch elements are sub-orbital
1591 *                   6 - satellite has decayed
1592 *
1593 *  locals        :
1594 *    am          -
1595 *    axnl, aynl        -
1596 *    betal       -
1597 *    cosim   , sinim   , cosomm  , sinomm  , cnod    , snod    , cos2u   ,
1598 *    sin2u   , coseo1  , sineo1  , cosi    , sini    , cosip   , sinip   ,
1599 *    cosisq  , cossu   , sinsu   , cosu    , sinu
1600 *    delm        -
1601 *    delomg      -
1602 *    dndt        -
1603 *    eccm        -
1604 *    emsq        -
1605 *    ecose       -
1606 *    el2         -
1607 *    eo1         -
1608 *    eccp        -
1609 *    esine       -
1610 *    argpm       -
1611 *    argpp       -
1612 *    omgadf      -
1613 *    pl          -
1614 *    r           -
1615 *    rtemsq      -
1616 *    rdotl       -
1617 *    rl          -
1618 *    rvdot       -
1619 *    rvdotl      -
1620 *    su          -
1621 *    t2  , t3   , t4    , tc
1622 *    tem5, temp , temp1 , temp2  , tempa  , tempe  , templ
1623 *    u   , ux   , uy    , uz     , vx     , vy     , vz
1624 *    inclm       - inclination
1625 *    mm          - mean anomaly
1626 *    nm          - mean motion
1627 *    nodem       - right asc of ascending node
1628 *    xinc        -
1629 *    xincp       -
1630 *    xl          -
1631 *    xlm         -
1632 *    mp          -
1633 *    xmdf        -
1634 *    xmx         -
1635 *    xmy         -
1636 *    nodedf      -
1637 *    xnode       -
1638 *    nodep       -
1639 *    np          -
1640 *
1641 *  coupling      :
1642 *    getgravconst-
1643 *    dpper
1644 *    dpspace
1645 *
1646 *  references    :
1647 *    hoots, roehrich, norad spacetrack report #3 1980
1648 *    hoots, norad spacetrack report #6 1986
1649 *    hoots, schumacher and glover 2004
1650 *    vallado, crawford, hujsak, kelso  2006
1651   ----------------------------------------------------------------------------*/
1652 
sgp4(gravconsttype whichconst,elsetrec & satrec,double tsince,double r[3],double v[3])1653 bool sgp4(gravconsttype whichconst, elsetrec& satrec,  double tsince, double r[3],  double v[3])
1654 {
1655 	double am   , axnl  , aynl , betal , cosim , cnod  ,
1656 	       cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu,
1657 	       delm , delomg, em   , ecose , el2   , eo1   ,
1658 	       ep   , esine , argpm, argpp , argpdf, pl    , mrt ,
1659 	       mvt  , rdotl , rl   , rvdot , rvdotl, sinim ,
1660 	       sin2u, sineo1, sini , sinip , sinsu , sinu  ,
1661 	       snod , su    , t2   , t3    , t4    , tem5  , temp,
1662 	       temp1, temp2 , tempa, tempe , templ , u     , ux  ,
1663 	       uy   , uz    , vx   , vy    , vz    , inclm , mm  ,
1664 	       nm   , nodem , xinc , xincp , xl    , xlm   , mp  ,
1665 	       xmdf , xmx   , xmy  , nodedf, xnode , nodep , tc  , dndt ,
1666 	       twopi, x2o3  , j2   , j3    , tumin , j4    , xke , j3oj2,
1667 	       radiusearthkm, mu, vkmpersec;
1668 	int ktr;
1669 	coseo1 = 0.0;
1670 	mrt    = 0.0;
1671 	sineo1 = 0.0;
1672 	j2     = 0;
1673 	j4     = 0;
1674 	xke    = 0;
1675 	j3oj2  = 0;
1676 	radiusearthkm = 0;
1677 
1678 	/* ------------------ set mathematical constants --------------- */
1679 	// sgp4fix divisor for divide by zero check on inclination
1680 	// the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
1681 	// 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
1682 	const double temp4 =   1.5e-12;
1683 	twopi = 2.0 * M_PI;
1684 	x2o3  = 2.0 / 3.0;
1685 	// sgp4fix identify constants and allow alternate values
1686 	getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
1687 	vkmpersec     = radiusearthkm * xke/60.0;
1688 
1689 	/* --------------------- clear sgp4 error flag ----------------- */
1690 	satrec.t     = tsince;
1691 	satrec.error = 0;
1692 
1693 	/* ------- update for secular gravity and atmospheric drag ----- */
1694 	xmdf    = satrec.mo + satrec.mdot * satrec.t;
1695 	argpdf  = satrec.argpo + satrec.argpdot * satrec.t;
1696 	nodedf  = satrec.nodeo + satrec.nodedot * satrec.t;
1697 	argpm   = argpdf;
1698 	mm      = xmdf;
1699 	t2      = satrec.t * satrec.t;
1700 	nodem   = nodedf + satrec.nodecf * t2;
1701 	tempa   = 1.0 - satrec.cc1 * satrec.t;
1702 	tempe   = satrec.bstar * satrec.cc4 * satrec.t;
1703 	templ   = satrec.t2cof * t2;
1704 
1705 	if (satrec.isimp != 1)
1706 	{
1707 		delomg = satrec.omgcof * satrec.t;
1708 		delm   = satrec.xmcof * (std::pow((1.0 + satrec.eta * std::cos(xmdf)), 3) - satrec.delmo);
1709 		temp   = delomg + delm;
1710 		mm     = xmdf + temp;
1711 		argpm  = argpdf - temp;
1712 		t3     = t2 * satrec.t;
1713 		t4     = t3 * satrec.t;
1714 		tempa  = tempa - satrec.d2 * t2 - satrec.d3 * t3 - satrec.d4 * t4;
1715 		tempe  = tempe + satrec.bstar * satrec.cc5 * (std::sin(mm) - satrec.sinmao);
1716 		templ  = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + satrec.t * satrec.t5cof);
1717 	}
1718 
1719 	nm    = satrec.no;
1720 	em    = satrec.ecco;
1721 	inclm = satrec.inclo;
1722 	if (satrec.method == 'd')
1723 	{
1724 		tc = satrec.t;
1725 		dspace(satrec.irez,
1726 		       satrec.d2201, satrec.d2211, satrec.d3210,
1727 		       satrec.d3222, satrec.d4410, satrec.d4422,
1728 		       satrec.d5220, satrec.d5232, satrec.d5421,
1729 		       satrec.d5433, satrec.dedt,  satrec.del1,
1730 		       satrec.del2,  satrec.del3,  satrec.didt,
1731 		       satrec.dmdt,  satrec.dnodt, satrec.domdt,
1732 		       satrec.argpo, satrec.argpdot, satrec.t, tc,
1733 		       satrec.gsto, satrec.xfact, satrec.xlamo,
1734 		       satrec.no, satrec.atime,
1735 		       em, argpm, inclm, satrec.xli, mm, satrec.xni,
1736 		       nodem, dndt, nm);
1737 	} // if method = d
1738 
1739 	if (nm <= 0.0)
1740 	{
1741 		//         printf("# error nm %f\n", nm);
1742 		satrec.error = 2;
1743 		// sgp4fix add return
1744 		return false;
1745 	}
1746 	am = std::pow((xke / nm),x2o3) * tempa * tempa;
1747 	nm = xke / std::pow(am, 1.5);
1748 	em = em - tempe;
1749 	// fix tolerance for error recognition
1750 	// sgp4fix am is fixed from the previous nm check
1751 	if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ )
1752 	{
1753 		//         printf("# error em %f\n", em);
1754 		satrec.error = 1;
1755 		// sgp4fix to return if there is an error in eccentricity
1756 		return false;
1757 	}
1758 	// sgp4fix fix tolerance to avoid a divide by zero
1759 	if (em < 1.0e-6)
1760 		em  = 1.0e-6;
1761 	mm     = mm + satrec.no * templ;
1762 	xlm    = mm + argpm + nodem;
1763 	// NOTE: Never read!
1764 	// emsq   = em * em;
1765 	// temp   = 1.0 - emsq;
1766 
1767 	nodem  = std::fmod(nodem, twopi);
1768 	argpm  = std::fmod(argpm, twopi);
1769 	xlm    = std::fmod(xlm, twopi);
1770 	mm     = std::fmod(xlm - argpm - nodem, twopi);
1771 
1772 	/* ----------------- compute extra mean quantities ------------- */
1773 	sinim = std::sin(inclm);
1774 	cosim = std::cos(inclm);
1775 
1776 	/* -------------------- add lunar-solar periodics -------------- */
1777 	ep     = em;
1778 	xincp  = inclm;
1779 	argpp  = argpm;
1780 	nodep  = nodem;
1781 	mp     = mm;
1782 	sinip  = sinim;
1783 	cosip  = cosim;
1784 	if (satrec.method == 'd')
1785 	{
1786 		dpper(satrec.e3,   satrec.ee2,  satrec.peo,
1787 		      satrec.pgho, satrec.pho,  satrec.pinco,
1788 		      satrec.plo,  satrec.se2,  satrec.se3,
1789 		      satrec.sgh2, satrec.sgh3, satrec.sgh4,
1790 		      satrec.sh2,  satrec.sh3,  satrec.si2,
1791 		      satrec.si3,  satrec.sl2,  satrec.sl3,
1792 		      satrec.sl4,  satrec.t,    satrec.xgh2,
1793 		      satrec.xgh3, satrec.xgh4, satrec.xh2,
1794 		      satrec.xh3,  satrec.xi2,  satrec.xi3,
1795 		      satrec.xl2,  satrec.xl3,  satrec.xl4,
1796 		      satrec.zmol, satrec.zmos, satrec.inclo,
1797 		      'n', ep, xincp, nodep, argpp, mp, satrec.operationmode);
1798 		if (xincp < 0.0)
1799 		{
1800 			xincp = -xincp;
1801 			nodep = nodep + M_PI;
1802 			argpp = argpp - M_PI;
1803 		}
1804 		if ((ep < 0.0 ) || ( ep > 1.0))
1805 		{
1806 			//            printf("# error ep %f\n", ep);
1807 			satrec.error = 3;
1808 			// sgp4fix add return
1809 			return false;
1810 		}
1811 	} // if method = d
1812 
1813 	/* -------------------- long period periodics ------------------ */
1814 	if (satrec.method == 'd')
1815 	{
1816 		sinip = std::sin(xincp);
1817 		cosip = std::cos(xincp);
1818 		satrec.aycof = -0.5*j3oj2*sinip;
1819 		// sgp4fix for divide by zero for xincp = 180 deg
1820 		if (std::fabs(cosip+1.0) > 1.5e-12)
1821 			satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip);
1822 		else
1823 			satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4;
1824 	}
1825 	axnl = ep * std::cos(argpp);
1826 	temp = 1.0 / (am * (1.0 - ep * ep));
1827 	aynl = ep* std::sin(argpp) + temp * satrec.aycof;
1828 	xl   = mp + argpp + nodep + temp * satrec.xlcof * axnl;
1829 
1830 	/* --------------------- solve kepler's equation --------------- */
1831 	u    = std::fmod(xl - nodep, twopi);
1832 	eo1  = u;
1833 	tem5 = 9999.9;
1834 	ktr = 1;
1835 	//   sgp4fix for kepler iteration
1836 	//   the following iteration needs better limits on corrections
1837 	while (( std::fabs(tem5) >= 1.0e-12) && (ktr <= 10) )
1838 	{
1839 		sineo1 = std::sin(eo1);
1840 		coseo1 = std::cos(eo1);
1841 		tem5   = 1.0 - coseo1 * axnl - sineo1 * aynl;
1842 		tem5   = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
1843 		if(std::fabs(tem5) >= 0.95)
1844 			tem5 = tem5 > 0.0 ? 0.95 : -0.95;
1845 		eo1    = eo1 + tem5;
1846 		ktr = ktr + 1;
1847 	}
1848 
1849 	/* ------------- short period preliminary quantities ----------- */
1850 	ecose = axnl*coseo1 + aynl*sineo1;
1851 	esine = axnl*sineo1 - aynl*coseo1;
1852 	el2   = axnl*axnl + aynl*aynl;
1853 	pl    = am*(1.0-el2);
1854 	if (pl < 0.0)
1855 	{
1856 		//         printf("# error pl %f\n", pl);
1857 		satrec.error = 4;
1858 		// sgp4fix add return
1859 		return false;
1860 	}
1861 	else
1862 	{
1863 		rl     = am * (1.0 - ecose);
1864 		rdotl  = std::sqrt(am) * esine/rl;
1865 		rvdotl = std::sqrt(pl) / rl;
1866 		betal  = std::sqrt(1.0 - el2);
1867 		temp   = esine / (1.0 + betal);
1868 		sinu   = am / rl * (sineo1 - aynl - axnl * temp);
1869 		cosu   = am / rl * (coseo1 - axnl + aynl * temp);
1870 		su     = std::atan2(sinu, cosu);
1871 		sin2u  = (cosu + cosu) * sinu;
1872 		cos2u  = 1.0 - 2.0 * sinu * sinu;
1873 		temp   = 1.0 / pl;
1874 		temp1  = 0.5 * j2 * temp;
1875 		temp2  = temp1 * temp;
1876 
1877 		/* -------------- update for short period periodics ------------ */
1878 		if (satrec.method == 'd')
1879 		{
1880 			cosisq                 = cosip * cosip;
1881 			satrec.con41  = 3.0*cosisq - 1.0;
1882 			satrec.x1mth2 = 1.0 - cosisq;
1883 			satrec.x7thm1 = 7.0*cosisq - 1.0;
1884 		}
1885 		mrt   = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) +
1886 			0.5 * temp1 * satrec.x1mth2 * cos2u;
1887 		su    = su - 0.25 * temp2 * satrec.x7thm1 * sin2u;
1888 		xnode = nodep + 1.5 * temp2 * cosip * sin2u;
1889 		xinc  = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
1890 		mvt   = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke;
1891 		rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + 1.5 * satrec.con41) / xke;
1892 
1893 		/* --------------------- orientation vectors ------------------- */
1894 		sinsu = std::sin(su);
1895 		cossu = std::cos(su);
1896 		snod  = std::sin(xnode);
1897 		cnod  = std::cos(xnode);
1898 		sini  = std::sin(xinc);
1899 		cosi  = std::cos(xinc);
1900 		xmx   = -snod * cosi;
1901 		xmy   =  cnod * cosi;
1902 		ux    =  xmx * sinsu + cnod * cossu;
1903 		uy    =  xmy * sinsu + snod * cossu;
1904 		uz    =  sini * sinsu;
1905 		vx    =  xmx * cossu - cnod * sinsu;
1906 		vy    =  xmy * cossu - snod * sinsu;
1907 		vz    =  sini * cossu;
1908 
1909 		/* --------- position and velocity (in km and km/sec) ---------- */
1910 		r[0] = (mrt * ux)* radiusearthkm;
1911 		r[1] = (mrt * uy)* radiusearthkm;
1912 		r[2] = (mrt * uz)* radiusearthkm;
1913 		v[0] = (mvt * ux + rvdot * vx) * vkmpersec;
1914 		v[1] = (mvt * uy + rvdot * vy) * vkmpersec;
1915 		v[2] = (mvt * uz + rvdot * vz) * vkmpersec;
1916 	}  // if pl > 0
1917 
1918 	// sgp4fix for decaying satellites
1919 	if (mrt < 1.0)
1920 	{
1921 		//         printf("# decay condition %11.6f \n",mrt);
1922 		satrec.error = 6;
1923 		return false;
1924 	}
1925 
1926 	return true;
1927 }  // end sgp4
1928 
1929 
1930 /* -----------------------------------------------------------------------------
1931 *
1932 *                           function gstime
1933 *
1934 *  this function finds the greenwich sidereal time.
1935 *
1936 *  author        : david vallado                  719-573-2600    1 mar 2001
1937 *
1938 *  inputs          description                    range / units
1939 *    jdut1       - julian date in ut1             days from 4713 bc
1940 *
1941 *  outputs       :
1942 *    gstime      - greenwich sidereal time        0 to 2pi rad
1943 *
1944 *  locals        :
1945 *    temp        - temporary variable for doubles   rad
1946 *    tut1        - julian centuries from the
1947 *                  jan 1, 2000 12 h epoch (ut1)
1948 *
1949 *  coupling      :
1950 *    none
1951 *
1952 *  references    :
1953 *    vallado       2004, 191, eq 3-45
1954 * --------------------------------------------------------------------------- */
1955 
gstime(double jdut1)1956 double gstime(double jdut1)
1957 {
1958 	const double twopi = 2.0 * M_PI;
1959 	const double deg2rad = M_PI / 180.0;
1960 	double       temp, tut1;
1961 
1962 	tut1 = (jdut1 - 2451545.0) / 36525.0;
1963 	temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 +
1964 	       (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841;  // sec
1965 	temp = std::fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad
1966 
1967 	// ------------------------ check quadrants ---------------------
1968 	if (temp < 0.0)
1969 		temp += twopi;
1970 
1971 	return temp;
1972 }  // end gstime
1973 
1974 /* -----------------------------------------------------------------------------
1975 *
1976 *                           function getgravconst
1977 *
1978 *  this function gets constants for the propagator. note that mu is identified to
1979 *    facilitiate comparisons with newer models. the common useage is wgs72.
1980 *
1981 *  author        : david vallado                  719-573-2600   21 jul 2006
1982 *
1983 *  inputs        :
1984 *    whichconst  - which set of constants to use  wgs72old, wgs72, wgs84
1985 *
1986 *  outputs       :
1987 *    tumin       - minutes in one time unit
1988 *    mu          - earth gravitational parameter
1989 *    radiusearthkm - radius of the earth in km
1990 *    xke         - reciprocal of tumin
1991 *    j2, j3, j4  - un-normalized zonal harmonic values
1992 *    j3oj2       - j3 divided by j2
1993 *
1994 *  locals        :
1995 *
1996 *  coupling      :
1997 *    none
1998 *
1999 *  references    :
2000 *    norad spacetrack report #3
2001 *    vallado, crawford, hujsak, kelso  2006
2002   --------------------------------------------------------------------------- */
2003 
getgravconst(gravconsttype whichconst,double & tumin,double & mu,double & radiusearthkm,double & xke,double & j2,double & j3,double & j4,double & j3oj2)2004 void getgravconst(gravconsttype whichconst, double& tumin, double& mu, double& radiusearthkm,
2005 		  double& xke, double& j2, double& j3, double& j4, double& j3oj2)
2006 {
2007 	switch (whichconst)
2008 	{
2009 		case wgs72old:
2010 			// -- wgs-72 low precision str#3 constants --
2011 			mu     = 398600.79964;        // in km3 / s2
2012 			radiusearthkm = 6378.135;     // km
2013 			xke    = 0.0743669161;
2014 			tumin  = 1.0 / xke;
2015 			j2     =   0.001082616;
2016 			j3     =  -0.00000253881;
2017 			j4     =  -0.00000165597;
2018 			j3oj2  =  j3 / j2;
2019 			break;
2020 		case wgs72:
2021 			// ------------ wgs-72 constants ------------
2022 			mu     = 398600.8;            // in km3 / s2
2023 			radiusearthkm = 6378.135;     // km
2024 			xke    = 60.0 / std::sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
2025 			tumin  = 1.0 / xke;
2026 			j2     =   0.001082616;
2027 			j3     =  -0.00000253881;
2028 			j4     =  -0.00000165597;
2029 			j3oj2  =  j3 / j2;
2030 			break;
2031 		case wgs84:
2032 			// ------------ wgs-84 constants ------------
2033 			mu     = 398600.5;            // in km3 / s2
2034 			radiusearthkm = 6378.137;     // km
2035 			xke    = 60.0 / std::sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
2036 			tumin  = 1.0 / xke;
2037 			j2     =   0.00108262998905;
2038 			j3     =  -0.00000253215306;
2039 			j4     =  -0.00000161098761;
2040 			j3oj2  =  j3 / j2;
2041 			break;
2042 		default:
2043 			fprintf(stderr,"unknown gravity option (%d)\n",whichconst);
2044 			break;
2045 	}
2046 }   // end getgravconst
2047 
2048 
2049 
2050 
2051 
2052