| 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(sat, pos, dpos, TSINCE)
|
|---|
| 82 | SatData *sat;
|
|---|
| 83 | Vec3 *pos;
|
|---|
| 84 | Vec3 *dpos;
|
|---|
| 85 | double TSINCE;
|
|---|
| 86 | {
|
|---|
| 87 | int i;
|
|---|
| 88 |
|
|---|
| 89 | double A1, A3OVK2, AO, BETAO, BETAO2, C1SQ, C2, C3, COEF, COEF1,
|
|---|
| 90 | DEL1, DELO, EETA, EOSQ, ETASQ, PERIGE, PINVSQ, PSISQ, QOMS24,
|
|---|
| 91 | S4, TEMP, TEMP1, TEMP2, TEMP3, THETA2, THETA4, TSI, X1M5TH,
|
|---|
| 92 | XHDOT1;
|
|---|
| 93 |
|
|---|
| 94 | double A, AXN, AYN, AYNL, BETA, BETAL, CAPU, COS2U, COSEPW, COSIK,
|
|---|
| 95 | COSNOK, COSU, COSUK, DELM, DELOMG, E, ECOSE, ELSQ, EPW, ESINE,
|
|---|
| 96 | OMEGA, OMGADF, PL, R, RDOT, RDOTK, RFDOT, RFDOTK, RK, SIN2U,
|
|---|
| 97 | SINEPW, SINIK, SINNOK, SINU, SINUK, TCUBE, TEMP4, TEMP5, TEMP6,
|
|---|
| 98 | TEMPA, TEMPE, TEMPL, TFOUR, TSQ, U, UK, UX, UY, UZ, VX, VY, VZ,
|
|---|
| 99 | XINCK, XL, XLL, XLT, XMDF, XMP, XMX, XMY, XN, XNODDF, XNODE,
|
|---|
| 100 | XNODEK;
|
|---|
| 101 |
|
|---|
| 102 | #if 0
|
|---|
| 103 | A1 = A3OVK2 = AO = BETAO = BETAO2 = C1SQ = C2 = C3 = COEF = COEF1 =
|
|---|
| 104 | DEL1 = DELO = EETA = EOSQ = ETASQ = PERIGE = PINVSQ = PSISQ = QOMS24 =
|
|---|
| 105 | S4 = TEMP = TEMP1 = TEMP2 = TEMP3 = THETA2 = THETA4 = TSI = X1M5TH =
|
|---|
| 106 | XHDOT1 = signaling_nan();
|
|---|
| 107 |
|
|---|
| 108 | A = AXN = AYN = AYNL = BETA = BETAL = CAPU = COS2U = COSEPW = COSIK =
|
|---|
| 109 | COSNOK = COSU = COSUK = DELM = DELOMG = E = ECOSE = ELSQ = EPW =
|
|---|
| 110 | ESINE = OMEGA = OMGADF = PL = R = RDOT = RDOTK = RFDOT = RFDOTK =
|
|---|
| 111 | RK = SIN2U = SINEPW = SINIK = SINNOK = SINU = SINUK = TCUBE = TEMP4 =
|
|---|
| 112 | TEMP5 = TEMP6 = TEMPA = TEMPE = TEMPL = TFOUR = TSQ = U = UK = UX =
|
|---|
| 113 | UY = UZ = VX = VY = VZ = XINCK = XL = XLL = XLT = XMDF = XMP = XMX =
|
|---|
| 114 | XMY = XN = XNODDF = XNODE = XNODEK = signaling_nan();
|
|---|
| 115 | #endif
|
|---|
| 116 |
|
|---|
| 117 | if(!sat->prop.sgp4) {
|
|---|
| 118 | sat->prop.sgp4 = (struct sgp4_data *) malloc(sizeof(struct sgp4_data));
|
|---|
| 119 |
|
|---|
| 120 | /*
|
|---|
| 121 | * RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP)
|
|---|
| 122 | * FROM INPUT ELEMENTS
|
|---|
| 123 | */
|
|---|
| 124 |
|
|---|
| 125 | A1 = pow((XKE/XNO), TOTHRD);
|
|---|
| 126 | COSIO = cos(XINCL);
|
|---|
| 127 | THETA2 = COSIO * COSIO;
|
|---|
| 128 | X3THM1 = 3.0 * THETA2 - 1.0;
|
|---|
| 129 | EOSQ = EO * EO;
|
|---|
| 130 | BETAO2 = 1.0 - EOSQ;
|
|---|
| 131 | BETAO = sqrt(BETAO2);
|
|---|
| 132 | DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
|
|---|
| 133 | AO = A1 * (1.0 - DEL1 * (.5 * TOTHRD +
|
|---|
| 134 | DEL1 * (1.0 + 134.0 /81.0 * DEL1)));
|
|---|
| 135 | DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
|
|---|
| 136 | XNODP = XNO / (1.0 + DELO);
|
|---|
| 137 | AODP=AO / (1.0 - DELO);
|
|---|
| 138 |
|
|---|
| 139 | /*
|
|---|
| 140 | * INITIALIZATION
|
|---|
| 141 | *
|
|---|
| 142 | * FOR PERIGEE LESS THAN 220 KILOMETERS, THE ISIMP FLAG IS SET AND
|
|---|
| 143 | * THE EQUATIONS ARE TRUNCATED TO LINEAR VARIATION IN SQRT A AND
|
|---|
| 144 | * QUADRATIC VARIATION IN MEAN ANOMALY. ALSO, THE C3 TERM, THE
|
|---|
| 145 | * DELTA OMEGA TERM, AND THE DELTA M TERM ARE DROPPED.
|
|---|
| 146 | */
|
|---|
| 147 |
|
|---|
| 148 | sat->prop.sgp4->sgp4_flags = 0;
|
|---|
| 149 |
|
|---|
| 150 | /* IF((AODP*(1.-EO)/AE) .LT. (220./XKMPER+AE)) ISIMP=1 */
|
|---|
| 151 |
|
|---|
| 152 | if((AODP * (1.0 - EO) / AE) < (220.0 / XKMPER + AE))
|
|---|
| 153 | sat->prop.sgp4->sgp4_flags |= SGP4_SIMPLE;
|
|---|
| 154 |
|
|---|
| 155 | /*
|
|---|
| 156 | * FOR PERIGEE BELOW 156 KM, THE VALUES OF
|
|---|
| 157 | * S AND QOMS2T ARE ALTERED
|
|---|
| 158 | */
|
|---|
| 159 |
|
|---|
| 160 | S4 = S;
|
|---|
| 161 | QOMS24 = QOMS2T;
|
|---|
| 162 | PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;
|
|---|
| 163 |
|
|---|
| 164 | if(PERIGE < 156.0) {
|
|---|
| 165 | S4 = PERIGE - 78.0;
|
|---|
| 166 |
|
|---|
| 167 | if(PERIGE <= 98.0)
|
|---|
| 168 | S4 = 20.0;
|
|---|
| 169 |
|
|---|
| 170 | QOMS24 = pow(((120.0 - S4) * AE / XKMPER), 4.0);
|
|---|
| 171 | S4 = S4 / XKMPER + AE;
|
|---|
| 172 | }
|
|---|
| 173 |
|
|---|
| 174 | PINVSQ=1.0 / (AODP * AODP * BETAO2 * BETAO2);
|
|---|
| 175 | TSI = 1.0 / (AODP - S4);
|
|---|
| 176 | ETA = AODP * EO * TSI;
|
|---|
| 177 | ETASQ = ETA * ETA;
|
|---|
| 178 | EETA = EO * ETA;
|
|---|
| 179 |
|
|---|
| 180 | PSISQ = fabs(1.0 - ETASQ);
|
|---|
| 181 |
|
|---|
| 182 | COEF = QOMS24 * pow(TSI, 4.0);
|
|---|
| 183 | COEF1 = COEF / pow(PSISQ, 3.5);
|
|---|
| 184 |
|
|---|
| 185 | C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
|
|---|
| 186 | EETA * (4.0 + ETASQ)) + .75 *
|
|---|
| 187 | CK2 * TSI /
|
|---|
| 188 | PSISQ * X3THM1 * (8.0 +
|
|---|
| 189 | 3.0 * ETASQ * (8.0 + ETASQ)));
|
|---|
| 190 |
|
|---|
| 191 |
|
|---|
| 192 | C1 = BSTAR * C2;
|
|---|
| 193 |
|
|---|
| 194 | SINIO = sin(XINCL);
|
|---|
| 195 |
|
|---|
| 196 | A3OVK2 = -XJ3 / CK2 * pow(AE, 3.0);
|
|---|
| 197 |
|
|---|
| 198 | C3 = COEF * TSI * A3OVK2 * XNODP * AE * SINIO / EO;
|
|---|
| 199 |
|
|---|
| 200 | X1MTH2 = 1.0 - THETA2;
|
|---|
| 201 | C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
|
|---|
| 202 | (ETA * (2.0 + .5 * ETASQ) +
|
|---|
| 203 | EO * (.5 + 2.0 * ETASQ) -
|
|---|
| 204 | 2.0 * CK2 * TSI / (AODP * PSISQ) *
|
|---|
| 205 | (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ * (1.5 - .5 * EETA)) +
|
|---|
| 206 | .75 * X1MTH2 * (2.0 * ETASQ - EETA * (1.0 + ETASQ)) *
|
|---|
| 207 | cos(2.0 * OMEGAO)));
|
|---|
| 208 |
|
|---|
| 209 | C5 = 2.0 * COEF1 * AODP * BETAO2 * (1.0 +
|
|---|
| 210 | 2.75 * (ETASQ + EETA) +
|
|---|
| 211 | EETA * ETASQ);
|
|---|
| 212 | THETA4 = THETA2 * THETA2;
|
|---|
| 213 | TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
|
|---|
| 214 | TEMP2 = TEMP1 * CK2 * PINVSQ;
|
|---|
| 215 | TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
|
|---|
| 216 |
|
|---|
| 217 | XMDOT = XNODP +
|
|---|
| 218 | .5 * TEMP1 * BETAO * X3THM1 +
|
|---|
| 219 | .0625 * TEMP2 * BETAO * (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
|
|---|
| 220 |
|
|---|
| 221 | X1M5TH = 1.0 - 5.0 * THETA2;
|
|---|
| 222 |
|
|---|
| 223 | OMGDOT = -.5 * TEMP1 * X1M5TH +
|
|---|
| 224 | .0625 * TEMP2 * (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
|
|---|
| 225 | TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
|
|---|
| 226 |
|
|---|
| 227 | XHDOT1 = -TEMP1 * COSIO;
|
|---|
| 228 |
|
|---|
| 229 | XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
|
|---|
| 230 | 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
|
|---|
| 231 |
|
|---|
| 232 | OMGCOF = BSTAR * C3 * cos(OMEGAO);
|
|---|
| 233 |
|
|---|
| 234 | XMCOF = -TOTHRD * COEF * BSTAR * AE / EETA;
|
|---|
| 235 | XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
|
|---|
| 236 | T2COF = 1.5 * C1;
|
|---|
| 237 | XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 *COSIO) / (1.0 + COSIO);
|
|---|
| 238 |
|
|---|
| 239 | AYCOF = .25 * A3OVK2 * SINIO;
|
|---|
| 240 | DELMO = pow(1.0 + ETA * cos(XMO), 3.0);
|
|---|
| 241 | SINMO = sin(XMO);
|
|---|
| 242 |
|
|---|
| 243 | X7THM1 = 7.0 * THETA2 - 1.0;
|
|---|
| 244 |
|
|---|
| 245 | /* IF(ISIMP .EQ. 1) GO TO 90 */
|
|---|
| 246 | if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) {
|
|---|
| 247 | C1SQ = C1 * C1;
|
|---|
| 248 | D2 = 4.0 * AODP * TSI * C1SQ;
|
|---|
| 249 | TEMP = D2 * TSI * C1 / 3.0;
|
|---|
| 250 | D3 = (17.0 * AODP + S4) * TEMP;
|
|---|
| 251 | D4 = .5 * TEMP * AODP * TSI * (221.0 * AODP + 31.0 * S4) * C1;
|
|---|
| 252 | T3COF = D2 + 2.0 * C1SQ;
|
|---|
| 253 | T4COF = .25 * (3.0 * D3 + C1 * (12.0 * D2 + 10.0 * C1SQ));
|
|---|
| 254 | T5COF = .2 * (3.0 * D4 +
|
|---|
| 255 | 12.0 * C1 * D3 +
|
|---|
| 256 | 6.0 * D2 * D2 +
|
|---|
| 257 | 15.0 * C1SQ * (2.0 * D2 + C1SQ));
|
|---|
| 258 | }
|
|---|
| 259 | }
|
|---|
| 260 |
|
|---|
| 261 | /*
|
|---|
| 262 | * UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG
|
|---|
| 263 | */
|
|---|
| 264 |
|
|---|
| 265 | XMDF = XMO + XMDOT * TSINCE;
|
|---|
| 266 | OMGADF = OMEGAO + OMGDOT * TSINCE;
|
|---|
| 267 | XNODDF = XNODEO + XNODOT * TSINCE;
|
|---|
| 268 | OMEGA = OMGADF;
|
|---|
| 269 | XMP = XMDF;
|
|---|
| 270 | TSQ = TSINCE * TSINCE;
|
|---|
| 271 | XNODE = XNODDF + XNODCF * TSQ;
|
|---|
| 272 | TEMPA = 1.0 - C1 * TSINCE;
|
|---|
| 273 | TEMPE = BSTAR * C4 * TSINCE;
|
|---|
| 274 | TEMPL = T2COF * TSQ;
|
|---|
| 275 | if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) {
|
|---|
| 276 | DELOMG = OMGCOF * TSINCE;
|
|---|
| 277 | DELM = XMCOF * (pow(1.0 + ETA * cos(XMDF), 3) - DELMO);
|
|---|
| 278 | TEMP = DELOMG + DELM;
|
|---|
| 279 | XMP = XMDF + TEMP;
|
|---|
| 280 | OMEGA = OMGADF - TEMP;
|
|---|
| 281 | TCUBE = TSQ * TSINCE;
|
|---|
| 282 | TFOUR = TSINCE * TCUBE;
|
|---|
| 283 | TEMPA = TEMPA - D2 * TSQ - D3 * TCUBE - D4 * TFOUR;
|
|---|
| 284 | TEMPE = TEMPE + BSTAR * C5 * (sin(XMP) - SINMO);
|
|---|
| 285 | TEMPL = TEMPL + T3COF * TCUBE + TFOUR * (T4COF + TSINCE * T5COF);
|
|---|
| 286 | }
|
|---|
| 287 |
|
|---|
| 288 | A = AODP * TEMPA * TEMPA;
|
|---|
| 289 | E = EO - TEMPE;
|
|---|
| 290 | XL = XMP + OMEGA + XNODE + XNODP * TEMPL;
|
|---|
| 291 | BETA = sqrt(1.0 - E * E);
|
|---|
| 292 | XN = XKE / pow(A, 1.5);
|
|---|
| 293 |
|
|---|
| 294 | /*
|
|---|
| 295 | * LONG PERIOD PERIODICS
|
|---|
| 296 | */
|
|---|
| 297 |
|
|---|
| 298 | AXN = E * cos(OMEGA);
|
|---|
| 299 | TEMP = 1.0 / (A * BETA * BETA);
|
|---|
| 300 | XLL = TEMP * XLCOF * AXN;
|
|---|
| 301 | AYNL = TEMP * AYCOF;
|
|---|
| 302 | XLT = XL + XLL;
|
|---|
| 303 | AYN = E * sin(OMEGA) + AYNL;
|
|---|
| 304 |
|
|---|
| 305 | /*
|
|---|
| 306 | * SOLVE KEPLERS EQUATION
|
|---|
| 307 | */
|
|---|
| 308 |
|
|---|
| 309 | CAPU = fmod(XLT - XNODE, TWOPI);
|
|---|
| 310 | TEMP2 = CAPU;
|
|---|
| 311 |
|
|---|
| 312 | for(i = 0; i < 10; i++) {
|
|---|
| 313 | SINEPW = sin(TEMP2);
|
|---|
| 314 | COSEPW = cos(TEMP2);
|
|---|
| 315 | TEMP3 = AXN * SINEPW;
|
|---|
| 316 | TEMP4 = AYN * COSEPW;
|
|---|
| 317 | TEMP5 = AXN * COSEPW;
|
|---|
| 318 | TEMP6 = AYN * SINEPW;
|
|---|
| 319 | EPW = (CAPU - TEMP4 + TEMP3 - TEMP2) / (1.0 - TEMP5 - TEMP6) + TEMP2;
|
|---|
| 320 |
|
|---|
| 321 | if(fabs(EPW - TEMP2) <= E6A)
|
|---|
| 322 | break;
|
|---|
| 323 |
|
|---|
| 324 | TEMP2 = EPW;
|
|---|
| 325 | }
|
|---|
| 326 |
|
|---|
| 327 | /*
|
|---|
| 328 | * SHORT PERIOD PRELIMINARY QUANTITIES
|
|---|
| 329 | */
|
|---|
| 330 |
|
|---|
| 331 | ECOSE = TEMP5 + TEMP6;
|
|---|
| 332 | ESINE = TEMP3 - TEMP4;
|
|---|
| 333 | ELSQ = AXN * AXN + AYN * AYN;
|
|---|
| 334 | TEMP = 1.0 - ELSQ;
|
|---|
| 335 | PL = A * TEMP;
|
|---|
| 336 | R = A * (1.0 - ECOSE);
|
|---|
| 337 |
|
|---|
| 338 | TEMP1 = 1.0 / R;
|
|---|
| 339 | RDOT = XKE * sqrt(A) * ESINE * TEMP1;
|
|---|
| 340 | RFDOT = XKE * sqrt(PL) * TEMP1;
|
|---|
| 341 | TEMP2 = A * TEMP1;
|
|---|
| 342 | BETAL = sqrt(TEMP);
|
|---|
| 343 | TEMP3 = 1.0 / (1.0 + BETAL);
|
|---|
| 344 |
|
|---|
| 345 | COSU = TEMP2 * (COSEPW - AXN + AYN * ESINE * TEMP3);
|
|---|
| 346 | SINU = TEMP2 * (SINEPW - AYN - AXN * ESINE * TEMP3);
|
|---|
| 347 |
|
|---|
| 348 | U = actan(SINU, COSU);
|
|---|
| 349 |
|
|---|
| 350 | SIN2U = 2.0 * SINU * COSU;
|
|---|
| 351 | COS2U = 2.0 * COSU * COSU - 1.0;
|
|---|
| 352 |
|
|---|
| 353 | TEMP = 1.0 / PL;
|
|---|
| 354 | TEMP1 = CK2 * TEMP;
|
|---|
| 355 | TEMP2 = TEMP1 * TEMP;
|
|---|
| 356 |
|
|---|
| 357 | /*
|
|---|
| 358 | * UPDATE FOR SHORT PERIODICS
|
|---|
| 359 | */
|
|---|
| 360 |
|
|---|
| 361 | RK = R * (1.0 - 1.5 * TEMP2 * BETAL * X3THM1) +
|
|---|
| 362 | .5 * TEMP1 * X1MTH2 * COS2U;
|
|---|
| 363 |
|
|---|
| 364 | UK = U - .25 * TEMP2 * X7THM1 * SIN2U;
|
|---|
| 365 |
|
|---|
| 366 | XNODEK = XNODE + 1.5 * TEMP2 * COSIO * SIN2U;
|
|---|
| 367 | XINCK = XINCL + 1.5 * TEMP2 * COSIO * SINIO * COS2U;
|
|---|
| 368 | RDOTK = RDOT - XN * TEMP1 * X1MTH2 * SIN2U;
|
|---|
| 369 | RFDOTK = RFDOT + XN * TEMP1 * (X1MTH2 * COS2U + 1.5 * X3THM1);
|
|---|
| 370 |
|
|---|
| 371 | /*
|
|---|
| 372 | * ORIENTATION VECTORS
|
|---|
| 373 | */
|
|---|
| 374 |
|
|---|
| 375 | SINUK = sin(UK);
|
|---|
| 376 | COSUK = cos(UK);
|
|---|
| 377 | SINIK = sin(XINCK);
|
|---|
| 378 | COSIK = cos(XINCK);
|
|---|
| 379 | SINNOK = sin(XNODEK);
|
|---|
| 380 | COSNOK = cos(XNODEK);
|
|---|
| 381 |
|
|---|
| 382 | XMX = -SINNOK * COSIK;
|
|---|
| 383 | XMY = COSNOK * COSIK;
|
|---|
| 384 | UX = XMX * SINUK + COSNOK * COSUK;
|
|---|
| 385 | UY = XMY * SINUK + SINNOK * COSUK;
|
|---|
| 386 | UZ = SINIK * SINUK;
|
|---|
| 387 | VX = XMX * COSUK - COSNOK * SINUK;
|
|---|
| 388 | VY = XMY * COSUK - SINNOK * SINUK;
|
|---|
| 389 | VZ = SINIK * COSUK;
|
|---|
| 390 |
|
|---|
| 391 | /*
|
|---|
| 392 | * POSITION AND VELOCITY
|
|---|
| 393 | */
|
|---|
| 394 |
|
|---|
| 395 | pos->x = RK * UX;
|
|---|
| 396 | pos->y = RK * UY;
|
|---|
| 397 | pos->z = RK * UZ;
|
|---|
| 398 |
|
|---|
| 399 | dpos->x = RDOTK * UX + RFDOTK * VX;
|
|---|
| 400 | dpos->y = RDOTK * UY + RFDOTK * VY;
|
|---|
| 401 | dpos->z = RDOTK * UZ + RFDOTK * VZ;
|
|---|
| 402 | }
|
|---|
| 403 |
|
|---|
| 404 | /* For RCS Only -- Do Not Edit */
|
|---|
| 405 | static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: sgp4.c,v $ $Date: 2001-04-10 14:40:47 $ $Revision: 1.1.1.1 $ $Name: not supported by cvs2svn $"};
|
|---|
![(please configure the [header_logo] section in trac.ini)](/PI/chrome/site/your_project_logo.png){kind=logo}
