sgp4/deep_space.rs
1use crate::gp;
2use crate::model;
3use crate::propagator;
4use crate::third_body;
5use core::cmp::Ordering;
6
7#[cfg(not(feature = "std"))]
8use num_traits::Float;
9
10#[cfg(not(feature = "std"))]
11use num_traits::Euclid;
12
13// θ̇ = 4.37526908801129966 × 10⁻³ rad.min⁻¹
14#[allow(clippy::excessive_precision)]
15const SIDEREAL_SPEED: f64 = 4.37526908801129966e-3;
16
17// eₛ = 0.01675
18const SOLAR_ECCENTRICITY: f64 = 0.01675;
19
20// eₗ = 0.05490
21const LUNAR_ECCENTRICITY: f64 = 0.05490;
22
23// nₛ = 1.19459 × 10⁻⁵ rad.min⁻¹
24const SOLAR_MEAN_MOTION: f64 = 1.19459e-5;
25
26// nₗ = 1.5835218 × 10⁻⁴ rad.min⁻¹
27const LUNAR_MEAN_MOTION: f64 = 1.5835218e-4;
28
29// Cₛ = 2.9864797 × 10⁻⁶ rad.min⁻¹
30const SOLAR_PERTURBATION_COEFFICIENT: f64 = 2.9864797e-6;
31
32// Cₗ = 4.7968065 × 10⁻⁷ rad.min⁻¹
33const LUNAR_PERTURBATION_COEFFICIENT: f64 = 4.7968065e-7;
34
35// |Δt| = 720 min
36const DELTA_T: f64 = 720.0;
37
38// λ₃₁ = 0.13130908
39const LAMBDA31: f64 = 0.13130908;
40
41// λ₂₂ = 2.8843198
42const LAMBDA22: f64 = 2.8843198;
43
44// λ₃₃ = 0.37448087
45const LAMBDA33: f64 = 0.37448087;
46
47// G₂₂ = 5.7686396
48const G22: f64 = 5.7686396;
49
50// G₃₂ = 0.95240898
51const G32: f64 = 0.95240898;
52
53// G₄₄ = 1.8014998
54const G44: f64 = 1.8014998;
55
56// G₅₂ = 1.0508330
57const G52: f64 = 1.0508330;
58
59// G₅₄ = 4.4108898
60const G54: f64 = 4.4108898;
61
62/// Represents the state of the deep space resonnance integrator
63///
64/// Use [Constants::initial_state](struct.Constants.html#method.initial_state) to initialize a resonance state.
65#[derive(Copy, Clone)]
66pub struct ResonanceState {
67 t: f64,
68 mean_motion: f64,
69 lambda: f64,
70}
71
72impl ResonanceState {
73 pub(crate) fn new(mean_motion_0: f64, lambda_0: f64) -> ResonanceState {
74 ResonanceState {
75 t: 0.0,
76 mean_motion: mean_motion_0,
77 lambda: lambda_0,
78 }
79 }
80
81 /// Returns the integrator's time in minutes since epoch
82 ///
83 /// The integrator time changes monotonically in Δt = 720 min increments
84 /// or Δt = -720 min decrements, depending on the propagation time sign.
85 pub fn t(&self) -> f64 {
86 self.t
87 }
88
89 #[allow(clippy::too_many_arguments)]
90 fn integrate(
91 &mut self,
92 geopotential: &model::Geopotential,
93 argument_of_perigee_0: f64,
94 lambda_dot_0: f64,
95 resonance: &propagator::Resonance,
96 sidereal_time_0: f64,
97 t: f64,
98 p22: f64,
99 p23: f64,
100 ) -> (f64, f64) {
101 if (self.t != 0.0 && self.t.is_sign_positive() != t.is_sign_positive())
102 || t.abs() < self.t.abs()
103 {
104 panic!("the resonance integration state must be manually reset if the target times are non-monotonic");
105 }
106 // θ = θ₀ + 4.37526908801129966 × 10⁻³ t rem 2π
107 #[allow(clippy::excessive_precision)]
108 let sidereal_time =
109 (sidereal_time_0 + t * 4.37526908801129966e-3) % (2.0 * core::f64::consts::PI);
110 let (delta_t, ordering) = if t > 0.0 {
111 (DELTA_T, Ordering::Less)
112 } else {
113 (-DELTA_T, Ordering::Greater)
114 };
115 loop {
116 // λ̇ᵢ = nᵢ + λ̇₀
117 let lambda_dot = self.mean_motion + lambda_dot_0;
118 let (ni_dot, ni_ddot) = match resonance {
119 propagator::Resonance::OneDay { dr1, dr2, dr3 } => (
120 // ṅᵢ = 𝛿ᵣ₁ sin(λᵢ - λ₃₁) + 𝛿ᵣ₂ sin(2 (λᵢ - λ₂₂)) + 𝛿ᵣ₃ sin(3 (λᵢ - λ₃₃))
121 dr1 * (self.lambda - LAMBDA31).sin()
122 + dr2 * (2.0 * (self.lambda - LAMBDA22)).sin()
123 + dr3 * (3.0 * (self.lambda - LAMBDA33)).sin(),
124 // n̈ᵢ = (𝛿ᵣ₁ cos(λᵢ - λ₃₁) + 𝛿ᵣ₂ cos(2 (λᵢ - λ₂₂)) + 𝛿ᵣ₃ cos(3 (λᵢ - λ₃₃))) λ̇ᵢ
125 (dr1 * (self.lambda - LAMBDA31).cos()
126 + 2.0 * dr2 * (2.0 * (self.lambda - LAMBDA22)).cos()
127 + 3.0 * dr3 * (3.0 * (self.lambda - LAMBDA33)).cos())
128 * lambda_dot,
129 ),
130 propagator::Resonance::HalfDay {
131 d2201,
132 d2211,
133 d3210,
134 d3222,
135 d4410,
136 d4422,
137 d5220,
138 d5232,
139 d5421,
140 d5433,
141 k14,
142 } => {
143 // ωᵢ = ω₀ + ω̇ tᵢ
144 let argument_of_perigee_i = argument_of_perigee_0 + k14 * self.t;
145 (
146 // ṅᵢ = Σ₍ₗₘₚₖ₎ Dₗₘₚₖ sin((l - 2 p) ωᵢ + m / 2 λᵢ - Gₗₘ)
147 // (l, m, p, k) ∈ {(2, 2, 0, -1), (2, 2, 1, 1), (3, 2, 1, 0),
148 // (3, 2, 2, 2), (4, 4, 1, 0), (4, 4, 2, 2), (5, 2, 2, 0),
149 // (5, 2, 3, 2), (5, 4, 2, 1), (5, 4, 3, 3)}
150 d2201 * (2.0 * argument_of_perigee_i + self.lambda - G22).sin()
151 + d2211 * (self.lambda - G22).sin()
152 + d3210 * (argument_of_perigee_i + self.lambda - G32).sin()
153 + d3222 * (-argument_of_perigee_i + self.lambda - G32).sin()
154 + d4410 * (2.0 * argument_of_perigee_i + 2.0 * self.lambda - G44).sin()
155 + d4422 * (2.0 * self.lambda - G44).sin()
156 + d5220 * (argument_of_perigee_i + self.lambda - G52).sin()
157 + d5232 * (-argument_of_perigee_i + self.lambda - G52).sin()
158 + d5421 * (argument_of_perigee_i + 2.0 * self.lambda - G54).sin()
159 + d5433 * (-argument_of_perigee_i + 2.0 * self.lambda - G54).sin(),
160 // n̈ᵢ = (Σ₍ₗₘₚₖ₎ m / 2 Dₗₘₚₖ cos((l - 2 p) ωᵢ + m / 2 λᵢ - Gₗₘ)) λ̇ᵢ
161 // (l, m, p, k) ∈ {(2, 2, 0, -1), (2, 2, 1, 1), (3, 2, 1, 0),
162 // (3, 2, 2, 2), (4, 4, 1, 0), (4, 4, 2, 2), (5, 2, 2, 0),
163 // (5, 2, 3, 2), (5, 4, 2, 1), (5, 4, 3, 3)}
164 (d2201 * (2.0 * argument_of_perigee_i + self.lambda - G22).cos()
165 + d2211 * (self.lambda - G22).cos()
166 + d3210 * (argument_of_perigee_i + self.lambda - G32).cos()
167 + d3222 * (-argument_of_perigee_i + self.lambda - G32).cos()
168 + d5220 * (argument_of_perigee_i + self.lambda - G52).cos()
169 + d5232 * (-argument_of_perigee_i + self.lambda - G52).cos()
170 + 2.0
171 * (d4410
172 * (2.0 * argument_of_perigee_i + 2.0 * self.lambda - G44)
173 .cos()
174 + d4422 * (2.0 * self.lambda - G44).cos()
175 + d5421
176 * (argument_of_perigee_i + 2.0 * self.lambda - G54).cos()
177 + d5433
178 * (-argument_of_perigee_i + 2.0 * self.lambda - G54)
179 .cos()))
180 * lambda_dot,
181 )
182 }
183 };
184 if (t - delta_t)
185 .partial_cmp(&self.t)
186 .unwrap_or(Ordering::Equal)
187 == ordering
188 {
189 return (
190 // p₂₈ = (kₑ / (nᵢ + ṅᵢ (t - tᵢ) + ¹/₂ n̈ᵢ (t - tᵢ)²))²ᐟ³
191 (geopotential.ke
192 / (self.mean_motion
193 + ni_dot * (t - self.t)
194 + ni_ddot * (t - self.t).powi(2) * 0.5))
195 .powf(2.0 / 3.0),
196 match resonance {
197 propagator::Resonance::OneDay { .. } => {
198 // p₂₉ = λᵢ + λ̇ᵢ (t - tᵢ) + ¹/₂ ṅᵢ (t - tᵢ)² - p₂₂ - p₂₃ + θ
199 self.lambda
200 + lambda_dot * (t - self.t)
201 + ni_dot * (t - self.t).powi(2) * 0.5
202 - p22
203 - p23
204 + sidereal_time
205 }
206 propagator::Resonance::HalfDay { .. } => {
207 // p₂₉ = λᵢ + λ̇ᵢ (t - tᵢ) + ¹/₂ ṅᵢ (t - tᵢ)² - 2 p₂₂ + 2 θ
208 self.lambda
209 + lambda_dot * (t - self.t)
210 + ni_dot * (t - self.t).powi(2) * 0.5
211 - 2.0 * p22
212 + 2.0 * sidereal_time
213 }
214 },
215 );
216 }
217
218 // tᵢ₊₁ = tᵢ + Δt
219 self.t += delta_t;
220
221 // nᵢ₊₁ = nᵢ + ṅᵢ Δt + n̈ᵢ (Δt² / 2)
222 self.mean_motion += ni_dot * delta_t + ni_ddot * (DELTA_T.powi(2) / 2.0);
223
224 // λᵢ₊₁ = λᵢ + λ̇ᵢ Δt + ṅᵢ (Δt² / 2)
225 self.lambda += lambda_dot * delta_t + ni_dot * (DELTA_T.powi(2) / 2.0);
226 }
227 }
228}
229
230#[allow(clippy::too_many_arguments)]
231pub(crate) fn constants(
232 geopotential: model::Geopotential,
233 epoch_to_sidereal_time: impl Fn(f64) -> f64,
234 epoch: f64,
235 orbit_0: propagator::Orbit,
236 p1: f64,
237 a0: f64,
238 c1: f64,
239 b0: f64,
240 c4: f64,
241 k0: f64,
242 k1: f64,
243 k14: f64,
244 p2: f64,
245 p14: f64,
246 p15: f64,
247) -> propagator::Constants {
248 // d₁₉₀₀ = 365.25 (y₂₀₀₀ + 100)
249 let d1900 = (epoch + 100.0) * 365.25;
250 let (solar_perturbations, solar_dots) = third_body::perturbations_and_dots(
251 orbit_0.inclination,
252 orbit_0.eccentricity,
253 orbit_0.argument_of_perigee,
254 orbit_0.mean_motion,
255 // sin Iₛ = 0.39785416
256 0.39785416,
257 // cos Iₛ = 0.91744867
258 0.91744867,
259 // sin(Ω₀ - Ωₛ) = sin Ω₀
260 orbit_0.right_ascension.sin(),
261 // cos(Ω₀ - Ωₛ) = cos Ω₀
262 orbit_0.right_ascension.cos(),
263 SOLAR_ECCENTRICITY,
264 // sin ωₛ = -0.98088458
265 -0.98088458,
266 // cos ωₛ = 0.1945905
267 0.1945905,
268 SOLAR_PERTURBATION_COEFFICIENT,
269 SOLAR_MEAN_MOTION,
270 // Mₛ₀ = (6.2565837 + 0.017201977 d₁₉₀₀) rem 2π
271 (6.2565837 + 0.017201977 * d1900) % (2.0 * core::f64::consts::PI),
272 p2,
273 b0,
274 );
275
276 // Ωₗₑ = 4.523602 - 9.2422029 × 10⁻⁴ d₁₉₀₀ rem 2π
277 let lunar_right_ascension_epsilon =
278 (4.5236020 - 9.2422029e-4 * d1900) % (2.0 * core::f64::consts::PI);
279
280 // cos Iₗ = 0.91375164 - 0.03568096 Ωₗₑ
281 let lunar_inclination_cosine = 0.91375164 - 0.03568096 * lunar_right_ascension_epsilon.cos();
282
283 // sin Iₗ = (1 - cos²Iₗ)¹ᐟ²
284 let lunar_inclination_sine = (1.0 - lunar_inclination_cosine.powi(2)).sqrt();
285
286 // sin Ωₗ = 0.089683511 sin Ωₗₑ / sin Iₗ
287 let lunar_right_ascension_sine =
288 0.089683511 * lunar_right_ascension_epsilon.sin() / lunar_inclination_sine;
289
290 // cos Ωₗ = (1 - sin²Ωₗ)¹ᐟ²
291 let lunar_right_ascension_cosine = (1.0 - lunar_right_ascension_sine.powi(2)).sqrt();
292
293 // ωₗ = 5.8351514 + 0.001944368 d₁₉₀₀
294 // 0.39785416 sin Ωₗₑ / sin Iₗ
295 // + tan⁻¹ ------------------------------------------ - Ωₗₑ
296 // cos Ωₗ cos Ωₗₑ + 0.91744867 sin Ωₗ sin Ωₗₑ
297 let lunar_argument_of_perigee = 5.8351514
298 + 0.001944368 * d1900
299 + (0.39785416 * lunar_right_ascension_epsilon.sin() / lunar_inclination_sine).atan2(
300 lunar_right_ascension_cosine * lunar_right_ascension_epsilon.cos()
301 + 0.91744867 * lunar_right_ascension_sine * lunar_right_ascension_epsilon.sin(),
302 )
303 - lunar_right_ascension_epsilon;
304 let (lunar_perturbations, lunar_dots) = third_body::perturbations_and_dots(
305 orbit_0.inclination,
306 orbit_0.eccentricity,
307 orbit_0.argument_of_perigee,
308 orbit_0.mean_motion,
309 lunar_inclination_sine,
310 lunar_inclination_cosine,
311 // sin(Ω₀ - Ωₗ) = sin Ω₀ cos Ωₗ - cos Ω₀ sin Ωₗ
312 orbit_0.right_ascension.sin() * lunar_right_ascension_cosine
313 - orbit_0.right_ascension.cos() * lunar_right_ascension_sine,
314 // cos(Ω₀ - Ωₗ) = cos Ωₗ cos Ω₀ + sin Ωₗ sin Ω₀
315 lunar_right_ascension_cosine * orbit_0.right_ascension.cos()
316 + lunar_right_ascension_sine * orbit_0.right_ascension.sin(),
317 LUNAR_ECCENTRICITY,
318 lunar_argument_of_perigee.sin(),
319 lunar_argument_of_perigee.cos(),
320 LUNAR_PERTURBATION_COEFFICIENT,
321 LUNAR_MEAN_MOTION,
322 // Mₗ₀ = (-1.1151842 + 0.228027132 d₁₉₀₀) rem 2π
323 (-1.1151842 + 0.228027132 * d1900) % (2.0 * core::f64::consts::PI),
324 p2,
325 b0,
326 );
327 propagator::Constants {
328 geopotential,
329
330 // Ω̇ = p₁₄ + (Ω̇ₛ + Ω̇ₗ)
331 right_ascension_dot: p14 + (solar_dots.right_ascension + lunar_dots.right_ascension),
332
333 // ω̇ = k₁₄ + (ω̇ₛ + ω̇ₗ)
334 argument_of_perigee_dot: k14
335 + (solar_dots.argument_of_perigee + lunar_dots.argument_of_perigee),
336
337 // Ṁ = p₁₅ + (Ṁₛ + Ṁₗ)
338 mean_anomaly_dot: p15 + (solar_dots.mean_anomaly + lunar_dots.mean_anomaly),
339 c1,
340 c4,
341 k0,
342 k1,
343 method: propagator::Method::DeepSpace {
344 eccentricity_dot: solar_dots.eccentricity + lunar_dots.eccentricity,
345 inclination_dot: solar_dots.inclination + lunar_dots.inclination,
346 solar_perturbations,
347 lunar_perturbations,
348 resonant: if (orbit_0.mean_motion < 0.0052359877 && orbit_0.mean_motion > 0.0034906585)
349 || (orbit_0.mean_motion >= 8.26e-3
350 && orbit_0.mean_motion <= 9.24e-3
351 && orbit_0.eccentricity >= 0.5)
352 {
353 let sidereal_time_0 = epoch_to_sidereal_time(epoch);
354 if orbit_0.mean_motion < 0.0052359877 && orbit_0.mean_motion > 0.0034906585 {
355 propagator::Resonant::Yes {
356 // λ₀ = M₀ + Ω₀ + ω₀ − θ₀ rem 2π
357 lambda_0: (orbit_0.mean_anomaly
358 + orbit_0.right_ascension
359 + orbit_0.argument_of_perigee
360 - sidereal_time_0)
361 % (2.0 * core::f64::consts::PI),
362
363 // λ̇₀ = p₁₅ + (k₁₄ + p₁₄) − θ̇ + (Ṁₛ + Ṁₗ) + (ω̇ₛ + ω̇ₗ) + (Ω̇ₛ + Ω̇ₗ) - n₀"
364 lambda_dot_0: p15 + (k14 + p14) - SIDEREAL_SPEED
365 + (solar_dots.mean_anomaly + lunar_dots.mean_anomaly)
366 + (solar_dots.argument_of_perigee + lunar_dots.argument_of_perigee)
367 + (solar_dots.right_ascension + lunar_dots.right_ascension)
368 - orbit_0.mean_motion,
369 sidereal_time_0,
370 resonance: {
371 // p₁₇ = 3 (n / a₀")²
372 let p17 = 3.0 * (orbit_0.mean_motion / a0).powi(2);
373 propagator::Resonance::OneDay {
374 // 𝛿ᵣ₁ = p₁₇ (¹⁵/₁₆ sin²I₀ (1 + 3 p₁) - ³/₄ (1 + p₁))
375 // (1 + 2 e₀²) 2.1460748 × 10⁻⁶ / a₀"²
376 dr1: p17
377 * (0.9375
378 * orbit_0.inclination.sin().powi(2)
379 * (1.0 + 3.0 * p1)
380 - 0.75 * (1.0 + p1))
381 * (1.0 + 2.0 * orbit_0.eccentricity.powi(2))
382 * 2.1460748e-6
383 / a0,
384
385 // 𝛿ᵣ₂ = 2 p₁₇ (³/₄ (1 + p₁)²)
386 // (1 + e₀² (- ⁵/₂ + ¹³/₁₆ e₀²)) 1.7891679 × 10⁻⁶
387 dr2: 2.0
388 * p17
389 * (0.75 * (1.0 + p1).powi(2))
390 * (1.0
391 + orbit_0.eccentricity.powi(2)
392 * (-2.5 + 0.8125 * orbit_0.eccentricity.powi(2)))
393 * 1.7891679e-6,
394
395 // 𝛿ᵣ₃ = 3 p₁₇ (¹⁵/₈ (1 + p₁)³) (1 + e₀² (- 6 + 6.60937 e₀²))
396 // 2.2123015 × 10⁻⁷ / a₀"²
397 dr3: 3.0
398 * p17
399 * (1.875 * (1.0 + p1).powi(3))
400 * (1.0
401 + orbit_0.eccentricity.powi(2)
402 * (-6.0 + 6.60937 * orbit_0.eccentricity.powi(2)))
403 * 2.2123015e-7
404 / a0,
405 }
406 },
407 }
408 } else {
409 propagator::Resonant::Yes {
410 // λ₀ = M₀ + 2 Ω₀ − 2 θ₀ rem 2π
411 lambda_0: (orbit_0.mean_anomaly
412 + orbit_0.right_ascension
413 + orbit_0.right_ascension
414 - sidereal_time_0
415 - sidereal_time_0)
416 % (2.0 * core::f64::consts::PI),
417
418 // λ̇₀ = p₁₅ + (Ṁₛ + Ṁₗ) + 2 (p₁₄ + (Ω̇ₛ + Ω̇ₗ) - θ̇) - n₀"
419 lambda_dot_0: p15
420 + (solar_dots.mean_anomaly + lunar_dots.mean_anomaly)
421 + 2.0
422 * (p14 + (solar_dots.right_ascension + lunar_dots.right_ascension)
423 - SIDEREAL_SPEED)
424 - orbit_0.mean_motion,
425 sidereal_time_0,
426 resonance: {
427 // p₁₈ = 3 n₀"² / a₀"²
428 let p18 = 3.0 * orbit_0.mean_motion.powi(2) * (1.0 / a0).powi(2);
429
430 // p₁₉ = p₁₈ / a₀"
431 let p19 = p18 * (1.0 / a0);
432
433 // p₂₀ = p₁₉ / a₀"
434 let p20 = p19 * (1.0 / a0);
435
436 // p₂₁ = p₂₀ / a₀"
437 let p21 = p20 * (1.0 / a0);
438
439 // F₂₂₀ = ³/₄ (1 + 2 p₁ + p₁²)
440 let f220 = 0.75 * (1.0 + 2.0 * p1 + p1.powi(2));
441
442 // G₂₁₁ = │ 3.616 - 13.247 e₀ + 16.29 e₀² if e₀ ≤ 0.65
443 // │ - 72.099 + 331.819 e₀ - 508.738 e₀² + 266.724 e₀³ otherwise
444 // G₃₁₀ = │ - 19.302 + 117.39 e₀ - 228.419 e₀² + 156.591 e₀³ if e₀ ≤ 0.65
445 // │ - 346.844 + 1582.851 e₀ - 2415.925 e₀² + 1246.113 e₀³ otherwise
446 // G₃₂₂ = │ - 18.9068 + 109.7927 e₀ - 214.6334 e₀² + 146.5816 e₀³ if e₀ ≤ 0.65
447 // │ - 342.585 + 1554.908 e₀ - 2366.899 e₀² + 1215.972 e₀³ otherwise
448 // G₄₁₀ = │ - 41.122 + 242.694 e₀ - 471.094 e₀² + 313.953 e₀³ if e₀ ≤ 0.65
449 // │ - 1052.797 + 4758.686 e₀ - 7193.992 e₀² + 3651.957 e₀³ otherwise
450 // G₄₂₂ = │ - 146.407 + 841.88 e₀ - 1629.014 e₀² + 1083.435 e₀³ if e₀ ≤ 0.65
451 // │ - 3581.69 + 16178.11 e₀ - 24462.77 e₀² + 12422.52 e₀³ otherwise
452 let (g211, g310, g322, g410, g422) = if orbit_0.eccentricity <= 0.65 {
453 (
454 3.616 - 13.247 * orbit_0.eccentricity
455 + 16.29 * orbit_0.eccentricity.powi(2),
456 -19.302 + 117.39 * orbit_0.eccentricity
457 - 228.419 * orbit_0.eccentricity.powi(2)
458 + 156.591 * orbit_0.eccentricity.powi(3),
459 -18.9068 + 109.7927 * orbit_0.eccentricity
460 - 214.6334 * orbit_0.eccentricity.powi(2)
461 + 146.5816 * orbit_0.eccentricity.powi(3),
462 -41.122 + 242.694 * orbit_0.eccentricity
463 - 471.094 * orbit_0.eccentricity.powi(2)
464 + 313.953 * orbit_0.eccentricity.powi(3),
465 -146.407 + 841.88 * orbit_0.eccentricity
466 - 1629.014 * orbit_0.eccentricity.powi(2)
467 + 1083.435 * orbit_0.eccentricity.powi(3),
468 )
469 } else {
470 (
471 -72.099 + 331.819 * orbit_0.eccentricity
472 - 508.738 * orbit_0.eccentricity.powi(2)
473 + 266.724 * orbit_0.eccentricity.powi(3),
474 -346.844 + 1582.851 * orbit_0.eccentricity
475 - 2415.925 * orbit_0.eccentricity.powi(2)
476 + 1246.113 * orbit_0.eccentricity.powi(3),
477 -342.585 + 1554.908 * orbit_0.eccentricity
478 - 2366.899 * orbit_0.eccentricity.powi(2)
479 + 1215.972 * orbit_0.eccentricity.powi(3),
480 -1052.797 + 4758.686 * orbit_0.eccentricity
481 - 7193.992 * orbit_0.eccentricity.powi(2)
482 + 3651.957 * orbit_0.eccentricity.powi(3),
483 -3581.69 + 16178.11 * orbit_0.eccentricity
484 - 24462.77 * orbit_0.eccentricity.powi(2)
485 + 12422.52 * orbit_0.eccentricity.powi(3),
486 )
487 };
488
489 // G₅₂₀ = │ - 532.114 + 3017.977 e₀ - 5740.032 e₀² + 3708.276 e₀³ if e₀ ≤ 0.65
490 // │ 1464.74 - 4664.75 e₀ + 3763.64 e₀² if 0.65 < e₀ < 0.715
491 // │ - 5149.66 + 29936.92 e₀ - 54087.36 e₀² + 31324.56 e₀³ otherwise
492 let g520 = if orbit_0.eccentricity <= 0.65 {
493 -532.114 + 3017.977 * orbit_0.eccentricity
494 - 5740.032 * orbit_0.eccentricity.powi(2)
495 + 3708.276 * orbit_0.eccentricity.powi(3)
496 } else if orbit_0.eccentricity < 0.715 {
497 1464.74 - 4664.75 * orbit_0.eccentricity
498 + 3763.64 * orbit_0.eccentricity.powi(2)
499 } else {
500 -5149.66 + 29936.92 * orbit_0.eccentricity
501 - 54087.36 * orbit_0.eccentricity.powi(2)
502 + 31324.56 * orbit_0.eccentricity.powi(3)
503 };
504
505 // G₅₃₂ = │ - 853.666 + 4690.25 e₀ - 8624.77 e₀² + 5341.4 e₀³ if e₀ < 0.7
506 // │ - 40023.88 + 170470.89 e₀ - 242699.48 e₀² + 115605.82 e₀³ otherwise
507 // G₅₂₁ = │ - 822.71072 + 4568.6173 e₀ - 8491.4146 e₀² + 5337.524 e₀³ if e₀ < 0.7
508 // │ - 51752.104 + 218913.95 e₀ - 309468.16 e₀² + 146349.42 e₀³ otherwise
509 // G₅₃₃ = │ - 919.2277 + 4988.61 e₀ - 9064.77 e₀² + 5542.21 e₀³ if e₀ < 0.7
510 // │ - 37995.78 + 161616.52 e₀ - 229838.2 e₀² + 109377.94 e₀³ otherwise
511 let (g532, g521, g533) = if orbit_0.eccentricity < 0.7 {
512 (
513 -853.666 + 4690.25 * orbit_0.eccentricity
514 - 8624.77 * orbit_0.eccentricity.powi(2)
515 + 5341.4 * orbit_0.eccentricity.powi(3),
516 -822.71072 + 4568.6173 * orbit_0.eccentricity
517 - 8491.4146 * orbit_0.eccentricity.powi(2)
518 + 5337.524 * orbit_0.eccentricity.powi(3),
519 -919.2277 + 4988.61 * orbit_0.eccentricity
520 - 9064.77 * orbit_0.eccentricity.powi(2)
521 + 5542.21 * orbit_0.eccentricity.powi(3),
522 )
523 } else {
524 (
525 -40023.88 + 170470.89 * orbit_0.eccentricity
526 - 242699.48 * orbit_0.eccentricity.powi(2)
527 + 115605.82 * orbit_0.eccentricity.powi(3),
528 -51752.104 + 218913.95 * orbit_0.eccentricity
529 - 309468.16 * orbit_0.eccentricity.powi(2)
530 + 146349.42 * orbit_0.eccentricity.powi(3),
531 -37995.78 + 161616.52 * orbit_0.eccentricity
532 - 229838.2 * orbit_0.eccentricity.powi(2)
533 + 109377.94 * orbit_0.eccentricity.powi(3),
534 )
535 };
536
537 propagator::Resonance::HalfDay {
538 // D₂₂₀₋₁ = p₁₈ 1.7891679 × 10⁻⁶ F₂₂₀ (- 0.306 - 0.44 (e₀ - 0.64))
539 d2201: p18
540 * 1.7891679e-6
541 * f220
542 * (-0.306 - (orbit_0.eccentricity - 0.64) * 0.44),
543
544 // D₂₂₁₁ = p₁₈ 1.7891679 × 10⁻⁶ (³/₂ sin²I₀) G₂₁₁
545 d2211: p18
546 * 1.7891679e-6
547 * (1.5 * orbit_0.inclination.sin().powi(2))
548 * g211,
549
550 // D₃₂₁₀ = p₁₉ 3.7393792 × 10⁻⁷ (¹⁵/₈ sin I₀ (1 - 2 p₁ - 3 p₁²)) G₃₁₀
551 d3210: p19
552 * 3.7393792e-7
553 * (1.875
554 * orbit_0.inclination.sin()
555 * (1.0 - 2.0 * p1 - 3.0 * p1.powi(2)))
556 * g310,
557
558 // D₃₂₂₂ = p₁₉ 3.7393792 × 10⁻⁷ (- ¹⁵/₈ sin I₀ (1 + 2 p₁ - 3 p₁²)) G₃₂₂
559 d3222: p19
560 * 3.7393792e-7
561 * (-1.875
562 * orbit_0.inclination.sin()
563 * (1.0 + 2.0 * p1 - 3.0 * p1.powi(2)))
564 * g322,
565
566 // D₄₄₁₀ = 2 p₂₀ 7.3636953 × 10⁻⁹ (35 sin²I₀ F₂₂₀) G₄₁₀
567 d4410: 2.0
568 * p20
569 * 7.3636953e-9
570 * (35.0 * orbit_0.inclination.sin().powi(2) * f220)
571 * g410,
572
573 // D₄₄₂₂ = 2 p₂₀ 7.3636953 × 10⁻⁹ (³¹⁵/₈ sin⁴I₀) G₄₂₂
574 d4422: 2.0
575 * p20
576 * 7.3636953e-9
577 * (39.375 * orbit_0.inclination.sin().powi(4))
578 * g422,
579
580 // D₅₂₂₀ = p₂₁ 1.1428639 × 10⁻⁷ (³¹⁵/₃₂ sin I₀
581 // (sin²I₀ (1 - 2 p₁ - 5 p₁²)
582 // + 0.33333333 (- 2 + 4 p₁ + 6 p₁²))) G₅₂₀
583 d5220: p21
584 * 1.1428639e-7
585 * (9.84375
586 * orbit_0.inclination.sin()
587 * (orbit_0.inclination.sin().powi(2)
588 * (1.0 - 2.0 * p1 - 5.0 * p1.powi(2))
589 + 0.33333333 * (-2.0 + 4.0 * p1 + 6.0 * p1.powi(2))))
590 * g520,
591
592 // D₅₂₃₂ = p₂₁ 1.1428639 × 10⁻⁷ (sin I₀
593 // (4.92187512 sin²I₀ (- 2 - 4 p₁ + 10 p₁²)
594 // + 6.56250012 (1 + p₁ - 3 p₁²))) G₅₃₂
595 d5232: p21
596 * 1.1428639e-7
597 * (orbit_0.inclination.sin()
598 * (4.92187512
599 * orbit_0.inclination.sin().powi(2)
600 * (-2.0 - 4.0 * p1 + 10.0 * p1.powi(2))
601 + 6.56250012 * (1.0 + 2.0 * p1 - 3.0 * p1.powi(2))))
602 * g532,
603
604 // D₅₄₂₁ = 2 p₂₁ 2.1765803 × 10⁻⁹ (⁹⁴⁵/₃₂ sin I₀
605 // (2 - 8 p₁ + p₁² (- 12 + 8 p₁ + 10 p₁²))) G₅₂₁
606 d5421: 2.0
607 * p21
608 * 2.1765803e-9
609 * (29.53125
610 * orbit_0.inclination.sin()
611 * (2.0 - 8.0 * p1
612 + p1.powi(2) * (-12.0 + 8.0 * p1 + 10.0 * p1.powi(2))))
613 * g521,
614
615 // D₅₄₃₃ = 2 p₂₁ 2.1765803 × 10⁻⁹ (⁹⁴⁵/₃₂ sin I₀
616 // (- 2 - 8 p₁ + p₁² (12 + 8 p₁ - 10 p₁²))) G₅₃₃
617 d5433: 2.0
618 * p21
619 * 2.1765803e-9
620 * (29.53125
621 * orbit_0.inclination.sin()
622 * (-2.0 - 8.0 * p1
623 + p1.powi(2) * (12.0 + 8.0 * p1 - 10.0 * p1.powi(2))))
624 * g533,
625 k14,
626 }
627 },
628 }
629 }
630 } else {
631 propagator::Resonant::No { a0 }
632 },
633 },
634 orbit_0,
635 }
636}
637
638impl propagator::Constants {
639 #[allow(clippy::too_many_arguments, clippy::type_complexity)]
640 pub(crate) fn deep_space_orbital_elements(
641 &self,
642 eccentricity_dot: f64,
643 inclination_dot: f64,
644 solar_perturbations: &third_body::Perturbations,
645 lunar_perturbations: &third_body::Perturbations,
646 resonant: &propagator::Resonant,
647 state: Option<&mut ResonanceState>,
648 t: f64,
649 p22: f64,
650 p23: f64,
651 afspc_compatibility_mode: bool,
652 ) -> core::result::Result<(propagator::Orbit, f64, f64, f64, f64, f64, f64), gp::Error> {
653 let (p28, p29) = match resonant {
654 propagator::Resonant::No { a0 } => {
655 assert!(
656 state.is_none(),
657 "state must be None with a non-resonant deep-space propagator",
658 );
659 (
660 // p₂₈ = a₀"
661 *a0,
662 // p₂₉ = M₀ + Ṁ t
663 self.orbit_0.mean_anomaly + self.mean_anomaly_dot * t,
664 )
665 }
666 propagator::Resonant::Yes {
667 lambda_dot_0,
668 sidereal_time_0,
669 resonance,
670 ..
671 } => match state {
672 Some(state) => state.integrate(
673 &self.geopotential,
674 self.orbit_0.argument_of_perigee,
675 *lambda_dot_0,
676 resonance,
677 *sidereal_time_0,
678 t,
679 p22,
680 p23,
681 ),
682 _ => panic!("state cannot be None with a deep space propagator"),
683 },
684 };
685 let (solar_delta_eccentricity, solar_delta_inclination, solar_delta_mean_motion, ps4, ps5) =
686 solar_perturbations.long_period_periodic_effects(
687 SOLAR_ECCENTRICITY,
688 SOLAR_MEAN_MOTION,
689 t,
690 );
691 let (lunar_delta_eccentricity, lunar_delta_inclination, lunar_delta_mean_motion, pl4, pl5) =
692 lunar_perturbations.long_period_periodic_effects(
693 LUNAR_ECCENTRICITY,
694 LUNAR_MEAN_MOTION,
695 t,
696 );
697
698 // I = I₀ + İ t + (δIₛ + δIₗ)
699 let inclination = self.orbit_0.inclination
700 + inclination_dot * t
701 + (solar_delta_inclination + lunar_delta_inclination);
702 let (right_ascension, argument_of_perigee) = if inclination >= 0.2 {
703 (
704 // Ω = p₂₂ + (pₛ₅ + pₗ₅) / sin I
705 p22 + (ps5 + pl5) / inclination.sin(),
706 // ω = p₂₃ + (pₛ₄ + pₗ₄) - cos I (pₛ₅ + pₗ₅) / sin I
707 p23 + (ps4 + pl4) - inclination.cos() * ((ps5 + pl5) / inclination.sin()),
708 )
709 } else {
710 // sin I sin p₂₂ + (pₛ₅ + pₗ₅) cos p₂₂ + (δIₛ + δIₗ) cos I sin p₂₂
711 // p₃₀ = tan⁻¹ -------------------------------------------------------------
712 // sin I cos p₂₂ - (pₛ₅ + pₗ₅) sin p₂₂ + (δIₛ + δIₗ) cos I cos p₂₂
713 let p30 = (inclination.sin() * p22.sin()
714 + ((ps5 + pl5) * p22.cos()
715 + (solar_delta_inclination + lunar_delta_inclination)
716 * inclination.cos()
717 * p22.sin()))
718 .atan2(
719 inclination.sin() * p22.cos()
720 + (-(ps5 + pl5) * p22.sin()
721 + (solar_delta_inclination + lunar_delta_inclination)
722 * inclination.cos()
723 * p22.cos()),
724 );
725
726 // Ω = │ p₃₀ + 2π if p₃₀ + π < p₂₂ rem 2π
727 // │ p₃₀ - 2π if p₃₀ - π > p₂₂ rem 2π
728 // │ p₃₀ otherwise
729 let right_ascension =
730 if p30 < p22 % (2.0 * core::f64::consts::PI) - core::f64::consts::PI {
731 p30 + (2.0 * core::f64::consts::PI)
732 } else if p30 > p22 % (2.0 * core::f64::consts::PI) + core::f64::consts::PI {
733 p30 - (2.0 * core::f64::consts::PI)
734 } else {
735 p30
736 };
737 (
738 right_ascension,
739 // ω = │ p₂₃ + (pₛ₄ + pₗ₄) + cos I ((p₂₂ rem 2π) - Ω)
740 // │ - (δIₛ + δIₗ) (p₂₂ mod 2π) sin I if AFSPC compatibility mode
741 // ω = │ p₂₃ + (pₛ₄ + pₗ₄) + cos I ((p₂₂ rem 2π) - Ω)
742 // │ - (δIₛ + δIₗ) (p₂₂ rem 2π) sin I otherwise
743 p23 + (ps4 + pl4)
744 + inclination.cos() * (p22 % (2.0 * core::f64::consts::PI) - right_ascension)
745 - (solar_delta_inclination + lunar_delta_inclination)
746 * if afspc_compatibility_mode {
747 #[cfg(feature = "std")]
748 {
749 p22.rem_euclid(2.0 * core::f64::consts::PI)
750 }
751 #[cfg(not(feature = "std"))]
752 {
753 Euclid::rem_euclid(&p22, &(2.0 * core::f64::consts::PI))
754 }
755 } else {
756 p22 % (2.0 * core::f64::consts::PI)
757 }
758 * inclination.sin(),
759 )
760 };
761
762 // p₃₁ = e₀ + ė t - C₄ t
763 let p31 = self.orbit_0.eccentricity + eccentricity_dot * t - self.c4 * t;
764 if !(-0.001..1.0).contains(&p31) {
765 Err(gp::Error::OutOfRangeEccentricity {
766 eccentricity: p31,
767 t,
768 })
769 } else {
770 // e = │ 10⁻⁶ + (δeₛ + δeₗ) if p₃₁ < 10⁻⁶
771 // │ p₃₁ + (δeₛ + δeₗ) otherwise
772 let eccentricity =
773 (p31).max(1.0e-6) + (solar_delta_eccentricity + lunar_delta_eccentricity);
774 if !(0.0..=1.0).contains(&eccentricity) {
775 Err(gp::Error::OutOfRangePerturbedEccentricity { eccentricity, t })
776 } else {
777 // a = p₂₈ (1 - C₁ t)²
778 let a = p28 * (1.0 - self.c1 * t).powi(2);
779 Ok((
780 propagator::Orbit {
781 inclination,
782 right_ascension,
783 eccentricity,
784 argument_of_perigee,
785
786 // M = p₂₉ + (δMₛ + δMₗ) + n₀" k₁ t²
787 mean_anomaly: p29
788 + (solar_delta_mean_motion + lunar_delta_mean_motion)
789 + self.orbit_0.mean_motion * self.k1 * t.powi(2),
790
791 // n = kₑ / a³ᐟ²
792 mean_motion: self.geopotential.ke / a.powf(1.5),
793 },
794 a,
795 // 1 J₃
796 // p₃₂ = - - -- sin I
797 // 2 J₂
798 -0.5 * (self.geopotential.j3 / self.geopotential.j2) * inclination.sin(),
799 // p₃₃ = 1 - cos²I
800 1.0 - inclination.cos().powi(2),
801 // p₃₄ = 7 cos²I - 1
802 7.0 * inclination.cos().powi(2) - 1.0,
803 // │ 1 J₃ 3 + 5 cos I
804 // p₃₅ = │ - - -- sin I ----------- if |1 + cos I| > 1.5 × 10⁻¹²
805 // │ 4 J₂ 1 + cos I
806 // │ 1 J₃ 3 + 5 cos I
807 // │ - - -- sin I ----------- otherwise
808 // │ 4 J₂ 1.5 × 10⁻¹²
809 if (1.0 + inclination.cos()).abs() > 1.5e-12 {
810 -0.25
811 * (self.geopotential.j3 / self.geopotential.j2)
812 * inclination.sin()
813 * (3.0 + 5.0 * inclination.cos())
814 / (1.0 + inclination.cos())
815 } else {
816 -0.25
817 * (self.geopotential.j3 / self.geopotential.j2)
818 * inclination.sin()
819 * (3.0 + 5.0 * inclination.cos())
820 / 1.5e-12
821 },
822 // p₃₆ = 3 cos²I - 1
823 3.0 * inclination.cos().powi(2) - 1.0,
824 ))
825 }
826 }
827 }
828}