nyx_space/tools/
lambert.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 crate::errors::NyxError;
20use crate::linalg::Vector3;
21use std::f64::consts::PI;
22
23const TAU: f64 = 2.0 * PI;
24const LAMBERT_EPSILON: f64 = 1e-4; // General epsilon
25const LAMBERT_EPSILON_TIME: f64 = 1e-4; // Time epsilon
26const LAMBERT_EPSILON_RAD: f64 = (5e-5 / 180.0) * PI; // 0.00005 degrees
27/// Maximum number of iterations allowed in the Lambert problem solver.
28/// This is a safety measure to prevent infinite loops in case a solution cannot be found.
29const MAX_ITERATIONS: usize = 1000;
30
31/// Define the transfer kind for a Lambert
32pub enum TransferKind {
33    Auto,
34    ShortWay,
35    LongWay,
36    NRevs(u8),
37}
38
39impl TransferKind {
40    /// Calculate the direction multiplier based on the transfer kind.
41    ///
42    /// # Arguments
43    ///
44    /// * `r_final` - The final radius vector.
45    /// * `r_init` - The initial radius vector.
46    ///
47    /// # Returns
48    ///
49    /// * `Result<f64, NyxError>` - The direction multiplier or an error if the transfer kind is not supported.
50    fn direction_of_motion(
51        self,
52        r_final: &Vector3<f64>,
53        r_init: &Vector3<f64>,
54    ) -> Result<f64, NyxError> {
55        match self {
56            TransferKind::Auto => {
57                let mut dnu = r_final[1].atan2(r_final[0]) - r_init[1].atan2(r_final[1]);
58                if dnu > TAU {
59                    dnu -= TAU;
60                } else if dnu < 0.0 {
61                    dnu += TAU;
62                }
63
64                if dnu > std::f64::consts::PI {
65                    Ok(-1.0)
66                } else {
67                    Ok(1.0)
68                }
69            }
70            TransferKind::ShortWay => Ok(1.0),
71            TransferKind::LongWay => Ok(-1.0),
72            _ => Err(NyxError::LambertMultiRevNotSupported),
73        }
74    }
75}
76
77#[derive(Debug)]
78pub struct LambertSolution {
79    pub v_init: Vector3<f64>,
80    pub v_final: Vector3<f64>,
81    pub phi: f64,
82}
83
84/// Solve the Lambert boundary problem using a standard secant method.
85///
86/// Given the initial and final radii, a time of flight, and a gravitational parameters, it returns the needed initial and final velocities
87/// along with φ which is the square of the difference in eccentric anomaly. Note that the direction of motion
88/// is computed directly in this function to simplify the generation of Pork chop plots.
89///
90/// # Arguments
91///
92/// * `r_init` - The initial radius vector.
93/// * `r_final` - The final radius vector.
94/// * `tof` - The time of flight.
95/// * `gm` - The gravitational parameter.
96/// * `kind` - The kind of transfer (auto, short way, long way, or number of revolutions).
97///
98/// # Returns
99///
100/// `Result<LambertSolution, NyxError>` - The solution to the Lambert problem or an error if the problem could not be solved.
101pub fn standard(
102    r_init: Vector3<f64>,
103    r_final: Vector3<f64>,
104    tof: f64,
105    gm: f64,
106    kind: TransferKind,
107) -> Result<LambertSolution, NyxError> {
108    let r_init_norm = r_init.norm();
109    let r_final_norm = r_final.norm();
110    let r_norm_product = r_init_norm * r_final_norm;
111    let cos_dnu = r_init.dot(&r_final) / r_norm_product;
112
113    let dm = kind.direction_of_motion(&r_final, &r_init)?;
114
115    let nu_init = r_init[1].atan2(r_init[0]);
116    let nu_final = r_final[1].atan2(r_final[0]);
117
118    let a = dm * (r_norm_product * (1.0 + cos_dnu)).sqrt();
119
120    if nu_final - nu_init < LAMBERT_EPSILON_RAD && a.abs() < LAMBERT_EPSILON {
121        return Err(NyxError::TargetsTooClose);
122    }
123
124    let mut phi_upper = 4.0 * PI.powi(2);
125    let mut phi_lower = -4.0 * PI.powi(2);
126    let mut phi = 0.0;
127
128    let mut c2: f64 = 1.0 / 2.0;
129    let mut c3: f64 = 1.0 / 6.0;
130    let mut iter: usize = 0;
131    let mut cur_tof: f64 = 0.0;
132    let mut y = 0.0;
133
134    while (cur_tof - tof).abs() > LAMBERT_EPSILON_TIME {
135        if iter > MAX_ITERATIONS {
136            return Err(NyxError::MaxIterReached {
137                msg: format!("Lambert solver failed after {MAX_ITERATIONS} iterations"),
138            });
139        }
140        iter += 1;
141
142        y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt();
143        if a > 0.0 && y < 0.0 {
144            for _ in 0..500 {
145                phi += 0.1;
146                y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt();
147                if y >= 0.0 {
148                    break;
149                }
150            }
151            if y < 0.0 {
152                return Err(NyxError::LambertNotReasonablePhi);
153            }
154        }
155
156        let chi = (y / c2).sqrt();
157        cur_tof = (chi.powi(3) * c3 + a * y.sqrt()) / gm.sqrt();
158
159        if cur_tof < tof {
160            phi_lower = phi;
161        } else {
162            phi_upper = phi;
163        }
164
165        phi = (phi_upper + phi_lower) / 2.0;
166
167        if phi > LAMBERT_EPSILON {
168            let sqrt_phi = phi.sqrt();
169            let (s_sphi, c_sphi) = sqrt_phi.sin_cos();
170            c2 = (1.0 - c_sphi) / phi;
171            c3 = (sqrt_phi - s_sphi) / phi.powi(3).sqrt();
172        } else if phi < -LAMBERT_EPSILON {
173            let sqrt_phi = (-phi).sqrt();
174            c2 = (1.0 - sqrt_phi.cosh()) / phi;
175            c3 = (sqrt_phi.sinh() - sqrt_phi) / (-phi).powi(3).sqrt();
176        } else {
177            c2 = 0.5;
178            c3 = 1.0 / 6.0;
179        }
180    }
181
182    let f = 1.0 - y / r_init_norm;
183    let g_dot = 1.0 - y / r_final_norm;
184    let g = a * (y / gm).sqrt();
185
186    Ok(LambertSolution {
187        v_init: (r_final - f * r_init) / g,
188        v_final: (1.0 / g) * (g_dot * r_final - r_init),
189        phi,
190    })
191}
192
193#[test]
194fn test_lambert_vallado_shortway() {
195    let ri = Vector3::new(15945.34, 0.0, 0.0);
196    let rf = Vector3::new(12214.83899, 10249.46731, 0.0);
197    let tof_s = 76.0 * 60.0;
198    let gm = 3.98600433e5;
199
200    let exp_vi = Vector3::new(2.058913, 2.915965, 0.0);
201    let exp_vf = Vector3::new(-3.451565, 0.910315, 0.0);
202
203    let sol = standard(ri, rf, tof_s, gm, TransferKind::ShortWay).unwrap();
204
205    assert!((sol.v_init - exp_vi).norm() < 1e-6);
206    assert!((sol.v_final - exp_vf).norm() < 1e-6);
207}
208
209#[test]
210fn test_lambert_vallado_lonway() {
211    let ri = Vector3::new(15945.34, 0.0, 0.0);
212    let rf = Vector3::new(12214.83899, 10249.46731, 0.0);
213    let tof_s = 76.0 * 60.0;
214    let gm = 3.98600433e5;
215
216    let exp_vi = Vector3::new(-3.811158, -2.003854, 0.0);
217    let exp_vf = Vector3::new(4.207569, 0.914724, 0.0);
218
219    let sol = standard(ri, rf, tof_s, gm, TransferKind::LongWay).unwrap();
220
221    assert!((sol.v_init - exp_vi).norm() < 1e-6);
222    assert!((sol.v_final - exp_vf).norm() < 1e-6);
223}
nyx_space/tools/lambert.rs

nyx_space/tools/
lambert.rs
