nyx_space/tools/lambert/
godding.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use super::{
20    LambertError, LambertInput, LambertSolution, TransferKind, LAMBERT_EPSILON,
21    LAMBERT_EPSILON_RAD, LAMBERT_EPSILON_TIME, MAX_ITERATIONS,
22};
23
24use core::f64::consts::PI;
25
26/// Solve the Lambert boundary problem using Gooding's method.
27///
28/// This is an implementation of R. H. Gooding's method for solving Lambert's problem, as described in "A procedure for the solution of Lambert's orbital boundary-value problem".
29/// Given the initial and final radii, a time of flight, and a gravitational parameters, it returns the needed initial and final velocities
30/// along with φ which is the square of the difference in eccentric anomaly. Note that the direction of motion
31/// is computed directly in this function to simplify the generation of Pork chop plots.
32///
33/// # Arguments
34///
35/// * `r_init` - The initial radius vector.
36/// * `r_final` - The final radius vector.
37/// * `tof_s` - The time of flight in seconds.
38/// * `mu_km3_s2` - The gravitational parameter in km^3/s^2.
39/// * `kind` - The kind of transfer (auto, short way, long way, or number of revolutions).
40///
41/// # Returns
42///
43/// `Result<LambertSolution, NyxError>` - The solution to the Lambert problem or an error if the problem could not be solved.
44pub fn gooding(input: LambertInput, kind: TransferKind) -> Result<LambertSolution, LambertError> {
45    let r_init = input.initial_state.radius_km;
46    let r_final = input.final_state.radius_km;
47    let tof_s = (input.final_state.epoch - input.initial_state.epoch).to_seconds();
48    let mu_km3_s2 = input.mu_km2_s3();
49
50    let r_init_norm = r_init.norm();
51    let r_final_norm = r_final.norm();
52    let r_norm_product = r_init_norm * r_final_norm;
53    let cos_dnu = r_init.dot(&r_final) / r_norm_product;
54
55    let dm = kind.direction_of_motion(&r_final, &r_init)?;
56
57    let nu_init = r_init[1].atan2(r_init[0]);
58    let nu_final = r_final[1].atan2(r_final[0]);
59
60    let a = dm * (r_norm_product * (1.0 + cos_dnu)).sqrt();
61
62    if nu_final - nu_init < LAMBERT_EPSILON_RAD && a.abs() < LAMBERT_EPSILON {
63        return Err(LambertError::TargetsTooClose);
64    }
65
66    let mut phi_upper = 4.0 * PI.powi(2);
67    let mut phi_lower = -4.0 * PI.powi(2);
68    let mut phi = 0.0;
69
70    let mut c2: f64 = 1.0 / 2.0;
71    let mut c3: f64 = 1.0 / 6.0;
72    let mut iter: usize = 0;
73    let mut cur_tof: f64 = 0.0;
74    let mut y = 0.0;
75
76    while (cur_tof - tof_s).abs() > LAMBERT_EPSILON_TIME {
77        if iter > MAX_ITERATIONS {
78            return Err(LambertError::SolverMaxIter {
79                maxiter: MAX_ITERATIONS,
80            });
81        }
82        iter += 1;
83
84        y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt();
85        if a > 0.0 && y < 0.0 {
86            for _ in 0..500 {
87                phi += 0.1;
88                y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt();
89                if y >= 0.0 {
90                    break;
91                }
92            }
93            if y < 0.0 {
94                return Err(LambertError::NotReasonablePhi);
95            }
96        }
97
98        let chi = (y / c2).sqrt();
99        cur_tof = (chi.powi(3) * c3 + a * y.sqrt()) / mu_km3_s2.sqrt();
100
101        if cur_tof < tof_s {
102            phi_lower = phi;
103        } else {
104            phi_upper = phi;
105        }
106
107        phi = (phi_upper + phi_lower) / 2.0;
108
109        if phi > LAMBERT_EPSILON {
110            let sqrt_phi = phi.sqrt();
111            let (s_sphi, c_sphi) = sqrt_phi.sin_cos();
112            c2 = (1.0 - c_sphi) / phi;
113            c3 = (sqrt_phi - s_sphi) / phi.powi(3).sqrt();
114        } else if phi < -LAMBERT_EPSILON {
115            let sqrt_phi = (-phi).sqrt();
116            c2 = (1.0 - sqrt_phi.cosh()) / phi;
117            c3 = (sqrt_phi.sinh() - sqrt_phi) / (-phi).powi(3).sqrt();
118        } else {
119            c2 = 0.5;
120            c3 = 1.0 / 6.0;
121        }
122    }
123
124    let f = 1.0 - y / r_init_norm;
125    let g_dot = 1.0 - y / r_final_norm;
126    let g = a * (y / mu_km3_s2).sqrt();
127
128    Ok(LambertSolution {
129        v_init_km_s: (r_final - f * r_init) / g,
130        v_final_km_s: (1.0 / g) * (g_dot * r_final - r_init),
131        phi_rad: phi,
132        input,
133    })
134}
135
136#[cfg(test)]
137mod ut_lambert_gooding {
138
139    use super::{gooding, TransferKind};
140    use crate::{linalg::Vector3, tools::lambert::LambertInput};
141    use anise::{frames::Frame, prelude::Orbit};
142    use hifitime::{Epoch, Unit};
143    #[test]
144    fn test_lambert_vallado_shortway() {
145        let frame = Frame {
146            ephemeris_id: 301,
147            orientation_id: 0,
148            mu_km3_s2: Some(3.98600433e5),
149            shape: None,
150            frozen_epoch: None,
151            force_inertial: false,
152        };
153        let initial_state = Orbit {
154            radius_km: Vector3::new(15945.34, 0.0, 0.0),
155            velocity_km_s: Vector3::zeros(),
156            epoch: Epoch::from_gregorian_utc_at_midnight(2025, 1, 1),
157            frame,
158        };
159        let final_state = Orbit {
160            radius_km: Vector3::new(12214.83899, 10249.46731, 0.0),
161            velocity_km_s: Vector3::zeros(),
162            epoch: Epoch::from_gregorian_utc_at_midnight(2025, 1, 1) + Unit::Minute * 76.0,
163            frame,
164        };
165
166        let input = LambertInput::from_planetary_states(initial_state, final_state).unwrap();
167
168        let exp_vi = Vector3::new(2.058913, 2.915965, 0.0);
169        let exp_vf = Vector3::new(-3.451565, 0.910315, 0.0);
170
171        let sol = gooding(input, TransferKind::ShortWay).unwrap();
172
173        assert!((sol.v_init_km_s - exp_vi).norm() < 1e-6);
174        assert!((sol.v_final_km_s - exp_vf).norm() < 1e-6);
175
176        println!("{sol:?}");
177        println!("{}\n{}", sol.transfer_orbit(), sol.arrival_orbit());
178        println!(
179            "v_inf+ = {} km/s -- c3 = {} km^2/s^2",
180            sol.v_inf_outgoing_km_s().norm(),
181            sol.c3_km2_s2()
182        );
183        println!("v_inf- = {} km/s", sol.v_inf_incoming_km_s().norm());
184    }
185
186    #[test]
187    fn test_lambert_vallado_longway() {
188        let frame = Frame {
189            ephemeris_id: 301,
190            orientation_id: 0,
191            mu_km3_s2: Some(3.98600433e5),
192            shape: None,
193            frozen_epoch: None,
194            force_inertial: false,
195        };
196        let initial_state = Orbit {
197            radius_km: Vector3::new(15945.34, 0.0, 0.0),
198            velocity_km_s: Vector3::zeros(),
199            epoch: Epoch::from_gregorian_utc_at_midnight(2025, 1, 1),
200            frame,
201        };
202        let final_state = Orbit {
203            radius_km: Vector3::new(12214.83899, 10249.46731, 0.0),
204            velocity_km_s: Vector3::zeros(),
205            epoch: Epoch::from_gregorian_utc_at_midnight(2025, 1, 1) + Unit::Minute * 76.0,
206            frame,
207        };
208
209        let input = LambertInput::from_planetary_states(initial_state, final_state).unwrap();
210        let exp_vi = Vector3::new(-3.811158, -2.003854, 0.0);
211        let exp_vf = Vector3::new(4.207569, 0.914724, 0.0);
212
213        let sol = gooding(input, TransferKind::LongWay).unwrap();
214
215        assert!((sol.v_init_km_s - exp_vi).norm() < 1e-6);
216        assert!((sol.v_final_km_s - exp_vf).norm() < 1e-6);
217    }
218
219    #[test]
220    fn test_lambert_gooding_direction_toggle_ih_negative() {
221        // Symmetry lock: on the Gooding path, the direction toggle is a
222        // simple ±1 on the chord term, independent of i_h.z. This test
223        // pins that invariant so ShortWay/LongWay can never collapse
224        // to a single solution on i_h.z<0 geometries.
225        let frame = Frame {
226            ephemeris_id: 301,
227            orientation_id: 0,
228            mu_km3_s2: Some(3.98600433e5),
229            shape: None,
230            frozen_epoch: None,
231            force_inertial: false,
232        };
233        let t0 = Epoch::from_gregorian_utc_at_midnight(2025, 1, 1);
234
235        let r1 = Vector3::new(7000.0, 0.0, 0.0);
236        let r2 = Vector3::new(0.0, -7000.0, -500.0);
237
238        let initial = Orbit {
239            radius_km: r1,
240            velocity_km_s: Vector3::zeros(),
241            epoch: t0,
242            frame,
243        };
244        let final_ = Orbit {
245            radius_km: r2,
246            velocity_km_s: Vector3::zeros(),
247            epoch: t0 + Unit::Minute * 60.0,
248            frame,
249        };
250        let input = LambertInput::from_planetary_states(initial, final_).unwrap();
251
252        let short = gooding(input, TransferKind::ShortWay).unwrap();
253        let long = gooding(input, TransferKind::LongWay).unwrap();
254
255        assert!(
256            (short.v_init_km_s - long.v_init_km_s).norm() > 1e-3,
257            "Gooding short/long-way collapsed on i_h.z<0 geometry",
258        );
259    }
260}
nyx_space/tools/lambert/godding.rs

nyx_space/tools/lambert/
godding.rs
