sgp 1.0.0

Simplified General Perturbations models (SGP8/SGP4) in Rust.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232

// Copyright (c) [2025] [Qin Chen <qchen2015@hotmail.com>]
// [SGP] is licensed under Mulan PSL v2.
// You can use this software according to the terms and conditions of the Mulan PSL v2.
// You may obtain a copy of Mulan PSL v2 at:
//          http://license.coscl.org.cn/MulanPSL2
// THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
// EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
// MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
// See the Mulan PSL v2 for more details.

use crate::sgp_common::{actan, fmod2p, SatData, Vector3};

/// Port of the MATLAB `sgp4` routine (refs/SGP4/sgp4.m).
///
/// Inputs:
/// - `tsince`: minutes since epoch
/// - `satdata`: satellite orbital parameters
///
/// Returns (position_km, velocity_km_per_s)
pub fn sgp4(tsince: f64, satdata: &SatData) -> (Vector3, Vector3) {
    let ae: f64 = 1.0;
    let tothrd: f64 = 2.0 / 3.0;
    let xj3: f64 = -2.53881e-6;
    let e6a: f64 = 1.0E-6;
    let xkmper: f64 = 6378.135; // Earth equatorial radius [km]
    let ge: f64 = 398600.8; // Earth gravitational constant [km^3/s^2]
    let ck2: f64 = 1.0826158e-3 / 2.0;
    let ck4: f64 = -3.0 * -1.65597e-6 / 8.0;

    // Constants
    let s: f64 = ae + 78.0 / xkmper;
    let qo: f64 = ae + 120.0 / xkmper;
    let xke: f64 = ((3600.0_f64 * ge) / (xkmper.powi(3))).sqrt();
    let qoms2t: f64 = (qo - s).powi(4);
    let temp2: f64 = xke / (satdata.xno);
    let a1: f64 = temp2.powf(tothrd);
    let cosio: f64 = (satdata.xincl).cos();
    let theta2: f64 = cosio * cosio;
    let x3thm1 = 3.0 * theta2 - 1.0;
    let eosq: f64 = satdata.eo * satdata.eo;
    let betao2: f64 = 1.0 - eosq;
    let betao: f64 = betao2.sqrt();
    let del1: f64 = 1.5 * ck2 * x3thm1 / ((a1 * a1) * betao * betao2);
    let ao: f64 = a1 * (1.0 - del1 * ((1.0 / 3.0) + del1 * (1.0 + (134.0 / 81.0) * del1)));
    let delo: f64 = 1.5 * ck2 * x3thm1 / ((ao * ao) * betao * betao2);
    let xnodp: f64 = (satdata.xno) / (1.0 + delo);
    let aodp: f64 = ao / (1.0 - delo);
    // Initialization
    let mut isimp = 0;
    if aodp * (1.0 - satdata.eo) / ae < 220.0 / xkmper + ae {
        isimp = 1;
    }
    // For perigee below 156 km, the values of s and qoms2t are altered.
    let mut s4: f64 = s;
    let mut qoms24: f64 = qoms2t;
    let perige = (aodp * (1.0 - satdata.eo) - ae) * xkmper;
    if perige < 156.0 {
        s4 = perige - 78.0;
        if perige <= 98.0 {
            s4 = 20.0;
        }
    qoms24 = ((120.0 - s4) * ae / xkmper).powi(4);
        s4 = s4 / xkmper + ae;
    }
    let pinvsq: f64 = 1.0 / ((aodp * aodp) * (betao2 * betao2));
    let tsi: f64 = 1.0 / (aodp - s4);
    let eta: f64 = aodp * (satdata.eo) * tsi;
    let etasq: f64 = eta * eta;
    let eeta: f64 = (satdata.eo) * eta;
    let psisq: f64 = (1.0 - etasq).abs();
    let coef: f64 = qoms24 * tsi.powi(4);
    let coef1: f64 = coef / psisq.powf(3.5);
    let c2: f64 = coef1 * xnodp * (aodp * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq))
        + 0.75 * ck2 * tsi / psisq * x3thm1 * (8.0 + 3.0 * etasq * (8.0 + etasq)));
    let c1: f64 = (satdata.bstar) * c2;
    let sinio: f64 = (satdata.xincl).sin();
    let a3ovk2: f64 = -xj3 / ck2 * (ae.powi(3));
    let c3: f64 = coef * tsi * a3ovk2 * xnodp * ae * sinio / (satdata.eo.max(1e-12));
    let x1mth2: f64 = 1.0 - theta2;
    let c4: f64 = 2.0 * xnodp * coef1 * aodp * betao2 * (eta * (2.0 + 0.5 * etasq)
        + (satdata.eo) * (0.5 + 2.0 * etasq) - 2.0 * ck2 * tsi / (aodp * psisq)
        * (-3.0 * x3thm1 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta))
            + 0.75 * x1mth2 * (2.0 * etasq - eeta * (1.0 + etasq)) * (satdata.omegao * 2.0).cos()));
    let c5: f64 = 2.0 * coef1 * aodp * betao2 * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq);
    let theta4: f64 = theta2 * theta2;
    let temp1: f64 = 3.0 * ck2 * pinvsq * xnodp;
    let temp2b: f64 = temp1 * ck2 * pinvsq;
    let temp3: f64 = 1.25 * ck4 * pinvsq * pinvsq * xnodp;
    let xmdot: f64 = xnodp + 0.5 * temp1 * betao * x3thm1 + 0.0625 * temp2b * betao * (13.0 - 78.0 * theta2 + 137.0 * theta4);
    let x1m5th: f64 = 1.0 - 5.0 * theta2;
    let omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2b * (7.0 - 114.0 * theta2 + 395.0 * theta4) + temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4);
    let xhdot1 = -temp1 * cosio;
    let xnodot = xhdot1 + (0.5 * temp2b * (4.0 - 19.0 * theta2) + 2.0 * temp3 * (3.0 - 7.0 * theta2)) * cosio;
    let omgcof = (satdata.bstar) * c3 * (satdata.omegao).cos();
    let xmcof = -(2.0 / 3.0) * coef * (satdata.bstar) * ae / (eeta.max(1e-12));
    let xnodcf = 3.5 * betao2 * xhdot1 * c1;
    let t2cof = 1.5 * c1;
    let xlcof = 0.125 * a3ovk2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio);
    let aycof = 0.25 * a3ovk2 * sinio;
    let delmo = (1.0 + eta * (satdata.xmo).cos()).powi(3);
    let sinmo = (satdata.xmo).sin();
    let x7thm1 = 7.0 * theta2 - 1.0;
    // additional terms
    let (_c1sq, d2, _tempd, d3, d4, t3cof, t4cof, t5cof) = if isimp == 0 {
        let c1sq = c1 * c1;
        let d2 = 4.0 * aodp * tsi * c1sq;
        let tempd = d2 * tsi * c1 / 3.0;
        let d3 = (17.0 * aodp + s4) * tempd;
        let d4 = 0.5 * tempd * aodp * tsi * (221.0 * aodp + 31.0 * s4) * c1;
        let t3cof = d2 + 2.0 * c1sq;
        let t4cof = 0.25 * (3.0 * d3 + c1 * (12.0 * d2 + 10.0 * c1sq));
        let t5cof = 0.2 * (3.0 * d4 + 12.0 * c1 * d3 + 6.0 * d2 * d2 + 15.0 * c1sq * (2.0 * d2 + c1sq));
        (c1sq, d2, tempd, d3, d4, t3cof, t4cof, t5cof)
    } else {
        (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
    };
    // Update for secular gravity and atmospheric drag.
    let xmdf = satdata.xmo + xmdot * tsince;
    let omgadf = satdata.omegao + omgdot * tsince;
    let xnoddf = satdata.xnodeo + xnodot * tsince;
    let mut omega = omgadf;
    let mut xmp = xmdf;
    let tsq = tsince * tsince;
    let xnode = xnoddf + xnodcf * tsq;
    let mut tempa = 1.0 - c1 * tsince;
    let mut tempe = (satdata.bstar) * c4 * tsince;
    let mut templ = t2cof * tsq;
    if isimp == 0 {
        let delomg = omgcof * tsince;
        let delm = xmcof * (((1.0 + eta * xmdf.cos()).powf(3.0)) - delmo);
        let temp = delomg + delm;
        xmp = xmdf + temp;
        omega = omgadf - temp;
        let tcube = tsq * tsince;
        let tfour = tsince * tcube;
        tempa = tempa - d2 * tsq - d3 * tcube - d4 * tfour;
    tempe += (satdata.bstar) * c5 * ((xmp).sin() - sinmo);
        templ = templ + t3cof * tcube + tfour * (t4cof + tsince * t5cof);
    }
    let a = aodp * (tempa * tempa);
    let e = (satdata.eo) - tempe;
    let xl = xmp + omega + xnode + xnodp * templ;
    let beta = (1.0_f64 - (e * e)).sqrt();
    let xn = xke / a.powf(tothrd);
    // Long period periodics
    let axn = e * omega.cos();
    let temp = 1.0 / (a * (beta * beta));
    let xll = temp * xlcof * axn;
    let aynl = temp * aycof;
    let xlt = xl + xll;
    let ayn = e * omega.sin() + aynl;
    // Solve Kepler's Equation
    let capu = fmod2p(xlt - xnode);
    let temp2w = capu;
    let mut epw = temp2w;
    for _i in 0..10 {
        let sinepw = epw.sin();
        let cosepw = epw.cos();
        let temp3 = axn * sinepw;
        let temp4 = ayn * cosepw;
        let temp5 = axn * cosepw;
        let temp6 = ayn * sinepw;
        let new_epw = (capu - temp4 + temp3 - epw) / (1.0 - temp5 - temp6) + epw;
        let delta = new_epw - epw;
        epw = new_epw;
        if delta.abs() <= e6a {
            break;
        }
    }
    // Short period preliminary quantities
    let ecose = axn * epw.cos() + ayn * epw.sin();
    let esine = axn * epw.sin() - ayn * epw.cos();
    let elsq = (axn * axn) + (ayn * ayn);
    let temp = 1.0 - elsq;
    let pl = a * temp;
    let r = a * (1.0 - ecose);
    let temp1 = 1.0 / r;
    let rdot = xke * a.sqrt() * esine * temp1;
    let rfdot = xke * pl.sqrt() * temp1;
    let temp2c = a * temp1;
    let betal = temp.sqrt();
    let temp3c = 1.0 / (1.0 + betal);
    let cosu = temp2c * (epw.cos() - axn + ayn * esine * temp3c);
    let sinu = temp2c * (epw.sin() - ayn - axn * esine * temp3c);
    let u = actan(sinu, cosu);
    let sin2u = 2.0 * sinu * cosu;
    let cos2u = 2.0 * (cosu * cosu) - 1.0;
    let temp4c = 1.0 / pl;
    let temp1c = ck2 * temp4c;
    let temp2d = temp1c * temp4c;
    // Update for short periodics
    let rk = r * (1.0 - 1.5 * temp2d * betal * x3thm1) + 0.5 * temp1c * x1mth2 * cos2u;
    let uk = u - 0.25 * temp2d * x7thm1 * sin2u;
    let xnodek = xnode + 1.5 * temp2d * cosio * sin2u;
    let xinck = satdata.xincl + 1.5 * temp2d * cosio * sinio * cos2u;
    let rdotk = rdot - xn * temp1c * x1mth2 * sin2u;
    let rfdotk = rfdot + xn * temp1c * (x1mth2 * cos2u + 1.5 * x3thm1);
    // Orientation vectors
    let mv1 = -xnodek.sin() * xinck.cos();
    let mv2 = xnodek.cos() * xinck.cos();
    let mv3 = xinck.sin();
    let nv1 = xnodek.cos();
    let nv2 = xnodek.sin();
    let nv3 = 0.0;
    let uv1 = mv1 * uk.sin() + nv1 * uk.cos();
    let uv2 = mv2 * uk.sin() + nv2 * uk.cos();
    let uv3 = mv3 * uk.sin() + nv3 * uk.cos();
    let vv1 = mv1 * uk.cos() - nv1 * uk.sin();
    let vv2 = mv2 * uk.cos() - nv2 * uk.sin();
    let vv3 = mv3 * uk.cos() - nv3 * uk.sin();
    // position + velocity
    let pos_x = rk * uv1;
    let pos_y = rk * uv2;
    let pos_z = rk * uv3;
    let vel_x = rdotk * uv1 + rfdotk * vv1;
    let vel_y = rdotk * uv2 + rfdotk * vv2;
    let vel_z = rdotk * uv3 + rfdotk * vv3;

    // Convert_Sat_State: scale to km and km/s
    let pos = Vector3 {
        x: pos_x * xkmper,
        y: pos_y * xkmper,
        z: pos_z * xkmper,
    };
    let vel = Vector3 {
        x: vel_x * xkmper / 60.0,
        y: vel_y * xkmper / 60.0,
        z: vel_z * xkmper / 60.0,
    };

    (pos, vel)
}