Update deepspace pascal code
Daniel Warner
parent
4388e439f0
commit
ef30e2d052
511
SGDP4.cpp
511
SGDP4.cpp
|
|
@ -11,6 +11,60 @@ SGDP4::SGDP4(void) {
|
||||||
SGDP4::~SGDP4(void) {
|
SGDP4::~SGDP4(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void SGDP4::ProcessTle(const Tle& tle) {
|
||||||
|
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
|
||||||
|
XMO MeanAnomoly() Mo
|
||||||
|
XNODEO AscendingNode() thetao
|
||||||
|
OMEGAO ArgumentPerigee() wo
|
||||||
|
EO Eccentricity() eo
|
||||||
|
XINCL Inclination() io
|
||||||
|
XNO MeanMotion() no
|
||||||
|
BSTAR BStar() bstar
|
||||||
|
|
||||||
|
double MeanAnomoly() const {
|
||||||
|
return mean_anomoly_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double AscendingNode() const {
|
||||||
|
return ascending_node_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double ArgumentPerigee() const {
|
||||||
|
return argument_perigee_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Eccentricity() const {
|
||||||
|
return eccentricity_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double MeanMotion() const {
|
||||||
|
return mean_motion_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double BStar() const {
|
||||||
|
return bstar_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double RecoveredSemiMajorAxis() const {
|
||||||
|
return recovered_semi_major_axis;
|
||||||
|
}
|
||||||
|
|
||||||
|
double RecoveredMeanMotion() const {
|
||||||
|
return recovered_mean_motion_;
|
||||||
|
}
|
||||||
|
|
||||||
|
X, Y, Z, XDOT, YDOT, ZDOT
|
||||||
|
EPOCH
|
||||||
|
|
||||||
|
COMMON / C1 / CK2, CK4, E6A, QOMS2T, S, TOTHRD,
|
||||||
|
1 XJ3, XKE, XKMPER, XMNPDA, AE
|
||||||
|
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
void SGDP4::Initialize(const Tle& tle) {
|
void SGDP4::Initialize(const Tle& tle) {
|
||||||
|
|
||||||
cosio_ = 0.0;
|
cosio_ = 0.0;
|
||||||
|
|
@ -109,9 +163,14 @@ void SGDP4::Initialize(const Tle& tle) {
|
||||||
* quadratic variation in mean anomly. also, the c3 term, the
|
* quadratic variation in mean anomly. also, the c3 term, the
|
||||||
* delta omega term and the delta m term are dropped
|
* delta omega term and the delta m term are dropped
|
||||||
*/
|
*/
|
||||||
|
use_deep_space_ = false;
|
||||||
use_simple_model_ = false;
|
use_simple_model_ = false;
|
||||||
if (rp < (220.0 / Globals::XKMPER() + Globals::AE()))
|
if (period >= 225.0) {
|
||||||
use_simple_model_ = true;
|
use_deep_space_ = true;
|
||||||
|
} else {
|
||||||
|
if (rp < (220.0 / Globals::XKMPER() + Globals::AE()))
|
||||||
|
use_simple_model_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
double s4_ = Globals::S();
|
double s4_ = Globals::S();
|
||||||
double qoms24_ = Globals::QOMS2T();
|
double qoms24_ = Globals::QOMS2T();
|
||||||
|
|
@ -124,15 +183,10 @@ void SGDP4::Initialize(const Tle& tle) {
|
||||||
if (perigee <= 98.0) {
|
if (perigee <= 98.0) {
|
||||||
s4_ = 20.0;
|
s4_ = 20.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
qoms24_ = pow((120.0 - s4_) * Globals::AE() / Globals::XKMPER(), 4.0);
|
qoms24_ = pow((120.0 - s4_) * Globals::AE() / Globals::XKMPER(), 4.0);
|
||||||
s4_ = s4_ / Globals::XKMPER() + Globals::AE();
|
s4_ = s4_ / Globals::XKMPER() + Globals::AE();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (period >= 225.0) {
|
|
||||||
use_deep_space_ = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
double pinvsq = 1.0 / (aodp_ * aodp_ * betao2 * betao2);
|
double pinvsq = 1.0 / (aodp_ * aodp_ * betao2 * betao2);
|
||||||
|
|
||||||
double tsi = 1.0 / (aodp_ - s4_);
|
double tsi = 1.0 / (aodp_ - s4_);
|
||||||
|
|
@ -180,7 +234,6 @@ void SGDP4::Initialize(const Tle& tle) {
|
||||||
|
|
||||||
if (!use_deep_space_) {
|
if (!use_deep_space_) {
|
||||||
|
|
||||||
// check result (different to telsko)
|
|
||||||
double c3 = 0.0;
|
double c3 = 0.0;
|
||||||
if (tle_data_0_.eo > 1.0e-4) {
|
if (tle_data_0_.eo > 1.0e-4) {
|
||||||
c3 = coef * tsi * a3ovk2_ * tle_data_0_.xno * Globals::AE() *
|
c3 = coef * tsi * a3ovk2_ * tle_data_0_.xno * Globals::AE() *
|
||||||
|
|
@ -432,7 +485,312 @@ void SGDP4::FindPosition(double tsince) {
|
||||||
}
|
}
|
||||||
|
|
||||||
#if 0
|
#if 0
|
||||||
* ENTRANCE FOR DEEP SPACE SECULAR EFFECTS
|
SUBROUTINE DEEP
|
||||||
|
COMMON / E1 / XMO, XNODEO, OMEGAO, EO, XINCL, XNO, XNDT2O,
|
||||||
|
1 XNDD6O, BSTAR, X, Y, Z, XDOT, YDOT, ZDOT, EPOCH, DS50
|
||||||
|
COMMON / C1 / CK2, CK4, E6A, QOMS2T, S, TOTHRD,
|
||||||
|
1 XJ3, XKE, XKMPER, XMNPDA, AE
|
||||||
|
COMMON / C2 / DE2RA, PI, PIO2, TWOPI, X3PIO2
|
||||||
|
DOUBLE PRECISION EPOCH, DS50
|
||||||
|
DOUBLE PRECISION
|
||||||
|
* DAY, PREEP, XNODCE, ATIME, DELT, SAVTSN, STEP2, STEPN, STEPP
|
||||||
|
|
||||||
|
double ZNS = 1.19459E-5;
|
||||||
|
double C1SS = 2.9864797E-6;
|
||||||
|
double ZES = .01675;
|
||||||
|
double ZNL = 1.5835218E-4;
|
||||||
|
double C1L = 4.7968065E-7;
|
||||||
|
double ZEL = .05490;
|
||||||
|
double ZCOSIS = .91744867;
|
||||||
|
double ZSINI = .39785416;
|
||||||
|
double ZSINGS = -.98088458;
|
||||||
|
double ZCOSGS = .1945905;
|
||||||
|
double ZCOSHS = 1.0;
|
||||||
|
double ZSINHS = 0.0;
|
||||||
|
double Q22 = 1.7891679E-6;
|
||||||
|
double Q31 = 2.1460748E-6;
|
||||||
|
double Q33 = 2.2123015E-7;
|
||||||
|
double G22 = 5.7686396;
|
||||||
|
double G32 = 0.95240898;
|
||||||
|
double G44 = 1.8014998;
|
||||||
|
double G52 = 1.0508330;
|
||||||
|
double G54 = 4.4108898;
|
||||||
|
double ROOT22 = 1.7891679E-6;
|
||||||
|
double ROOT32 = 3.7393792E-7;
|
||||||
|
double ROOT44 = 7.3636953E-9;
|
||||||
|
double ROOT52 = 1.1428639E-7;
|
||||||
|
double ROOT54 = 2.1765803E-9;
|
||||||
|
double THDT = 4.3752691E-3;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* ENTRANCE FOR DEEP SPACE INITIALIZATION
|
||||||
|
*/
|
||||||
|
ENTRY DPINIT(EQSQ, SINIQ, COSIQ, RTEQSQ, AO, COSQ2, SINOMO, COSOMO,
|
||||||
|
1 BSQ, XLLDOT, OMGDT, XNODOT, XNODP)
|
||||||
|
THGR = THETAG(EPOCH)
|
||||||
|
EQ = EO
|
||||||
|
XNQ = XNODP
|
||||||
|
AQNV = 1. / AO
|
||||||
|
XQNCL = XINCL
|
||||||
|
XMAO = XMO
|
||||||
|
XPIDOT = OMGDT + XNODOT
|
||||||
|
SINQ = SIN(XNODEO)
|
||||||
|
COSQ = COS(XNODEO)
|
||||||
|
OMEGAQ = OMEGAO
|
||||||
|
* INITIALIZE LUNAR SOLAR TERMS
|
||||||
|
5 DAY = DS50 + 18261.5D0
|
||||||
|
IF(DAY.EQ.PREEP) GO TO 10
|
||||||
|
PREEP = DAY
|
||||||
|
XNODCE = 4.5236020 - 9.2422029E-4 * DAY
|
||||||
|
59
|
||||||
|
STEM = DSIN(XNODCE)
|
||||||
|
CTEM = DCOS(XNODCE)
|
||||||
|
ZCOSIL = .91375164 - .03568096 * CTEM
|
||||||
|
ZSINIL = SQRT(1. - ZCOSIL*ZCOSIL)
|
||||||
|
ZSINHL = .089683511 * STEM / ZSINIL
|
||||||
|
ZCOSHL = SQRT(1. - ZSINHL*ZSINHL)
|
||||||
|
C = 4.7199672 + .22997150 * DAY
|
||||||
|
GAM = 5.8351514 + .0019443680 * DAY
|
||||||
|
ZMOL = FMOD2P(C - GAM)
|
||||||
|
ZX = .39785416 * STEM / ZSINIL
|
||||||
|
ZY = ZCOSHL*CTEM + 0.91744867 * ZSINHL*STEM
|
||||||
|
ZX = ACTAN(ZX, ZY)
|
||||||
|
ZX = GAM + ZX - XNODCE
|
||||||
|
ZCOSGL = COS(ZX)
|
||||||
|
ZSINGL = SIN(ZX)
|
||||||
|
ZMOS = 6.2565837D0 + .017201977D0*DAY
|
||||||
|
ZMOS = FMOD2P(ZMOS)
|
||||||
|
* DO SOLAR TERMS
|
||||||
|
10 LS = 0
|
||||||
|
SAVTSN = 1.D20
|
||||||
|
ZCOSG = ZCOSGS
|
||||||
|
ZSING = ZSINGS
|
||||||
|
ZCOSI = ZCOSIS
|
||||||
|
ZSINI = ZSINIS
|
||||||
|
ZCOSH = COSQ
|
||||||
|
ZSINH = SINQ
|
||||||
|
CC = C1SS
|
||||||
|
ZN = ZNS
|
||||||
|
ZE = ZES
|
||||||
|
ZMO = ZMOS
|
||||||
|
XNOI = 1. / XNQ
|
||||||
|
ASSIGN 30 TO LS
|
||||||
|
20 A1 = ZCOSG*ZCOSH + ZSING*ZCOSI*ZSINH
|
||||||
|
A3 = -ZSING*ZCOSH + ZCOSG*ZCOSI*ZSINH
|
||||||
|
A7 = -ZCOSG*ZSINH + ZSING*ZCOSI*ZCOSH
|
||||||
|
A8 = ZSING*ZSINI
|
||||||
|
A9 = ZSING*ZSINH + ZCOSG*ZCOSI*ZCOSH
|
||||||
|
A10 = ZCOSG*ZSINI
|
||||||
|
A2 = COSIQ*A7 + SINIQ*A8
|
||||||
|
A4 = COSIQ*A9 + SINIQ*A10
|
||||||
|
A5 = -SINIQ*A7 + COSIQ*A8
|
||||||
|
A6 = -SINIQ*A9 + COSIQ*A10
|
||||||
|
C
|
||||||
|
X1 = A1*COSOMO + A2*SINOMO
|
||||||
|
X2 = A3*COSOMO + A4*SINOMO
|
||||||
|
X3 = -A1*SINOMO + A2*COSOMO
|
||||||
|
60
|
||||||
|
X4 = -A3*SINOMO + A4*COSOMO
|
||||||
|
X5 = A5*SINOMO
|
||||||
|
X6 = A6*SINOMO
|
||||||
|
X7 = A5*COSOMO
|
||||||
|
X8 = A6*COSOMO
|
||||||
|
C
|
||||||
|
Z31 = 12. * X1*X1 - 3. * X3*X3
|
||||||
|
Z32 = 24. * X1*X2 - 6. * X3*X4
|
||||||
|
Z33 = 12. * X2*X2 - 3. * X4*X4
|
||||||
|
Z1 = 3. * (A1*A1 + A2*A2) + Z31*EQSQ
|
||||||
|
Z2 = 6. * (A1*A3 + A2*A4) + Z32*EQSQ
|
||||||
|
Z3 = 3. * (A3*A3 + A4*A4) + Z33*EQSQ
|
||||||
|
Z11 = -6. * A1*A5 + EQSQ *(-24. * X1*X7 - 6. * X3*X5)
|
||||||
|
Z12 = -6. * (A1*A6 + A3*A5) + EQSQ *(-24. * (X2*X7 + X1*X8) - 6. * (X3*X6 + X4*X5))
|
||||||
|
Z13 = -6. * A3*A6 + EQSQ *(-24. * X2*X8 - 6. * X4*X6)
|
||||||
|
Z21 = 6. * A2*A5 + EQSQ *(24. * X1*X5 - 6. * X3*X7)
|
||||||
|
Z22 = 6. * (A4*A5 + A2*A6) + EQSQ *(24. * (X2*X5 + X1*X6) - 6. * (X4*X7 + X3*X8))
|
||||||
|
Z23 = 6. * A4*A6 + EQSQ *(24. * X2*X6 - 6. * X4*X8)
|
||||||
|
Z1 = Z1 + Z1 + BSQ*Z31
|
||||||
|
Z2 = Z2 + Z2 + BSQ*Z32
|
||||||
|
Z3 = Z3 + Z3 + BSQ*Z33
|
||||||
|
S3 = CC*XNOI
|
||||||
|
S2 = -.5 * S3 / RTEQSQ
|
||||||
|
S4 = S3*RTEQSQ
|
||||||
|
S1 = -15. * EQ*S4
|
||||||
|
S5 = X1*X3 + X2*X4
|
||||||
|
S6 = X2*X3 + X1*X4
|
||||||
|
S7 = X2*X4 - X1*X3
|
||||||
|
SE = S1*ZN*S5
|
||||||
|
SI = S2*ZN*(Z11 + Z13)
|
||||||
|
SL = -ZN*S3*(Z1 + Z3 - 14. - 6. * EQSQ)
|
||||||
|
SGH = S4*ZN*(Z31 + Z33 - 6.)
|
||||||
|
SH = -ZN*S2*(Z21 + Z23)
|
||||||
|
IF(XQNCL.LT.5.2359877E-2) SH = 0.0
|
||||||
|
EE2 = 2. * S1*S6
|
||||||
|
E3 = 2. * S1*S7
|
||||||
|
XI2 = 2. * S2*Z12
|
||||||
|
XI3 = 2. * S2*(Z13 - Z11)
|
||||||
|
XL2 = -2. * S3*Z2
|
||||||
|
XL3 = -2. * S3*(Z3 - Z1)
|
||||||
|
XL4 = -2. * S3*(-21. - 9. * EQSQ) * ZE
|
||||||
|
XGH2 = 2. * S4*Z32
|
||||||
|
XGH3 = 2. * S4*(Z33 - Z31)
|
||||||
|
XGH4 = -18. * S4*ZE
|
||||||
|
XH2 = -2. * S2*Z22
|
||||||
|
XH3 = -2. * S2*(Z23 - Z21)
|
||||||
|
GO TO LS
|
||||||
|
61
|
||||||
|
* DO LUNAR TERMS
|
||||||
|
30 SSE = SE
|
||||||
|
SSI = SI
|
||||||
|
SSL = SL
|
||||||
|
SSH = SH / SINIQ
|
||||||
|
SSG = SGH - COSIQ*SSH
|
||||||
|
SE2 = EE2
|
||||||
|
SI2 = XI2
|
||||||
|
SL2 = XL2
|
||||||
|
SGH2 = XGH2
|
||||||
|
SH2 = XH2
|
||||||
|
SE3 = E3
|
||||||
|
SI3 = XI3
|
||||||
|
SL3 = XL3
|
||||||
|
SGH3 = XGH3
|
||||||
|
SH3 = XH3
|
||||||
|
SL4 = XL4
|
||||||
|
SGH4 = XGH4
|
||||||
|
LS = 1
|
||||||
|
ZCOSG = ZCOSGL
|
||||||
|
ZSING = ZSINGL
|
||||||
|
ZCOSI = ZCOSIL
|
||||||
|
ZSINI = ZSINIL
|
||||||
|
ZCOSH = ZCOSHL*COSQ + ZSINHL*SINQ
|
||||||
|
ZSINH = SINQ*ZCOSHL - COSQ*ZSINHL
|
||||||
|
ZN = ZNL
|
||||||
|
CC = C1L
|
||||||
|
ZE = ZEL
|
||||||
|
ZMO = ZMOL
|
||||||
|
ASSIGN 40 TO LS
|
||||||
|
GO TO 20
|
||||||
|
40 SSE = SSE + SE
|
||||||
|
SSI = SSI + SI
|
||||||
|
SSL = SSL + SL
|
||||||
|
SSG = SSG + SGH - COSIQ / SINIQ*SH
|
||||||
|
SSH = SSH + SH / SINIQ
|
||||||
|
* GEOPOTENTIAL RESONANCE INITIALIZATION FOR 12 HOUR ORBITS
|
||||||
|
IRESFL = 0
|
||||||
|
ISYNFL = 0
|
||||||
|
IF(XNQ.LT.(.0052359877).AND.XNQ.GT.(.0034906585)) GO TO 70
|
||||||
|
IF(XNQ.LT.(8.26E-3) .OR. XNQ.GT.(9.24E-3)) RETURN
|
||||||
|
IF(EQ.LT.0.5) RETURN
|
||||||
|
IRESFL = 1
|
||||||
|
EOC = EQ*EQSQ
|
||||||
|
G201 = -.306 - (EQ - .64)*.440
|
||||||
|
62
|
||||||
|
IF(EQ.GT.(.65)) GO TO 45
|
||||||
|
G211 = 3.616 - 13.247 * EQ + 16.290 * EQSQ
|
||||||
|
G310 = -19.302 + 117.390 * EQ - 228.419 * EQSQ + 156.591 * EOC
|
||||||
|
G322 = -18.9068 + 109.7927 * EQ - 214.6334 * EQSQ + 146.5816 * EOC
|
||||||
|
G410 = -41.122 + 242.694 * EQ - 471.094 * EQSQ + 313.953 * EOC
|
||||||
|
G422 = -146.407 + 841.880 * EQ - 1629.014 * EQSQ + 1083.435 * EOC
|
||||||
|
G520 = -532.114 + 3017.977 * EQ - 5740 * EQSQ + 3708.276 * EOC
|
||||||
|
GO TO 55
|
||||||
|
45 G211 = -72.099 + 331.819 * EQ - 508.738 * EQSQ + 266.724 * EOC
|
||||||
|
G310 = -346.844 + 1582.851 * EQ - 2415.925 * EQSQ + 1246.113 * EOC
|
||||||
|
G322 = -342.585 + 1554.908 * EQ - 2366.899 * EQSQ + 1215.972 * EOC
|
||||||
|
G410 = -1052.797 + 4758.686 * EQ - 7193.992 * EQSQ + 3651.957 * EOC
|
||||||
|
G422 = -3581.69 + 16178.11 * EQ - 24462.77 * EQSQ + 12422.52 * EOC
|
||||||
|
IF(EQ.GT.(.715)) GO TO 50
|
||||||
|
G520 = 1464.74 - 4664.75 * EQ + 3763.64 * EQSQ
|
||||||
|
GO TO 55
|
||||||
|
50 G520 = -5149.66 + 29936.92 * EQ - 54087.36 * EQSQ + 31324.56 * EOC
|
||||||
|
55 IF(EQ.GE.(.7)) GO TO 60
|
||||||
|
G533 = -919.2277 + 4988.61 * EQ - 9064.77 * EQSQ + 5542.21 * EOC
|
||||||
|
G521 = -822.71072 + 4568.6173 * EQ - 8491.4146 * EQSQ + 5337.524 * EOC
|
||||||
|
G532 = -853.666 + 4690.25 * EQ - 8624.77 * EQSQ + 5341.4 * EOC
|
||||||
|
GO TO 65
|
||||||
|
60 G533 = -37995.78 + 161616.52 * EQ - 229838.2 * EQSQ + 109377.94 * EOC
|
||||||
|
G521 = -51752.104 + 218913.95 * EQ - 309468.16 * EQSQ + 146349.42 * EOC
|
||||||
|
G532 = -40023.88 + 170470.89 * EQ - 242699.48 * EQSQ + 115605.82 * EOC
|
||||||
|
65 SINI2 = SINIQ*SINIQ
|
||||||
|
F220 = .75 * (1. + 2. * COSIQ + COSQ2)
|
||||||
|
F221 = 1.5 * SINI2
|
||||||
|
F321 = 1.875 * SINIQ*(1. - 2. * COSIQ - 3. * COSQ2)
|
||||||
|
F322 = -1.875 * SINIQ*(1. + 2. * COSIQ - 3. * COSQ2)
|
||||||
|
F441 = 35. * SINI2*F220
|
||||||
|
F442 = 39.3750 * SINI2*SINI2
|
||||||
|
F522 = 9.84375 * SINIQ*(SINI2*(1. - 2. * COSIQ - 5. * COSQ2)
|
||||||
|
1 + .33333333 * (-2. + 4. * COSIQ + 6. * COSQ2))
|
||||||
|
F523 = SINIQ*(4.92187512 * SINI2*(-2. - 4. * COSIQ + 10. * COSQ2)
|
||||||
|
* +6.56250012 * (1. + 2. * COSIQ - 3. * COSQ2))
|
||||||
|
F542 = 29.53125 * SINIQ*(2. - 8. * COSIQ + COSQ2*(-12. + 8. * COSIQ
|
||||||
|
* +10. * COSQ2))
|
||||||
|
F543 = 29.53125 * SINIQ*(-2. - 8. * COSIQ + COSQ2*(12. + 8. * COSIQ - 10. * COSQ2))
|
||||||
|
XNO2 = XNQ*XNQ
|
||||||
|
AINV2 = AQNV*AQNV
|
||||||
|
TEMP1 = 3. * XNO2*AINV2
|
||||||
|
TEMP = TEMP1*ROOT22
|
||||||
|
D2201 = TEMP*F220*G201
|
||||||
|
D2211 = TEMP*F221*G211
|
||||||
|
TEMP1 = TEMP1*AQNV
|
||||||
|
TEMP = TEMP1*ROOT32
|
||||||
|
D3210 = TEMP*F321*G310
|
||||||
|
63
|
||||||
|
D3222 = TEMP*F322*G322
|
||||||
|
TEMP1 = TEMP1*AQNV
|
||||||
|
TEMP = 2. * TEMP1*ROOT44
|
||||||
|
D4410 = TEMP*F441*G410
|
||||||
|
D4422 = TEMP*F442*G422
|
||||||
|
TEMP1 = TEMP1*AQNV
|
||||||
|
TEMP = TEMP1*ROOT52
|
||||||
|
D5220 = TEMP*F522*G520
|
||||||
|
D5232 = TEMP*F523*G532
|
||||||
|
TEMP = 2. * TEMP1*ROOT54
|
||||||
|
D5421 = TEMP*F542*G521
|
||||||
|
D5433 = TEMP*F543*G533
|
||||||
|
XLAMO = XMAO + XNODEO + XNODEO - THGR - THGR
|
||||||
|
BFACT = XLLDOT + XNODOT + XNODOT - THDT - THDT
|
||||||
|
BFACT = BFACT + SSL + SSH + SSH
|
||||||
|
GO TO 80
|
||||||
|
* SYNCHRONOUS RESONANCE TERMS INITIALIZATION
|
||||||
|
70 IRESFL = 1
|
||||||
|
ISYNFL = 1
|
||||||
|
G200 = 1.0 + EQSQ*(-2.5 + .8125 * EQSQ)
|
||||||
|
G310 = 1.0 + 2.0 * EQSQ
|
||||||
|
G300 = 1.0 + EQSQ*(-6.0 + 6.60937 * EQSQ)
|
||||||
|
F220 = .75 * (1. + COSIQ)*(1. + COSIQ)
|
||||||
|
F311 = .9375 * SINIQ*SINIQ*(1. + 3. * COSIQ) - .75 * (1. + COSIQ)
|
||||||
|
F330 = 1. + COSIQ
|
||||||
|
F330 = 1.875 * F330*F330*F330
|
||||||
|
DEL1 = 3. * XNQ*XNQ*AQNV*AQNV
|
||||||
|
DEL2 = 2. * DEL1*F220*G200*Q22
|
||||||
|
DEL3 = 3. * DEL1*F330*G300*Q33*AQNV
|
||||||
|
DEL1 = DEL1*F311*G310*Q31*AQNV
|
||||||
|
FASX2 = .13130908
|
||||||
|
FASX4 = 2.8843198
|
||||||
|
FASX6 = .37448087
|
||||||
|
XLAMO = XMAO + XNODEO + OMEGAO - THGR
|
||||||
|
BFACT = XLLDOT + XPIDOT - THDT
|
||||||
|
BFACT = BFACT + SSL + SSG + SSH
|
||||||
|
80 XFACT = BFACT - XNQ
|
||||||
|
C
|
||||||
|
C INITIALIZE INTEGRATOR
|
||||||
|
C
|
||||||
|
XLI = XLAMO
|
||||||
|
XNI = XNQ
|
||||||
|
ATIME = 0.D0
|
||||||
|
STEPP = 720.D0
|
||||||
|
STEPN = -720.D0
|
||||||
|
STEP2 = 259200.D0
|
||||||
|
64
|
||||||
|
RETURN
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* ENTRANCE FOR DEEP SPACE SECULAR EFFECTS
|
||||||
|
*/
|
||||||
ENTRY DPSEC(XLL, OMGASM, XNODES, EM, XINC, XN, T)
|
ENTRY DPSEC(XLL, OMGASM, XNODES, EM, XINC, XN, T)
|
||||||
XLL = XLL + SSL*T
|
XLL = XLL + SSL*T
|
||||||
OMGASM = OMGASM + SSG*T
|
OMGASM = OMGASM + SSG*T
|
||||||
|
|
@ -527,72 +885,71 @@ GO TO 125
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
c
|
/*
|
||||||
c entrances for lunar - solar periodics
|
* ENTRANCES FOR LUNAR-SOLAR PERIODICS
|
||||||
c
|
*/
|
||||||
66
|
ENTRY DPPER(EM, XINC, OMGASM, XNODES, XLL)
|
||||||
c
|
SINIS = SIN(XINC)
|
||||||
entry dpper(em, xinc, omgasm, xnodes, xll)
|
COSIS = COS(XINC)
|
||||||
sinis = sin(xinc)
|
IF(DABS(SAVTSN - T).LT.(30.D0)) GO TO 210
|
||||||
cosis = cos(xinc)
|
SAVTSN = T
|
||||||
if (dabs(savtsn - t).lt.(30.d0)) go to 210
|
ZM = ZMOS + ZNS*T
|
||||||
savtsn = t
|
205 ZF = ZM + 2. * ZES*SIN(ZM)
|
||||||
zm = zmos + zns * t
|
SINZF = SIN(ZF)
|
||||||
205 zf = zm + 2. * zes * sin(zm)
|
F2 = .5 * SINZF*SINZF - .25
|
||||||
sinzf = sin(zf)
|
F3 = -.5 * SINZF*COS(ZF)
|
||||||
f2 = .5 * sinzf * sinzf - .25
|
SES = SE2*F2 + SE3*F3
|
||||||
f3 = -.5 * sinzf * cos(zf)
|
SIS = SI2*F2 + SI3*F3
|
||||||
ses = se2 * f2 + se3 * f3
|
SLS = SL2*F2 + SL3*F3 + SL4*SINZF
|
||||||
sis = si2 * f2 + si3 * f3
|
SGHS = SGH2*F2 + SGH3*F3 + SGH4*SINZF
|
||||||
sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf
|
SHS = SH2*F2 + SH3*F3
|
||||||
sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf
|
ZM = ZMOL + ZNL*T
|
||||||
shs = sh2 * f2 + sh3 * f3
|
ZF = ZM + 2. * ZEL*SIN(ZM)
|
||||||
zm = zmol + znl * t
|
SINZF = SIN(ZF)
|
||||||
zf = zm + 2. * zel * sin(zm)
|
F2 = .5 * SINZF*SINZF - .25
|
||||||
sinzf = sin(zf)
|
F3 = -.5 * SINZF*COS(ZF)
|
||||||
f2 = .5 * sinzf * sinzf - .25
|
SEL = EE2*F2 + E3*F3
|
||||||
f3 = -.5 * sinzf * cos(zf)
|
SIL = XI2*F2 + XI3*F3
|
||||||
sel = ee2 * f2 + e3 * f3
|
SLL = XL2*F2 + XL3*F3 + XL4*SINZF
|
||||||
sil = xi2 * f2 + xi3 * f3
|
SGHL = XGH2*F2 + XGH3*F3 + XGH4*SINZF
|
||||||
sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf
|
SHL = XH2*F2 + XH3*F3
|
||||||
sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf
|
PE = SES + SEL
|
||||||
shl = xh2 * f2 + xh3 * f3
|
PINC = SIS + SIL
|
||||||
pe = ses + sel
|
PL = SLS + SLL
|
||||||
pinc = sis + sil
|
210 PGH = SGHS + SGHL
|
||||||
pl = sls + sll
|
PH = SHS + SHL
|
||||||
210 pgh = sghs + sghl
|
XINC = XINC + PINC
|
||||||
ph = shs + shl
|
EM = EM + PE
|
||||||
xinc = xinc + pinc
|
IF(XQNCL.LT.(.2)) GO TO 220
|
||||||
em = em + pe
|
GO TO 218
|
||||||
if (xqncl.lt.(.2)) go to 220
|
C
|
||||||
go to 218
|
C APPLY PERIODICS DIRECTLY
|
||||||
c
|
C
|
||||||
c apply periodics directly
|
218 PH = PH / SINIQ
|
||||||
c
|
PGH = PGH - COSIQ*PH
|
||||||
218 ph = ph / siniq
|
OMGASM = OMGASM + PGH
|
||||||
pgh = pgh - cosiq * ph
|
XNODES = XNODES + PH
|
||||||
omgasm = omgasm + pgh
|
XLL = XLL + PL
|
||||||
xnodes = xnodes + ph
|
GO TO 230
|
||||||
xll = xll + pl
|
C
|
||||||
go to 230
|
C APPLY PERIODICS WITH LYDDANE MODIFICATION
|
||||||
c
|
C
|
||||||
c apply periodics with lyddane modification
|
220 SINOK = SIN(XNODES)
|
||||||
c
|
67
|
||||||
220 sinok = sin(xnodes)
|
COSOK = COS(XNODES)
|
||||||
67
|
ALFDP = SINIS*SINOK
|
||||||
cosok = cos(xnodes)
|
BETDP = SINIS*COSOK
|
||||||
alfdp = sinis * sinok
|
DALF = PH*COSOK + PINC*COSIS*SINOK
|
||||||
betdp = sinis * cosok
|
DBET = -PH*SINOK + PINC*COSIS*COSOK
|
||||||
dalf = ph * cosok + pinc * cosis * sinok
|
ALFDP = ALFDP + DALF
|
||||||
dbet = -ph * sinok + pinc * cosis * cosok
|
BETDP = BETDP + DBET
|
||||||
alfdp = alfdp + dalf
|
XLS = XLL + OMGASM + COSIS*XNODES
|
||||||
betdp = betdp + dbet
|
DLS = PL + PGH - PINC*XNODES*SINIS
|
||||||
xls = xll + omgasm + cosis * xnodes
|
XLS = XLS + DLS
|
||||||
dls = pl + pgh - pinc * xnodes * sinis
|
XNODES = ACTAN(ALFDP, BETDP)
|
||||||
xls = xls + dls
|
XLL = XLL + PL
|
||||||
xnodes = actan(alfdp, betdp)
|
OMGASM = XLS - XLL - COS(XINC) * XNODES
|
||||||
xll = xll + pl
|
230 CONTINUE
|
||||||
omgasm = xls - xll - cos(xinc) * xnodes
|
RETURN
|
||||||
230 continue
|
END
|
||||||
}
|
|
||||||
#endif
|
#endif
|
||||||
Loading…
Reference in New Issue