| 1 | #include <stdlib.h>
|
|---|
| 2 | #include <math.h>
|
|---|
| 3 |
|
|---|
| 4 | #include "sattypes.h"
|
|---|
| 5 | #include "vector.h"
|
|---|
| 6 | #include "satspec.h"
|
|---|
| 7 |
|
|---|
| 8 | #define XMO (sat->elem->se_XMO)
|
|---|
| 9 | #define XNODEO (sat->elem->se_XNODEO)
|
|---|
| 10 | #define OMEGAO (sat->elem->se_OMEGAO)
|
|---|
| 11 | #define EO (sat->elem->se_EO)
|
|---|
| 12 | #define XINCL (sat->elem->se_XINCL)
|
|---|
| 13 | #define XNO (sat->elem->se_XNO)
|
|---|
| 14 | #define XNDT20 (sat->elem->se_XNDT20)
|
|---|
| 15 | #define XNDD60 (sat->elem->se_XNDD60)
|
|---|
| 16 | #define BSTAR (sat->elem->se_BSTAR)
|
|---|
| 17 | #define EPOCH (sat->elem->se_EPOCH)
|
|---|
| 18 |
|
|---|
| 19 | #define X (pos->sl_X)
|
|---|
| 20 | #define XDOT (pos->sl_XDOT)
|
|---|
| 21 | #define Y (pos->sl_Y)
|
|---|
| 22 | #define YDOT (pos->sl_YDOT)
|
|---|
| 23 | #define Z (pos->sl_Z)
|
|---|
| 24 | #define ZDOT (pos->sl_ZDOT)
|
|---|
| 25 |
|
|---|
| 26 | #define AODP (sat->prop.sgp4->sgp4_AODP)
|
|---|
| 27 | #define AYCOF (sat->prop.sgp4->sgp4_AYCOF)
|
|---|
| 28 | #define C1 (sat->prop.sgp4->sgp4_C1)
|
|---|
| 29 | #define C4 (sat->prop.sgp4->sgp4_C4)
|
|---|
| 30 | #define C5 (sat->prop.sgp4->sgp4_C5)
|
|---|
| 31 | #define COSIO (sat->prop.sgp4->sgp4_COSIO)
|
|---|
| 32 | #define D2 (sat->prop.sgp4->sgp4_D2)
|
|---|
| 33 | #define D3 (sat->prop.sgp4->sgp4_D3)
|
|---|
| 34 | #define D4 (sat->prop.sgp4->sgp4_D4)
|
|---|
| 35 | #define DELMO (sat->prop.sgp4->sgp4_DELMO)
|
|---|
| 36 | #define ETA (sat->prop.sgp4->sgp4_ETA)
|
|---|
| 37 | #define OMGCOF (sat->prop.sgp4->sgp4_OMGCOF)
|
|---|
| 38 | #define OMGDOT (sat->prop.sgp4->sgp4_OMGDOT)
|
|---|
| 39 | #define SINIO (sat->prop.sgp4->sgp4_SINIO)
|
|---|
| 40 | #define SINMO (sat->prop.sgp4->sgp4_SINMO)
|
|---|
| 41 | #define T2COF (sat->prop.sgp4->sgp4_T2COF)
|
|---|
| 42 | #define T3COF (sat->prop.sgp4->sgp4_T3COF)
|
|---|
| 43 | #define T4COF (sat->prop.sgp4->sgp4_T4COF)
|
|---|
| 44 | #define T5COF (sat->prop.sgp4->sgp4_T5COF)
|
|---|
| 45 | #define X1MTH2 (sat->prop.sgp4->sgp4_X1MTH2)
|
|---|
| 46 | #define X3THM1 (sat->prop.sgp4->sgp4_X3THM1)
|
|---|
| 47 | #define X7THM1 (sat->prop.sgp4->sgp4_X7THM1)
|
|---|
| 48 | #define XLCOF (sat->prop.sgp4->sgp4_XLCOF)
|
|---|
| 49 | #define XMCOF (sat->prop.sgp4->sgp4_XMCOF)
|
|---|
| 50 | #define XMDOT (sat->prop.sgp4->sgp4_XMDOT)
|
|---|
| 51 | #define XNODCF (sat->prop.sgp4->sgp4_XNODCF)
|
|---|
| 52 | #define XNODOT (sat->prop.sgp4->sgp4_XNODOT)
|
|---|
| 53 | #define XNODP (sat->prop.sgp4->sgp4_XNODP)
|
|---|
| 54 |
|
|---|
| 55 | #define CK2 (5.413080e-04)
|
|---|
| 56 | #define CK4 (6.209887e-07)
|
|---|
| 57 | #define QOMS2T (1.880279e-09)
|
|---|
| 58 | #define S (1.012229e+00)
|
|---|
| 59 |
|
|---|
| 60 | #define AE (1.0)
|
|---|
| 61 | #define DE2RA (.174532925E-1)
|
|---|
| 62 | #define E6A (1.E-6)
|
|---|
| 63 | #define PI (3.14159265)
|
|---|
| 64 | #define PIO2 (1.57079633)
|
|---|
| 65 | #define QO (120.0)
|
|---|
| 66 | #define SO (78.0)
|
|---|
| 67 | #define TOTHRD (.66666667)
|
|---|
| 68 | #define TWOPI (6.2831853)
|
|---|
| 69 | #define X3PIO2 (4.71238898)
|
|---|
| 70 | #define XJ2 (1.082616E-3)
|
|---|
| 71 | #define XJ3 (-.253881E-5)
|
|---|
| 72 | #define XJ4 (-1.65597E-6)
|
|---|
| 73 | #define XKE (.743669161E-1)
|
|---|
| 74 | #define XKMPER (6378.135)
|
|---|
| 75 | #define XMNPDA (1440.0)
|
|---|
| 76 |
|
|---|
| 77 | /* compute position and velocity vectors for the satellite defined in sat->elem
|
|---|
| 78 | * at its epoch + TSINCE.
|
|---|
| 79 | */
|
|---|
| 80 | void
|
|---|
| 81 | sgp4(SatData *sat, Vec3 *pos, Vec3 *dpos, double TSINCE)
|
|---|
| 82 | {
|
|---|
| 83 | int i;
|
|---|
| 84 |
|
|---|
| 85 | double A1, A3OVK2, AO, BETAO, BETAO2, C1SQ, C2, C3, COEF, COEF1,
|
|---|
| 86 | DEL1, DELO, EETA, EOSQ, ETASQ, PERIGE, PINVSQ, PSISQ, QOMS24,
|
|---|
| 87 | S4, TEMP, TEMP1, TEMP2, TEMP3=0, THETA2, THETA4, TSI, X1M5TH,
|
|---|
| 88 | XHDOT1;
|
|---|
| 89 |
|
|---|
| 90 | double A, AXN, AYN, AYNL, BETA, BETAL, CAPU, COS2U, COSEPW=0, COSIK,
|
|---|
| 91 | COSNOK, COSU, COSUK, DELM, DELOMG, E, ECOSE, ELSQ, EPW, ESINE,
|
|---|
| 92 | OMEGA, OMGADF, PL, R, RDOT, RDOTK, RFDOT, RFDOTK, RK, SIN2U,
|
|---|
| 93 | SINEPW=0, SINIK, SINNOK, SINU, SINUK, TCUBE, TEMP4=0, TEMP5=0, TEMP6=0,
|
|---|
| 94 | TEMPA, TEMPE, TEMPL, TFOUR, TSQ, U, UK, UX, UY, UZ, VX, VY, VZ,
|
|---|
| 95 | XINCK, XL, XLL, XLT, XMDF, XMP, XMX, XMY, XN, XNODDF, XNODE,
|
|---|
| 96 | XNODEK;
|
|---|
| 97 |
|
|---|
| 98 | #if 0
|
|---|
| 99 | A1 = A3OVK2 = AO = BETAO = BETAO2 = C1SQ = C2 = C3 = COEF = COEF1 =
|
|---|
| 100 | DEL1 = DELO = EETA = EOSQ = ETASQ = PERIGE = PINVSQ = PSISQ = QOMS24 =
|
|---|
| 101 | S4 = TEMP = TEMP1 = TEMP2 = TEMP3 = THETA2 = THETA4 = TSI = X1M5TH =
|
|---|
| 102 | XHDOT1 = signaling_nan();
|
|---|
| 103 |
|
|---|
| 104 | A = AXN = AYN = AYNL = BETA = BETAL = CAPU = COS2U = COSEPW = COSIK =
|
|---|
| 105 | COSNOK = COSU = COSUK = DELM = DELOMG = E = ECOSE = ELSQ = EPW =
|
|---|
| 106 | ESINE = OMEGA = OMGADF = PL = R = RDOT = RDOTK = RFDOT = RFDOTK =
|
|---|
| 107 | RK = SIN2U = SINEPW = SINIK = SINNOK = SINU = SINUK = TCUBE = TEMP4 =
|
|---|
| 108 | TEMP5 = TEMP6 = TEMPA = TEMPE = TEMPL = TFOUR = TSQ = U = UK = UX =
|
|---|
| 109 | UY = UZ = VX = VY = VZ = XINCK = XL = XLL = XLT = XMDF = XMP = XMX =
|
|---|
| 110 | XMY = XN = XNODDF = XNODE = XNODEK = signaling_nan();
|
|---|
| 111 | #endif
|
|---|
| 112 |
|
|---|
| 113 | if(!sat->prop.sgp4) {
|
|---|
| 114 | sat->prop.sgp4 = (struct sgp4_data *) malloc(sizeof(struct sgp4_data));
|
|---|
| 115 |
|
|---|
| 116 | /*
|
|---|
| 117 | * RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP)
|
|---|
| 118 | * FROM INPUT ELEMENTS
|
|---|
| 119 | */
|
|---|
| 120 |
|
|---|
| 121 | A1 = pow((XKE/XNO), TOTHRD);
|
|---|
| 122 | COSIO = cos(XINCL);
|
|---|
| 123 | THETA2 = COSIO * COSIO;
|
|---|
| 124 | X3THM1 = 3.0 * THETA2 - 1.0;
|
|---|
| 125 | EOSQ = EO * EO;
|
|---|
| 126 | BETAO2 = 1.0 - EOSQ;
|
|---|
| 127 | BETAO = sqrt(BETAO2);
|
|---|
| 128 | DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
|
|---|
| 129 | AO = A1 * (1.0 - DEL1 * (.5 * TOTHRD +
|
|---|
| 130 | DEL1 * (1.0 + 134.0 /81.0 * DEL1)));
|
|---|
| 131 | DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
|
|---|
| 132 | XNODP = XNO / (1.0 + DELO);
|
|---|
| 133 | AODP=AO / (1.0 - DELO);
|
|---|
| 134 |
|
|---|
| 135 | /*
|
|---|
| 136 | * INITIALIZATION
|
|---|
| 137 | *
|
|---|
| 138 | * FOR PERIGEE LESS THAN 220 KILOMETERS, THE ISIMP FLAG IS SET AND
|
|---|
| 139 | * THE EQUATIONS ARE TRUNCATED TO LINEAR VARIATION IN SQRT A AND
|
|---|
| 140 | * QUADRATIC VARIATION IN MEAN ANOMALY. ALSO, THE C3 TERM, THE
|
|---|
| 141 | * DELTA OMEGA TERM, AND THE DELTA M TERM ARE DROPPED.
|
|---|
| 142 | */
|
|---|
| 143 |
|
|---|
| 144 | sat->prop.sgp4->sgp4_flags = 0;
|
|---|
| 145 |
|
|---|
| 146 | /* IF((AODP*(1.-EO)/AE) .LT. (220./XKMPER+AE)) ISIMP=1 */
|
|---|
| 147 |
|
|---|
| 148 | if((AODP * (1.0 - EO) / AE) < (220.0 / XKMPER + AE))
|
|---|
| 149 | sat->prop.sgp4->sgp4_flags |= SGP4_SIMPLE;
|
|---|
| 150 |
|
|---|
| 151 | /*
|
|---|
| 152 | * FOR PERIGEE BELOW 156 KM, THE VALUES OF
|
|---|
| 153 | * S AND QOMS2T ARE ALTERED
|
|---|
| 154 | */
|
|---|
| 155 |
|
|---|
| 156 | S4 = S;
|
|---|
| 157 | QOMS24 = QOMS2T;
|
|---|
| 158 | PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;
|
|---|
| 159 |
|
|---|
| 160 | if(PERIGE < 156.0) {
|
|---|
| 161 | S4 = PERIGE - 78.0;
|
|---|
| 162 |
|
|---|
| 163 | if(PERIGE <= 98.0)
|
|---|
| 164 | S4 = 20.0;
|
|---|
| 165 |
|
|---|
| 166 | QOMS24 = pow(((120.0 - S4) * AE / XKMPER), 4.0);
|
|---|
| 167 | S4 = S4 / XKMPER + AE;
|
|---|
| 168 | }
|
|---|
| 169 |
|
|---|
| 170 | PINVSQ=1.0 / (AODP * AODP * BETAO2 * BETAO2);
|
|---|
| 171 | TSI = 1.0 / (AODP - S4);
|
|---|
| 172 | ETA = AODP * EO * TSI;
|
|---|
| 173 | ETASQ = ETA * ETA;
|
|---|
| 174 | EETA = EO * ETA;
|
|---|
| 175 |
|
|---|
| 176 | PSISQ = fabs(1.0 - ETASQ);
|
|---|
| 177 |
|
|---|
| 178 | COEF = QOMS24 * pow(TSI, 4.0);
|
|---|
| 179 | COEF1 = COEF / pow(PSISQ, 3.5);
|
|---|
| 180 |
|
|---|
| 181 | C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
|
|---|
| 182 | EETA * (4.0 + ETASQ)) + .75 *
|
|---|
| 183 | CK2 * TSI /
|
|---|
| 184 | PSISQ * X3THM1 * (8.0 +
|
|---|
| 185 | 3.0 * ETASQ * (8.0 + ETASQ)));
|
|---|
| 186 |
|
|---|
| 187 |
|
|---|
| 188 | C1 = BSTAR * C2;
|
|---|
| 189 |
|
|---|
| 190 | SINIO = sin(XINCL);
|
|---|
| 191 |
|
|---|
| 192 | A3OVK2 = -XJ3 / CK2 * pow(AE, 3.0);
|
|---|
| 193 |
|
|---|
| 194 | C3 = COEF * TSI * A3OVK2 * XNODP * AE * SINIO / EO;
|
|---|
| 195 |
|
|---|
| 196 | X1MTH2 = 1.0 - THETA2;
|
|---|
| 197 | C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
|
|---|
| 198 | (ETA * (2.0 + .5 * ETASQ) +
|
|---|
| 199 | EO * (.5 + 2.0 * ETASQ) -
|
|---|
| 200 | 2.0 * CK2 * TSI / (AODP * PSISQ) *
|
|---|
| 201 | (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ * (1.5 - .5 * EETA)) +
|
|---|
| 202 | .75 * X1MTH2 * (2.0 * ETASQ - EETA * (1.0 + ETASQ)) *
|
|---|
| 203 | cos(2.0 * OMEGAO)));
|
|---|
| 204 |
|
|---|
| 205 | C5 = 2.0 * COEF1 * AODP * BETAO2 * (1.0 +
|
|---|
| 206 | 2.75 * (ETASQ + EETA) +
|
|---|
| 207 | EETA * ETASQ);
|
|---|
| 208 | THETA4 = THETA2 * THETA2;
|
|---|
| 209 | TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
|
|---|
| 210 | TEMP2 = TEMP1 * CK2 * PINVSQ;
|
|---|
| 211 | TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
|
|---|
| 212 |
|
|---|
| 213 | XMDOT = XNODP +
|
|---|
| 214 | .5 * TEMP1 * BETAO * X3THM1 +
|
|---|
| 215 | .0625 * TEMP2 * BETAO * (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
|
|---|
| 216 |
|
|---|
| 217 | X1M5TH = 1.0 - 5.0 * THETA2;
|
|---|
| 218 |
|
|---|
| 219 | OMGDOT = -.5 * TEMP1 * X1M5TH +
|
|---|
| 220 | .0625 * TEMP2 * (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
|
|---|
| 221 | TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
|
|---|
| 222 |
|
|---|
| 223 | XHDOT1 = -TEMP1 * COSIO;
|
|---|
| 224 |
|
|---|
| 225 | XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
|
|---|
| 226 | 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
|
|---|
| 227 |
|
|---|
| 228 | OMGCOF = BSTAR * C3 * cos(OMEGAO);
|
|---|
| 229 |
|
|---|
| 230 | XMCOF = -TOTHRD * COEF * BSTAR * AE / EETA;
|
|---|
| 231 | XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
|
|---|
| 232 | T2COF = 1.5 * C1;
|
|---|
| 233 | XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 *COSIO) / (1.0 + COSIO);
|
|---|
| 234 |
|
|---|
| 235 | AYCOF = .25 * A3OVK2 * SINIO;
|
|---|
| 236 | DELMO = pow(1.0 + ETA * cos(XMO), 3.0);
|
|---|
| 237 | SINMO = sin(XMO);
|
|---|
| 238 |
|
|---|
| 239 | X7THM1 = 7.0 * THETA2 - 1.0;
|
|---|
| 240 |
|
|---|
| 241 | /* IF(ISIMP .EQ. 1) GO TO 90 */
|
|---|
| 242 | if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) {
|
|---|
| 243 | C1SQ = C1 * C1;
|
|---|
| 244 | D2 = 4.0 * AODP * TSI * C1SQ;
|
|---|
| 245 | TEMP = D2 * TSI * C1 / 3.0;
|
|---|
| 246 | D3 = (17.0 * AODP + S4) * TEMP;
|
|---|
| 247 | D4 = .5 * TEMP * AODP * TSI * (221.0 * AODP + 31.0 * S4) * C1;
|
|---|
| 248 | T3COF = D2 + 2.0 * C1SQ;
|
|---|
| 249 | T4COF = .25 * (3.0 * D3 + C1 * (12.0 * D2 + 10.0 * C1SQ));
|
|---|
| 250 | T5COF = .2 * (3.0 * D4 +
|
|---|
| 251 | 12.0 * C1 * D3 +
|
|---|
| 252 | 6.0 * D2 * D2 +
|
|---|
| 253 | 15.0 * C1SQ * (2.0 * D2 + C1SQ));
|
|---|
| 254 | }
|
|---|
| 255 | }
|
|---|
| 256 |
|
|---|
| 257 | /*
|
|---|
| 258 | * UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG
|
|---|
| 259 | */
|
|---|
| 260 |
|
|---|
| 261 | XMDF = XMO + XMDOT * TSINCE;
|
|---|
| 262 | OMGADF = OMEGAO + OMGDOT * TSINCE;
|
|---|
| 263 | XNODDF = XNODEO + XNODOT * TSINCE;
|
|---|
| 264 | OMEGA = OMGADF;
|
|---|
| 265 | XMP = XMDF;
|
|---|
| 266 | TSQ = TSINCE * TSINCE;
|
|---|
| 267 | XNODE = XNODDF + XNODCF * TSQ;
|
|---|
| 268 | TEMPA = 1.0 - C1 * TSINCE;
|
|---|
| 269 | TEMPE = BSTAR * C4 * TSINCE;
|
|---|
| 270 | TEMPL = T2COF * TSQ;
|
|---|
| 271 | if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) {
|
|---|
| 272 | DELOMG = OMGCOF * TSINCE;
|
|---|
| 273 | DELM = XMCOF * (pow(1.0 + ETA * cos(XMDF), 3) - DELMO);
|
|---|
| 274 | TEMP = DELOMG + DELM;
|
|---|
| 275 | XMP = XMDF + TEMP;
|
|---|
| 276 | OMEGA = OMGADF - TEMP;
|
|---|
| 277 | TCUBE = TSQ * TSINCE;
|
|---|
| 278 | TFOUR = TSINCE * TCUBE;
|
|---|
| 279 | TEMPA = TEMPA - D2 * TSQ - D3 * TCUBE - D4 * TFOUR;
|
|---|
| 280 | TEMPE = TEMPE + BSTAR * C5 * (sin(XMP) - SINMO);
|
|---|
| 281 | TEMPL = TEMPL + T3COF * TCUBE + TFOUR * (T4COF + TSINCE * T5COF);
|
|---|
| 282 | }
|
|---|
| 283 |
|
|---|
| 284 | A = AODP * TEMPA * TEMPA;
|
|---|
| 285 | E = EO - TEMPE;
|
|---|
| 286 | XL = XMP + OMEGA + XNODE + XNODP * TEMPL;
|
|---|
| 287 | BETA = sqrt(1.0 - E * E);
|
|---|
| 288 | XN = XKE / pow(A, 1.5);
|
|---|
| 289 |
|
|---|
| 290 | /*
|
|---|
| 291 | * LONG PERIOD PERIODICS
|
|---|
| 292 | */
|
|---|
| 293 |
|
|---|
| 294 | AXN = E * cos(OMEGA);
|
|---|
| 295 | TEMP = 1.0 / (A * BETA * BETA);
|
|---|
| 296 | XLL = TEMP * XLCOF * AXN;
|
|---|
| 297 | AYNL = TEMP * AYCOF;
|
|---|
| 298 | XLT = XL + XLL;
|
|---|
| 299 | AYN = E * sin(OMEGA) + AYNL;
|
|---|
| 300 |
|
|---|
| 301 | /*
|
|---|
| 302 | * SOLVE KEPLERS EQUATION
|
|---|
| 303 | */
|
|---|
| 304 |
|
|---|
| 305 | CAPU = fmod(XLT - XNODE, TWOPI);
|
|---|
| 306 | TEMP2 = CAPU;
|
|---|
| 307 |
|
|---|
| 308 | for(i = 0; i < 10; i++) {
|
|---|
| 309 | SINEPW = sin(TEMP2);
|
|---|
| 310 | COSEPW = cos(TEMP2);
|
|---|
| 311 | TEMP3 = AXN * SINEPW;
|
|---|
| 312 | TEMP4 = AYN * COSEPW;
|
|---|
| 313 | TEMP5 = AXN * COSEPW;
|
|---|
| 314 | TEMP6 = AYN * SINEPW;
|
|---|
| 315 | EPW = (CAPU - TEMP4 + TEMP3 - TEMP2) / (1.0 - TEMP5 - TEMP6) + TEMP2;
|
|---|
| 316 |
|
|---|
| 317 | if(fabs(EPW - TEMP2) <= E6A)
|
|---|
| 318 | break;
|
|---|
| 319 |
|
|---|
| 320 | TEMP2 = EPW;
|
|---|
| 321 | }
|
|---|
| 322 |
|
|---|
| 323 | /*
|
|---|
| 324 | * SHORT PERIOD PRELIMINARY QUANTITIES
|
|---|
| 325 | */
|
|---|
| 326 |
|
|---|
| 327 | ECOSE = TEMP5 + TEMP6;
|
|---|
| 328 | ESINE = TEMP3 - TEMP4;
|
|---|
| 329 | ELSQ = AXN * AXN + AYN * AYN;
|
|---|
| 330 | TEMP = 1.0 - ELSQ;
|
|---|
| 331 | PL = A * TEMP;
|
|---|
| 332 | R = A * (1.0 - ECOSE);
|
|---|
| 333 |
|
|---|
| 334 | TEMP1 = 1.0 / R;
|
|---|
| 335 | RDOT = XKE * sqrt(A) * ESINE * TEMP1;
|
|---|
| 336 | RFDOT = XKE * sqrt(PL) * TEMP1;
|
|---|
| 337 | TEMP2 = A * TEMP1;
|
|---|
| 338 | BETAL = sqrt(TEMP);
|
|---|
| 339 | TEMP3 = 1.0 / (1.0 + BETAL);
|
|---|
| 340 |
|
|---|
| 341 | COSU = TEMP2 * (COSEPW - AXN + AYN * ESINE * TEMP3);
|
|---|
| 342 | SINU = TEMP2 * (SINEPW - AYN - AXN * ESINE * TEMP3);
|
|---|
| 343 |
|
|---|
| 344 | U = actan(SINU, COSU);
|
|---|
| 345 |
|
|---|
| 346 | SIN2U = 2.0 * SINU * COSU;
|
|---|
| 347 | COS2U = 2.0 * COSU * COSU - 1.0;
|
|---|
| 348 |
|
|---|
| 349 | TEMP = 1.0 / PL;
|
|---|
| 350 | TEMP1 = CK2 * TEMP;
|
|---|
| 351 | TEMP2 = TEMP1 * TEMP;
|
|---|
| 352 |
|
|---|
| 353 | /*
|
|---|
| 354 | * UPDATE FOR SHORT PERIODICS
|
|---|
| 355 | */
|
|---|
| 356 |
|
|---|
| 357 | RK = R * (1.0 - 1.5 * TEMP2 * BETAL * X3THM1) +
|
|---|
| 358 | .5 * TEMP1 * X1MTH2 * COS2U;
|
|---|
| 359 |
|
|---|
| 360 | UK = U - .25 * TEMP2 * X7THM1 * SIN2U;
|
|---|
| 361 |
|
|---|
| 362 | XNODEK = XNODE + 1.5 * TEMP2 * COSIO * SIN2U;
|
|---|
| 363 | XINCK = XINCL + 1.5 * TEMP2 * COSIO * SINIO * COS2U;
|
|---|
| 364 | RDOTK = RDOT - XN * TEMP1 * X1MTH2 * SIN2U;
|
|---|
| 365 | RFDOTK = RFDOT + XN * TEMP1 * (X1MTH2 * COS2U + 1.5 * X3THM1);
|
|---|
| 366 |
|
|---|
| 367 | /*
|
|---|
| 368 | * ORIENTATION VECTORS
|
|---|
| 369 | */
|
|---|
| 370 |
|
|---|
| 371 | SINUK = sin(UK);
|
|---|
| 372 | COSUK = cos(UK);
|
|---|
| 373 | SINIK = sin(XINCK);
|
|---|
| 374 | COSIK = cos(XINCK);
|
|---|
| 375 | SINNOK = sin(XNODEK);
|
|---|
| 376 | COSNOK = cos(XNODEK);
|
|---|
| 377 |
|
|---|
| 378 | XMX = -SINNOK * COSIK;
|
|---|
| 379 | XMY = COSNOK * COSIK;
|
|---|
| 380 | UX = XMX * SINUK + COSNOK * COSUK;
|
|---|
| 381 | UY = XMY * SINUK + SINNOK * COSUK;
|
|---|
| 382 | UZ = SINIK * SINUK;
|
|---|
| 383 | VX = XMX * COSUK - COSNOK * SINUK;
|
|---|
| 384 | VY = XMY * COSUK - SINNOK * SINUK;
|
|---|
| 385 | VZ = SINIK * COSUK;
|
|---|
| 386 |
|
|---|
| 387 | /*
|
|---|
| 388 | * POSITION AND VELOCITY
|
|---|
| 389 | */
|
|---|
| 390 |
|
|---|
| 391 | pos->x = RK * UX;
|
|---|
| 392 | pos->y = RK * UY;
|
|---|
| 393 | pos->z = RK * UZ;
|
|---|
| 394 |
|
|---|
| 395 | dpos->x = RDOTK * UX + RFDOTK * VX;
|
|---|
| 396 | dpos->y = RDOTK * UY + RFDOTK * VY;
|
|---|
| 397 | dpos->z = RDOTK * UZ + RFDOTK * VZ;
|
|---|
| 398 | }
|
|---|
| 399 |
|
|---|
| 400 | /* For RCS Only -- Do Not Edit */
|
|---|
| 401 | static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: sgp4.c,v $ $Date: 2011-09-21 16:17:51 $ $Revision: 1.9 $ $Name: not supported by cvs2svn $"};
|
|---|
![(please configure the [header_logo] section in trac.ini)](/PI/chrome/site/your_project_logo.png){kind=logo}
