source: Sophya/trunk/SophyaExt/XephemAstroLib/sdp4.c@ 2044

Last change on this file since 2044 was 1719, checked in by cmv, 24 years ago

Adapted to version 3.5 xephem cmv 22/10/2001
File size: 12.3 KB
RevLine 
[1457]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
97void
98sdp4 (sat, pos, dpos, TSINCE)
99SatData *sat;
100Vec3 *pos;
101Vec3 *dpos;
102double TSINCE;
103{
104 int i;
105
106 /* private temporary variables used only in init section */
107 double A1,A3OVK2,AO,C2,COEF,COEF1,DEL1,DELO,EETA,ETA,
108 ETASQ,PERIGE,PINVSQ,PSISQ,QOMS24,S4,THETA4,TSI,X1M5TH,XHDOT1;
109
110 /* private temporary variables */
111 double A,AXN,AYN,AYNL,BETA,BETAL,CAPU,COS2U,COSEPW,
[1719]112 COSIK,COSNOK,COSU,COSUK,E,ECOSE,ELSQ,EM=0,EPW,ESINE,OMGADF,PL,
[1457]113 R,RDOT,RDOTK,RFDOT,RFDOTK,RK,SIN2U,SINEPW,SINIK,SINNOK,
114 SINU,SINUK,TEMP,TEMP1,TEMP2,TEMP3,TEMP4,TEMP5,TEMP6,TEMPA,
[1719]115 TEMPE,TEMPL,TSQ,U,UK,UX,UY,UZ,VX,VY,VZ,XINC=0,XINCK,XL,XLL,XLT,
[1457]116 XMAM,XMDF,XMX,XMY,XN,XNODDF,XNODE,XNODEK;
117
118#if 0
119 A1=A3OVK2=AO=C2=COEF=COEF1=DEL1=DELO=EETA=ETA = signaling_nan();
120 ETASQ=PERIGE=PINVSQ=PSISQ=QOMS24=S4=THETA4=TSI=X1M5TH=XHDOT1 = signaling_nan();
121
122 A=AXN=AYN=AYNL=BETA=BETAL=CAPU=COS2U=COSEPW = signaling_nan();
123 COSIK=COSNOK=COSU=COSUK=E=ECOSE=ELSQ=EM=EPW=ESINE=OMGADF=PL = signaling_nan();
124 R=RDOT=RDOTK=RFDOT=RFDOTK=RK=SIN2U=SINEPW=SINIK=SINNOK = signaling_nan();
125 SINU=SINUK=TEMP=TEMP1=TEMP2=TEMP3=TEMP4=TEMP5=TEMP6=TEMPA = signaling_nan();
126 TEMPE=TEMPL=TSQ=U=UK=UX=UY=UZ=VX=VY=VZ=XINC=XINCK=XL=XLL=XLT = signaling_nan();
127 XMAM=XMDF=XMX=XMY=XN=XNODDF=XNODE=XNODEK = signaling_nan();
128#endif
129
130 if(TSINCE != 0.0 && !sat->prop.sdp4) {
131 /*
132 * Yes, this is a recursive call.
133 */
134 sdp4(sat, pos, dpos, 0.0);
135 }
136
137/* IF (IFLAG .EQ. 0) GO TO 100 */
138/* if(!IFLAG) */
139 if(!sat->prop.sdp4) {
140 sat->prop.sdp4 = (struct sdp4_data *) malloc(sizeof(struct sdp4_data));
141
142/* init_sdp4(sat->prop.sdp4); */
143
144/* RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP) */
145/* FROM INPUT ELEMENTS */
146
147 A1=pow((XKE/XNO), TOTHRD);
148 COSIO=cos(XINCL);
149 THETA2=COSIO*COSIO;
150 X3THM1=3.0 * THETA2 - 1.0;
151 EOSQ = EO * EO;
152 BETAO2 = 1.0 - EOSQ;
153 BETAO = sqrt(BETAO2);
154 DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
155 AO = A1 * (1.0 - DEL1 * (0.5 * TOTHRD +
156 DEL1 * (1.0 + 134.0 / 81.0 * DEL1)));
157 DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
158 XNODP = XNO / (1.0 + DELO);
159 AODP = AO / (1.0 - DELO);
160
161/* INITIALIZATION */
162
163/* FOR PERIGEE BELOW 156 KM, THE VALUES OF
164* S AND QOMS2T ARE ALTERED */
165
166 S4 = S;
167 QOMS24 = QOMS2T;
168 PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;
169
170/* IF(PERIGE .GE. 156.) GO TO 10 */
171
172 if(PERIGE < 156.0) {
173 S4 = PERIGE - 78.0;
174
175 if(PERIGE <= 98.0) { /* GO TO 9 */
176 S4 = 20.0;
177 }
178
179 QOMS24 = pow((120.0 - S4) * AE / XKMPER, 4.0); /* 9 */
180 S4 = S4 / XKMPER + AE;
181 }
182 PINVSQ = 1.0 / (AODP * AODP * BETAO2 * BETAO2); /* 10 */
183 SING = sin(OMEGAO);
184 COSG = cos(OMEGAO);
185 TSI = 1.0 / (AODP - S4);
186 ETA = AODP * EO * TSI;
187 ETASQ = ETA * ETA;
188 EETA = EO * ETA;
189 PSISQ = fabs(1.0 - ETASQ);
190 COEF = QOMS24 * pow(TSI, 4.0);
191 COEF1 = COEF / pow(PSISQ, 3.5);
192 C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
193 EETA * (4.0 + ETASQ)) +
194 .75 * CK2 * TSI / PSISQ * X3THM1 *
195 (8.0 + 3.0 * ETASQ * (8.0 + ETASQ)));
196 C1 = BSTAR * C2;
197 SINIO = sin(XINCL);
198 A3OVK2 = -XJ3 / CK2 * AE * AE * AE; /* A3OVK2=-XJ3/CK2*AE**3; */
199 X1MTH2 = 1.0 - THETA2;
200 C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
201 (ETA * (2.0 + .5 * ETASQ) + EO * (.5 + 2.0 * ETASQ) -
202 2.0 * CK2 * TSI / (AODP * PSISQ) *
203 (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ *
204 (1.5 - .5 * EETA)) +
205 .75 * X1MTH2 * (2.0 * ETASQ - EETA *
206 (1.0 + ETASQ)) * cos(2.0 * OMEGAO)));
207 THETA4 = THETA2 * THETA2;
208 TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
209 TEMP2 = TEMP1 * CK2 * PINVSQ;
210 TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
211 XMDOT = XNODP + 0.5 * TEMP1 * BETAO * X3THM1 + .0625 * TEMP2 * BETAO *
212 (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
213 X1M5TH=1.0 - 5.0 * THETA2;
214 OMGDOT = -.5 * TEMP1 * X1M5TH + .0625 * TEMP2 *
215 (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
216 TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
217 XHDOT1 = -TEMP1 * COSIO;
218 XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
219 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
220 XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
221 T2COF = 1.5 * C1;
222 XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 * COSIO) / (1.0 + COSIO);
223 AYCOF = .25 * A3OVK2 * SINIO;
224 X7THM1 = 7.0 * THETA2 - 1.0;
225/* 90 IFLAG=0 */
226
227#ifdef SDP_DEEP_DEBUG
228 printf("calling dpinit\n");
229 printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
230 EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
231 SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP);
232#endif
233 dpinit(sat, EOSQ, SINIO, COSIO, BETAO, AODP, THETA2,
234 SING, COSG, BETAO2, XMDOT, OMGDOT, XNODOT, XNODP);
235
236/* CALL DPINIT(EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
237 1 SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP) */
238
239/* UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG */
240 }
241
242 XMDF = XMO + XMDOT * TSINCE; /* 100 */
243 OMGADF = OMEGAO + OMGDOT * TSINCE;
244 XNODDF = XNODEO + XNODOT * TSINCE;
245 TSQ = TSINCE * TSINCE;
246 XNODE = XNODDF + XNODCF * TSQ;
247 TEMPA = 1.0 - C1 * TSINCE;
248 TEMPE = BSTAR * C4 * TSINCE;
249 TEMPL = T2COF * TSQ;
250 XN = XNODP;
251
252 if(TSINCE == 0.0) {
253 XMDF_seco = XMDF;
254 OMGADF_seco = OMGADF;
255 XNODE_seco = XNODE;
256 EM_seco = EM;
257 XINC_seco = XINC;
258 XN_seco = XN;
259 }
260
261 dpsec(sat, &XMDF, &OMGADF, &XNODE, &EM, &XINC, &XN, TSINCE);
262
263 if(TSINCE == 0.0) {
264 XMDF_seco = XMDF - XMDF_seco;
265 OMGADF_seco = OMGADF - OMGADF_seco;
266 XNODE_seco = XNODE - XNODE_seco;
267 EM_seco = EM - EM_seco;
268 XINC_seco = XINC - XINC_seco;
269 XN_seco = XN - XN_seco;
270
271#if 0
272 printf("XMDF_seco = %e\n", XMDF_seco);
273 printf("OMGADF_seco = %e\n", OMGADF_seco);
274 printf("XNODE_seco = %e\n", XNODE_seco);
275 printf("EM_seco = %e\n", EM_seco);
276 printf("XINC_seco = %e\n", XINC_seco);
277 printf("XN_seco = %e\n", XN_seco);
278#endif
279 }
280
281 /*
282 XMDF -= XMDF_seco;
283 OMGADF -= OMGADF_seco;
284 XNODE -= XNODE_seco;
285 EM -= EM_seco;
286 XINC -= XINC_seco;
287 XN -= XN_seco;
288 */
289
290 A = pow(XKE/XN, TOTHRD) * TEMPA * TEMPA;
291 E = EM - TEMPE;
292#ifdef SDP_DEEP_DEBUG
293 printf("*** E = %f\n", E);
294#endif
295 XMAM = XMDF + XNODP * TEMPL;
296/* CALL DPPER(E,XINC,OMGADF,XNODE,XMAM) */
297
298#ifdef SDP_DEEP_DEBUG
299 printf("%12s %12s %12s %12s %12s\n",
300 "E", "XINC", "OMGADF", "XNODE", "XMAM");
301 printf("%12f %12f %12f %12f %12f\n",
302 E, XINC, OMGADF, XNODE, XMAM);
303#endif
304
305 if(TSINCE == 0.0) {
306 E_pero = E;
307 XINC_pero = XINC;
308 OMGADF_pero = OMGADF;
309 XNODE_pero = XNODE;
310 XMAM_pero = XMAM;
311 }
312
313 dpper(sat, &E, &XINC, &OMGADF, &XNODE, &XMAM, TSINCE);
314
315 if(TSINCE == 0.0) {
316 E_pero = E - E_pero;
317 XINC_pero = XINC - XINC_pero;
318 OMGADF_pero = OMGADF - OMGADF_pero;
319 XNODE_pero = XNODE - XNODE_pero;
320 XMAM_pero = XMAM - XMAM_pero;
321
322#if 0
323 printf("E_pero = %e\n", E_pero);
324 printf("XINC_pero = %e\n", XINC_pero);
325 printf("OMGADF_pero = %e\n", OMGADF_pero);
326 printf("XNODE_pero = %e\n", XNODE_pero);
327 printf("XMAM_pero = %e\n\n", XMAM_pero);
328#endif
329 }
330
331 /*
332 E -= E_pero;
333 XINC -= XINC_pero;
334 OMGADF -= OMGADF_pero;
335 XNODE -= XNODE_pero;
336 XMAM -= XMAM_pero;
337 */
338 XL = XMAM + OMGADF + XNODE;
339 BETA = sqrt(1.0 - E * E);
340 XN = XKE / pow(A, 1.5);
341
342/* LONG PERIOD PERIODICS */
343
344 AXN = E * cos(OMGADF);
345 TEMP=1./(A*BETA*BETA);
346 XLL=TEMP*XLCOF*AXN;
347 AYNL=TEMP*AYCOF;
348 XLT=XL+XLL;
349 AYN=E*sin(OMGADF)+AYNL;
350
351/* SOLVE KEPLERS EQUATION */
352
353 CAPU=fmod(XLT-XNODE, TWOPI);
354 TEMP2=CAPU;
355/* DO 130 I=1,10*/
356 for(i = 1; i < 10; i++) {
357 SINEPW=sin(TEMP2);
358 COSEPW=cos(TEMP2);
359 TEMP3=AXN*SINEPW;
360 TEMP4=AYN*COSEPW;
361 TEMP5=AXN*COSEPW;
362 TEMP6=AYN*SINEPW;
363 EPW=(CAPU-TEMP4+TEMP3-TEMP2)/(1.-TEMP5-TEMP6)+TEMP2;
364/* IF(ABS(EPW-TEMP2) .LE. E6A) GO TO 140 */
365 if(fabs(EPW-TEMP2) <= E6A)
366 break;
367 TEMP2=EPW; /* 130 */
368 }
369
370/* SHORT PERIOD PRELIMINARY QUANTITIES */
371
372 ECOSE=TEMP5+TEMP6; /* 140 */
373 ESINE=TEMP3-TEMP4;
374 ELSQ=AXN*AXN+AYN*AYN;
375 TEMP=1.-ELSQ;
376 PL=A*TEMP;
377 R=A*(1.-ECOSE);
378 TEMP1=1./R;
379 RDOT=XKE*sqrt(A)*ESINE*TEMP1;
380 RFDOT=XKE*sqrt(PL)*TEMP1;
381 TEMP2=A*TEMP1;
382 BETAL=sqrt(TEMP);
383 TEMP3=1./(1.+BETAL);
384 COSU=TEMP2*(COSEPW-AXN+AYN*ESINE*TEMP3);
385 SINU=TEMP2*(SINEPW-AYN-AXN*ESINE*TEMP3);
386 U=actan(SINU,COSU);
387 SIN2U=2.*SINU*COSU;
388 COS2U=2.*COSU*COSU-1.0;
389 TEMP=1./PL;
390 TEMP1=CK2*TEMP;
391 TEMP2=TEMP1*TEMP;
392
393/* UPDATE FOR SHORT PERIODICS */
394
395 RK=R*(1.-1.5*TEMP2*BETAL*X3THM1)+.5*TEMP1*X1MTH2*COS2U;
396 UK=U - .25 * TEMP2 * X7THM1 * SIN2U;
397 XNODEK=XNODE+1.5*TEMP2*COSIO*SIN2U;
398 XINCK=XINC+1.5*TEMP2*COSIO*SINIO*COS2U;
399 RDOTK=RDOT-XN*TEMP1*X1MTH2*SIN2U;
400 RFDOTK=RFDOT+XN*TEMP1*(X1MTH2*COS2U+1.5*X3THM1);
401
402/* ORIENTATION VECTORS */
403 SINUK=sin(UK);
404 COSUK=cos(UK);
405 SINIK=sin(XINCK);
406 COSIK=cos(XINCK);
407 SINNOK=sin(XNODEK);
408 COSNOK=cos(XNODEK);
409 XMX=-SINNOK*COSIK;
410 XMY=COSNOK*COSIK;
411 UX=XMX*SINUK+COSNOK*COSUK;
412 UY=XMY*SINUK+SINNOK*COSUK;
413 UZ=SINIK*SINUK;
414 VX=XMX*COSUK-COSNOK*SINUK;
415 VY=XMY*COSUK-SINNOK*SINUK;
416 VZ=SINIK*COSUK;
417#if 0
418 printf("UX = %f VX = %f RK = %f RDOTK = %f RFDOTK = %f\n",
419 UX, VX, RK, RDOTK, RFDOTK);
420#endif
421/* POSITION AND VELOCITY */
422
423 pos->x = RK*UX;
424 pos->y = RK*UY;
425 pos->z = RK*UZ;
426 dpos->x = RDOTK*UX+RFDOTK*VX;
427 dpos->y = RDOTK*UY+RFDOTK*VY;
428 dpos->z = RDOTK*UZ+RFDOTK*VZ;
429/* RETURN
430 END */
431}
432
433/* For RCS Only -- Do Not Edit */
[1719]434static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: sdp4.c,v $ $Date: 2001-10-22 12:08:28 $ $Revision: 1.2 $ $Name: not supported by cvs2svn $"};
Note: See TracBrowser for help on using the repository browser.