nyx_space/tools/lambert/
mod.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::cosmic::AstroError;
20use crate::errors::LambertError;
21use crate::linalg::Vector3;
22use std::f64::consts::PI;
23
24mod godding;
25mod izzo;
26
27use anise::errors::PhysicsError;
28use anise::prelude::Orbit;
29pub use godding::gooding;
30pub use izzo::izzo;
31
32const TAU: f64 = 2.0 * PI;
33const LAMBERT_EPSILON: f64 = 1e-4; // General epsilon
34const LAMBERT_EPSILON_TIME: f64 = 1e-4; // Time epsilon
35const LAMBERT_EPSILON_RAD: f64 = (5e-5 / 180.0) * PI; // 0.00005 degrees
36/// Maximum number of iterations allowed in the Lambert problem solver.
37/// This is a safety measure to prevent infinite loops in case a solution cannot be found.
38const MAX_ITERATIONS: usize = 1000;
39
40/// Define the transfer kind for a Lambert
41pub enum TransferKind {
42    Auto,
43    ShortWay,
44    LongWay,
45    NRevs(u8),
46}
47
48impl TransferKind {
49    /// Calculate the direction multiplier based on the transfer kind.
50    ///
51    /// # Arguments
52    ///
53    /// * `r_final` - The final radius vector.
54    /// * `r_init` - The initial radius vector.
55    ///
56    /// # Returns
57    ///
58    /// * `Result<f64, NyxError>` - The direction multiplier or an error if the transfer kind is not supported.
59    fn direction_of_motion(
60        self,
61        r_final: &Vector3<f64>,
62        r_init: &Vector3<f64>,
63    ) -> Result<f64, LambertError> {
64        match self {
65            TransferKind::Auto => {
66                let mut dnu = r_final[1].atan2(r_final[0]) - r_init[1].atan2(r_init[0]);
67                if dnu < 0.0 {
68                    dnu += TAU;
69                }
70
71                if dnu > std::f64::consts::PI {
72                    Ok(-1.0)
73                } else {
74                    Ok(1.0)
75                }
76            }
77            TransferKind::ShortWay => Ok(1.0),
78            TransferKind::LongWay => Ok(-1.0),
79            _ => Err(LambertError::MultiRevNotSupported),
80        }
81    }
82}
83
84#[derive(Copy, Clone, Debug)]
85pub struct LambertInput {
86    pub initial_state: Orbit,
87    pub final_state: Orbit,
88}
89
90impl LambertInput {
91    pub fn from_planetary_states(
92        initial_state: Orbit,
93        final_state: Orbit,
94    ) -> Result<Self, AstroError> {
95        if final_state.frame != initial_state.frame {
96            return Err(AstroError::AstroPhysics {
97                source: PhysicsError::FrameMismatch {
98                    action: "Lambert solver requires both states to be in the same frame",
99                    frame1: final_state.frame.into(),
100                    frame2: initial_state.frame.into(),
101                },
102            });
103        }
104        // Ensure that the GM is set
105        initial_state
106            .frame
107            .mu_km3_s2()
108            .map_err(|e| AstroError::AstroPhysics { source: e })?;
109
110        Ok(Self {
111            initial_state,
112            final_state,
113        })
114    }
115
116    /// Return the gravitational parameter of this Lambert problem
117    pub fn mu_km2_s3(&self) -> f64 {
118        self.initial_state.frame.mu_km3_s2().unwrap()
119    }
120}
121
122/// A solution to the Lambert problem.
123#[derive(Copy, Clone, Debug)]
124pub struct LambertSolution {
125    pub v_init_km_s: Vector3<f64>,
126    pub v_final_km_s: Vector3<f64>,
127    /// Turn angle ONLY computed with Godding method
128    pub phi_rad: f64,
129    pub input: LambertInput,
130}
131
132impl LambertSolution {
133    /// Return the v infinity vector at departure, in km/s
134    pub fn v_inf_outgoing_km_s(&self) -> Vector3<f64> {
135        self.input.initial_state.velocity_km_s - self.v_init_km_s
136    }
137
138    /// Return v infinity vector at arrival, in km/s
139    pub fn v_inf_incoming_km_s(&self) -> Vector3<f64> {
140        self.input.final_state.velocity_km_s - self.v_final_km_s
141    }
142
143    /// Return the transfer orbit computed from setting the departure velocity to the initial state.
144    pub fn transfer_orbit(mut self) -> Orbit {
145        self.input.initial_state.velocity_km_s = self.v_init_km_s;
146        self.input.initial_state
147    }
148
149    /// Return the arrival orbit computed from setting the arrival velocity to the final state.
150    pub fn arrival_orbit(mut self) -> Orbit {
151        self.input.final_state.velocity_km_s = self.v_final_km_s;
152        self.input.final_state
153    }
154
155    /// Return the declination of the departure v infinity (i.e. the outgoing asymptote velocity vector), in degrees
156    pub fn v_inf_outgoing_declination_deg(&self) -> f64 {
157        // Must negate compared to the departure location
158        let v_inf_km_s = -self.v_inf_outgoing_km_s();
159        (v_inf_km_s.z / v_inf_km_s.norm()).asin().to_degrees()
160    }
161
162    /// Return the right ascention of the departure v infinity (i.e. the outgoing asymptote velocity vector), in degrees
163    pub fn v_inf_outgoing_right_ascension_deg(&self) -> f64 {
164        // Must negate compared to the departure location
165        let v_inf_km_s = -self.v_inf_outgoing_km_s();
166        (v_inf_km_s.y.atan2(v_inf_km_s.x)).to_degrees()
167    }
168
169    /// Returns the c3 computed as the departure v infinity norm squared
170    pub fn c3_km2_s2(&self) -> f64 {
171        self.v_inf_outgoing_km_s().norm_squared()
172    }
173}
174
175#[cfg(test)]
176mod ut_lambert_transfer_kind {
177    use super::{TransferKind, Vector3};
178
179    #[test]
180    fn test_auto_direction_of_motion_long_way() {
181        // Regression: Δν = 3π/2 (r_init on +x, r_final on -y) must select
182        // the long way (-1.0). The pre-fix expression
183        // `r_init[1].atan2(r_final[1])` yielded the short way (+1.0).
184        let r_init = Vector3::new(8000.0, 0.0, 0.0);
185        let r_final = Vector3::new(0.0, -8000.0, 0.0);
186
187        let dm = TransferKind::Auto
188            .direction_of_motion(&r_final, &r_init)
189            .unwrap();
190
191        assert_eq!(dm, -1.0, "Auto should pick long-way for Δν=3π/2; got {dm}");
192    }
193
194    #[test]
195    fn test_auto_direction_of_motion_short_way() {
196        // Sanity companion: Δν = π/2 (+x → +y) must select short way.
197        let r_init = Vector3::new(8000.0, 0.0, 0.0);
198        let r_final = Vector3::new(0.0, 8000.0, 0.0);
199
200        let dm = TransferKind::Auto
201            .direction_of_motion(&r_final, &r_init)
202            .unwrap();
203
204        assert_eq!(dm, 1.0, "Auto should pick short-way for Δν=π/2; got {dm}");
205    }
206}
nyx_space/tools/lambert/mod.rs

nyx_space/tools/lambert/
mod.rs
