sgp 1.0.4

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
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330

// 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 `sgp8` routine.
/// Returns (position_km, velocity_km_per_min)
pub fn sgp8(tsince: f64, satdata: &SatData) -> (Vector3, Vector3) {
    let ae: f64 = 1.0;
    let xmnpda: f64 = 1440.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 rho = 2.461 * 0.00001 * xkmper;
    let s = ae + 78.0 / xkmper;
    let qo = ae + 120.0 / xkmper;
    let xke = ((3600.0 * ge) / (xkmper.powi(3))).sqrt();
    let qoms2t = (qo - s).powi(4);
    let temp2 = xke / (satdata.xno);
    let a1 = temp2.powf(tothrd);
    let cosio = (satdata.xincl).cos();
    let sinio = (satdata.xincl).sin();
    let delta1 = 1.5 * ck2 * (3.0 * cosio * cosio - 1.0)
        / (a1.powi(2) * (1.0 - (satdata.eo * satdata.eo)).powf(1.5));
    let a0 = a1
        * (1.0
            - (1.0 / 3.0) * delta1
            - delta1 * delta1
            - (134.0 / 81.0) * delta1 * delta1 * delta1);
    let delta0 = 1.5 * ck2 * (3.0 * cosio * cosio - 1.0)
        / (a0.powi(2) * (1.0 - (satdata.eo * satdata.eo)).powf(1.5));
    let noii = satdata.xno / (1.0 + delta0);
    let aoii = a0 / (1.0 - delta0);
    // B term
    let b = 2.0 * (satdata.bstar) / rho;
    // constants II
    let beta = (1.0 - (satdata.eo * satdata.eo)).sqrt();
    let theta = cosio;
    let temp = (noii * ck2) / (aoii.powi(2) * beta * beta * beta);
    let m1dot = -1.5 * temp * (1.0 - 3.0 * (theta * theta));
    let omega1dot = -1.5 * (temp / beta) * (1.0 - 5.0 * (theta * theta));
    let xnode1dot = -3.0 * (temp / beta) * theta;
    let m2dot = (3.0 / 16.0)
        * (temp * ck2 / (aoii.powi(2) * beta.powi(4)))
        * (13.0 - 78.0 * (theta * theta) + 137.0 * (theta.powi(4)));
    let omega2dot = (3.0 / 16.0)
        * (temp * ck2 / (aoii.powi(2) * beta.powi(5)))
        * (7.0 - 114.0 * (theta * theta) + 395.0 * (theta.powi(4)))
        + 1.25 * (noii * ck4) / (aoii.powi(4) * beta.powi(8))
            * (3.0 - 36.0 * (theta * theta) + 49.0 * (theta.powi(4)));
    let xnode2dot =
        1.5 * (temp * ck2 / (aoii.powi(2) * beta.powi(5))) * theta * (4.0 - 19.0 * (theta * theta))
            + 2.5 * (noii * ck4) / (aoii.powi(4) * beta.powi(8))
                * theta
                * (3.0 - 7.0 * (theta * theta));
    let ldot = noii + m1dot + m2dot;
    let omegadot = omega1dot + omega2dot;
    let xnodedot = xnode1dot + xnode2dot;
    let tsi = 1.0 / (aoii * (beta * beta) - s);
    let eta = satdata.eo * s * tsi;
    let phi = (1.0 - (eta * eta)).sqrt();
    let alpha = (1.0 + (satdata.eo * satdata.eo)).sqrt();
    let c0 = 0.5 * b * rho * qoms2t * noii * aoii * tsi.powf(4.0) / (alpha * phi.powf(7.0));
    let c1 = 1.5 * noii * alpha.powf(4.0) * c0;
    let d1 = tsi * (1.0 / (phi * phi)) / (aoii * (beta * beta));
    let d2 = 12.0 + 36.0 * (eta * eta) + 4.5 * eta.powi(4);
    let d3 = 15.0 * (eta * eta) + 2.5 * eta.powi(4);
    let d4 = 5.0 * eta + (15.0 / 4.0) * eta.powi(3);
    let d5 = tsi / (phi * phi);
    let b1 = -ck2 * (1.0 - 3.0 * (theta * theta));
    let b2 = -ck2 * (1.0 - (theta * theta));
    let a30 = -xj3 * (ae.powi(3)) / ck2;
    let b3 = a30 * sinio;
    let c2 = d1 * d3 * b2;
    let c3 = d4 * d5 * b3;
    let n0dot = c1
        * ((2.0
            + eta.powi(2) * (3.0 + 34.0 * (satdata.eo * satdata.eo))
            + 5.0 * satdata.eo * eta * (4.0 + eta.powi(2))
            + 8.5 * (satdata.eo * satdata.eo))
            + d1 * d2 * b1
            + c2 * (2.0 * (satdata.omegao).cos())
            + c3 * (satdata.omegao).sin());
    let d6 = 30.0 * eta + 22.5 * eta.powi(3);
    let d7 = 5.0 * eta + 12.5 * eta.powi(3);
    let d8 = 1.0 + 6.75 * eta.powi(2) + eta.powi(4);
    let c4 = d1 * d7 * b2;
    let c5 = d5 * d8 * b3;
    let e0dot = -c0
        * (eta * (4.0 + eta.powi(2) + (satdata.eo * satdata.eo) * (15.5 + 7.0 * eta.powi(2)))
            + satdata.eo * (5.0 + 15.0 * eta.powi(2))
            + d1 * d6 * b1
            + c4 * (2.0 * (satdata.omegao).cos())
            + c5 * (satdata.omegao).sin());
    let alphadottoalpha = satdata.eo * e0dot / (alpha * alpha);
    let c6 = (1.0 / 3.0) * (n0dot / noii);
    let tsidottotsi = 2.0 * aoii * tsi * (c6 * (beta * beta) + satdata.eo * e0dot);
    let etadot = (e0dot + satdata.eo * tsidottotsi) * s * tsi;
    let phidottophi = -eta * etadot / (phi * phi);
    let c0dot_to_c0 = c6 + 4.0 * tsidottotsi - alphadottoalpha - 7.0 * phidottophi;
    let c1dot_to_c1 = n0dot / noii + 4.0 * alphadottoalpha + c0dot_to_c0;
    let d9 = 6.0 * eta
        + 20.0 * satdata.eo
        + 15.0 * satdata.eo * eta.powi(2)
        + 68.0 * (satdata.eo * satdata.eo) * eta;
    let d10 = 20.0 * eta + 5.0 * eta.powi(3) + 17.0 * satdata.eo + 68.0 * satdata.eo * eta.powi(2);
    let d11 = eta * (72.0 + 18.0 * eta.powi(2));
    let d12 = eta * (30.0 + 10.0 * eta.powi(2));
    let d13 = 5.0 + 11.25 * eta.powi(2);
    let d14 = tsidottotsi - 2.0 * phidottophi;
    let d15 = 2.0 * (c6 + satdata.eo * e0dot / (beta * beta));
    let d1dot = d1 * (d14 + d15);
    let d2dot = etadot * d11;
    let d3dot = etadot * d12;
    let d4dot = etadot * d13;
    let d5dot = d5 * d14;
    let c2dot = b2 * (d1dot * d3 + d1 * d3dot);
    let c3dot = b3 * (d5dot * d4 + d5 * d4dot);
    let d16 = d9 * etadot
        + d10 * e0dot
        + b1 * (d1dot * d2 + d1 * d2dot)
        + c2dot * (2.0 * (satdata.omegao).cos())
        + c3dot * (satdata.omegao).sin()
        + omega1dot * (c3 * (satdata.omegao).cos() - 2.0 * c2 * (2.0 * (satdata.omegao).sin()));
    let n0ddot = n0dot * c1dot_to_c1 + c1 * d16;
    let e0ddot = e0dot * c0dot_to_c0
        - c0 * ((4.0
            + 3.0 * eta.powi(2)
            + 30.0 * satdata.eo * eta
            + 15.5 * (satdata.eo * satdata.eo)
            + 21.0 * eta.powi(2) * (satdata.eo * satdata.eo))
            * etadot
            + (5.0
                + 15.0 * eta.powi(2)
                + 31.0 * satdata.eo * eta
                + 14.0 * satdata.eo * eta.powi(3))
                * e0dot
            + b1 * (d1dot * d6 + d1 * etadot * (30.0 + 67.5 * eta.powi(2)))
            + b2 * (d1dot * d7 + d1 * etadot * (5.0 + 37.5 * eta.powi(2)))
                * (2.0 * (satdata.omegao).cos())
            + b3 * (d5dot * d8 + d5 * eta * etadot * (13.5 + 4.0 * eta.powi(2)))
                * (satdata.omegao).sin()
            + omega1dot
                * (c5 * (satdata.omegao).cos() - 2.0 * c4 * (2.0 * (satdata.omegao).sin())));
    let d17 = n0ddot / noii - (n0dot / noii).powi(2);
    let tsiddottotsi = 2.0 * (tsidottotsi - c6) * tsidottotsi
        + 2.0
            * aoii
            * tsi
            * ((1.0 / 3.0) * d17 * (beta * beta) - 2.0 * c6 * satdata.eo * e0dot
                + (e0dot * e0dot)
                + satdata.eo * e0ddot);
    let etaddot = (e0ddot + 2.0 * e0dot * tsidottotsi) * s * tsi + eta * tsiddottotsi;
    let d18 = tsiddottotsi - tsidottotsi.powi(2);
    let d19 =
        -(phidottophi * phidottophi) * (1.0 + 1.0 / (eta * eta)) - eta * etaddot / (phi * phi);
    let d1ddot = d1dot * (d14 + d15)
        + d1 * (d18 - 2.0 * d19
            + (2.0 / 3.0) * d17
            + 2.0 * alpha.powi(2) * (e0dot * e0dot) / beta.powi(4)
            + 2.0 * satdata.eo * e0ddot / (beta * beta));
    let n0tdot = n0dot
        * (2.0 * (2.0 / 3.0) * d17
            + 3.0 * ((e0dot * e0dot) + satdata.eo * e0ddot) / (alpha * alpha)
            - 6.0 * (satdata.eo * e0dot / (alpha * alpha)).powi(2)
            + 4.0 * d18
            - 7.0 * d19)
        + c1dot_to_c1 * n0ddot;
    let n0tdot = n0tdot
        + c1 * (c1dot_to_c1 * d16
            + d9 * etaddot
            + d10 * e0ddot
            + (etadot * etadot)
                * (6.0 + 30.0 * satdata.eo * eta + 68.0 * (satdata.eo * satdata.eo))
            + etadot * e0dot * (40.0 + 30.0 * eta.powi(2) + 272.0 * satdata.eo * eta)
            + (e0dot * e0dot) * (17.0 + 68.0 * eta.powi(2))
            + b1 * (d1ddot * d2
                + 2.0 * d1dot * d2dot
                + d1 * (etaddot * d11 + (etadot * etadot) * (72.0 + 54.0 * eta.powi(2))))
            + b2 * (d1ddot * d3
                + 2.0 * d1dot * d3dot
                + d1 * (etaddot * d12 + (etadot * etadot) * (30.0 + 30.0 * eta.powi(2))))
                * (2.0 * satdata.omegao.cos())
            + b3 * ((d5dot * d14 + d5 * (d18 - 2.0 * d19)) * d4
                + 2.0 * d4dot * d5dot
                + d5 * (etaddot * d13 + 22.5 * eta * (etadot * etadot)))
                * satdata.omegao.sin()
            + omega1dot
                * ((7.0 * (1.0 / 3.0) * (n0dot / noii)
                    + 4.0 * satdata.eo * e0dot / (beta * beta))
                    * (c3 * satdata.omegao.cos() - 2.0 * c2 * (2.0 * satdata.omegao.sin()))
                    + (2.0 * c3dot * satdata.omegao.cos()
                        - 4.0 * c2dot * (2.0 * satdata.omegao.sin())
                        - omega1dot
                            * (c3 * satdata.omegao.sin() + 4.0 * c2 * satdata.omegao.cos()))));
    let smalln0ddot = n0ddot * 1.0e9;
    let temp = (smalln0ddot * smalln0ddot) - n0dot * 1.0e18 * n0tdot;
    let p = (temp + (smalln0ddot * smalln0ddot)) / temp;
    let gamma = -n0tdot / (n0ddot * (p - 2.0));
    let nd = n0dot / (p * gamma);
    let q = 1.0 - e0ddot / (e0dot * gamma);
    let ed = e0dot / (q * gamma);
    // atmospheric drag and gravitation
    let (n, _edot, e, z1) = if (n0dot / noii * xmnpda).abs() < 0.00216 {
        let n = noii + n0dot * tsince;
        let edot = -(2.0 / 3.0) * n0dot * (1.0 - satdata.eo) / noii;
        let e = satdata.eo + edot * tsince;
        let z1 = 0.5 * n0dot * tsince.powi(2);
        (n, edot, e, z1)
    } else {
        let n = noii + nd * (1.0 - (1.0 - gamma * tsince).powf(p));
        let edot = e0dot;
        let e = satdata.eo + ed * (1.0 - (1.0 - gamma * tsince).powf(q));
        let mut z1 = gamma * tsince;
        z1 = 1.0 - z1;
        z1 = z1.powf(p + 1.0);
        z1 -= 1.0;
        z1 /= gamma * (p + 1.0);
        z1 += tsince;
        z1 = z1 * n0dot / (p * gamma);
        (n, edot, e, z1)
    };
    let omega = satdata.omegao + omegadot * tsince + (7.0 * z1) / (3.0 * noii) * omega1dot;
    let xnode = satdata.xnodeo + xnodedot * tsince + xnode1dot * (7.0 * z1) / (3.0 * noii);
    let mut m = satdata.xmo + tsince * ldot + z1 + m1dot * (7.0 * z1) / (3.0 * noii);
    // Kepler
    m = fmod2p(m);
    let mut ew = m + satdata.eo * m.sin() * (1.0 + e * m.cos());
    for _iter in 0..10 {
        let delta = (m + e * ew.sin() - ew) / (1.0 - e * ew.cos());
        ew += delta;
        if delta.abs() <= e6a {
            break;
        }
    }
    // short-period periodics
    let a = (xke / n).powf(tothrd);
    let beta2 = (1.0 - e * e).sqrt();
    let sinf = beta2 * ew.sin() / (1.0 - e * ew.cos());
    let cosf = (ew.cos() - e) / (1.0 - e * ew.cos());
    let f = actan(sinf, cosf);
    let sinu = sinf * omega.cos() + cosf * omega.sin();
    let cosu = cosf * omega.cos() - sinf * omega.sin();
    let rii = a * (1.0 - e * e) / (1.0 + e * cosf);
    let delta_r = (0.5 * ck2 * (1.0 / (a * (1.0 - e * e))))
        * ((1.0 - (theta * theta)) * (2.0 * (cosu * cosu) - 1.0)
            - 3.0 * (3.0 * (theta * theta) - 1.0))
        - (0.25 * a30 * sinio) * sinu;
    let temp2 = 3.0
        * (0.5 * ck2 * (1.0 / (a * (1.0 - e * e))).powi(2))
        * sinio
        * (2.0 * (cosu * cosu) - 1.0)
        - (0.25 * a30 * (1.0 / (a * (1.0 - e * e)))) * e * omega.sin();
    let delta_i = temp2 * cosio;
    let delta_u = (satdata.xincl / 2.0).sin()
        * ((0.5 * ck2 * (1.0 / (a * (1.0 - e * e))).powi(2))
            * (0.5 * (1.0 - 7.0 * (theta * theta)) * (2.0 * sinu * cosu)
                - 3.0 * (1.0 - 5.0 * (theta * theta)) * (f - m + e * sinf))
            - (0.25 * a30 * (1.0 / (a * (1.0 - e * e)))) * sinio * cosu * (2.0 + e * cosf))
        - 0.5 * (0.25 * a30 * (1.0 / (a * (1.0 - e * e)))) * (theta * theta) * e * omega.cos()
            / (satdata.xincl / 2.0).cos();
    let lambda = f
        + omega
        + xnode
        + (0.5 * ck2 * (1.0 / (a * (1.0 - e * e))).powi(2))
            * (0.5 * (1.0 + 6.0 * cosio - 7.0 * (theta * theta)) * (2.0 * sinu * cosu)
                - 3.0 * ((1.0 - 5.0 * (theta * theta)) + 2.0 * cosio) * (f - m + e * sinf))
        + (0.25 * a30 * (1.0 / (a * (1.0 - e * e))))
            * sinio
            * (cosio * e * omega.cos() / (1.0 + cosio) - (2.0 + e * cosf) * cosu);
    let y4 = (satdata.xincl / 2.0).sin() * sinu
        + cosu * delta_u
        + 0.5 * sinu * (satdata.xincl / 2.0).cos() * delta_i;
    let y5 = (satdata.xincl / 2.0).sin() * cosu - sinu * delta_u
        + 0.5 * cosu * (satdata.xincl / 2.0).cos() * delta_i;
    let r = rii + delta_r;
    let rdot = n * a * e * sinf / beta2
        + (-n * (a / rii).powi(2))
            * (1.0
                * ck2
                * (1.0 / (a * (1.0 - e * e)))
                * (1.0 - (theta * theta))
                * (2.0 * sinu * cosu)
                + (0.25 * a30 * sinio) * cosu);
    let rfdot = n * (a.powi(2)) * beta2 / rii
        + (-n * (a / rii).powi(2)) * delta_r
        + a * (n * (a / rii)) * sinio * temp2;
    // Unit-orientation vector
    let cos_i2 = (1.0 - (y4 * y4) - (y5 * y5)).sqrt();
    let uv1 = 2.0 * y4 * (y5 * lambda.sin() - y4 * lambda.cos()) + lambda.cos();
    let uv2 = -2.0 * y4 * (y5 * lambda.cos() + y4 * lambda.sin()) + lambda.sin();
    let uv3 = 2.0 * y4 * cos_i2;
    let vv1 = 2.0 * y5 * (y5 * lambda.sin() - y4 * lambda.cos()) - lambda.sin();
    let vv2 = -2.0 * y5 * (y5 * lambda.cos() + y4 * lambda.sin()) + lambda.cos();
    let vv3 = 2.0 * y5 * cos_i2;
    // position + velocity (in normalized units)
    let pos_x = r * uv1;
    let pos_y = r * uv2;
    let pos_z = r * uv3;
    let vel_x = rdot * uv1 + rfdot * vv1;
    let vel_y = rdot * uv2 + rfdot * vv2;
    let vel_z = rdot * uv3 + rfdot * vv3;

    // Convert_Sat_State: scale to km and km/min
    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)
}