/* > sgdp4.c * * 1.00 around 1980 - Felix R. Hoots & Ronald L. Roehrich, from original * SDP4.FOR and SGP4.FOR * ************************************************************************ * * Made famous by the spacetrack report No.3: * "Models for Propogation of NORAD Element Sets" * Edited and subsequently distributed by Dr. T. S. Kelso. * ************************************************************************ * * This conversion by: * Paul S. Crawford and Andrew R. Brooks * Dundee University * * NOTE ! * This code is supplied "as is" and without warranty of any sort. * * (c) 1994-2004, Paul Crawford, Andrew Brooks * ************************************************************************ * * 1.07 arb Oct 1994 - Transcribed by arb Oct 1994 into 'C', then * modified to fit Dundee systems by psc. * * 1.08 psc Mon Nov 7 1994 - replaced original satpos.c with SGP4 model. * * 1.09 psc Wed Nov 9 1994 - Corrected a few minor translation errors after * testing with example two-line elements. * * 1.10 psc Mon Nov 21 1994 - A few optimising tweeks. * * 1.11 psc Wed Nov 30 1994 - No longer uses eloset() and minor error in the * SGP4 code corrected. * * 2.00 psc Tue Dec 13 1994 - arb discovered the archive.afit.af.mil FTP site * with the original FORTRAN code in machine form. * Tidied up and added support for the SDP4 model. * * 2.01 psc Fri Dec 23 1994 - Tested out the combined SGP4/SDP4 code against * the original FORTRAN versions. * * 2.02 psc Mon Jan 02 1995 - Few more tweeks and tidied up the * doccumentation for more general use. * * 3.00 psc Mon May 29 1995 - Cleaned up for general use & distrabution (to * remove Dundee specific features). * * 3.01 psc Mon Jan 12 2004 - Minor bug fix for day calculation. * * 3.02 psc Mon Jul 10 2006 - Added if(rk < (real)1.0) test for sub-orbital decay. * * 3.03 psc Sat Aug 05 2006 - Added trap for divide-by-zero when calculating xlcof. * */ static const char SCCSid[] = "@(#)sgdp4.c 3.03 (C) 1995 psc SatLib: Orbital Model"; #include #include #include /* ================ single / double precision fix-ups =============== */ #include "sgdp4h.h" #define ECC_ZERO ((real)0.0) /* Zero eccentricity case ? */ #define ECC_ALL ((real)1.0e-4) /* For all drag terms in GSFC case. */ #define ECC_EPS ((real)1.0e-6) /* Too low for computing further drops. */ #define ECC_LIMIT_LOW ((real)-1.0e-3) /* Exit point for serious decaying of orbits. */ #define ECC_LIMIT_HIGH ((real)(1.0 - ECC_EPS)) /* Too close to 1 */ #define EPS_COSIO (1.5e-12) /* Minimum divisor allowed for (...)/(1+cos(IO)) */ #define TOTHRD (2.0/3.0) #if defined( SGDP4_SNGL ) || 0 #define NR_EPS ((real)(1.0e-6)) /* Minimum ~1e-6 min for float. */ #else #define NR_EPS ((real)(1.0e-12)) /* Minimum ~1e-14 for double. */ //#define NR_EPS ((real)(1.0e-14)) /* Minimum ~1e-14 for double. */ //#define NR_EPS ((real)(1.0e-8)) /* Minimum ~1e-14 for double. */ #endif #define Q0 ((real)120.0) #define S0 ((real)78.0) #define XJ2 ((real)1.082616e-3) #define XJ3 ((real)-2.53881e-6) #define XJ4 ((real)-1.65597e-6) #define XKMPER (6378.135) /* Km per earth radii */ #define XMNPDA (1440.0) /* Minutes per day */ #define AE (1.0) /* Earth radius in "chosen units". */ #if 0 /* Original code constants. */ #define XKE (0.743669161e-1) #define CK2 ((real)5.413080e-4) /* (0.5 * XJ2 * AE * AE) */ #define CK4 ((real)0.62098875e-6) /* (-0.375 * XJ4 * AE * AE * AE * AE) */ #define QOMS2T ((real)1.88027916e-9) /* (pow((Q0 - S0)*AE/XKMPER, 4.0)) */ #define KS ((real)1.01222928) /* (AE * (1.0 + S0/XKMPER)) */ #else /* GSFC improved coeficient resolution. */ #define XKE ((real)7.43669161331734132e-2) #define CK2 ((real)(0.5 * XJ2 * AE * AE)) #define CK4 ((real)(-0.375 * XJ4 * AE * AE * AE * AE)) #define QOMS2T ((real)1.880279159015270643865e-9) /* (pow((Q0 - S0)*AE/XKMPER, 4.0)) */ #define KS ((real)(AE * (1.0 + S0/XKMPER))) #endif static const real a3ovk2 = (real)(-XJ3 / CK2 * (AE * AE * AE)); /* ================= Copy of the orbital elements ==================== */ static double xno; /* Mean motion (rad/min) */ static real xmo; /* Mean "mean anomaly" at epoch (rad). */ static real eo; /* Eccentricity. */ static real xincl; /* Equatorial inclination (rad). */ static real omegao; /* Mean argument of perigee at epoch (rad). */ static real xnodeo; /* Mean longitude of ascending node (rad, east). */ static real bstar; /* Drag term. */ double SGDP4_jd0; /* Julian Day for epoch (available to outside functions. */ /* ================== Local "global" variables for SGP4 ================= */ static int imode = SGDP4_NOT_INIT; static real sinIO, cosIO, sinXMO, cosXMO; static real c1, c2, c3, c4, c5, d2, d3, d4; static real omgcof, xmcof, xlcof, aycof; static real t2cof, t3cof, t4cof, t5cof; static real xnodcf, delmo, x7thm1, x3thm1, x1mth2; static real aodp, eta, omgdot, xnodot; static double xnodp, xmdot; static long Isat=0; /* 16-bit compilers need 'long' integer for higher space catalogue numbers. */ double perigee, period, apogee; long Icount = 0; int MaxNR=0; extern int Set_LS_zero; /* From deep.c */ /* ======================================================================= The init_sgdp4() function passes all of the required orbital elements to the sgdp4() function together with the pre-calculated constants. There is some basic error traps and the detemination of the orbital model is made. For near-earth satellites (xnodp < 225 minutes according to the NORAD classification) the SGP4 model is used, with truncated terms for low perigee heights when the drag terms are high. For deep-space satellites the SDP4 model is used and the deep-space terms initialised (a slow process). For orbits with an eccentricity of less than ECC_EPS the model reverts to a very basic circular model. This is not physically meaningfull but such a circluar orbit is not either! It is fast though. Callinr arguments: orb : Input, structure with the orbital elements from NORAD 2-line element data in radian form. The return value indicates the orbital model used. ======================================================================= */ int init_sgdp4(orbit_t *orb) { LOCAL_REAL theta2, theta4, xhdot1, x1m5th; LOCAL_REAL s4, del1, del0; LOCAL_REAL betao, betao2, coef, coef1; LOCAL_REAL etasq, eeta, qoms24; LOCAL_REAL pinvsq, tsi, psisq, c1sq; LOCAL_DOUBLE a0, a1, epoch; real temp0, temp1, temp2, temp3; long iday, iyear; /* Copy over elements. */ /* Convert year to Gregorian with century as 1994 or 94 type ? */ iyear = (long)orb->ep_year; if (iyear < 1957) { /* Assume 0 and 100 both refer to 2000AD */ iyear += (iyear < 57 ? 2000 : 1900); } if (iyear < 1901 || iyear > 2099) { fatal_error("init_sgdp4: Satellite ep_year error %ld", iyear); imode = SGDP4_ERROR; return imode; } Isat = orb->satno; /* Compute days from 1st Jan 1900 (works 1901 to 2099 only). */ iday = ((iyear - 1901)*1461L)/4L + 364L + 1L; SGDP4_jd0 = JD1900 + iday + (orb->ep_day - 1.0); /* Julian day number. */ epoch = (iyear - 1900) * 1.0e3 + orb->ep_day; /* YYDDD.DDDD as from 2-line. */ #ifdef DEBUG fprintf(stderr, "Epoch = %f SGDP4_jd0 = %f\n", epoch, SGDP4_jd0); #endif eo = (real)orb->ecc; xno = (double)orb->rev * TWOPI/XMNPDA; /* Radian / unit time. */ xincl = (real)orb->eqinc; xnodeo = (real)orb->ascn; omegao = (real)orb->argp; xmo = (real)orb->mnan; bstar = (real)orb->bstar; /* A few simple error checks here. */ if (eo < (real)0.0 || eo > ECC_LIMIT_HIGH) { fatal_error("init_sgdp4: Eccentricity out of range for %ld (%le)", Isat, (double)eo); imode = SGDP4_ERROR; return imode; } if (xno < 0.035*TWOPI/XMNPDA || xno > 18.0*TWOPI/XMNPDA) { fatal_error("init_sgdp4: Mean motion out of range %ld (%le)", Isat, xno); imode = SGDP4_ERROR; return imode; } if (xincl < (real)0.0 || xincl > (real)PI) { fatal_error("init_sgdp4: Equatorial inclination out of range %ld (%le)", Isat, DEG(xincl)); imode = SGDP4_ERROR; return imode; } /* Start the initialisation. */ if (eo < ECC_ZERO) imode = SGDP4_ZERO_ECC; /* Special mode for "ideal" circular orbit. */ else imode = SGDP4_NOT_INIT; /* Recover original mean motion (xnodp) and semimajor axis (aodp) from input elements. */ SINCOS(xincl, &sinIO, &cosIO); theta2 = cosIO * cosIO; theta4 = theta2 * theta2; x3thm1 = (real)3.0 * theta2 - (real)1.0; x1mth2 = (real)1.0 - theta2; x7thm1 = (real)7.0 * theta2 - (real)1.0; a1 = pow(XKE / xno, TOTHRD); betao2 = (real)1.0 - eo * eo; betao = SQRT(betao2); temp0 = (real)(1.5 * CK2) * x3thm1 / (betao * betao2); del1 = temp0 / (a1 * a1); a0 = a1 * (1.0 - del1 * (1.0/3.0 + del1 * (1.0 + del1 * 134.0/81.0))); del0 = temp0 / (a0 * a0); xnodp = xno / (1.0 + del0); aodp = (real)(a0 / (1.0 - del0)); perigee = (aodp * (1.0 - eo) - AE) * XKMPER; apogee = (aodp * (1.0 + eo) - AE) * XKMPER; period = (TWOPI * 1440.0 / XMNPDA) / xnodp; /* printf("Perigee = %lf km period = %lf min del0 = %e\n", perigee, period, del0); */ if (perigee <= 0.0) { fprintf(stderr, "# Satellite %ld sub-orbital (apogee = %.1f km, perigee = %.1f km)\n", Isat, apogee, perigee); } if (imode == SGDP4_ZERO_ECC) return imode; if (period >= 225.0 && Set_LS_zero < 2) { imode = SGDP4_DEEP_NORM; /* Deep-Space model(s). */ } else if (perigee < 220.0) { /* For perigee less than 220 km the imode flag is set so 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. */ imode = SGDP4_NEAR_SIMP; /* Near-space, simplified equations. */ } else { imode = SGDP4_NEAR_NORM; /* Near-space, normal equations. */ } /* For perigee below 156 km the values of S and QOMS2T are altered */ if (perigee < 156.0) { s4 = (real)(perigee - 78.0); if(s4 < (real)20.0) { fprintf(stderr, "# Very low s4 constant for sat %ld (perigee = %.2f)\n", Isat, perigee); s4 = (real)20.0; } else { fprintf(stderr, "# Changing s4 constant for sat %ld (perigee = %.2f)\n", Isat, perigee); } qoms24 = POW4((real)((120.0 - s4) * (AE / XKMPER))); s4 = (real)(s4 / XKMPER + AE); } else { s4 = KS; qoms24 = QOMS2T; } pinvsq = (real)1.0 / (aodp * aodp * betao2 * betao2); tsi = (real)1.0 / (aodp - s4); eta = aodp * eo * tsi; etasq = eta * eta; eeta = eo * eta; psisq = FABS((real)1.0 - etasq); coef = qoms24 * POW4(tsi); coef1 = coef / POW(psisq, 3.5); c2 = coef1 * (real)xnodp * (aodp * ((real)1.0 + (real)1.5 * etasq + eeta * ((real)4.0 + etasq)) + (real)(0.75 * CK2) * tsi / psisq * x3thm1 * ((real)8.0 + (real)3.0 * etasq * ((real)8.0 + etasq))); c1 = bstar * c2; c4 = (real)2.0 * (real)xnodp * coef1 * aodp * betao2 * (eta * ((real)2.0 + (real)0.5 * etasq) + eo * ((real)0.5 + (real)2.0 * etasq) - (real)(2.0 * CK2) * tsi / (aodp * psisq) * ((real)-3.0 * x3thm1 * ((real)1.0 - (real)2.0 * eeta + etasq * ((real)1.5 - (real)0.5 * eeta)) + (real)0.75 * x1mth2 * ((real)2.0 * etasq - eeta * ((real)1.0 + etasq)) * COS((real)2.0 * omegao))); c5 = c3 = omgcof = (real)0.0; if (imode == SGDP4_NEAR_NORM) { /* BSTAR drag terms for normal near-space 'normal' model only. */ c5 = (real)2.0 * coef1 * aodp * betao2 * ((real)1.0 + (real)2.75 * (etasq + eeta) + eeta * etasq); if(eo > ECC_ALL) { c3 = coef * tsi * a3ovk2 * (real)xnodp * (real)AE * sinIO / eo; } omgcof = bstar * c3 * COS(omegao); } temp1 = (real)(3.0 * CK2) * pinvsq * (real)xnodp; temp2 = temp1 * CK2 * pinvsq; temp3 = (real)(1.25 * CK4) * pinvsq * pinvsq * (real)xnodp; xmdot = xnodp + ((real)0.5 * temp1 * betao * x3thm1 + (real)0.0625 * temp2 * betao * ((real)13.0 - (real)78.0 * theta2 + (real)137.0 * theta4)); x1m5th = (real)1.0 - (real)5.0 * theta2; omgdot = (real)-0.5 * temp1 * x1m5th + (real)0.0625 * temp2 * ((real)7.0 - (real)114.0 * theta2 + (real)395.0 * theta4) + temp3 * ((real)3.0 - (real)36.0 * theta2 + (real)49.0 * theta4); xhdot1 = -temp1 * cosIO; xnodot = xhdot1 + ((real)0.5 * temp2 * ((real)4.0 - (real)19.0 * theta2) + (real)2.0 * temp3 * ((real)3.0 - (real)7.0 * theta2)) * cosIO; xmcof = (real)0.0; if(eo > ECC_ALL) { xmcof = (real)(-TOTHRD * AE) * coef * bstar / eeta; } xnodcf = (real)3.5 * betao2 * xhdot1 * c1; t2cof = (real)1.5 * c1; /* Check for possible divide-by-zero for X/(1+cosIO) when calculating xlcof */ temp0 = (real)1.0 + cosIO; if(fabs(temp0) < EPS_COSIO) temp0 = (real)SIGN(EPS_COSIO, temp0); xlcof = (real)0.125 * a3ovk2 * sinIO * ((real)3.0 + (real)5.0 * cosIO) / temp0; aycof = (real)0.25 * a3ovk2 * sinIO; SINCOS(xmo, &sinXMO, &cosXMO); delmo = CUBE((real)1.0 + eta * cosXMO); if (imode == SGDP4_NEAR_NORM) { c1sq = c1 * c1; d2 = (real)4.0 * aodp * tsi * c1sq; temp0 = d2 * tsi * c1 / (real)3.0; d3 = ((real)17.0 * aodp + s4) * temp0; d4 = (real)0.5 * temp0 * aodp * tsi * ((real)221.0 * aodp + (real)31.0 * s4) * c1; t3cof = d2 + (real)2.0 * c1sq; t4cof = (real)0.25 * ((real)3.0 * d3 + c1 * ((real)12.0 * d2 + (real)10.0 * c1sq)); t5cof = (real)0.2 * ((real)3.0 * d4 + (real)12.0 * c1 * d3 + (real)6.0 * d2 * d2 + (real)15.0 * c1sq * ((real)2.0 * d2 + c1sq)); } else if (imode == SGDP4_DEEP_NORM) { #ifdef NO_DEEP_SPACE fatal_error("init_sgdp4: Deep space equations not supported"); #else imode = SGDP4_dpinit(epoch, omegao, xnodeo, xmo, eo, xincl, aodp, xmdot, omgdot, xnodot, xnodp); #endif /* !NO_DEEP_SPACE */ } return imode; } /* ======================================================================= The sgdp4() function computes the Keplarian elements that describe the position and velocity of the satellite. Depending on the initialisation (and the compile options) the deep-space perturbations are also included allowing sensible predictions for most satellites. These output elements can be transformed to Earth Centered Inertial coordinates (X-Y-Z) and/or to sub-satellite latitude and longitude as required. The terms for the velocity solution are often not required so the 'withvel' flag can be used to by-pass that step as required. This function is normally called through another since the input 'tsince' is the time from epoch. Calling arguments: tsince : Input, time from epoch (minutes). withvel : Input, non-zero if velocity terms required. kep : Output, the Keplarian position / velocity of the satellite. The return value indicates the orbital mode used. ======================================================================= */ int sgdp4(double tsince, int withvel, kep_t *kep) { LOCAL_REAL rk, uk, xnodek, xinck, em, xinc; LOCAL_REAL xnode, delm, axn, ayn, omega; LOCAL_REAL capu, epw, elsq, invR, beta2, betal; LOCAL_REAL sinu, sin2u, cosu, cos2u; LOCAL_REAL a, e, r, u, pl; LOCAL_REAL sinEPW, cosEPW, sinOMG, cosOMG; LOCAL_DOUBLE xmp, xl, xlt; const int MAXI = 10; #ifndef NO_DEEP_SPACE LOCAL_DOUBLE xn, xmam; #endif /* !NO_DEEP_SPACE */ real esinE, ecosE, maxnr; real temp0, temp1, temp2, temp3; real tempa, tempe, templ; int ii; #ifdef SGDP4_SNGL real ts = (real)tsince; #else #define ts tsince #endif /* ! SGDP4_SNGL */ /* Update for secular gravity and atmospheric drag. */ em = eo; xinc = xincl; xmp = (double)xmo + xmdot * tsince; xnode = xnodeo + ts * (xnodot + ts * xnodcf); omega = omegao + omgdot * ts; switch(imode) { case SGDP4_ZERO_ECC: /* Not a "real" orbit but OK for fast computation searches. */ kep->smjaxs = kep->radius = (double)aodp * XKMPER/AE; kep->theta = fmod(PI + xnodp * tsince, TWOPI) - PI; kep->eqinc = (double)xincl; kep->ascn = xnodeo; kep->argp = 0; kep->ecc = 0; kep->rfdotk = 0; if(withvel) kep->rfdotk = aodp * xnodp * (XKMPER/AE*XMNPDA/86400.0); /* For km/sec */ else kep->rfdotk = 0; return imode; case SGDP4_NEAR_SIMP: tempa = (real)1.0 - ts * c1; tempe = bstar * ts * c4; templ = ts * ts * t2cof; a = aodp * tempa * tempa; e = em - tempe; xl = xmp + omega + xnode + xnodp * templ; break; case SGDP4_NEAR_NORM: delm = xmcof * (CUBE((real)1.0 + eta * COS(xmp)) - delmo); temp0 = ts * omgcof + delm; xmp += (double)temp0; omega -= temp0; tempa = (real)1.0 - (ts * (c1 + ts * (d2 + ts * (d3 + ts * d4)))); tempe = bstar * (c4 * ts + c5 * (SIN(xmp) - sinXMO)); templ = ts * ts * (t2cof + ts * (t3cof + ts * (t4cof + ts * t5cof))); //xmp += (double)temp0; a = aodp * tempa * tempa; e = em - tempe; xl = xmp + omega + xnode + xnodp * templ; break; #ifndef NO_DEEP_SPACE case SGDP4_DEEP_NORM: case SGDP4_DEEP_RESN: case SGDP4_DEEP_SYNC: tempa = (real)1.0 - ts * c1; tempe = bstar * ts * c4; templ = ts * ts * t2cof; xn = xnodp; SGDP4_dpsec(&xmp, &omega, &xnode, &em, &xinc, &xn, tsince); a = POW(XKE / xn, TOTHRD) * tempa * tempa; e = em - tempe; xmam = xmp + xnodp * templ; SGDP4_dpper(&e, &xinc, &omega, &xnode, &xmam, tsince); if (xinc < (real)0.0) { xinc = (-xinc); xnode += (real)PI; omega -= (real)PI; } xl = xmam + omega + xnode; /* Re-compute the perturbed values. */ SINCOS(xinc, &sinIO, &cosIO); { real theta2 = cosIO * cosIO; x3thm1 = (real)3.0 * theta2 - (real)1.0; x1mth2 = (real)1.0 - theta2; x7thm1 = (real)7.0 * theta2 - (real)1.0; /* Check for possible divide-by-zero for X/(1+cosIO) when calculating xlcof */ temp0 = (real)1.0 + cosIO; if(fabs(temp0) < EPS_COSIO) temp0 = (real)SIGN(EPS_COSIO, temp0); xlcof = (real)0.125 * a3ovk2 * sinIO * ((real)3.0 + (real)5.0 * cosIO) / temp0; aycof = (real)0.25 * a3ovk2 * sinIO; } break; #endif /* ! NO_DEEP_SPACE */ default: fatal_error("sgdp4: Orbit not initialised"); return SGDP4_ERROR; } if(a < (real)1.0) { fprintf(stderr, "sgdp4: Satellite %05ld crashed at %.3f (a = %.3f Earth radii)\n", Isat, ts, a); return SGDP4_ERROR; } if(e < ECC_LIMIT_LOW) { fprintf(stderr, "sgdp4: Satellite %05ld modified eccentricity too low (ts = %.3f, e = %e < %e)\n", Isat, ts, e, ECC_LIMIT_LOW); return SGDP4_ERROR; } if(e < ECC_EPS) { /*fprintf(stderr, "# ecc %f at %.3f for for %05ld\n", e, ts, Isat);*/ e = ECC_EPS; } else if(e > ECC_LIMIT_HIGH) { /*fprintf(stderr, "# ecc %f at %.3f for for %05ld\n", e, ts, Isat);*/ e = ECC_LIMIT_HIGH; } beta2 = (real)1.0 - e * e; /* Long period periodics */ SINCOS(omega, &sinOMG, &cosOMG); temp0 = (real)1.0 / (a * beta2); axn = e * cosOMG; ayn = e * sinOMG + temp0 * aycof; xlt = xl + temp0 * xlcof * axn; elsq = axn * axn + ayn * ayn; if (elsq >= (real)1.0) { fprintf(stderr, "sgdp4: SQR(e) >= 1 (%.3f at tsince = %.3f for sat %05ld)\n", elsq, tsince, Isat); return SGDP4_ERROR; } /* Sensibility check for N-R correction. */ kep->ecc = sqrt(elsq); /* * Solve Kepler's equation using Newton-Raphson root solving. Here 'capu' is * almost the "Mean anomaly", initialise the "Eccentric Anomaly" term 'epw'. * The fmod() saves reduction of angle to +/-2pi in SINCOS() and prevents * convergence problems. * * Later modified to support 2nd order NR method which saves roughly 1 iteration * for only a couple of arithmetic operations. */ epw = capu = fmod(xlt - xnode, TWOPI); maxnr = kep->ecc; for(ii = 0; ii < MAXI; ii++) { double nr, f, df; SINCOS(epw, &sinEPW, &cosEPW); ecosE = axn * cosEPW + ayn * sinEPW; esinE = axn * sinEPW - ayn * cosEPW; f = capu - epw + esinE; if (fabs(f) < NR_EPS) break; df = 1.0 - ecosE; /* 1st order Newton-Raphson correction. */ nr = f / df; if (ii == 0 && FABS(nr) > 1.25*maxnr) nr = SIGN(maxnr, nr); #if 1 /* 2nd order Newton-Raphson correction. */ else nr = f / (df + 0.5*esinE*nr); /* f/(df - 0.5*d2f*f/df) */ #endif epw += nr; /* Newton-Raphson correction of -F/DF. */ //if (fabs(nr) < NR_EPS) break; } /* Short period preliminary quantities */ temp0 = (real)1.0 - elsq; betal = SQRT(temp0); pl = a * temp0; r = a * ((real)1.0 - ecosE); invR = (real)1.0 / r; temp2 = a * invR; temp3 = (real)1.0 / ((real)1.0 + betal); cosu = temp2 * (cosEPW - axn + ayn * esinE * temp3); sinu = temp2 * (sinEPW - ayn - axn * esinE * temp3); u = ATAN2(sinu, cosu); sin2u = (real)2.0 * sinu * cosu; cos2u = (real)2.0 * cosu * cosu - (real)1.0; temp0 = (real)1.0 / pl; temp1 = CK2 * temp0; temp2 = temp1 * temp0; /* Update for short term periodics to position terms. */ rk = r * ((real)1.0 - (real)1.5 * temp2 * betal * x3thm1) + (real)0.5 * temp1 * x1mth2 * cos2u; uk = u - (real)0.25 * temp2 * x7thm1 * sin2u; xnodek = xnode + (real)1.5 * temp2 * cosIO * sin2u; xinck = xinc + (real)1.5 * temp2 * cosIO * sinIO * cos2u; if(rk < (real)1.0) { #if 1 fprintf(stderr, "sgdp4: Satellite %05ld crashed at %.3f (rk = %.3f Earth radii)\n", Isat, ts, rk); #endif return SGDP4_ERROR; } kep->radius = rk * XKMPER/AE; /* Into km */ kep->theta = uk; kep->eqinc = xinck; kep->ascn = xnodek; kep->argp = omega; kep->smjaxs = a * XKMPER/AE; /* Short period velocity terms ?. */ if (withvel) { /* xn = XKE / pow(a, 1.5); */ temp0 = SQRT(a); temp2 = (real)XKE / (a * temp0); kep->rdotk = ((real)XKE * temp0 * esinE * invR - temp2 * temp1 * x1mth2 * sin2u) * (XKMPER/AE*XMNPDA/86400.0); /* Into km/sec */ kep->rfdotk = ((real)XKE * SQRT(pl) * invR + temp2 * temp1 * (x1mth2 * cos2u + (real)1.5 * x3thm1)) * (XKMPER/AE*XMNPDA/86400.0); } else { kep->rdotk = kep->rfdotk = 0; } #ifndef SGDP4_SNGL #undef ts #endif return imode; } /* ==================================================================== Transformation from "Kepler" type coordinates to cartesian XYZ form. Calling arguments: K : Kepler structure as filled by sgdp4(); pos : XYZ structure for position. vel : same for velocity. ==================================================================== */ void kep2xyz(kep_t *K, xyz_t *pos, xyz_t *vel) { real xmx, xmy; real ux, uy, uz, vx, vy, vz; real sinT, cosT, sinI, cosI, sinS, cosS; /* Orientation vectors for X-Y-Z format. */ SINCOS((real)K->theta, &sinT, &cosT); SINCOS((real)K->eqinc, &sinI, &cosI); SINCOS((real)K->ascn, &sinS, &cosS); xmx = -sinS * cosI; xmy = cosS * cosI; ux = xmx * sinT + cosS * cosT; uy = xmy * sinT + sinS * cosT; uz = sinI * sinT; /* Position and velocity */ if(pos != NULL) { pos->x = K->radius * ux; pos->y = K->radius * uy; pos->z = K->radius * uz; } if(vel != NULL) { vx = xmx * cosT - cosS * sinT; vy = xmy * cosT - sinS * sinT; vz = sinI * cosT; vel->x = K->rdotk * ux + K->rfdotk * vx; vel->y = K->rdotk * uy + K->rfdotk * vy; vel->z = K->rdotk * uz + K->rfdotk * vz; } } /* ====================================================================== Compute the satellite position and/or velocity for a given time (in the form of Julian day number.) Calling arguments are: jd : Time as Julian day number. pos : Pointer to posiition vector, km (NULL if not required). vel : Pointer to velocity vector, km/sec (NULL if not required). ====================================================================== */ int satpos_xyz(double jd, xyz_t *pos, xyz_t *vel) { kep_t K; int withvel, rv; double tsince; tsince = (jd - SGDP4_jd0) * XMNPDA; #ifdef DEBUG fprintf(stderr, "Tsince = %f\n", tsince); #endif if(vel != NULL) withvel = 1; else withvel = 0; rv = sgdp4(tsince, withvel, &K); kep2xyz(&K, pos, vel); return rv; } /* ==================== End of file sgdp4.c ========================== */