/* position is stationary relative to the earth's surface. */
void
Calculate_User_PosVel(double _time,
geodetic_t *geodetic,
vector_t *obs_pos,
vector_t *obs_vel)
{
/* Reference: The 1992 Astronomical Almanac, page K11. */
double c,sq,achcp;
geodetic->theta = FMod2p(ThetaG_JD(_time) + geodetic->lon);/*LMST*/
c = 1/sqrt(1 + __f*(__f - 2)*Sqr(sin(geodetic->lat)));
sq = Sqr(1 - __f)*c;
achcp = (xkmper*c + geodetic->alt)*cos(geodetic->lat);
obs_pos->x = achcp*cos(geodetic->theta);/*kilometers*/
obs_pos->y = achcp*sin(geodetic->theta);
obs_pos->z = (xkmper*sq + geodetic->alt)*sin(geodetic->lat);
obs_vel->x = -mfactor*obs_pos->y;/*kilometers/second*/
obs_vel->y = mfactor*obs_pos->x;
obs_vel->z = 0;
Magnitude(obs_pos);
Magnitude(obs_vel);
} /*Procedure Calculate_User_PosVel*/
void SGP4(float tsince, PredicThirteen::tle_t * tle, PredicThirteen::vector_t * pos, PredicThirteen::vector_t * vel)
{
/* This function is used to calculate the position and velocity */
/* of near-earth (period < 225 minutes) satellites. tsince is */
/* time since epoch in minutes, tle is a pointer to a tle_t */
/* structure with Keplerian orbital elements and pos and vel */
/* are vector_t structures returning ECI satellite position and */
/* velocity. Use Convert_Sat_State() to convert to km and km/s. */
static float aodp, aycof, c1, c4, c5, cosio, d2, d3, d4, delmo,
omgcof, eta, omgdot, sinio, xnodp, sinmo, t2cof, t3cof, t4cof,
t5cof, x1mth2, x3thm1, x7thm1, xmcof, xmdot, xnodcf, xnodot, xlcof;
float cosuk, sinuk, rfdotk, vx, vy, vz, ux, uy, uz, xmy, xmx, cosnok,
sinnok, cosik, sinik, rdotk, xinck, xnodek, uk, rk, cos2u, sin2u,
u, sinu, cosu, betal, rfdot, rdot, r, pl, elsq, esine, ecose, epw,
cosepw, x1m5th, xhdot1, tfour, sinepw, capu, ayn, xlt, aynl, xll,
axn, xn, beta, xl, e, a, tcube, delm, delomg, templ, tempe, tempa,
xnode, tsq, xmp, omega, xnoddf, omgadf, xmdf, a1, a3ovk2, ao,
betao, betao2, c1sq, c2, c3, coef, coef1, del1, delo, eeta, eosq,
etasq, perigee, pinvsq, psisq, qoms24, s4, temp, temp1, temp2,
temp3, temp4, temp5, temp6, theta2, theta4, tsi;
int i;
DEBUG_PRINTLN("----------------------------------------");
printTle(tle);
/* Initialization */
if (isFlagClear(SGP4_INITIALIZED_FLAG))
{
SetFlag(SGP4_INITIALIZED_FLAG);
/* Recover original mean motion (xnodp) and */
/* semimajor axis (aodp) from input elements. */
a1=pow(xke/tle->xno,tothrd);
printVar("a1", a1);
cosio=cos(tle->xincl);
printVar("Tle->xincl", tle->xincl);
theta2=cosio*cosio;
x3thm1=3*theta2-1.0;
printVar("theta2",theta2);
printVar("cosio",cosio);
eosq=tle->eo*tle->eo;
betao2=1.0-eosq;
betao=sqrt(betao2);
del1=1.5*ck2*x3thm1/(a1*a1*betao*betao2);
ao=a1*(1.0-del1*(0.5*tothrd+del1*(1.0+134.0/81.0*del1)));
delo=1.5*ck2*x3thm1/(ao*ao*betao*betao2);
xnodp=tle->xno/(1.0+delo);
aodp=ao/(1.0-delo);
printVar("aodp", aodp);
DEBUG_PRINTLN("----------------------------------------");
/* For perigee less than 220 kilometers, the "simple" */
/* flag is set and the equations are truncated to linear */
/* variation in sqrt a and quadratic variation in mean */
/* anomaly. Also, the c3 term, the delta omega term, and */
/* the delta m term are dropped. */
if ((aodp*(1-tle->eo)/ae)<(220/xkmper+ae))
SetFlag(SIMPLE_FLAG);
else
ClearFlag(SIMPLE_FLAG);
/* For perigees below 156 km, the */
/* values of s and qoms2t are altered. */
s4=s;
qoms24=qoms2t;
perigee=(aodp*(1-tle->eo)-ae)*xkmper;
printVar("perigee", perigee);
if (perigee<156.0)
{
if (perigee<=98.0)
s4=20;
else
s4=perigee-78.0;
qoms24=pow((120-s4)*ae/xkmper,4);
s4=s4/xkmper+ae;
}
DEBUG_PRINTLN("----------------------------------------");
pinvsq=1/(aodp*aodp*betao2*betao2);
tsi=1/(aodp-s4);
eta=aodp*tle->eo*tsi;
etasq=eta*eta;
printVar("etasq",etasq);
eeta=tle->eo*eta;
psisq=fabs(1-etasq);
coef=qoms24*pow(tsi,4);
coef1=coef/pow(psisq,3.5);
printVar("coef1",coef1);
c2=coef1*xnodp*(aodp*(1+1.5*etasq+eeta*(4+etasq))+0.75*ck2*tsi/psisq*x3thm1*(8+3*etasq*(8+etasq)));
printVar("xnodp", xnodp);
printVar("etasq",etasq);
printVar("eeta",eeta);
printVar("ck2",ck2);
printVar("tsi",tsi);
printVar("psisq",psisq);
printVar("x3thm1",x3thm1);
printVar("c2",c2);
c1=tle->bstar*c2;
printVar("c1*1000000",c1*1000000);
printVar("BSTAR: ", tle->bstar);
sinio=sin(tle->xincl);
a3ovk2=-xj3/ck2*pow(ae,3);
c3=coef*tsi*a3ovk2*xnodp*ae*sinio/tle->eo;
x1mth2=1-theta2;
printVar("x1mth2", x1mth2);
DEBUG_PRINTLN("----------------------------------------");
DEBUG_PRINTLN("----------------------------------------");
c4=2*xnodp*coef1*aodp*betao2*(eta*(2+0.5*etasq)+tle->eo*(0.5+2*etasq)-2*ck2*tsi/(aodp*psisq)*(-3*x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*tle->omegao)));
c5=2*coef1*aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
theta4=theta2*theta2;
temp1=3*ck2*pinvsq*xnodp;
temp2=temp1*ck2*pinvsq;
temp3=1.25*ck4*pinvsq*pinvsq*xnodp;
xmdot=xnodp+0.5*temp1*betao*x3thm1+0.0625*temp2*betao*(13-78*theta2+137*theta4);
x1m5th=1-5*theta2;
omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+395*theta4)+temp3*(3-36*theta2+49*theta4);
xhdot1=-temp1*cosio;
xnodot=xhdot1+(0.5*temp2*(4-19*theta2)+2*temp3*(3-7*theta2))*cosio;
omgcof=tle->bstar*c3*cos(tle->omegao);
xmcof=-tothrd*coef*tle->bstar*ae/eeta;
xnodcf=3.5*betao2*xhdot1*c1;
t2cof=1.5*c1;
printVar("t2cof*1000000", t2cof*1000000);
xlcof=0.125*a3ovk2*sinio*(3+5*cosio)/(1+cosio);
aycof=0.25*a3ovk2*sinio;
delmo=pow(1+eta*cos(tle->xmo),3);
sinmo=sin(tle->xmo);
x7thm1=7*theta2-1;
printVar("x7thm1", x7thm1);
if (isFlagClear(SIMPLE_FLAG))
{
c1sq=c1*c1;
d2=4*aodp*tsi*c1sq;
temp=d2*tsi*c1/3;
d3=(17*aodp+s4)*temp;
d4=0.5*temp*aodp*tsi*(221*aodp+31*s4)*c1;
t3cof=d2+2*c1sq;
t4cof=0.25*(3*d3+c1*(12*d2+10*c1sq));
t5cof=0.2*(3*d4+12*c1*d3+6*d2*d2+15*c1sq*(2*d2+c1sq));
}
}
/* Update for secular gravity and atmospheric drag. */
xmdf=tle->xmo+xmdot*tsince;
omgadf=tle->omegao+omgdot*tsince;
xnoddf=tle->xnodeo+xnodot*tsince;
omega=omgadf;
xmp=xmdf;
tsq=tsince*tsince;
printVar("tsince*1000000", tsince*1000000);
printVar("tsq", tsq);
xnode=xnoddf+xnodcf*tsq;
tempa=1-c1*tsince;
tempe=tle->bstar*c4*tsince;
templ=t2cof*tsq;
printVar("templ", templ);
if (isFlagClear(SIMPLE_FLAG))
{
delomg=omgcof*tsince;
delm=xmcof*(pow(1+eta*cos(xmdf),3)-delmo);
temp=delomg+delm;
xmp=xmdf+temp;
omega=omgadf-temp;
tcube=tsq*tsince;
tfour=tsince*tcube;
tempa=tempa-d2*tsq-d3*tcube-d4*tfour;
tempe=tempe+tle->bstar*c5*(sin(xmp)-sinmo);
templ=templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof);
printVar("templ", templ);
}
a=aodp*pow(tempa,2);
e=tle->eo-tempe;
xl=xmp+omega+xnode+xnodp*templ;
beta=sqrt(1-e*e);
xn=xke/pow(a,1.5);
/* Long period periodics */
axn=e*cos(omega);
temp=1/(a*beta*beta);
xll=temp*xlcof*axn;
aynl=temp*aycof;
xlt=xl+xll;
ayn=e*sin(omega)+aynl;
/* Solve Kepler's Equation */
capu=FMod2p(xlt-xnode);
temp2=capu;
i=0;
do
{
sinepw=sin(temp2);
cosepw=cos(temp2);
temp3=axn*sinepw;
temp4=ayn*cosepw;
temp5=axn*cosepw;
temp6=ayn*sinepw;
epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
if (fabs(epw-temp2)<= e6a)
break;
temp2=epw;
} while (i++<10);
/* Short p2eriod preliminary quantities */
ecose=temp5+temp6;
esine=temp3-temp4;
elsq=axn*axn+ayn*ayn;
temp=1-elsq;
pl=a*temp;
r=a*(1-ecose);
temp1=1/r;
rdot=xke*sqrt(a)*esine*temp1;
rfdot=xke*sqrt(pl)*temp1;
temp2=a*temp1;
betal=sqrt(temp);
temp3=1/(1+betal);
cosu=temp2*(cosepw-axn+ayn*esine*temp3);
sinu=temp2*(sinepw-ayn-axn*esine*temp3);
u=AcTan(sinu,cosu);
sin2u=2*sinu*cosu;
cos2u=2*cosu*cosu-1;
temp=1/pl;
temp1=ck2*temp;
temp2=temp1*temp;
/* Update for short periodics */
rk=r*(1-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u;
uk=u-0.25*temp2*x7thm1*sin2u;
xnodek=xnode+1.5*temp2*cosio*sin2u;
xinck=tle->xincl+1.5*temp2*cosio*sinio*cos2u;
rdotk=rdot-xn*temp1*x1mth2*sin2u;
rfdotk=rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1);
/* Orientation vectors */
sinuk=sin(uk);
cosuk=cos(uk);
sinik=sin(xinck);
cosik=cos(xinck);
sinnok=sin(xnodek);
cosnok=cos(xnodek);
xmx=-sinnok*cosik;
xmy=cosnok*cosik;
ux=xmx*sinuk+cosnok*cosuk;
uy=xmy*sinuk+sinnok*cosuk;
uz=sinik*sinuk;
vx=xmx*cosuk-cosnok*sinuk;
vy=xmy*cosuk-sinnok*sinuk;
vz=sinik*cosuk;
/* Position and velocity */
pos->x=rk*ux;
pos->y=rk*uy;
pos->z=rk*uz;
vel->x=rdotk*ux+rfdotk*vx;
vel->y=rdotk*uy+rfdotk*vy;
vel->z=rdotk*uz+rfdotk*vz;
DEBUG_PRINTLN("----------------------------------------");
DEBUG_PRINT("Position: ");
printVector(pos);
DEBUG_PRINT("Velocity; ");
printVector(vel);
/* Phase in radians */
phase=xlt-xnode-omgadf+twopi;
if (phase<0.0)
phase+=twopi;
phase=FMod2p(phase);
}
/* DEEP */
void Deep_dpinit( const tle_t *tle, deep_arg_t *deep_arg)
{
const double sinq = sin(tle->xnodeo);
const double cosq = cos(tle->xnodeo);
const double aqnv = 1/deep_arg->aodp;
const double c1ss = 2.9864797E-6;
/* 1900 Jan 0.5 = JD 2415020. */
const double days_since_1900 = tle->epoch - 2415020.;
/* zcosi, zsini start as cos & sin of obliquity of earth's */
/* orbit = 23.444100 degrees... matches obliquity in 1963; */
/* probably just a slightly inaccurate value: */
const double zcosi0 = 0.91744867;
const double zsini0 = 0.39785416;
double zcosi = zcosi0;
double zsini = zsini0;
/* zcosg, zsing start as cos & sin of -78.779197 degrees */
double zsing = -0.98088458;
double zcosg = 0.1945905;
double bfact, cc = c1ss, se;
double ze = zes, zn = zns_per_min;
double sgh, sh, si;
double zsinh = sinq, zcosh = cosq;
double sl;
int iteration;
deep_arg->thgr = ThetaG( tle->epoch);
deep_arg->xnq = deep_arg->xnodp;
deep_arg->xqncl = tle->xincl;
deep_arg->omegaq = tle->omegao;
/* If the epoch has changed, recompute (or initialize) the lunar and */
/* solar terms... except that now that zcosil, etc. are within the */
/* deep_arg structure, instead of static, they must _always_ be */
/* recomputed. So I've commented out the 'if' part. (Revision made */
/* 14 May 2005) */
/* if( days_since_1900 != deep_arg->preep) */
{
const double lunar_asc_node = 4.5236020 - 9.2422029E-4 * days_since_1900;
const double sin_asc_node = sin(lunar_asc_node);
const double cos_asc_node = cos(lunar_asc_node);
const double c_minus_gam = znl_per_day * days_since_1900 - 1.1151842;
/* gam = longitude of perigee for the moon, in radians: */
const double gam = 5.8351514 + 0.0019443680 * days_since_1900;
double zx, zy;
deep_arg->preep = days_since_1900;
deep_arg->zcosil = 0.91375164 - 0.03568096 * cos_asc_node;
deep_arg->zsinil = sqrt(1. - deep_arg->zcosil * deep_arg->zcosil);
deep_arg->zsinhl = 0.089683511 * sin_asc_node / deep_arg->zsinil;
deep_arg->zcoshl = sqrt(1. - deep_arg->zsinhl*deep_arg->zsinhl);
deep_arg->zmol = FMod2p( c_minus_gam);
zx = zsini0 * sin_asc_node / deep_arg->zsinil;
zy = deep_arg->zcoshl * cos_asc_node + zcosi0 * deep_arg->zsinhl * sin_asc_node;
zx = atan2( zx, zy) + gam - lunar_asc_node;
deep_arg->zcosgl = cos( zx);
deep_arg->zsingl = sin( zx);
deep_arg->zmos = FMod2p( 6.2565837
+ zns_per_day * days_since_1900);
} /* End if( days_since_1900 != deep_arg->preep) */
/* Do solar terms */
deep_arg->savtsn = 1E20;
/* There was previously some convoluted logic here, but it boils */
/* down to this: we compute the solar terms, then the lunar terms. */
/* On a second pass, we recompute the solar terms, taking advantage */
/* of the improved data that resulted from computing lunar terms. */
for( iteration = 0; iteration < 2; iteration++)
{
const double c1l = 4.7968065E-7;
const double a1 = zcosg * zcosh + zsing * zcosi * zsinh;
const double a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
const double a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
const double a8 = zsing * zsini;
const double a9 = zsing * zsinh + zcosg * zcosi * zcosh;
const double a10 = zcosg * zsini;
const double a2 = deep_arg->cosio * a7 + deep_arg->sinio * a8;
const double a4 = deep_arg->cosio * a9 + deep_arg->sinio * a10;
const double a5 = -deep_arg->sinio * a7 + deep_arg->cosio * a8;
const double a6 = -deep_arg->sinio * a9 + deep_arg->cosio * a10;
const double x1 = a1 * deep_arg->cosg + a2 * deep_arg->sing;
const double x2 = a3 * deep_arg->cosg + a4 * deep_arg->sing;
const double x3 = -a1 * deep_arg->sing + a2 * deep_arg->cosg;
const double x4 = -a3 * deep_arg->sing + a4 * deep_arg->cosg;
const double x5 = a5 * deep_arg->sing;
const double x6 = a6 * deep_arg->sing;
const double x7 = a5 * deep_arg->cosg;
const double x8 = a6 * deep_arg->cosg;
const double z31 = 12 * x1 * x1 - 3 * x3 * x3;
const double z32 = 24 * x1 * x2 - 6 * x3 * x4;
const double z33 = 12 * x2 * x2 - 3 * x4 * x4;
const double z11 = -6 * a1 * a5 + deep_arg->eosq * (-24 * x1 * x7 - 6 * x3 * x5);
const double z12 = -6 * (a1 * a6 + a3 * a5) + deep_arg->eosq *
(-24 * (x2 * x7 + x1 * x8) - 6 * (x3 * x6 + x4 * x5));
const double z13 = -6 * a3 * a6 + deep_arg->eosq * (-24 * x2 * x8 - 6 * x4 * x6);
const double z21 = 6 * a2 * a5 + deep_arg->eosq * (24 * x1 * x5 - 6 * x3 * x7);
const double z22 = 6 * (a4 * a5 + a2 * a6) + deep_arg->eosq *
(24 * (x2 * x5 + x1 * x6) - 6 * (x4 * x7 + x3 * x8));
const double z23 = 6 * a4 * a6 + deep_arg->eosq * (24 * x2 * x6 - 6 * x4 * x8);
const double s3 = cc / deep_arg->xnq;
const double s2 = -0.5 * s3 / deep_arg->betao;
const double s4 = s3 * deep_arg->betao;
const double s1 = -15 * tle->eo * s4;
const double s5 = x1 * x3 + x2 * x4;
const double s6 = x2 * x3 + x1 * x4;
const double s7 = x2 * x4 - x1 * x3;
double z1 = 3 * (a1 * a1 + a2 * a2) + z31 * deep_arg->eosq;
double z2 = 6 * (a1 * a3 + a2 * a4) + z32 * deep_arg->eosq;
double z3 = 3 * (a3 * a3 + a4 * a4) + z33 * deep_arg->eosq;
z1 = z1 + z1 + deep_arg->betao2 * z31;
z2 = z2 + z2 + deep_arg->betao2 * z32;
z3 = z3 + z3 + deep_arg->betao2 * z33;
se = s1*zn*s5;
si = s2*zn*(z11+z13);
sl = -zn*s3*(z1+z3-14-6*deep_arg->eosq);
sgh = s4*zn*(z31+z33-6);
if (deep_arg->xqncl < pi / 60.) /* pi / 60 radians = 3 degrees */
sh = 0;
else
sh = -zn*s2*(z21+z23);
deep_arg->ee2 = 2*s1*s6;
deep_arg->e3 = 2*s1*s7;
deep_arg->xi2 = 2*s2*z12;
deep_arg->xi3 = 2*s2*(z13-z11);
deep_arg->xl2 = -2*s3*z2;
deep_arg->xl3 = -2*s3*(z3-z1);
deep_arg->xl4 = -2*s3*(-21-9*deep_arg->eosq)*ze;
deep_arg->xgh2 = 2*s4*z32;
deep_arg->xgh3 = 2*s4*(z33-z31);
deep_arg->xgh4 = -18*s4*ze;
deep_arg->xh2 = -2*s2*z22;
deep_arg->xh3 = -2*s2*(z23-z21);
if( !iteration) /* we compute lunar terms only on the first pass: */
{
deep_arg->sse = se;
deep_arg->ssi = si;
deep_arg->ssl = sl;
deep_arg->ssh = (deep_arg->sinio ? sh / deep_arg->sinio : 0.);
deep_arg->ssg = sgh-deep_arg->cosio*deep_arg->ssh;
deep_arg->se2 = deep_arg->ee2;
deep_arg->si2 = deep_arg->xi2;
deep_arg->sl2 = deep_arg->xl2;
deep_arg->sgh2 = deep_arg->xgh2;
deep_arg->sh2 = deep_arg->xh2;
deep_arg->se3 = deep_arg->e3;
deep_arg->si3 = deep_arg->xi3;
deep_arg->sl3 = deep_arg->xl3;
deep_arg->sgh3 = deep_arg->xgh3;
deep_arg->sh3 = deep_arg->xh3;
deep_arg->sl4 = deep_arg->xl4;
deep_arg->sgh4 = deep_arg->xgh4;
zcosg = deep_arg->zcosgl;
zsing = deep_arg->zsingl;
zcosi = deep_arg->zcosil;
zsini = deep_arg->zsinil;
zcosh = deep_arg->zcoshl*cosq+deep_arg->zsinhl*sinq;
zsinh = sinq*deep_arg->zcoshl-cosq*deep_arg->zsinhl;
zn = znl_per_min;
cc = c1l;
ze = zel;
}
}
deep_arg->sse += se;
deep_arg->ssi += si;
deep_arg->ssl += sl;
deep_arg->ssg += sgh;
if( deep_arg->sinio)
{
deep_arg->ssg -= sh * deep_arg->cosio / deep_arg->sinio;
deep_arg->ssh += sh / deep_arg->sinio;
}
/* "if mean motion is 1.893053 to 2.117652 revs/day, and ecc >= .5" */
if( deep_arg->xnq >= 0.00826 && deep_arg->xnq <= 0.00924 && tle->eo >= .5)
{ /* start of 12-hour orbit, e >.5 section */
/* 'root##' variables are somewhat inaccurate values for */
/* a few fully normalized sectorial/tesseral spherical */
/* harmonics of the Earth's gravitational potential: */
const double root22 = 1.7891679E-6;
const double root32 = 3.7393792E-7;
const double root44 = 7.3636953E-9;
const double root52 = 1.1428639E-7;
const double root54 = 2.1765803E-9;
const double g201 = -0.306 - (tle->eo - 0.64) * 0.440;
const double sini2 = deep_arg->sinio*deep_arg->sinio;
const double f220 = 0.75*(1+2*deep_arg->cosio+deep_arg->theta2);
const double f221 = 1.5 * sini2;
const double f321 = 1.875 * deep_arg->sinio * (1 - 2 *\
deep_arg->cosio - 3 * deep_arg->theta2);
const double f322 = -1.875*deep_arg->sinio*(1+2*
deep_arg->cosio-3*deep_arg->theta2);
const double f441 = 35 * sini2 * f220;
const double f442 = 39.3750 * sini2 * sini2;
const double f522 = 9.84375*deep_arg->sinio*(sini2*(1-2*deep_arg->cosio-5*
deep_arg->theta2)+0.33333333*(-2+4*deep_arg->cosio+
6*deep_arg->theta2));
const double f523 = deep_arg->sinio*(4.92187512*sini2*(-2-4*
deep_arg->cosio+10*deep_arg->theta2)+6.56250012
*(1+2*deep_arg->cosio-3*deep_arg->theta2));
const double f542 = 29.53125*deep_arg->sinio*(2-8*
deep_arg->cosio+deep_arg->theta2*
(-12+8*deep_arg->cosio+10*deep_arg->theta2));
const double f543 = 29.53125*deep_arg->sinio*(-2-8*deep_arg->cosio+
deep_arg->theta2*(12+8*deep_arg->cosio-10*
deep_arg->theta2));
double g410, g422, g520, g521, g532, g533;
double g211, g310, g322;
double temp, temp1;
deep_arg->resonance_flag = 1; /* it _is_ resonant... */
deep_arg->synchronous_flag = 0; /* but it's not synchronous */
/* Geopotential resonance initialization for 12 hour orbits: */
if (tle->eo <= 0.65)
{
g211 = 3.616-13.247*tle->eo+16.290*deep_arg->eosq;
g310 = eval_cubic_poly( tle->eo, -19.302, 117.390, -228.419, 156.591);
g322 = eval_cubic_poly( tle->eo, -18.9068, 109.7927, -214.6334, 146.5816);
g410 = eval_cubic_poly( tle->eo, -41.122, 242.694, -471.094, 313.953);
g422 = eval_cubic_poly( tle->eo, -146.407, 841.880, -1629.014, 1083.435);
g520 = eval_cubic_poly( tle->eo, -532.114, 3017.977, -5740.032, 3708.276);
/* NOTE: quadratic coeff was 5740 */
}
else
{
g211 = eval_cubic_poly( tle->eo, -72.099, 331.819, -508.738, 266.724);
g310 = eval_cubic_poly( tle->eo, -346.844, 1582.851, -2415.925, 1246.113);
g322 = eval_cubic_poly( tle->eo, -342.585, 1554.908, -2366.899, 1215.972);
g410 = eval_cubic_poly( tle->eo, -1052.797, 4758.686, -7193.992, 3651.957);
g422 = eval_cubic_poly( tle->eo, -3581.69, 16178.11, -24462.77, 12422.52);
if (tle->eo <= 0.715)
g520 = eval_cubic_poly( tle->eo, 1464.74, -4664.75, 3763.64, 0.);
else
g520 = eval_cubic_poly( tle->eo, -5149.66, 29936.92, -54087.36, 31324.56);
} /* End if (tle->eo <= 0.65) */
if (tle->eo < 0.7)
{
g533 = eval_cubic_poly( tle->eo, -919.2277, 4988.61, -9064.77, 5542.21);
g521 = eval_cubic_poly( tle->eo, -822.71072, 4568.6173, -8491.4146, 5337.524);
g532 = eval_cubic_poly( tle->eo, -853.666, 4690.25, -8624.77, 5341.4);
}
else
{
g533 = eval_cubic_poly( tle->eo, -37995.78, 161616.52, -229838.2, 109377.94);
g521 = eval_cubic_poly( tle->eo, -51752.104, 218913.95, -309468.16, 146349.42);
g532 = eval_cubic_poly( tle->eo, -40023.88, 170470.89, -242699.48, 115605.82);
} /* End if (tle->eo <= 0.7) */
temp1 = 3 * deep_arg->xnq * deep_arg->xnq * aqnv * aqnv;
temp = temp1*root22;
deep_arg->d2201 = temp * f220 * g201;
deep_arg->d2211 = temp * f221 * g211;
temp1 *= aqnv;
temp = temp1*root32;
deep_arg->d3210 = temp * f321 * g310;
deep_arg->d3222 = temp * f322 * g322;
temp1 *= aqnv;
temp = 2*temp1*root44;
deep_arg->d4410 = temp * f441 * g410;
deep_arg->d4422 = temp * f442 * g422;
temp1 *= aqnv;
temp = temp1*root52;
deep_arg->d5220 = temp * f522 * g520;
deep_arg->d5232 = temp * f523 * g532;
temp = 2*temp1*root54;
deep_arg->d5421 = temp * f542 * g521;
deep_arg->d5433 = temp * f543 * g533;
deep_arg->xlamo = tle->xmo+tle->xnodeo+tle->xnodeo-deep_arg->thgr-deep_arg->thgr;
bfact = deep_arg->xmdot + deep_arg->xnodot+
deep_arg->xnodot - thdt - thdt;
bfact += deep_arg->ssl + deep_arg->ssh + deep_arg->ssh;
} /* end of 12-hour orbit, e >.5 section */
else if( deep_arg->xnq < 1.2 * twopi / minutes_per_day &&
deep_arg->xnq > 0.8 * twopi / minutes_per_day)
{ /* "if mean motion is .8 to 1.2 revs/day" */
const double q22 = 1.7891679E-6;
const double q31 = 2.1460748E-6;
const double q33 = 2.2123015E-7;
const double cosio_plus_1 = 1. + deep_arg->cosio;
const double g200 = 1+deep_arg->eosq*(-2.5+0.8125*deep_arg->eosq);
const double g300 = 1+deep_arg->eosq*(-6+6.60937*deep_arg->eosq);
const double f311 = 0.9375*deep_arg->sinio*deep_arg->sinio*
(1+3*deep_arg->cosio)-0.75*cosio_plus_1;
const double g310 = 1+2*deep_arg->eosq;
const double f220 = 0.75 * cosio_plus_1 * cosio_plus_1;
const double f330 = 2.5 * f220 * cosio_plus_1;
deep_arg->resonance_flag = deep_arg->synchronous_flag = 1;
/* Synchronous resonance terms initialization */
deep_arg->del1 = 3*deep_arg->xnq*deep_arg->xnq*aqnv*aqnv;
deep_arg->del2 = 2*deep_arg->del1*f220*g200*q22;
deep_arg->del3 = 3*deep_arg->del1*f330*g300*q33*aqnv;
deep_arg->del1 *= f311*g310*q31*aqnv;
deep_arg->xlamo = tle->xmo+tle->xnodeo+tle->omegao-deep_arg->thgr;
bfact = deep_arg->xmdot + deep_arg->omgdot + deep_arg->xnodot - thdt;
bfact = bfact+deep_arg->ssl+deep_arg->ssg+deep_arg->ssh;
} /* End of geosych case */
else /* it's neither a high-e 12-hr orbit nor a geosynch: */
deep_arg->resonance_flag = deep_arg->synchronous_flag = 0;
if( deep_arg->resonance_flag)
{
deep_arg->xfact = bfact-deep_arg->xnq;
/* Initialize integrator */
deep_arg->xli = deep_arg->xlamo;
deep_arg->xni = deep_arg->xnq;
deep_arg->atime = 0;
}
/* End case dpinit: */
}
void DLL_FUNC SGP( const double tsince, const tle_t *tle, const double *params,
double *pos, double *vel)
{
double
temp, rdot, cosu, sinu, cos2u, sin2u, a, e,
p, rr, u, ecose, esine, omgas, cosik, xinck,
sinik, axnsl, aynsl,
sinuk, rvdot, cosuk, coseo1, sineo1, pl,
rk, uk, xl, su, ux, uy, uz, vx, vy, vz, pl2,
xnodek, cosnok, xnodes, el2, eo1, r1, sinnok,
xls, xmx, xmy, tem2, tem5;
int i;
/* Update for secular gravity and atmospheric drag */
a = tle->xno+(tle->xndt2o*2.+tle->xndd6o*3.*tsince)*tsince;
a = ao * pow( tle->xno / a, two_thirds);
e = e6a;
if (a > qo) e = 1.-qo/a;
p = a*(1.-e*e);
xnodes = tle->xnodeo+xnodot*tsince;
omgas = tle->omegao+omgdt*tsince;
r1 = xlo+(tle->xno+omgdt+xnodot+
(tle->xndt2o+tle->xndd6o*tsince)*tsince)*tsince;
xls = FMod2p(r1);
/* Long period periodics */
axnsl = e*cos(omgas);
aynsl = e*sin(omgas)-c6/p;
r1 = xls-c5/p*axnsl;
xl = FMod2p(r1);
/* Solve Kepler's equation */
r1 = xl-xnodes;
u = FMod2p(r1);
eo1 = u;
tem5 = 1.;
i = 0;
do
{
sineo1 = sin(eo1);
coseo1 = cos(eo1);
if (fabs(tem5) < e6a) break;
tem5 = 1.-coseo1*axnsl-sineo1*aynsl;
tem5 = (u-aynsl*coseo1+axnsl*sineo1-eo1)/tem5;
tem2 = fabs(tem5);
if (tem2 > 1.) tem5 = tem2/tem5;
eo1 += tem5;
}
while(i++ < 10);
/* Short period preliminary quantities */
ecose = axnsl*coseo1+aynsl*sineo1;
esine = axnsl*sineo1-aynsl*coseo1;
el2 = axnsl*axnsl+aynsl*aynsl;
pl = a*(1.-el2);
pl2 = pl*pl;
rr = a*(1.-ecose);
rdot = xke*sqrt(a)/rr*esine;
rvdot = xke*sqrt(pl)/rr;
temp = esine/(sqrt(1.-el2)+1.);
sinu = a/rr*(sineo1-aynsl-axnsl*temp);
cosu = a/rr*(coseo1-axnsl+aynsl*temp);
su = atan2(sinu, cosu);
/* Update for short periodics */
sin2u = (cosu+cosu)*sinu;
cos2u = 1.-2.*sinu*sinu;
rk = rr+d1o/pl*cos2u;
uk = su-d2o/pl2*sin2u;
xnodek = xnodes+d3o*sin2u/pl2;
xinck = tle->xincl+d4o/pl2*cos2u;
/* Orientation vectors */
sinuk = sin(uk);
cosuk = cos(uk);
sinnok = sin(xnodek);
cosnok = cos(xnodek);
sinik = sin(xinck);
cosik = cos(xinck);
xmx = -sinnok*cosik;
xmy = cosnok*cosik;
ux = xmx*sinuk+cosnok*cosuk;
uy = xmy*sinuk+sinnok*cosuk;
uz = sinik*sinuk;
vx = xmx*cosuk-cosnok*sinuk;
vy = xmy*cosuk-sinnok*sinuk;
vz = sinik*cosuk;
/* Position and velocity */
pos[0] = rk*ux*earth_radius_in_km;
pos[1] = rk*uy*earth_radius_in_km;
pos[2] = rk*uz*earth_radius_in_km;
if( vel)
{
vel[0] = (rdot*ux + rvdot * vx)*earth_radius_in_km;
vel[1] = (rdot*uy + rvdot * vy)*earth_radius_in_km;
vel[2] = (rdot*uz + rvdot * vz)*earth_radius_in_km;
}
} /* SGP */
void sgp4_predict(struct _sgp4 *m, double tsince, tle_t *tle, double pos[3], double vel[3])
{
/* This function is used to calculate the position and velocity */
/* of near-earth (period < 225 minutes) satellites. tsince is */
/* time since epoch in minutes, tle is a pointer to a tle_t */
/* structure with Keplerian orbital elements and pos and vel */
/* are vector_t structures returning ECI satellite position and */
/* velocity. Use Convert_Sat_State() to convert to km and km/s. */
double cosuk, sinuk, rfdotk, vx, vy, vz, ux, uy, uz, xmy, xmx, cosnok,
sinnok, cosik, sinik, rdotk, xinck, xnodek, uk, rk, cos2u, sin2u,
u, sinu, cosu, betal, rfdot, rdot, r, pl, elsq, esine, ecose, epw,
cosepw, x1m5th, xhdot1, tfour, sinepw, capu, ayn, xlt, aynl, xll,
axn, xn, beta, xl, e, a, tcube, delm, delomg, templ, tempe, tempa,
xnode, tsq, xmp, omega, xnoddf, omgadf, xmdf, a1, a3ovk2, ao,
betao, betao2, c1sq, c2, c3, coef, coef1, del1, delo, eeta, eosq,
etasq, perigee, pinvsq, psisq, qoms24, s4, temp, temp1, temp2,
temp3, temp4, temp5, temp6, theta2, theta4, tsi;
int i;
/* Initialization */
if (!m->initialized) {
//Set initialized flag:
m->initialized = true;
/* Recover original mean motion (m->xnodp) and */
/* semimajor axis (m->aodp) from input elements. */
a1=pow(xke/tle->xno,tothrd);
m->cosio=cos(tle->xincl);
theta2=m->cosio*m->cosio;
m->x3thm1=3*theta2-1.0;
eosq=tle->eo*tle->eo;
betao2=1.0-eosq;
betao=sqrt(betao2);
del1=1.5*ck2*m->x3thm1/(a1*a1*betao*betao2);
ao=a1*(1.0-del1*(0.5*tothrd+del1*(1.0+134.0/81.0*del1)));
delo=1.5*ck2*m->x3thm1/(ao*ao*betao*betao2);
m->xnodp=tle->xno/(1.0+delo);
m->aodp=ao/(1.0-delo);
/* For perigee less than 220 kilometers, the "simple" */
/* flag is set and the equations are truncated to linear */
/* variation in sqrt a and quadratic variation in mean */
/* anomaly. Also, the c3 term, the delta omega term, and */
/* the delta m term are dropped. */
if ((m->aodp*(1-tle->eo)/ae)<(220/xkmper+ae))
m->simpleFlag = true;
else
m->simpleFlag = false;
/* For perigees below 156 km, the */
/* values of s and qoms2t are altered. */
s4=s;
qoms24=qoms2t;
perigee=(m->aodp*(1-tle->eo)-ae)*xkmper;
if (perigee<156.0)
{
if (perigee<=98.0)
s4=20;
else
s4=perigee-78.0;
qoms24=pow((120-s4)*ae/xkmper,4);
s4=s4/xkmper+ae;
}
pinvsq=1/(m->aodp*m->aodp*betao2*betao2);
tsi=1/(m->aodp-s4);
m->eta=m->aodp*tle->eo*tsi;
etasq=m->eta*m->eta;
eeta=tle->eo*m->eta;
psisq=fabs(1-etasq);
coef=qoms24*pow(tsi,4);
coef1=coef/pow(psisq,3.5);
c2=coef1*m->xnodp*(m->aodp*(1+1.5*etasq+eeta*(4+etasq))+0.75*ck2*tsi/psisq*m->x3thm1*(8+3*etasq*(8+etasq)));
m->c1=tle->bstar*c2;
m->sinio=sin(tle->xincl);
a3ovk2=-xj3/ck2*pow(ae,3);
c3=coef*tsi*a3ovk2*m->xnodp*ae*m->sinio/tle->eo;
m->x1mth2=1-theta2;
m->c4=2*m->xnodp*coef1*m->aodp*betao2*(m->eta*(2+0.5*etasq)+tle->eo*(0.5+2*etasq)-2*ck2*tsi/(m->aodp*psisq)*(-3*m->x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*m->x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*tle->omegao)));
m->c5=2*coef1*m->aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
theta4=theta2*theta2;
temp1=3*ck2*pinvsq*m->xnodp;
temp2=temp1*ck2*pinvsq;
temp3=1.25*ck4*pinvsq*pinvsq*m->xnodp;
m->xmdot=m->xnodp+0.5*temp1*betao*m->x3thm1+0.0625*temp2*betao*(13-78*theta2+137*theta4);
x1m5th=1-5*theta2;
m->omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+395*theta4)+temp3*(3-36*theta2+49*theta4);
xhdot1=-temp1*m->cosio;
m->xnodot=xhdot1+(0.5*temp2*(4-19*theta2)+2*temp3*(3-7*theta2))*m->cosio;
m->omgcof=tle->bstar*c3*cos(tle->omegao);
m->xmcof=-tothrd*coef*tle->bstar*ae/eeta;
m->xnodcf=3.5*betao2*xhdot1*m->c1;
m->t2cof=1.5*m->c1;
m->xlcof=0.125*a3ovk2*m->sinio*(3+5*m->cosio)/(1+m->cosio);
m->aycof=0.25*a3ovk2*m->sinio;
m->delmo=pow(1+m->eta*cos(tle->xmo),3);
m->sinmo=sin(tle->xmo);
m->x7thm1=7*theta2-1;
if (!m->simpleFlag) {
c1sq=m->c1*m->c1;
m->d2=4*m->aodp*tsi*c1sq;
temp=m->d2*tsi*m->c1/3;
m->d3=(17*m->aodp+s4)*temp;
m->d4=0.5*temp*m->aodp*tsi*(221*m->aodp+31*s4)*m->c1;
m->t3cof=m->d2+2*c1sq;
m->t4cof=0.25*(3*m->d3+m->c1*(12*m->d2+10*c1sq));
m->t5cof=0.2*(3*m->d4+12*m->c1*m->d3+6*m->d2*m->d2+15*c1sq*(2*m->d2+c1sq));
}
}
/* Update for secular gravity and atmospheric drag. */
xmdf=tle->xmo+m->xmdot*tsince;
omgadf=tle->omegao+m->omgdot*tsince;
xnoddf=tle->xnodeo+m->xnodot*tsince;
omega=omgadf;
xmp=xmdf;
tsq=tsince*tsince;
xnode=xnoddf+m->xnodcf*tsq;
tempa=1-m->c1*tsince;
tempe=tle->bstar*m->c4*tsince;
templ=m->t2cof*tsq;
if (!m->simpleFlag) {
delomg=m->omgcof*tsince;
delm=m->xmcof*(pow(1+m->eta*cos(xmdf),3)-m->delmo);
temp=delomg+delm;
xmp=xmdf+temp;
omega=omgadf-temp;
tcube=tsq*tsince;
tfour=tsince*tcube;
tempa=tempa-m->d2*tsq-m->d3*tcube-m->d4*tfour;
tempe=tempe+tle->bstar*m->c5*(sin(xmp)-m->sinmo);
templ=templ+m->t3cof*tcube+tfour*(m->t4cof+tsince*m->t5cof);
}
a=m->aodp*pow(tempa,2);
e=tle->eo-tempe;
xl=xmp+omega+xnode+m->xnodp*templ;
beta=sqrt(1-e*e);
xn=xke/pow(a,1.5);
/* Long period periodics */
axn=e*cos(omega);
temp=1/(a*beta*beta);
xll=temp*m->xlcof*axn;
aynl=temp*m->aycof;
xlt=xl+xll;
ayn=e*sin(omega)+aynl;
/* Solve Kepler's Equation */
capu=FMod2p(xlt-xnode);
temp2=capu;
i=0;
do
{
sinepw=sin(temp2);
cosepw=cos(temp2);
temp3=axn*sinepw;
temp4=ayn*cosepw;
temp5=axn*cosepw;
temp6=ayn*sinepw;
epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
if (fabs(epw-temp2)<= e6a)
break;
temp2=epw;
} while (i++<10);
/* Short period preliminary quantities */
ecose=temp5+temp6;
esine=temp3-temp4;
elsq=axn*axn+ayn*ayn;
temp=1-elsq;
pl=a*temp;
r=a*(1-ecose);
temp1=1/r;
rdot=xke*sqrt(a)*esine*temp1;
rfdot=xke*sqrt(pl)*temp1;
temp2=a*temp1;
betal=sqrt(temp);
temp3=1/(1+betal);
cosu=temp2*(cosepw-axn+ayn*esine*temp3);
sinu=temp2*(sinepw-ayn-axn*esine*temp3);
u=AcTan(sinu,cosu);
sin2u=2*sinu*cosu;
cos2u=2*cosu*cosu-1;
temp=1/pl;
temp1=ck2*temp;
temp2=temp1*temp;
/* Update for short periodics */
rk=r*(1-1.5*temp2*betal*m->x3thm1)+0.5*temp1*m->x1mth2*cos2u;
uk=u-0.25*temp2*m->x7thm1*sin2u;
xnodek=xnode+1.5*temp2*m->cosio*sin2u;
xinck=tle->xincl+1.5*temp2*m->cosio*m->sinio*cos2u;
rdotk=rdot-xn*temp1*m->x1mth2*sin2u;
rfdotk=rfdot+xn*temp1*(m->x1mth2*cos2u+1.5*m->x3thm1);
/* Orientation vectors */
sinuk=sin(uk);
cosuk=cos(uk);
sinik=sin(xinck);
cosik=cos(xinck);
sinnok=sin(xnodek);
cosnok=cos(xnodek);
xmx=-sinnok*cosik;
xmy=cosnok*cosik;
ux=xmx*sinuk+cosnok*cosuk;
uy=xmy*sinuk+sinnok*cosuk;
uz=sinik*sinuk;
vx=xmx*cosuk-cosnok*sinuk;
vy=xmy*cosuk-sinnok*sinuk;
vz=sinik*cosuk;
/* Position and velocity */
pos[0] = rk*ux;
pos[1] = rk*uy;
pos[2] = rk*uz;
vel[0] = rdotk*ux+rfdotk*vx;
vel[1] = rdotk*uy+rfdotk*vy;
vel[2] = rdotk*uz+rfdotk*vz;
/* Phase in radians */
m->phase=xlt-xnode-omgadf+twopi;
if (m->phase<0.0)
m->phase+=twopi;
m->phase=FMod2p(m->phase);
}
int DLL_FUNC SGP( const double tsince, const tle_t *tle, const double *params,
double *pos, double *vel)
{
double
temp, rdot, cosu, sinu, cos2u, sin2u, a, e,
p, rr, u, ecose, esine, omgas, cosik, xinck,
sinik, axnsl, aynsl,
sinuk, rvdot, cosuk, coseo1, sineo1, pl,
rk, uk, xl, su, ux, uy, uz, vx, vy, vz, pl2,
xnodek, cosnok, xnodes, el2, eo1, r1, sinnok,
xls, xmx, xmy, tem2, tem5;
const double chicken_factor_on_eccentricity = 1.e-6;
int i, rval = 0;
/* Update for secular gravity and atmospheric drag */
a = tle->xno+(tle->xndt2o*2.+tle->xndd6o*3.*tsince)*tsince;
if( a < 0.)
rval = SXPX_ERR_NEGATIVE_MAJOR_AXIS;
e = e6a;
if( e > 1. - chicken_factor_on_eccentricity)
rval = SXPX_ERR_NEARLY_PARABOLIC;
if( rval)
{
for( i = 0; i < 3; i++)
{
pos[i] = 0.;
if( vel)
vel[i] = 0.;
}
return( rval);
}
a = ao * pow( tle->xno / a, two_thirds);
if( a * (1. - e) < 1. && a * (1. + e) < 1.) /* entirely within earth */
rval = SXPX_WARN_ORBIT_WITHIN_EARTH; /* remember, e can be negative */
if( a * (1. - e) < 1. || a * (1. + e) < 1.) /* perigee within earth */
rval = SXPX_WARN_PERIGEE_WITHIN_EARTH;
if (a > qo) e = 1.-qo/a;
p = a*(1.-e*e);
xnodes = tle->xnodeo+xnodot*tsince;
omgas = tle->omegao+omgdt*tsince;
r1 = xlo+(tle->xno+omgdt+xnodot+
(tle->xndt2o+tle->xndd6o*tsince)*tsince)*tsince;
xls = FMod2p(r1);
/* Long period periodics */
axnsl = e*cos(omgas);
aynsl = e*sin(omgas)-c6/p;
r1 = xls-c5/p*axnsl;
xl = FMod2p(r1);
/* Solve Kepler's equation */
r1 = xl-xnodes;
u = FMod2p(r1);
eo1 = u;
tem5 = 1.;
i = 0;
do
{
sineo1 = sin(eo1);
coseo1 = cos(eo1);
if (fabs(tem5) < e6a) break;
tem5 = 1.-coseo1*axnsl-sineo1*aynsl;
tem5 = (u-aynsl*coseo1+axnsl*sineo1-eo1)/tem5;
tem2 = fabs(tem5);
if (tem2 > 1.) tem5 = tem2/tem5;
eo1 += tem5;
}
while(i++ < 10);
/* Short period preliminary quantities */
ecose = axnsl*coseo1+aynsl*sineo1;
esine = axnsl*sineo1-aynsl*coseo1;
el2 = axnsl*axnsl+aynsl*aynsl;
pl = a*(1.-el2);
pl2 = pl*pl;
rr = a*(1.-ecose);
rdot = xke*sqrt(a)/rr*esine;
rvdot = xke*sqrt(pl)/rr;
temp = esine/(sqrt(1.-el2)+1.);
sinu = a/rr*(sineo1-aynsl-axnsl*temp);
cosu = a/rr*(coseo1-axnsl+aynsl*temp);
su = atan2(sinu, cosu);
/* Update for short periodics */
sin2u = (cosu+cosu)*sinu;
cos2u = 1.-2.*sinu*sinu;
rk = rr+d1o/pl*cos2u;
uk = su-d2o/pl2*sin2u;
xnodek = xnodes+d3o*sin2u/pl2;
xinck = tle->xincl+d4o/pl2*cos2u;
/* Orientation vectors */
sinuk = sin(uk);
cosuk = cos(uk);
sinnok = sin(xnodek);
cosnok = cos(xnodek);
sinik = sin(xinck);
cosik = cos(xinck);
xmx = -sinnok*cosik;
xmy = cosnok*cosik;
ux = xmx*sinuk+cosnok*cosuk;
uy = xmy*sinuk+sinnok*cosuk;
uz = sinik*sinuk;
vx = xmx*cosuk-cosnok*sinuk;
vy = xmy*cosuk-sinnok*sinuk;
vz = sinik*cosuk;
/* Position and velocity */
pos[0] = rk*ux*earth_radius_in_km;
pos[1] = rk*uy*earth_radius_in_km;
pos[2] = rk*uz*earth_radius_in_km;
if( vel)
{
vel[0] = (rdot*ux + rvdot * vx)*earth_radius_in_km;
vel[1] = (rdot*uy + rvdot * vy)*earth_radius_in_km;
vel[2] = (rdot*uz + rvdot * vz)*earth_radius_in_km;
}
return( rval);
} /* SGP */
