| 1 | #include <stdlib.h>
|
|---|
| 2 | #include <math.h>
|
|---|
| 3 | #undef SING
|
|---|
| 4 |
|
|---|
| 5 | #include "sattypes.h"
|
|---|
| 6 | #include "vector.h"
|
|---|
| 7 | #include "satspec.h"
|
|---|
| 8 |
|
|---|
| 9 | /* SDP4 3 NOV 80 */
|
|---|
| 10 | /* SUBROUTINE SDP4(IFLAG,TSINCE)
|
|---|
| 11 | COMMON/E1/XMO,XNODEO,OMEGAO,EO,XINCL,XNO,XNDT2O,
|
|---|
| 12 | 1 XNDD6O,BSTAR,X,Y,Z,XDOT,YDOT,ZDOT,EPOCH,DS50
|
|---|
| 13 | COMMON/C1/CK2,CK4,E6A,QOMS2T,S,TOTHRD,
|
|---|
| 14 | 1 XJ3,XKE,XKMPER,XMNPDA,AE
|
|---|
| 15 | DOUBLE PRECISION EPOCH, DS50
|
|---|
| 16 | */
|
|---|
| 17 |
|
|---|
| 18 | #define XMO (sat->elem->se_XMO)
|
|---|
| 19 | #define XNODEO (sat->elem->se_XNODEO)
|
|---|
| 20 | #define OMEGAO (sat->elem->se_OMEGAO)
|
|---|
| 21 | #define EO (sat->elem->se_EO)
|
|---|
| 22 | #define XINCL (sat->elem->se_XINCL)
|
|---|
| 23 | #define XNO (sat->elem->se_XNO)
|
|---|
| 24 | #define XNDT20 (sat->elem->se_XNDT20)
|
|---|
| 25 | #define XNDD60 (sat->elem->se_XNDD60)
|
|---|
| 26 | #define BSTAR (sat->elem->se_BSTAR)
|
|---|
| 27 | #define EPOCH (sat->elem->se_EPOCH)
|
|---|
| 28 |
|
|---|
| 29 | #define CK2 (5.413080e-04)
|
|---|
| 30 | #define CK4 (6.209887e-07)
|
|---|
| 31 | #define QOMS2T (1.880279e-09)
|
|---|
| 32 | #define S (1.012229e+00)
|
|---|
| 33 |
|
|---|
| 34 | #define AE (1.0)
|
|---|
| 35 | #define DE2RA (.174532925E-1)
|
|---|
| 36 | #define E6A (1.E-6)
|
|---|
| 37 | #define PI (3.14159265)
|
|---|
| 38 | #define PIO2 (1.57079633)
|
|---|
| 39 | #define QO (120.0)
|
|---|
| 40 | #define SO (78.0)
|
|---|
| 41 | #define TOTHRD (.66666667)
|
|---|
| 42 | #define TWOPI (6.2831853)
|
|---|
| 43 | #define X3PIO2 (4.71238898)
|
|---|
| 44 | #define XJ2 (1.082616E-3)
|
|---|
| 45 | #define XJ3 (-.253881E-5)
|
|---|
| 46 | #define XJ4 (-1.65597E-6)
|
|---|
| 47 | #define XKE (.743669161E-1)
|
|---|
| 48 | #define XKMPER (6378.135)
|
|---|
| 49 | #define XMNPDA (1440.)
|
|---|
| 50 |
|
|---|
| 51 | /* int IFLAG; */
|
|---|
| 52 |
|
|---|
| 53 | #define X (pos->sl_X)
|
|---|
| 54 | #define XDOT (pos->sl_XDOT)
|
|---|
| 55 | #define Y (pos->sl_Y)
|
|---|
| 56 | #define YDOT (pos->sl_YDOT)
|
|---|
| 57 | #define Z (pos->sl_Z)
|
|---|
| 58 | #define ZDOT (pos->sl_ZDOT)
|
|---|
| 59 |
|
|---|
| 60 | /* sat->prop.sdp4-> */
|
|---|
| 61 | #define AODP (sat->prop.sdp4->sdp4_AODP)
|
|---|
| 62 | #define AYCOF (sat->prop.sdp4->sdp4_AYCOF)
|
|---|
| 63 | #define BETAO (sat->prop.sdp4->sdp4_BETAO)
|
|---|
| 64 | #define BETAO2 (sat->prop.sdp4->sdp4_BETAO2)
|
|---|
| 65 | #define C1 (sat->prop.sdp4->sdp4_C1)
|
|---|
| 66 | #define C4 (sat->prop.sdp4->sdp4_C4)
|
|---|
| 67 | #define COSG (sat->prop.sdp4->sdp4_COSG)
|
|---|
| 68 | #define COSIO (sat->prop.sdp4->sdp4_COSIO)
|
|---|
| 69 | #define EOSQ (sat->prop.sdp4->sdp4_EOSQ)
|
|---|
| 70 | #define OMGDOT (sat->prop.sdp4->sdp4_OMGDOT)
|
|---|
| 71 | #define SING (sat->prop.sdp4->sdp4_SING)
|
|---|
| 72 | #define SINIO (sat->prop.sdp4->sdp4_SINIO)
|
|---|
| 73 | #define T2COF (sat->prop.sdp4->sdp4_T2COF)
|
|---|
| 74 | #define THETA2 (sat->prop.sdp4->sdp4_THETA2)
|
|---|
| 75 | #define X1MTH2 (sat->prop.sdp4->sdp4_X1MTH2)
|
|---|
| 76 | #define X3THM1 (sat->prop.sdp4->sdp4_X3THM1)
|
|---|
| 77 | #define X7THM1 (sat->prop.sdp4->sdp4_X7THM1)
|
|---|
| 78 | #define XLCOF (sat->prop.sdp4->sdp4_XLCOF)
|
|---|
| 79 | #define XMDOT (sat->prop.sdp4->sdp4_XMDOT)
|
|---|
| 80 | #define XNODCF (sat->prop.sdp4->sdp4_XNODCF)
|
|---|
| 81 | #define XNODOT (sat->prop.sdp4->sdp4_XNODOT)
|
|---|
| 82 | #define XNODP (sat->prop.sdp4->sdp4_XNODP)
|
|---|
| 83 |
|
|---|
| 84 | #define XMDF_seco (sat->prop.sdp4->sdp4_XMDF_seco)
|
|---|
| 85 | #define OMGADF_seco (sat->prop.sdp4->sdp4_OMGADF_seco)
|
|---|
| 86 | #define XNODE_seco (sat->prop.sdp4->sdp4_XNODE_seco)
|
|---|
| 87 | #define EM_seco (sat->prop.sdp4->sdp4_EM_seco)
|
|---|
| 88 | #define XINC_seco (sat->prop.sdp4->sdp4_XINC_seco)
|
|---|
| 89 | #define XN_seco (sat->prop.sdp4->sdp4_XN_seco)
|
|---|
| 90 |
|
|---|
| 91 | #define E_pero (sat->prop.sdp4->sdp4_E_pero)
|
|---|
| 92 | #define XINC_pero (sat->prop.sdp4->sdp4_XINC_pero)
|
|---|
| 93 | #define OMGADF_pero (sat->prop.sdp4->sdp4_OMGADF_pero)
|
|---|
| 94 | #define XNODE_pero (sat->prop.sdp4->sdp4_XNODE_pero)
|
|---|
| 95 | #define XMAM_pero (sat->prop.sdp4->sdp4_XMAM_pero)
|
|---|
| 96 |
|
|---|
| 97 | void
|
|---|
| 98 | sdp4 (SatData *sat, Vec3 *pos, Vec3 *dpos, double TSINCE)
|
|---|
| 99 | {
|
|---|
| 100 | int i;
|
|---|
| 101 |
|
|---|
| 102 | /* private temporary variables used only in init section */
|
|---|
| 103 | double A1,A3OVK2,AO,C2,COEF,COEF1,DEL1,DELO,EETA,ETA,
|
|---|
| 104 | ETASQ,PERIGE,PINVSQ,PSISQ,QOMS24,S4,THETA4,TSI,X1M5TH,XHDOT1;
|
|---|
| 105 |
|
|---|
| 106 | /* private temporary variables */
|
|---|
| 107 | double A,AXN,AYN,AYNL,BETA,BETAL,CAPU,COS2U,COSEPW=0,
|
|---|
| 108 | COSIK,COSNOK,COSU,COSUK,E,ECOSE,ELSQ,EM=0,EPW,ESINE,OMGADF,PL,
|
|---|
| 109 | R,RDOT,RDOTK,RFDOT,RFDOTK,RK,SIN2U,SINEPW=0,SINIK,SINNOK,
|
|---|
| 110 | SINU,SINUK,TEMP,TEMP1,TEMP2,TEMP3=0,TEMP4=0,TEMP5=0,TEMP6=0,TEMPA,
|
|---|
| 111 | TEMPE,TEMPL,TSQ,U,UK,UX,UY,UZ,VX,VY,VZ,XINC=0,XINCK,XL,XLL,XLT,
|
|---|
| 112 | XMAM,XMDF,XMX,XMY,XN,XNODDF,XNODE,XNODEK;
|
|---|
| 113 |
|
|---|
| 114 | #if 0
|
|---|
| 115 | A1=A3OVK2=AO=C2=COEF=COEF1=DEL1=DELO=EETA=ETA = signaling_nan();
|
|---|
| 116 | ETASQ=PERIGE=PINVSQ=PSISQ=QOMS24=S4=THETA4=TSI=X1M5TH=XHDOT1 = signaling_nan();
|
|---|
| 117 |
|
|---|
| 118 | A=AXN=AYN=AYNL=BETA=BETAL=CAPU=COS2U=COSEPW = signaling_nan();
|
|---|
| 119 | COSIK=COSNOK=COSU=COSUK=E=ECOSE=ELSQ=EM=EPW=ESINE=OMGADF=PL = signaling_nan();
|
|---|
| 120 | R=RDOT=RDOTK=RFDOT=RFDOTK=RK=SIN2U=SINEPW=SINIK=SINNOK = signaling_nan();
|
|---|
| 121 | SINU=SINUK=TEMP=TEMP1=TEMP2=TEMP3=TEMP4=TEMP5=TEMP6=TEMPA = signaling_nan();
|
|---|
| 122 | TEMPE=TEMPL=TSQ=U=UK=UX=UY=UZ=VX=VY=VZ=XINC=XINCK=XL=XLL=XLT = signaling_nan();
|
|---|
| 123 | XMAM=XMDF=XMX=XMY=XN=XNODDF=XNODE=XNODEK = signaling_nan();
|
|---|
| 124 | #endif
|
|---|
| 125 |
|
|---|
| 126 | if(TSINCE != 0.0 && !sat->prop.sdp4) {
|
|---|
| 127 | /*
|
|---|
| 128 | * Yes, this is a recursive call.
|
|---|
| 129 | */
|
|---|
| 130 | sdp4(sat, pos, dpos, 0.0);
|
|---|
| 131 | }
|
|---|
| 132 |
|
|---|
| 133 | /* IF (IFLAG .EQ. 0) GO TO 100 */
|
|---|
| 134 | /* if(!IFLAG) */
|
|---|
| 135 | if(!sat->prop.sdp4) {
|
|---|
| 136 | sat->prop.sdp4 = (struct sdp4_data *) malloc(sizeof(struct sdp4_data));
|
|---|
| 137 |
|
|---|
| 138 | /* init_sdp4(sat->prop.sdp4); */
|
|---|
| 139 |
|
|---|
| 140 | /* RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP) */
|
|---|
| 141 | /* FROM INPUT ELEMENTS */
|
|---|
| 142 |
|
|---|
| 143 | A1=pow((XKE/XNO), TOTHRD);
|
|---|
| 144 | COSIO=cos(XINCL);
|
|---|
| 145 | THETA2=COSIO*COSIO;
|
|---|
| 146 | X3THM1=3.0 * THETA2 - 1.0;
|
|---|
| 147 | EOSQ = EO * EO;
|
|---|
| 148 | BETAO2 = 1.0 - EOSQ;
|
|---|
| 149 | BETAO = sqrt(BETAO2);
|
|---|
| 150 | DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
|
|---|
| 151 | AO = A1 * (1.0 - DEL1 * (0.5 * TOTHRD +
|
|---|
| 152 | DEL1 * (1.0 + 134.0 / 81.0 * DEL1)));
|
|---|
| 153 | DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
|
|---|
| 154 | XNODP = XNO / (1.0 + DELO);
|
|---|
| 155 | AODP = AO / (1.0 - DELO);
|
|---|
| 156 |
|
|---|
| 157 | /* INITIALIZATION */
|
|---|
| 158 |
|
|---|
| 159 | /* FOR PERIGEE BELOW 156 KM, THE VALUES OF
|
|---|
| 160 | * S AND QOMS2T ARE ALTERED */
|
|---|
| 161 |
|
|---|
| 162 | S4 = S;
|
|---|
| 163 | QOMS24 = QOMS2T;
|
|---|
| 164 | PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;
|
|---|
| 165 |
|
|---|
| 166 | /* IF(PERIGE .GE. 156.) GO TO 10 */
|
|---|
| 167 |
|
|---|
| 168 | if(PERIGE < 156.0) {
|
|---|
| 169 | S4 = PERIGE - 78.0;
|
|---|
| 170 |
|
|---|
| 171 | if(PERIGE <= 98.0) { /* GO TO 9 */
|
|---|
| 172 | S4 = 20.0;
|
|---|
| 173 | }
|
|---|
| 174 |
|
|---|
| 175 | QOMS24 = pow((120.0 - S4) * AE / XKMPER, 4.0); /* 9 */
|
|---|
| 176 | S4 = S4 / XKMPER + AE;
|
|---|
| 177 | }
|
|---|
| 178 | PINVSQ = 1.0 / (AODP * AODP * BETAO2 * BETAO2); /* 10 */
|
|---|
| 179 | SING = sin(OMEGAO);
|
|---|
| 180 | COSG = cos(OMEGAO);
|
|---|
| 181 | TSI = 1.0 / (AODP - S4);
|
|---|
| 182 | ETA = AODP * EO * TSI;
|
|---|
| 183 | ETASQ = ETA * ETA;
|
|---|
| 184 | EETA = EO * ETA;
|
|---|
| 185 | PSISQ = fabs(1.0 - ETASQ);
|
|---|
| 186 | COEF = QOMS24 * pow(TSI, 4.0);
|
|---|
| 187 | COEF1 = COEF / pow(PSISQ, 3.5);
|
|---|
| 188 | C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
|
|---|
| 189 | EETA * (4.0 + ETASQ)) +
|
|---|
| 190 | .75 * CK2 * TSI / PSISQ * X3THM1 *
|
|---|
| 191 | (8.0 + 3.0 * ETASQ * (8.0 + ETASQ)));
|
|---|
| 192 | C1 = BSTAR * C2;
|
|---|
| 193 | SINIO = sin(XINCL);
|
|---|
| 194 | A3OVK2 = -XJ3 / CK2 * AE * AE * AE; /* A3OVK2=-XJ3/CK2*AE**3; */
|
|---|
| 195 | X1MTH2 = 1.0 - THETA2;
|
|---|
| 196 | C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
|
|---|
| 197 | (ETA * (2.0 + .5 * ETASQ) + EO * (.5 + 2.0 * ETASQ) -
|
|---|
| 198 | 2.0 * CK2 * TSI / (AODP * PSISQ) *
|
|---|
| 199 | (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ *
|
|---|
| 200 | (1.5 - .5 * EETA)) +
|
|---|
| 201 | .75 * X1MTH2 * (2.0 * ETASQ - EETA *
|
|---|
| 202 | (1.0 + ETASQ)) * cos(2.0 * OMEGAO)));
|
|---|
| 203 | THETA4 = THETA2 * THETA2;
|
|---|
| 204 | TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
|
|---|
| 205 | TEMP2 = TEMP1 * CK2 * PINVSQ;
|
|---|
| 206 | TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
|
|---|
| 207 | XMDOT = XNODP + 0.5 * TEMP1 * BETAO * X3THM1 + .0625 * TEMP2 * BETAO *
|
|---|
| 208 | (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
|
|---|
| 209 | X1M5TH=1.0 - 5.0 * THETA2;
|
|---|
| 210 | OMGDOT = -.5 * TEMP1 * X1M5TH + .0625 * TEMP2 *
|
|---|
| 211 | (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
|
|---|
| 212 | TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
|
|---|
| 213 | XHDOT1 = -TEMP1 * COSIO;
|
|---|
| 214 | XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
|
|---|
| 215 | 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
|
|---|
| 216 | XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
|
|---|
| 217 | T2COF = 1.5 * C1;
|
|---|
| 218 | XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 * COSIO) / (1.0 + COSIO);
|
|---|
| 219 | AYCOF = .25 * A3OVK2 * SINIO;
|
|---|
| 220 | X7THM1 = 7.0 * THETA2 - 1.0;
|
|---|
| 221 | /* 90 IFLAG=0 */
|
|---|
| 222 |
|
|---|
| 223 | #ifdef SDP_DEEP_DEBUG
|
|---|
| 224 | printf("calling dpinit\n");
|
|---|
| 225 | printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
|
|---|
| 226 | EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
|
|---|
| 227 | SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP);
|
|---|
| 228 | #endif
|
|---|
| 229 | dpinit(sat, EOSQ, SINIO, COSIO, BETAO, AODP, THETA2,
|
|---|
| 230 | SING, COSG, BETAO2, XMDOT, OMGDOT, XNODOT, XNODP);
|
|---|
| 231 |
|
|---|
| 232 | /* CALL DPINIT(EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
|
|---|
| 233 | 1 SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP) */
|
|---|
| 234 |
|
|---|
| 235 | /* UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG */
|
|---|
| 236 | }
|
|---|
| 237 |
|
|---|
| 238 | XMDF = XMO + XMDOT * TSINCE; /* 100 */
|
|---|
| 239 | OMGADF = OMEGAO + OMGDOT * TSINCE;
|
|---|
| 240 | XNODDF = XNODEO + XNODOT * TSINCE;
|
|---|
| 241 | TSQ = TSINCE * TSINCE;
|
|---|
| 242 | XNODE = XNODDF + XNODCF * TSQ;
|
|---|
| 243 | TEMPA = 1.0 - C1 * TSINCE;
|
|---|
| 244 | TEMPE = BSTAR * C4 * TSINCE;
|
|---|
| 245 | TEMPL = T2COF * TSQ;
|
|---|
| 246 | XN = XNODP;
|
|---|
| 247 |
|
|---|
| 248 | if(TSINCE == 0.0) {
|
|---|
| 249 | XMDF_seco = XMDF;
|
|---|
| 250 | OMGADF_seco = OMGADF;
|
|---|
| 251 | XNODE_seco = XNODE;
|
|---|
| 252 | EM_seco = EM;
|
|---|
| 253 | XINC_seco = XINC;
|
|---|
| 254 | XN_seco = XN;
|
|---|
| 255 | }
|
|---|
| 256 |
|
|---|
| 257 | dpsec(sat, &XMDF, &OMGADF, &XNODE, &EM, &XINC, &XN, TSINCE);
|
|---|
| 258 |
|
|---|
| 259 | if(TSINCE == 0.0) {
|
|---|
| 260 | XMDF_seco = XMDF - XMDF_seco;
|
|---|
| 261 | OMGADF_seco = OMGADF - OMGADF_seco;
|
|---|
| 262 | XNODE_seco = XNODE - XNODE_seco;
|
|---|
| 263 | EM_seco = EM - EM_seco;
|
|---|
| 264 | XINC_seco = XINC - XINC_seco;
|
|---|
| 265 | XN_seco = XN - XN_seco;
|
|---|
| 266 |
|
|---|
| 267 | #if 0
|
|---|
| 268 | printf("XMDF_seco = %e\n", XMDF_seco);
|
|---|
| 269 | printf("OMGADF_seco = %e\n", OMGADF_seco);
|
|---|
| 270 | printf("XNODE_seco = %e\n", XNODE_seco);
|
|---|
| 271 | printf("EM_seco = %e\n", EM_seco);
|
|---|
| 272 | printf("XINC_seco = %e\n", XINC_seco);
|
|---|
| 273 | printf("XN_seco = %e\n", XN_seco);
|
|---|
| 274 | #endif
|
|---|
| 275 | }
|
|---|
| 276 |
|
|---|
| 277 | /*
|
|---|
| 278 | XMDF -= XMDF_seco;
|
|---|
| 279 | OMGADF -= OMGADF_seco;
|
|---|
| 280 | XNODE -= XNODE_seco;
|
|---|
| 281 | EM -= EM_seco;
|
|---|
| 282 | XINC -= XINC_seco;
|
|---|
| 283 | XN -= XN_seco;
|
|---|
| 284 | */
|
|---|
| 285 |
|
|---|
| 286 | A = pow(XKE/XN, TOTHRD) * TEMPA * TEMPA;
|
|---|
| 287 | E = EM - TEMPE;
|
|---|
| 288 | #ifdef SDP_DEEP_DEBUG
|
|---|
| 289 | printf("*** E = %f\n", E);
|
|---|
| 290 | #endif
|
|---|
| 291 | XMAM = XMDF + XNODP * TEMPL;
|
|---|
| 292 | /* CALL DPPER(E,XINC,OMGADF,XNODE,XMAM) */
|
|---|
| 293 |
|
|---|
| 294 | #ifdef SDP_DEEP_DEBUG
|
|---|
| 295 | printf("%12s %12s %12s %12s %12s\n",
|
|---|
| 296 | "E", "XINC", "OMGADF", "XNODE", "XMAM");
|
|---|
| 297 | printf("%12f %12f %12f %12f %12f\n",
|
|---|
| 298 | E, XINC, OMGADF, XNODE, XMAM);
|
|---|
| 299 | #endif
|
|---|
| 300 |
|
|---|
| 301 | if(TSINCE == 0.0) {
|
|---|
| 302 | E_pero = E;
|
|---|
| 303 | XINC_pero = XINC;
|
|---|
| 304 | OMGADF_pero = OMGADF;
|
|---|
| 305 | XNODE_pero = XNODE;
|
|---|
| 306 | XMAM_pero = XMAM;
|
|---|
| 307 | }
|
|---|
| 308 |
|
|---|
| 309 | dpper(sat, &E, &XINC, &OMGADF, &XNODE, &XMAM, TSINCE);
|
|---|
| 310 |
|
|---|
| 311 | if(TSINCE == 0.0) {
|
|---|
| 312 | E_pero = E - E_pero;
|
|---|
| 313 | XINC_pero = XINC - XINC_pero;
|
|---|
| 314 | OMGADF_pero = OMGADF - OMGADF_pero;
|
|---|
| 315 | XNODE_pero = XNODE - XNODE_pero;
|
|---|
| 316 | XMAM_pero = XMAM - XMAM_pero;
|
|---|
| 317 |
|
|---|
| 318 | #if 0
|
|---|
| 319 | printf("E_pero = %e\n", E_pero);
|
|---|
| 320 | printf("XINC_pero = %e\n", XINC_pero);
|
|---|
| 321 | printf("OMGADF_pero = %e\n", OMGADF_pero);
|
|---|
| 322 | printf("XNODE_pero = %e\n", XNODE_pero);
|
|---|
| 323 | printf("XMAM_pero = %e\n\n", XMAM_pero);
|
|---|
| 324 | #endif
|
|---|
| 325 | }
|
|---|
| 326 |
|
|---|
| 327 | /*
|
|---|
| 328 | E -= E_pero;
|
|---|
| 329 | XINC -= XINC_pero;
|
|---|
| 330 | OMGADF -= OMGADF_pero;
|
|---|
| 331 | XNODE -= XNODE_pero;
|
|---|
| 332 | XMAM -= XMAM_pero;
|
|---|
| 333 | */
|
|---|
| 334 | XL = XMAM + OMGADF + XNODE;
|
|---|
| 335 | BETA = sqrt(1.0 - E * E);
|
|---|
| 336 | XN = XKE / pow(A, 1.5);
|
|---|
| 337 |
|
|---|
| 338 | /* LONG PERIOD PERIODICS */
|
|---|
| 339 |
|
|---|
| 340 | AXN = E * cos(OMGADF);
|
|---|
| 341 | TEMP=1./(A*BETA*BETA);
|
|---|
| 342 | XLL=TEMP*XLCOF*AXN;
|
|---|
| 343 | AYNL=TEMP*AYCOF;
|
|---|
| 344 | XLT=XL+XLL;
|
|---|
| 345 | AYN=E*sin(OMGADF)+AYNL;
|
|---|
| 346 |
|
|---|
| 347 | /* SOLVE KEPLERS EQUATION */
|
|---|
| 348 |
|
|---|
| 349 | CAPU=fmod(XLT-XNODE, TWOPI);
|
|---|
| 350 | TEMP2=CAPU;
|
|---|
| 351 | /* DO 130 I=1,10*/
|
|---|
| 352 | for(i = 1; i < 10; i++) {
|
|---|
| 353 | SINEPW=sin(TEMP2);
|
|---|
| 354 | COSEPW=cos(TEMP2);
|
|---|
| 355 | TEMP3=AXN*SINEPW;
|
|---|
| 356 | TEMP4=AYN*COSEPW;
|
|---|
| 357 | TEMP5=AXN*COSEPW;
|
|---|
| 358 | TEMP6=AYN*SINEPW;
|
|---|
| 359 | EPW=(CAPU-TEMP4+TEMP3-TEMP2)/(1.-TEMP5-TEMP6)+TEMP2;
|
|---|
| 360 | /* IF(ABS(EPW-TEMP2) .LE. E6A) GO TO 140 */
|
|---|
| 361 | if(fabs(EPW-TEMP2) <= E6A)
|
|---|
| 362 | break;
|
|---|
| 363 | TEMP2=EPW; /* 130 */
|
|---|
| 364 | }
|
|---|
| 365 |
|
|---|
| 366 | /* SHORT PERIOD PRELIMINARY QUANTITIES */
|
|---|
| 367 |
|
|---|
| 368 | ECOSE=TEMP5+TEMP6; /* 140 */
|
|---|
| 369 | ESINE=TEMP3-TEMP4;
|
|---|
| 370 | ELSQ=AXN*AXN+AYN*AYN;
|
|---|
| 371 | TEMP=1.-ELSQ;
|
|---|
| 372 | PL=A*TEMP;
|
|---|
| 373 | R=A*(1.-ECOSE);
|
|---|
| 374 | TEMP1=1./R;
|
|---|
| 375 | RDOT=XKE*sqrt(A)*ESINE*TEMP1;
|
|---|
| 376 | RFDOT=XKE*sqrt(PL)*TEMP1;
|
|---|
| 377 | TEMP2=A*TEMP1;
|
|---|
| 378 | BETAL=sqrt(TEMP);
|
|---|
| 379 | TEMP3=1./(1.+BETAL);
|
|---|
| 380 | COSU=TEMP2*(COSEPW-AXN+AYN*ESINE*TEMP3);
|
|---|
| 381 | SINU=TEMP2*(SINEPW-AYN-AXN*ESINE*TEMP3);
|
|---|
| 382 | U=actan(SINU,COSU);
|
|---|
| 383 | SIN2U=2.*SINU*COSU;
|
|---|
| 384 | COS2U=2.*COSU*COSU-1.0;
|
|---|
| 385 | TEMP=1./PL;
|
|---|
| 386 | TEMP1=CK2*TEMP;
|
|---|
| 387 | TEMP2=TEMP1*TEMP;
|
|---|
| 388 |
|
|---|
| 389 | /* UPDATE FOR SHORT PERIODICS */
|
|---|
| 390 |
|
|---|
| 391 | RK=R*(1.-1.5*TEMP2*BETAL*X3THM1)+.5*TEMP1*X1MTH2*COS2U;
|
|---|
| 392 | UK=U - .25 * TEMP2 * X7THM1 * SIN2U;
|
|---|
| 393 | XNODEK=XNODE+1.5*TEMP2*COSIO*SIN2U;
|
|---|
| 394 | XINCK=XINC+1.5*TEMP2*COSIO*SINIO*COS2U;
|
|---|
| 395 | RDOTK=RDOT-XN*TEMP1*X1MTH2*SIN2U;
|
|---|
| 396 | RFDOTK=RFDOT+XN*TEMP1*(X1MTH2*COS2U+1.5*X3THM1);
|
|---|
| 397 |
|
|---|
| 398 | /* ORIENTATION VECTORS */
|
|---|
| 399 | SINUK=sin(UK);
|
|---|
| 400 | COSUK=cos(UK);
|
|---|
| 401 | SINIK=sin(XINCK);
|
|---|
| 402 | COSIK=cos(XINCK);
|
|---|
| 403 | SINNOK=sin(XNODEK);
|
|---|
| 404 | COSNOK=cos(XNODEK);
|
|---|
| 405 | XMX=-SINNOK*COSIK;
|
|---|
| 406 | XMY=COSNOK*COSIK;
|
|---|
| 407 | UX=XMX*SINUK+COSNOK*COSUK;
|
|---|
| 408 | UY=XMY*SINUK+SINNOK*COSUK;
|
|---|
| 409 | UZ=SINIK*SINUK;
|
|---|
| 410 | VX=XMX*COSUK-COSNOK*SINUK;
|
|---|
| 411 | VY=XMY*COSUK-SINNOK*SINUK;
|
|---|
| 412 | VZ=SINIK*COSUK;
|
|---|
| 413 | #if 0
|
|---|
| 414 | printf("UX = %f VX = %f RK = %f RDOTK = %f RFDOTK = %f\n",
|
|---|
| 415 | UX, VX, RK, RDOTK, RFDOTK);
|
|---|
| 416 | #endif
|
|---|
| 417 | /* POSITION AND VELOCITY */
|
|---|
| 418 |
|
|---|
| 419 | pos->x = RK*UX;
|
|---|
| 420 | pos->y = RK*UY;
|
|---|
| 421 | pos->z = RK*UZ;
|
|---|
| 422 | dpos->x = RDOTK*UX+RFDOTK*VX;
|
|---|
| 423 | dpos->y = RDOTK*UY+RFDOTK*VY;
|
|---|
| 424 | dpos->z = RDOTK*UZ+RFDOTK*VZ;
|
|---|
| 425 | /* RETURN
|
|---|
| 426 | END */
|
|---|
| 427 | }
|
|---|
| 428 |
|
|---|
| 429 | /* For RCS Only -- Do Not Edit */
|
|---|
| 430 | static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: sdp4.c,v $ $Date: 2005-08-21 10:02:39 $ $Revision: 1.5 $ $Name: not supported by cvs2svn $"};
|
|---|
![(please configure the [header_logo] section in trac.ini)](/PI/chrome/site/your_project_logo.png){kind=logo}
