/*-----------------------------------------------------------------------------
*
* procedure sgp4init
*
* this procedure initializes variables for sgp4.
*
* author : david vallado 719-573-2600 28 jun 2005
* author : david vallado 719-573-2600 28 jun 2005
*
* inputs :
* opsmode - mode of operation afspc or improved 'a', 'i'
* satn - satellite number
* bstar - sgp4 type drag coefficient kg/m2er
* ecco - eccentricity
* epoch - epoch time in days from jan 0, 1950. 0 hr
* argpo - argument of perigee (output if ds)
* inclo - inclination
* mo - mean anomaly (output if ds)
* no - mean motion
* nodeo - right ascension of ascending node
*
* outputs :
* rec - common values for subsequent calls
* return code - non-zero on error.
* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
* 2 - mean motion less than 0.0
* 3 - pert elements, ecc < 0.0 or ecc > 1.0
* 4 - semi-latus rectum < 0.0
* 5 - epoch elements are sub-orbital
* 6 - satellite has decayed
*
* locals :
* cnodm , snodm , cosim , sinim , cosomm , sinomm
* cc1sq , cc2 , cc3
* coef , coef1
* cosio4 -
* day -
* dndt -
* em - eccentricity
* emsq - eccentricity squared
* eeta -
* etasq -
* gam -
* argpm - argument of perigee
* nodem -
* inclm - inclination
* mm - mean anomaly
* nm - mean motion
* perige - perigee
* pinvsq -
* psisq -
* qzms24 -
* rtemsq -
* s1, s2, s3, s4, s5, s6, s7 -
* sfour -
* ss1, ss2, ss3, ss4, ss5, ss6, ss7 -
* sz1, sz2, sz3
* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
* tc -
* temp -
* temp1, temp2, temp3 -
* tsi -
* xpidot -
* xhdot1 -
* z1, z2, z3 -
* z11, z12, z13, z21, z22, z23, z31, z32, z33 -
*
* coupling :
* getgravconst-
* initl -
* dscom -
* dpper -
* dsinit -
* sgp4 -
*
* references :
* hoots, roehrich, norad spacetrack report #3 1980
* hoots, norad spacetrack report #6 1986
* hoots, schumacher and glover 2004
* vallado, crawford, hujsak, kelso 2006
* ----------------------------------------------------------------------------*/
public void sgp4init(Satrec satrec, Sgp4initOptions options)
{
Globals globals = new Globals();
double pi = globals.pi;
double earthRadius = globals.earthRadius;
double j2 = globals.j2;
double j4 = globals.j4;
double j3oj2 = globals.j3oj2;
double x2o3 = globals.x2o3;
char opsmode = options.opsmode;
string satn = options.satn;
double epoch = options.epoch;
double xbstar = options.xbstar;
double xecco = options.xecco;
double xargpo = options.xargpo;
double xinclo = options.xinclo;
double xmo = options.xmo;
double xno = options.xno;
double xnodeo = options.xnodeo;
double cosim;
double sinim;
double cc1sq;
double cc2;
double cc3;
double coef;
double coef1;
double cosio4;
double em;
double emsq;
double eeta;
double etasq;
double argpm;
double nodem;
double inclm;
double mm;
double nm;
double perige;
double pinvsq;
double psisq;
double qzms24;
double s1;
double s2;
double s3;
double s4;
double s5;
double sfour;
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 tc;
double temp;
double temp1;
double temp2;
double temp3;
double tsi;
double xpidot;
double xhdot1;
double z1;
double z3;
double z11;
double z13;
double z21;
double z23;
double z31;
double z33;
/* ------------------------ initialization --------------------- */
// sgp4fix divisor for divide by zero check on inclination
// the old check used 1.0 + Math.cos(pi-1.0e-9), but then compared it to
// 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
double temp4 = 1.5e-12;
// ----------- set all near earth variables to zero ------------
satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0;
satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0;
satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0;
satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0;
satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0;
satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0;
satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0;
satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0;
satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0;
// ----------- set all deep space variables to zero ------------
satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0;
satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0;
satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0;
satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0;
satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0;
satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0;
satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0;
satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0;
satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0;
satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0;
satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0;
satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0;
satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0;
satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0;
satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0;
satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0;
satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0;
satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0;
satrec.xli = 0.0; satrec.xni = 0.0;
// sgp4fix - note the following variables are also passed directly via satrec.
// it is possible to streamline the sgp4init call by deleting the "x"
// variables, but the user would need to set the satrec.* values first. we
// include the additional assignments in case twoline2rv is not used.
satrec.bstar = xbstar;
satrec.ecco = xecco;
satrec.argpo = xargpo;
satrec.inclo = xinclo;
satrec.mo = xmo;
satrec.no = xno;
satrec.nodeo = xnodeo;
// sgp4fix add opsmode
satrec.operationmode = opsmode;
// ------------------------ earth constants -----------------------
// sgp4fix identify constants and allow alternate values
double ss = (78.0 / earthRadius) + 1.0;
// sgp4fix use multiply for speed instead of pow
double qzms2ttemp = (120.0 - 78.0) / earthRadius;
double qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp;
satrec.init = 'y';
satrec.t = 0.0;
InitlOptions initlOptions = new InitlOptions();
initlOptions.satn = satn;
initlOptions.ecco = satrec.ecco;
initlOptions.epoch = epoch;
initlOptions.inlco = satrec.inclo;
initlOptions.no = satrec.no;
initlOptions.method = satrec.method;
initlOptions.opsmode = satrec.operationmode;
Initl initl = new Initl();
InitlResult initlResult = new InitlResult();
initlResult = initl.initl(initlOptions); //TODO: Finish impl
double ao = initlResult.ao;
double con42 = initlResult.con42;
double cosio = initlResult.cosio;
double cosio2 = initlResult.cosio2;
double eccsq = initlResult.eccsq;
double omeosq = initlResult.omeosq;
double posq = initlResult.posq;
double rp = initlResult.rp;
double rteosq = initlResult.rteosq;
double sinio = initlResult.sinio;
satrec.no = initlResult.no;
satrec.con41 = initlResult.con41;
satrec.gsto = initlResult.gsto;
satrec.error = 0;
// sgp4fix remove this check as it is unnecessary
// the mrt check in sgp4 handles decaying satellite cases even if the starting
// condition is below the surface of te earth
// if (rp < 1.0)
// {
// printf("// *** satn%d epoch elts sub-orbital ***\n", satn);
// satrec.error = 5;
// }
if (omeosq >= 0.0 || satrec.no >= 0.0)
{
satrec.isimp = 0;
if (rp < (220.0 / earthRadius + 1.0))
{
satrec.isimp = 1;
}
sfour = ss;
qzms24 = qzms2t;
perige = (rp - 1.0) * earthRadius;
// - for perigees below 156 km, s and qoms2t are altered -
if (perige < 156.0)
{
sfour = perige - 78.0;
if (perige < 98.0)
{
sfour = 20.0;
}
// sgp4fix use multiply for speed instead of pow
double qzms24temp = (120.0 - sfour) / earthRadius;
qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp;
sfour = (sfour / earthRadius) + 1.0;
}
pinvsq = 1.0 / posq;
tsi = 1.0 / (ao - sfour);
satrec.eta = ao * satrec.ecco * tsi;
etasq = satrec.eta * satrec.eta;
eeta = satrec.ecco * satrec.eta;
psisq = Math.Abs(1.0 - etasq);
coef = qzms24 * Math.Pow(tsi, 4.0);
coef1 = coef / (Math.Pow(psisq, 3.5));
cc2 = coef1 * satrec.no * ((ao * (1.0 + (1.5 * etasq) + (eeta * (4.0 + etasq))))
+ (((0.375 * j2 * tsi) / psisq) * satrec.con41
* (8.0 + (3.0 * etasq * (8.0 + etasq)))));
satrec.cc1 = satrec.bstar * cc2;
cc3 = 0.0;
if (satrec.ecco > 1.0e-4)
{
cc3 = (-2.0 * coef * tsi * j3oj2 * satrec.no * sinio) / satrec.ecco;
}
satrec.x1mth2 = 1.0 - cosio2;
satrec.cc4 = 2.0 * satrec.no * coef1 * ao * omeosq * (
((satrec.eta * (2.0 + (0.5 * etasq)))
+ (satrec.ecco * (0.5 + (2.0 * etasq))))
- (((j2 * tsi) / (ao * psisq))
* ((-3.0 * satrec.con41 * ((1.0 - (2.0 * eeta)) + (etasq * (1.5 - (0.5 * eeta)))))
+ (0.75 * satrec.x1mth2
* ((2.0 * etasq) - (eeta * (1.0 + etasq)))
* Math.Cos(2.0 * satrec.argpo))))
);
satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + (2.75 * (etasq + eeta)) + (eeta * etasq));
cosio4 = cosio2 * cosio2;
temp1 = 1.5 * j2 * pinvsq * satrec.no;
temp2 = 0.5 * temp1 * j2 * pinvsq;
temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no;
satrec.mdot = satrec.no + (0.5 * temp1 * rteosq * satrec.con41)
+ (0.0625 * temp2 * rteosq * ((13.0 - (78.0 * cosio2)) + (137.0 * cosio4)));
satrec.argpdot = (-0.5 * temp1 * con42)
+ (0.0625 * temp2 * ((7.0 - (114.0 * cosio2)) + (395.0 * cosio4)))
+ (temp3 * ((3.0 - (36.0 * cosio2)) + (49.0 * cosio4)));
xhdot1 = -temp1 * cosio;
satrec.nodedot = xhdot1 + (((0.5 * temp2 * (4.0 - (19.0 * cosio2)))
+ (2.0 * temp3 * (3.0 - (7.0 * cosio2)))) * cosio);
xpidot = satrec.argpdot + satrec.nodedot;
satrec.omgcof = satrec.bstar * cc3 * Math.Cos(satrec.argpo);
satrec.xmcof = 0.0;
if (satrec.ecco > 1.0e-4)
{
satrec.xmcof = (-x2o3 * coef * satrec.bstar) / eeta;
}
satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1;
satrec.t2cof = 1.5 * satrec.cc1;
// sgp4fix for divide by zero with xinco = 180 deg
if (Math.Abs(cosio + 1.0) > 1.5e-12)
{
satrec.xlcof = (-0.25 * j3oj2 * sinio * (3.0 + (5.0 * cosio))) / (1.0 + cosio);
}
else
{
satrec.xlcof = (-0.25 * j3oj2 * sinio * (3.0 + (5.0 * cosio))) / temp4;
}
satrec.aycof = -0.5 * j3oj2 * sinio;
// sgp4fix use multiply for speed instead of pow
double delmotemp = 1.0 + (satrec.eta * Math.Cos(satrec.mo));
satrec.delmo = delmotemp * delmotemp * delmotemp;
satrec.sinmao = Math.Sin(satrec.mo);
satrec.x7thm1 = (7.0 * cosio2) - 1.0;
// --------------- deep space initialization -------------
if ((2 * pi) / satrec.no >= 225.0)
{
satrec.method = 'd';
satrec.isimp = 1;
tc = 0.0;
inclm = satrec.inclo;
DscomOptions dscomOptions = new DscomOptions();
dscomOptions.epoch = epoch;
dscomOptions.ep = satrec.ecco;
dscomOptions.argpp = satrec.argpo;
dscomOptions.tc = tc;
dscomOptions.inclp = satrec.inclo;
dscomOptions.nodep = satrec.nodeo;
dscomOptions.np = satrec.no;
dscomOptions.e3 = satrec.e3;
dscomOptions.ee2 = satrec.ee2;
dscomOptions.peo = satrec.peo;
dscomOptions.pgho = satrec.pgho;
dscomOptions.pho = satrec.pho;
dscomOptions.pinco = satrec.pinco;
dscomOptions.plo = satrec.plo;
dscomOptions.se2 = satrec.se2;
dscomOptions.se3 = satrec.se3;
dscomOptions.sgh2 = satrec.sgh2;
dscomOptions.sgh3 = satrec.sgh3;
dscomOptions.sgh4 = satrec.sgh4;
dscomOptions.sh2 = satrec.sh2;
dscomOptions.sh3 = satrec.sh3;
dscomOptions.si2 = satrec.si2;
dscomOptions.si3 = satrec.si3;
dscomOptions.sl2 = satrec.sl2;
dscomOptions.sl3 = satrec.sl3;
dscomOptions.sl4 = satrec.sl4;
dscomOptions.xgh2 = satrec.xgh2;
dscomOptions.xgh3 = satrec.xgh3;
dscomOptions.xgh4 = satrec.xgh4;
dscomOptions.xh2 = satrec.xh2;
dscomOptions.xh3 = satrec.xh3;
dscomOptions.xi2 = satrec.xi2;
dscomOptions.xi3 = satrec.xi3;
dscomOptions.xl2 = satrec.xl2;
dscomOptions.xl3 = satrec.xl3;
dscomOptions.xl4 = satrec.xl4;
dscomOptions.zmol = satrec.zmol;
dscomOptions.zmos = satrec.zmos;
Dscom dscom = new Dscom();
DscomResult dscomResult = new DscomResult();
dscomResult = dscom.dscom(dscomOptions);
satrec.e3 = dscomResult.e3;
satrec.ee2 = dscomResult.ee2;
satrec.peo = dscomResult.peo;
satrec.pgho = dscomResult.pgho;
satrec.pho = dscomResult.pho;
satrec.pinco = dscomResult.pinco;
satrec.plo = dscomResult.plo;
satrec.se2 = dscomResult.se2;
satrec.se3 = dscomResult.se3;
satrec.sgh2 = dscomResult.sgh2;
satrec.sgh3 = dscomResult.sgh3;
satrec.sgh4 = dscomResult.sgh4;
satrec.sh2 = dscomResult.sh2;
satrec.sh3 = dscomResult.sh3;
satrec.si2 = dscomResult.si2;
satrec.si3 = dscomResult.si3;
satrec.sl2 = dscomResult.sl2;
satrec.sl3 = dscomResult.sl3;
satrec.sl4 = dscomResult.sl4;
sinim = dscomResult.sinim;
cosim = dscomResult.cosim;
em = dscomResult.em;
emsq = dscomResult.emsq;
s1 = dscomResult.s1;
s2 = dscomResult.s2;
s3 = dscomResult.s3;
s4 = dscomResult.s4;
s5 = dscomResult.s5;
ss1 = dscomResult.ss1;
ss2 = dscomResult.ss2;
ss3 = dscomResult.ss3;
ss4 = dscomResult.ss4;
ss5 = dscomResult.ss5;
sz1 = dscomResult.sz1;
sz3 = dscomResult.sz3;
sz11 = dscomResult.sz11;
sz13 = dscomResult.sz13;
sz21 = dscomResult.sz21;
sz23 = dscomResult.sz23;
sz31 = dscomResult.sz31;
sz33 = dscomResult.sz33;
satrec.xgh2 = dscomResult.xgh2;
satrec.xgh3 = dscomResult.xgh3;
satrec.xgh4 = dscomResult.xgh4;
satrec.xh2 = dscomResult.xh2;
satrec.xh3 = dscomResult.xh3;
satrec.xi2 = dscomResult.xi2;
satrec.xi3 = dscomResult.xi3;
satrec.xl2 = dscomResult.xl2;
satrec.xl3 = dscomResult.xl3;
satrec.xl4 = dscomResult.xl4;
satrec.zmol = dscomResult.zmol;
satrec.zmos = dscomResult.zmos;
nm = dscomResult.nm;
z1 = dscomResult.z1;
z3 = dscomResult.z3;
z11 = dscomResult.z11;
z13 = dscomResult.z13;
z21 = dscomResult.z21;
z23 = dscomResult.z23;
z31 = dscomResult.z31;
z33 = dscomResult.z33;
DpperOptions dpperOptions = new DpperOptions();
dpperOptions.inclo = inclm;
dpperOptions.init = satrec.init;
dpperOptions.ep = satrec.ecco;
dpperOptions.inclp = satrec.inclo;
dpperOptions.nodep = satrec.nodeo;
dpperOptions.argpp = satrec.argpo;
dpperOptions.mp = satrec.mo;
dpperOptions.opsmode = satrec.operationmode;
Dpper dpper = new Dpper();
DpperResult dpperResult = new DpperResult();
dpperResult = dpper.dpper(satrec, dpperOptions);
satrec.ecco = dpperResult.ep;
satrec.inclo = dpperResult.inclp;
satrec.nodeo = dpperResult.nodep;
satrec.argpo = dpperResult.argpp;
satrec.mo = dpperResult.mp;
argpm = 0.0;
nodem = 0.0;
mm = 0.0;
DsinitOptions dsinitOptions = new DsinitOptions();
dsinitOptions.cosim = cosim;
dsinitOptions.emsq = emsq;
dsinitOptions.argpo = satrec.argpo;
dsinitOptions.s1 = s1;
dsinitOptions.s2 = s2;
dsinitOptions.s3 = s3;
dsinitOptions.s4 = s4;
dsinitOptions.s5 = s5;
dsinitOptions.sinim = sinim;
dsinitOptions.ss1 = ss1;
dsinitOptions.ss2 = ss2;
dsinitOptions.ss3 = ss3;
dsinitOptions.ss4 = ss4;
dsinitOptions.ss5 = ss5;
dsinitOptions.sz1 = sz1;
dsinitOptions.sz3 = sz3;
dsinitOptions.sz11 = sz11;
dsinitOptions.sz13 = sz13;
dsinitOptions.sz21 = sz21;
dsinitOptions.sz23 = sz23;
dsinitOptions.sz31 = sz31;
dsinitOptions.sz33 = sz33;
dsinitOptions.t = satrec.t;
dsinitOptions.tc = tc;
dsinitOptions.gsto = satrec.gsto;
dsinitOptions.mo = satrec.mo;
dsinitOptions.mdot = satrec.mdot;
dsinitOptions.no = satrec.no;
dsinitOptions.nodeo = satrec.nodeo;
dsinitOptions.nodedot = satrec.nodedot;
dsinitOptions.xpidot = xpidot;
dsinitOptions.z1 = z1;
dsinitOptions.z3 = z3;
dsinitOptions.z11 = z11;
dsinitOptions.z13 = z13;
dsinitOptions.z21 = z21;
dsinitOptions.z23 = z23;
dsinitOptions.z31 = z31;
dsinitOptions.z33 = z33;
dsinitOptions.ecco = satrec.ecco;
dsinitOptions.eccsq = eccsq;
dsinitOptions.em = em;
dsinitOptions.argpm = argpm;
dsinitOptions.inclm = inclm;
dsinitOptions.mm = mm;
dsinitOptions.nm = nm;
dsinitOptions.nodem = nodem;
dsinitOptions.irez = satrec.irez;
dsinitOptions.atime = satrec.atime;
dsinitOptions.d2201 = satrec.d2201;
dsinitOptions.d2211 = satrec.d2211;
dsinitOptions.d3210 = satrec.d3210;
dsinitOptions.d3222 = satrec.d3222;
dsinitOptions.d4410 = satrec.d4410;
dsinitOptions.d4422 = satrec.d4422;
dsinitOptions.d5220 = satrec.d5220;
dsinitOptions.d5232 = satrec.d5232;
dsinitOptions.d5421 = satrec.d5421;
dsinitOptions.d5433 = satrec.d5433;
dsinitOptions.dedt = satrec.dedt;
dsinitOptions.didt = satrec.didt;
dsinitOptions.dmdt = satrec.dmdt;
dsinitOptions.dnodt = satrec.dnodt;
dsinitOptions.domdt = satrec.domdt;
dsinitOptions.del1 = satrec.del1;
dsinitOptions.del2 = satrec.del2;
dsinitOptions.del3 = satrec.del3;
dsinitOptions.xfact = satrec.xfact;
dsinitOptions.xlamo = satrec.xlamo;
dsinitOptions.xli = satrec.xli;
dsinitOptions.xni = satrec.xni;
Dsinit dsinit = new Dsinit();
DsinitResult dsinitResult = new DsinitResult();
dsinitResult = dsinit.dsinit(dsinitOptions);
satrec.irez = dsinitResult.irez;
satrec.atime = dsinitResult.atime;
satrec.d2201 = dsinitResult.d2201;
satrec.d2211 = dsinitResult.d2211;
satrec.d3210 = dsinitResult.d3210;
satrec.d3222 = dsinitResult.d3222;
satrec.d4410 = dsinitResult.d4410;
satrec.d4422 = dsinitResult.d4422;
satrec.d5220 = dsinitResult.d5220;
satrec.d5232 = dsinitResult.d5232;
satrec.d5421 = dsinitResult.d5421;
satrec.d5433 = dsinitResult.d5433;
satrec.dedt = dsinitResult.dedt;
satrec.didt = dsinitResult.didt;
satrec.dmdt = dsinitResult.dmdt;
satrec.dnodt = dsinitResult.dnodt;
satrec.domdt = dsinitResult.domdt;
satrec.del1 = dsinitResult.del1;
satrec.del2 = dsinitResult.del2;
satrec.del3 = dsinitResult.del3;
satrec.xfact = dsinitResult.xfact;
satrec.xlamo = dsinitResult.xlamo;
satrec.xli = dsinitResult.xli;
satrec.xni = dsinitResult.xni;
}
// ----------- set variables if not deep space -----------
if (satrec.isimp != 1)
{
cc1sq = satrec.cc1 * satrec.cc1;
satrec.d2 = 4.0 * ao * tsi * cc1sq;
temp = (satrec.d2 * tsi * satrec.cc1) / 3.0;
satrec.d3 = ((17.0 * ao) + sfour) * temp;
satrec.d4 = 0.5 * temp * ao * tsi * ((221.0 * ao) + (31.0 * sfour)) * satrec.cc1;
satrec.t3cof = satrec.d2 + (2.0 * cc1sq);
satrec.t4cof = 0.25 * ((3.0 * satrec.d3)
+ (satrec.cc1 * ((12.0 * satrec.d2) + (10.0 * cc1sq))));
satrec.t5cof = 0.2 * (
(3.0 * satrec.d4)
+ (12.0 * satrec.cc1 * satrec.d3)
+ (6.0 * satrec.d2 * satrec.d2)
+ (15.0 * cc1sq * ((2.0 * satrec.d2) + cc1sq))
);
}
/* finally propogate to zero epoch to initialize all others. */
// sgp4fix take out check to let satellites process until they are actually below earth surface
// if(satrec.error == 0)
}
Sgp4 sgp4 = new Sgp4();
sgp4.sgp4(satrec, 0);
satrec.init = 'n';
/* eslint-enable no-param-reassign */
}
/* -----------------------------------------------------------------------------
*
* procedure dpper
*
* this procedure provides deep space long period periodic contributions
* to the mean elements. by design, these periodics are zero at epoch.
* this used to be dscom which included initialization, but it's really a
* recurring function.
*
* author : david vallado 719-573-2600 28 jun 2005
*
* inputs :
* e3 -
* ee2 -
* peo -
* pgho -
* pho -
* pinco -
* plo -
* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 -
* t -
* xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
* zmol -
* zmos -
* ep - eccentricity 0.0 - 1.0
* inclo - inclination - needed for lyddane modification
* nodep - right ascension of ascending node
* argpp - argument of perigee
* mp - mean anomaly
*
* outputs :
* ep - eccentricity 0.0 - 1.0
* inclp - inclination
* nodep - right ascension of ascending node
* argpp - argument of perigee
* mp - mean anomaly
*
* locals :
* alfdp -
* betdp -
* cosip , sinip , cosop , sinop ,
* dalf -
* dbet -
* dls -
* f2, f3 -
* pe -
* pgh -
* ph -
* pinc -
* pl -
* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis ,
* sll , sls
* xls -
* xnoh -
* zf -
* zm -
*
* coupling :
* none.
*
* references :
* hoots, roehrich, norad spacetrack report #3 1980
* hoots, norad spacetrack report #6 1986
* hoots, schumacher and glover 2004
* vallado, crawford, hujsak, kelso 2006
* ----------------------------------------------------------------------------*/
public DpperResult dpper(Satrec satrec, DpperOptions dpperOptions)
{
Globals globals = new Globals();
double pi = globals.pi;
double twoPi = globals.twoPi;
double e3 = satrec.e3;
double ee2 = satrec.ee2;
double peo = satrec.peo;
double pgho = satrec.pgho;
double pho = satrec.pho;
double pinco = satrec.pinco;
double plo = satrec.plo;
double se2 = satrec.se2;
double se3 = satrec.se3;
double sgh2 = satrec.sgh2;
double sgh3 = satrec.sgh3;
double sgh4 = satrec.sgh4;
double sh2 = satrec.sh2;
double sh3 = satrec.sh3;
double si2 = satrec.si2;
double si3 = satrec.si3;
double sl2 = satrec.sl2;
double sl3 = satrec.sl3;
double sl4 = satrec.sl4;
double t = satrec.t;
double xgh2 = satrec.xgh2;
double xgh3 = satrec.xgh3;
double xgh4 = satrec.xgh4;
double xh2 = satrec.xh2;
double xh3 = satrec.xh3;
double xi2 = satrec.xi2;
double xi3 = satrec.xi3;
double xl2 = satrec.xl2;
double xl3 = satrec.xl3;
double xl4 = satrec.xl4;
double zmol = satrec.zmol;
double zmos = satrec.zmos;
double init = dpperOptions.init;
double opsmode = dpperOptions.opsmode;
double ep = dpperOptions.ep;
double inclp = dpperOptions.inclp;
double nodep = dpperOptions.nodep;
double argpp = dpperOptions.argpp;
double mp = dpperOptions.mp;
// Copy satellite attributes into local variables for convenience
// and symmetry in writing formulae.
double alfdp = 0.0;
double betdp = 0.0;
double cosip = 0.0;
double sinip = 0.0;
double cosop = 0.0;
double sinop = 0.0;
double dalf = 0.0;
double dbet = 0.0;
double dls = 0.0;
double f2 = 0.0;
double f3 = 0.0;
double pe = 0.0;
double pgh = 0.0;
double ph = 0.0;
double pinc = 0.0;
double pl = 0.0;
double sinzf = 0.0;
double xls = 0.0;
double xnoh = 0.0;
double zf = 0.0;
double zm = 0.0;
// ---------------------- constants -----------------------------
double zns = 1.19459e-5;
double zes = 0.01675;
double znl = 1.5835218e-4;
double zel = 0.05490;
// --------------- calculate time varying periodics -----------
zm = zmos + (zns * t);
// be sure that the initial call has time set to zero
if (init == 'y')
{
zm = zmos;
}
zf = zm + (2.0 * zes * Math.Sin(zm));
sinzf = Math.Sin(zf);
f2 = (0.5 * sinzf * sinzf) - 0.25;
f3 = -0.5 * sinzf * Math.Cos(zf);
double ses = (se2 * f2) + (se3 * f3);
double sis = (si2 * f2) + (si3 * f3);
double sls = (sl2 * f2) + (sl3 * f3) + (sl4 * sinzf);
double sghs = (sgh2 * f2) + (sgh3 * f3) + (sgh4 * sinzf);
double shs = (sh2 * f2) + (sh3 * f3);
zm = zmol + (znl * t);
if (init == 'y')
{
zm = zmol;
}
zf = zm + (2.0 * zel * Math.Sin(zm));
sinzf = Math.Sin(zf);
f2 = (0.5 * sinzf * sinzf) - 0.25;
f3 = -0.5 * sinzf * Math.Cos(zf);
double sel = (ee2 * f2) + (e3 * f3);
double sil = (xi2 * f2) + (xi3 * f3);
double sll = (xl2 * f2) + (xl3 * f3) + (xl4 * sinzf);
double sghl = (xgh2 * f2) + (xgh3 * f3) + (xgh4 * sinzf);
double shll = (xh2 * f2) + (xh3 * f3);
pe = ses + sel;
pinc = sis + sil;
pl = sls + sll;
pgh = sghs + sghl;
ph = shs + shll;
if (init == 'n')
{
pe -= peo;
pinc -= pinco;
pl -= plo;
pgh -= pgho;
ph -= pho;
inclp += pinc;
ep += pe;
sinip = Math.Sin(inclp);
cosip = Math.Cos(inclp);
/* ----------------- apply periodics directly ------------ */
// sgp4fix for lyddane choice
// strn3 used original inclination - this is technically feasible
// gsfc used perturbed inclination - also technically feasible
// probably best to readjust the 0.2 limit value and limit discontinuity
// 0.2 rad = 11.45916 deg
// use next line for original strn3 approach and original inclination
// if (inclo >= 0.2)
// use next line for gsfc version and perturbed inclination
if (inclp >= 0.2)
{
ph /= sinip;
pgh -= cosip * ph;
argpp += pgh;
nodep += ph;
mp += pl;
}
else
{
// ---- apply periodics with lyddane modification ----
sinop = Math.Sin(nodep);
cosop = Math.Cos(nodep);
alfdp = sinip * sinop;
betdp = sinip * cosop;
dalf = (ph * cosop) + (pinc * cosip * sinop);
dbet = (-ph * sinop) + (pinc * cosip * cosop);
alfdp += dalf;
betdp += dbet;
nodep %= twoPi;
// sgp4fix for afspc written intrinsic functions
// nodep used without a trigonometric function ahead
if (nodep < 0.0 && opsmode == 'a')
{
nodep += twoPi;
}
xls = mp + argpp + (cosip * nodep);
dls = (pl + pgh) - (pinc * nodep * sinip);
xls += dls;
xnoh = nodep;
nodep = Math.Atan2(alfdp, betdp);
// sgp4fix for afspc written intrinsic functions
// nodep used without a trigonometric function ahead
if (nodep < 0.0 && opsmode == 'a')
{
nodep += twoPi;
}
if (Math.Abs(xnoh - nodep) > pi)
{
if (nodep < xnoh)
{
nodep += twoPi;
}
else
{
nodep -= twoPi;
}
}
mp += pl;
argpp = xls - mp - (cosip * nodep);
}
}
DpperResult dpperResult = new DpperResult();
dpperResult.ep = ep;
dpperResult.inclp = inclp;
dpperResult.nodep = nodep;
dpperResult.argpp = argpp;
dpperResult.mp = mp;
return(dpperResult);
}
/*----------------------------------------------------------------------------
*
* procedure sgp4
*
* this procedure is the sgp4 prediction model from space command. this is an
* updated and combined version of sgp4 and sdp4, which were originally
* published separately in spacetrack report //3. this version follows the
* methodology from the aiaa paper (2006) describing the history and
* development of the code.
*
* author : david vallado 719-573-2600 28 jun 2005
*
* inputs :
* satrec - initialised structure from sgp4init() call.
* tsince - time since epoch (minutes)
*
* outputs :
* r - position vector km
* v - velocity km/sec
* return code - non-zero on error.
* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
* 2 - mean motion less than 0.0
* 3 - pert elements, ecc < 0.0 or ecc > 1.0
* 4 - semi-latus rectum < 0.0
* 5 - epoch elements are sub-orbital
* 6 - satellite has decayed
*
* locals :
* am -
* axnl, aynl -
* betal -
* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u ,
* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip ,
* cosisq , cossu , sinsu , cosu , sinu
* delm -
* delomg -
* dndt -
* eccm -
* emsq -
* ecose -
* el2 -
* eo1 -
* eccp -
* esine -
* argpm -
* argpp -
* omgadf -
* pl -
* r -
* rtemsq -
* rdotl -
* rl -
* rvdot -
* rvdotl -
* su -
* t2 , t3 , t4 , tc
* tem5, temp , temp1 , temp2 , tempa , tempe , templ
* u , ux , uy , uz , vx , vy , vz
* inclm - inclination
* mm - mean anomaly
* nm - mean motion
* nodem - right asc of ascending node
* xinc -
* xincp -
* xl -
* xlm -
* mp -
* xmdf -
* xmx -
* xmy -
* nodedf -
* xnode -
* nodep -
* np -
*
* coupling :
* getgravconst-
* dpper
* dspace
*
* references :
* hoots, roehrich, norad spacetrack report //3 1980
* hoots, norad spacetrack report //6 1986
* hoots, schumacher and glover 2004
* vallado, crawford, hujsak, kelso 2006
* ----------------------------------------------------------------------------*/
public PositionAndVelocity sgp4(Satrec satrec, double tsince)
{
Globals globals = new Globals();
double pi = globals.pi;
double twoPi = globals.twoPi;
double earthRadius = globals.earthRadius;
double xke = globals.xke;
double vkmpersec = globals.vkmpersec;
double j2 = globals.j2;
double j3oj2 = globals.j3oj2;
double x2o3 = globals.x2o3;
double coseo1 = 0.0;
double sineo1 = 0.0;
double cosip = 0.0;
double sinip = 0.0;
double cosisq = 0.0;
double delm = 0.0;
double delomg = 0.0;
double eo1 = 0.0;
double argpm = 0.0;
double argpp = 0.0;
double su = 0.0;
double t3 = 0.0;
double t4 = 0.0;
double tc = 0.0;
double tem5 = 0.0;
double temp = 0.0;
double tempa = 0.0;
double tempe = 0.0;
double templ = 0.0;
double inclm = 0.0;
double mm = 0.0;
double nm = 0.0;
double nodem = 0.0;
double xincp = 0.0;
double xlm = 0.0;
double mp = 0.0;
double nodep = 0.0;
/* ------------------ set mathematical constants --------------- */
// sgp4fix divisor for divide by zero check on inclination
// the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
// 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
double temp4 = 1.5e-12;
// --------------------- clear sgp4 error flag -----------------
satrec.t = tsince;
satrec.error = 0;
// ------- update for secular gravity and atmospheric drag -----
double xmdf = satrec.mo + (satrec.mdot * satrec.t);
double argpdf = satrec.argpo + (satrec.argpdot * satrec.t);
double nodedf = satrec.nodeo + (satrec.nodedot * satrec.t);
argpm = argpdf;
mm = xmdf;
double t2 = satrec.t * satrec.t;
nodem = nodedf + (satrec.nodecf * t2);
tempa = 1.0 - (satrec.cc1 * satrec.t);
tempe = satrec.bstar * satrec.cc4 * satrec.t;
templ = satrec.t2cof * t2;
if (satrec.isimp != 1)
{
delomg = satrec.omgcof * satrec.t;
// sgp4fix use mutliply for speed instead of pow
double delmtemp = 1.0 + (satrec.eta * Math.Cos(xmdf));
delm = satrec.xmcof * ((delmtemp * delmtemp * delmtemp) - satrec.delmo);
temp = delomg + delm;
mm = xmdf + temp;
argpm = argpdf - temp;
t3 = t2 * satrec.t;
t4 = t3 * satrec.t;
tempa = tempa - (satrec.d2 * t2) - (satrec.d3 * t3) - (satrec.d4 * t4);
tempe += satrec.bstar * satrec.cc5 * (Math.Sin(mm) - satrec.sinmao);
templ = templ + (satrec.t3cof * t3) + (t4 * (satrec.t4cof + (satrec.t * satrec.t5cof)));
}
nm = satrec.no;
double em = satrec.ecco;
inclm = satrec.inclo;
if (satrec.method == 'd')
{
tc = satrec.t;
DspaceOptions dspaceOptions = new DspaceOptions();
dspaceOptions.irez = satrec.irez;
dspaceOptions.d2201 = satrec.d2201;
dspaceOptions.d2211 = satrec.d2211;
dspaceOptions.d3210 = satrec.d3210;
dspaceOptions.d3222 = satrec.d3222;
dspaceOptions.d4410 = satrec.d4410;
dspaceOptions.d4422 = satrec.d4422;
dspaceOptions.d5220 = satrec.d5220;
dspaceOptions.d5232 = satrec.d5232;
dspaceOptions.d5421 = satrec.d5421;
dspaceOptions.d5433 = satrec.d5433;
dspaceOptions.dedt = satrec.dedt;
dspaceOptions.del1 = satrec.del1;
dspaceOptions.del2 = satrec.del2;
dspaceOptions.del3 = satrec.del3;
dspaceOptions.didt = satrec.didt;
dspaceOptions.dmdt = satrec.dmdt;
dspaceOptions.dnodt = satrec.dnodt;
dspaceOptions.domdt = satrec.domdt;
dspaceOptions.argpo = satrec.argpo;
dspaceOptions.argpdot = satrec.argpdot;
dspaceOptions.t = satrec.t;
dspaceOptions.tc = tc;
dspaceOptions.gsto = satrec.gsto;
dspaceOptions.xfact = satrec.xfact;
dspaceOptions.xlamo = satrec.xlamo;
dspaceOptions.no = satrec.no;
dspaceOptions.atime = satrec.atime;
dspaceOptions.em = em;
dspaceOptions.argpm = argpm;
dspaceOptions.inclm = inclm;
dspaceOptions.xli = satrec.xli;
dspaceOptions.mm = mm;
dspaceOptions.xni = satrec.xni;
dspaceOptions.nodem = nodem;
dspaceOptions.nm = nm;
Dspace dspace = new Dspace();
DspaceResults dspaceResults = new DspaceResults();
dspaceResults = dspace.dspace(dspaceOptions);
em = dspaceResults.em;
argpm = dspaceResults.argpm;
inclm = dspaceResults.inclm;
mm = dspaceResults.mm;
nodem = dspaceResults.nodem;
nm = dspaceResults.nm;
}
if (nm <= 0.0)
{
// printf("// error nm %f\n", nm);
satrec.error = 2;
// sgp4fix add return
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
double am = (Math.Pow((xke / nm), x2o3)) * tempa * tempa;
nm = xke / (Math.Pow(am, 1.5));
em -= tempe;
// fix tolerance for error recognition
// sgp4fix am is fixed from the previous nm check
if (em >= 1.0 || em < -0.001) // || (am < 0.95)
// printf("// error em %f\n", em);
{
satrec.error = 1;
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
// sgp4fix fix tolerance to avoid a divide by zero
if (em < 1.0e-6)
{
em = 1.0e-6;
}
mm += satrec.no * templ;
xlm = mm + argpm + nodem;
nodem %= twoPi;
argpm %= twoPi;
xlm %= twoPi;
mm = (xlm - argpm - nodem) % twoPi;
// ----------------- compute extra mean quantities -------------
double sinim = Math.Sin(inclm);
double cosim = Math.Cos(inclm);
// -------------------- add lunar-solar periodics --------------
double ep = em;
xincp = inclm;
argpp = argpm;
nodep = nodem;
mp = mm;
sinip = sinim;
cosip = cosim;
if (satrec.method == 'd')
{
DpperOptions dpperParameters = new DpperOptions();
dpperParameters.inclo = satrec.inclo;
dpperParameters.init = 'n';
dpperParameters.ep = ep;
dpperParameters.inclp = xincp;
dpperParameters.nodep = nodep;
dpperParameters.argpp = argpp;
dpperParameters.mp = mp;
dpperParameters.opsmode = satrec.operationmode;
Dpper dpper = new Dpper();
DpperResult dpperResult = new DpperResult();
dpperResult = dpper.dpper(satrec, dpperParameters);
ep = dpperResult.ep;
nodep = dpperResult.nodep;
argpp = dpperResult.argpp;
mp = dpperResult.mp;
xincp = dpperResult.inclp;
if (xincp < 0.0)
{
xincp = -xincp;
nodep += pi;
argpp -= pi;
}
if (ep < 0.0 || ep > 1.0)
{
// printf("// error ep %f\n", ep);
satrec.error = 3;
// sgp4fix add return
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
}
// -------------------- long period periodics ------------------
if (satrec.method == 'd')
{
sinip = Math.Sin(xincp);
cosip = Math.Cos(xincp);
satrec.aycof = -0.5 * j3oj2 * sinip;
// sgp4fix for divide by zero for xincp = 180 deg
if (Math.Abs(cosip + 1.0) > 1.5e-12)
{
satrec.xlcof = (-0.25 * j3oj2 * sinip * (3.0 + (5.0 * cosip))) / (1.0 + cosip);
}
else
{
satrec.xlcof = (-0.25 * j3oj2 * sinip * (3.0 + (5.0 * cosip))) / temp4;
}
}
double axnl = ep * Math.Cos(argpp);
temp = 1.0 / (am * (1.0 - (ep * ep)));
double aynl = (ep * Math.Sin(argpp)) + (temp * satrec.aycof);
double xl = mp + argpp + nodep + (temp * satrec.xlcof * axnl);
// --------------------- solve kepler's equation ---------------
double u = (xl - nodep) % twoPi;
eo1 = u;
tem5 = 9999.9;
double ktr = 1;
// sgp4fix for kepler iteration
// the following iteration needs better limits on corrections
while (Math.Abs(tem5) >= 1.0e-12 && ktr <= 10)
{
sineo1 = Math.Sin(eo1);
coseo1 = Math.Cos(eo1);
tem5 = 1.0 - (coseo1 * axnl) - (sineo1 * aynl);
tem5 = (((u - (aynl * coseo1)) + (axnl * sineo1)) - eo1) / tem5;
if (Math.Abs(tem5) >= 0.95)
{
if (tem5 > 0.0)
{
tem5 = 0.95;
}
else
{
tem5 = -0.95;
}
}
eo1 += tem5;
ktr += 1;
}
// ------------- short period preliminary quantities -----------
double ecose = (axnl * coseo1) + (aynl * sineo1);
double esine = (axnl * sineo1) - (aynl * coseo1);
double el2 = (axnl * axnl) + (aynl * aynl);
double pl = am * (1.0 - el2);
if (pl < 0.0)
{
// printf("// error pl %f\n", pl);
satrec.error = 4;
// sgp4fix add return
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
double rl = am * (1.0 - ecose);
double rdotl = (Math.Sqrt(am) * esine) / rl;
double rvdotl = Math.Sqrt(pl) / rl;
double betal = Math.Sqrt(1.0 - el2);
temp = esine / (1.0 + betal);
double sinu = (am / rl) * (sineo1 - aynl - (axnl * temp));
double cosu = (am / rl) * ((coseo1 - axnl) + (aynl * temp));
su = Math.Atan2(sinu, cosu);
double sin2u = (cosu + cosu) * sinu;
double cos2u = 1.0 - (2.0 * sinu * sinu);
temp = 1.0 / pl;
double temp1 = 0.5 * j2 * temp;
double temp2 = temp1 * temp;
// -------------- update for short period periodics ------------
if (satrec.method == 'd')
{
cosisq = cosip * cosip;
satrec.con41 = (3.0 * cosisq) - 1.0;
satrec.x1mth2 = 1.0 - cosisq;
satrec.x7thm1 = (7.0 * cosisq) - 1.0;
}
double mrt = (rl * (1.0 - (1.5 * temp2 * betal * satrec.con41)))
+ (0.5 * temp1 * satrec.x1mth2 * cos2u);
// sgp4fix for decaying satellites
if (mrt < 1.0)
{
// printf("// decay condition %11.6f \n",mrt);
satrec.error = 6;
// return {
// position: false,
// velocity: false,
// };
PositionAndVelocity noValue = new PositionAndVelocity();
return(noValue); //TODO set this to 0
}
su -= 0.25 * temp2 * satrec.x7thm1 * sin2u;
double xnode = nodep + (1.5 * temp2 * cosip * sin2u);
double xinc = xincp + (1.5 * temp2 * cosip * sinip * cos2u);
double mvt = rdotl - ((nm * temp1 * satrec.x1mth2 * sin2u) / xke);
double rvdot = rvdotl + ((nm * temp1 * ((satrec.x1mth2 * cos2u) + (1.5 * satrec.con41))) / xke);
// --------------------- orientation vectors -------------------
double sinsu = Math.Sin(su);
double cossu = Math.Cos(su);
double snod = Math.Sin(xnode);
double cnod = Math.Cos(xnode);
double sini = Math.Sin(xinc);
double cosi = Math.Cos(xinc);
double xmx = -snod * cosi;
double xmy = cnod * cosi;
double ux = (xmx * sinsu) + (cnod * cossu);
double uy = (xmy * sinsu) + (snod * cossu);
double uz = sini * sinsu;
double vx = (xmx * cossu) - (cnod * sinsu);
double vy = (xmy * cossu) - (snod * sinsu);
double vz = sini * cossu;
// --------- position and velocity (in km and km/sec) ----------
PositionAndVelocity positionAndVelocity = new PositionAndVelocity();
positionAndVelocity.position_ECI.x = (mrt * ux) * earthRadius;
positionAndVelocity.position_ECI.y = (mrt * uy) * earthRadius;
positionAndVelocity.position_ECI.z = (mrt * uz) * earthRadius;
// double r = {
// x: (mrt * ux) * earthRadius,
// y: (mrt * uy) * earthRadius,
// z: (mrt * uz) * earthRadius,
// };
positionAndVelocity.velocity_ECI.x = ((mvt * ux) + (rvdot * vx)) * vkmpersec;
positionAndVelocity.velocity_ECI.y = ((mvt * uy) + (rvdot * vy)) * vkmpersec;
positionAndVelocity.velocity_ECI.z = ((mvt * uz) + (rvdot * vz)) * vkmpersec;
// double v = {
// x: ((mvt * ux) + (rvdot * vx)) * vkmpersec,
// y: ((mvt * uy) + (rvdot * vy)) * vkmpersec,
// z: ((mvt * uz) + (rvdot * vz)) * vkmpersec,
// };
// return {
// position: r,
// velocity: v,
// };
return(positionAndVelocity);
}