From 3b2c11ba10a79e419feb2a92eca473e977d14bbb Mon Sep 17 00:00:00 2001 From: ThibFrgsGmz Date: Sun, 11 Jun 2023 21:12:49 +1200 Subject: [PATCH 1/4] (feat) breakdown Lambert::stardard() function --- src/tools/lambert.rs | 146 +++++++++++++++++++++++++++---------------- 1 file changed, 92 insertions(+), 54 deletions(-) diff --git a/src/tools/lambert.rs b/src/tools/lambert.rs index 79066098..f18eae63 100644 --- a/src/tools/lambert.rs +++ b/src/tools/lambert.rs @@ -24,6 +24,9 @@ const TAU: f64 = 2.0 * PI; const LAMBERT_EPSILON: f64 = 1e-4; // General epsilon const LAMBERT_EPSILON_TIME: f64 = 1e-4; // Time epsilon const LAMBERT_EPSILON_RAD: f64 = (5e-5 / 180.0) * PI; // 0.00005 degrees +/// Maximum number of iterations allowed in the Lambert problem solver. +/// This is a safety measure to prevent infinite loops in case a solution cannot be found. +const MAX_ITERATIONS: usize = 1000; /// Define the transfer kind for a Lambert pub enum TransferKind { @@ -40,10 +43,87 @@ pub struct LambertSolution { pub phi: f64, } -/// Solves the Lambert boundary problem using a standard secant method. +/// Calculate the direction multiplier based on the transfer kind. +/// +/// # Arguments +/// +/// * `kind` - The kind of transfer (auto, short way, long way, or number of revolutions). +/// * `r_final` - The final radius vector. +/// * `r_init` - The initial radius vector. +/// +/// # Returns +/// +/// `Result` - The direction multiplier or an error if the transfer kind is not supported. +fn calculate_dm( + kind: &TransferKind, + r_final: &Vector3, + r_init: &Vector3, +) -> Result { + match kind { + TransferKind::Auto => { + let mut dnu = r_final[1].atan2(r_final[0]) - r_init[1].atan2(r_final[1]); + if dnu > TAU { + dnu -= TAU; + } else if dnu < 0.0 { + dnu += TAU; + } + + if dnu > std::f64::consts::PI { + Ok(-1.0) + } else { + Ok(1.0) + } + } + TransferKind::ShortWay => Ok(1.0), + TransferKind::LongWay => Ok(-1.0), + _ => Err(NyxError::LambertMultiRevNotSupported), + } +} + +/// Calculate the new values of phi, c2, and c3 based on the current value of phi. +/// +/// # Arguments +/// +/// `phi` - The current value of phi. +/// +/// # Returns +/// +/// `(f64, f64, f64)` - The new values of phi, c2, and c3. +fn calculate_phi_and_c2_c3(phi: f64) -> (f64, f64, f64) { + let mut c2 = 0.5; + let mut c3 = 1.0 / 6.0; + + if phi > LAMBERT_EPSILON { + let sqrt_phi = phi.sqrt(); + let (s_sphi, c_sphi) = sqrt_phi.sin_cos(); + c2 = (1.0 - c_sphi) / phi; + c3 = (sqrt_phi - s_sphi) / phi.powi(3).sqrt(); + } else if phi < -LAMBERT_EPSILON { + let sqrt_phi = (-phi).sqrt(); + c2 = (1.0 - sqrt_phi.cosh()) / phi; + c3 = (sqrt_phi.sinh() - sqrt_phi) / (-phi).powi(3).sqrt(); + } + + (phi, c2, c3) +} + +/// Solve the Lambert boundary problem using a standard secant method. +/// /// Given the initial and final radii, a time of flight, and a gravitational parameters, it returns the needed initial and final velocities /// along with φ which is the square of the difference in eccentric anomaly. Note that the direction of motion /// is computed directly in this function to simplify the generation of Pork chop plots. +/// +/// # Arguments +/// +/// * `r_init` - The initial radius vector. +/// * `r_final` - The final radius vector. +/// * `tof` - The time of flight. +/// * `gm` - The gravitational parameter. +/// * `kind` - The kind of transfer (auto, short way, long way, or number of revolutions). +/// +/// # Returns +/// +/// `Result` - The solution to the Lambert problem or an error if the problem could not be solved. pub fn standard( r_init: Vector3, r_final: Vector3, @@ -53,45 +133,24 @@ pub fn standard( ) -> Result { let r_init_norm = r_init.norm(); let r_final_norm = r_final.norm(); + let r_norm_product = r_init_norm * r_final_norm; + let cos_dnu = r_init.dot(&r_final) / r_norm_product; - let cos_dnu = r_init.dot(&r_final) / (r_init_norm * r_final_norm); + let dm = calculate_dm(&kind, &r_final, &r_init)?; - let dm = match kind { - TransferKind::Auto => { - let mut dnu = r_final[1].atan2(r_final[0]) - r_init[1].atan2(r_final[1]); - if dnu > TAU { - dnu -= TAU; - } else if dnu < 0.0 { - dnu += TAU; - } - - if dnu > std::f64::consts::PI { - -1.0 - } else { - 1.0 - } - } - TransferKind::ShortWay => 1.0, - TransferKind::LongWay => -1.0, - _ => return Err(NyxError::LambertMultiRevNotSupported), - }; - - // Compute the direction of motion let nu_init = r_init[1].atan2(r_init[0]); let nu_final = r_final[1].atan2(r_final[0]); - let a = dm * (r_init_norm * r_final_norm * (1.0 + cos_dnu)).sqrt(); + let a = dm * (r_norm_product * (1.0 + cos_dnu)).sqrt(); if nu_final - nu_init < LAMBERT_EPSILON_RAD && a.abs() < LAMBERT_EPSILON { return Err(NyxError::TargetsTooClose); } - // Define the search space (note that we do not support multirevs in this algorithm) let mut phi_upper = 4.0 * PI.powi(2); let mut phi_lower = -4.0 * PI.powi(2); - let mut phi = 0.0; // ??!? + let mut phi = 0.0; - // Initial guesses for c2 and c3 let mut c2: f64 = 1.0 / 2.0; let mut c3: f64 = 1.0 / 6.0; let mut iter: usize = 0; @@ -99,68 +158,47 @@ pub fn standard( let mut y = 0.0; while (cur_tof - tof).abs() > LAMBERT_EPSILON_TIME { - if iter > 1000 { + if iter > MAX_ITERATIONS { return Err(NyxError::MaxIterReached(format!( "Lambert solver failed after {} iterations", - 1000 + MAX_ITERATIONS ))); } iter += 1; y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt(); if a > 0.0 && y < 0.0 { - // Try to increase phi for _ in 0..500 { phi += 0.1; - // Recompute y y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt(); if y >= 0.0 { break; } } if y < 0.0 { - // If y is still negative, then our attempts have failed. return Err(NyxError::LambertNotReasonablePhi); } } let chi = (y / c2).sqrt(); - // Compute the current time of flight cur_tof = (chi.powi(3) * c3 + a * y.sqrt()) / gm.sqrt(); - // Update the next TOF we should use + if cur_tof < tof { phi_lower = phi; } else { phi_upper = phi; } - // Compute the next phi phi = (phi_upper + phi_lower) / 2.0; - - // Update c2 and c3 - if phi > LAMBERT_EPSILON { - let sqrt_phi = phi.sqrt(); - let (s_sphi, c_sphi) = sqrt_phi.sin_cos(); - c2 = (1.0 - c_sphi) / phi; - c3 = (sqrt_phi - s_sphi) / phi.powi(3).sqrt(); - } else if phi < -LAMBERT_EPSILON { - let sqrt_phi = (-phi).sqrt(); - c2 = (1.0 - sqrt_phi.cosh()) / phi; - c3 = (sqrt_phi.sinh() - sqrt_phi) / (-phi).powi(3).sqrt(); - } else { - // Reset c2 and c3 and try again - c2 = 0.5; - c3 = 1.0 / 6.0; - } + let (_phi, c2_new, c3_new) = calculate_phi_and_c2_c3(phi); + c2 = c2_new; + c3 = c3_new; } - // Time of flight matches! - let f = 1.0 - y / r_init_norm; let g_dot = 1.0 - y / r_final_norm; let g = a * (y / gm).sqrt(); - // Compute velocities Ok(LambertSolution { v_init: (r_final - f * r_init) / g, v_final: (1.0 / g) * (g_dot * r_final - r_init), From eb89cae210b70d506399d3d084b0d304904282e0 Mon Sep 17 00:00:00 2001 From: ThibFrgsGmz Date: Sun, 11 Jun 2023 21:18:34 +1200 Subject: [PATCH 2/4] (doc) bring back comments --- src/tools/lambert.rs | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/tools/lambert.rs b/src/tools/lambert.rs index f18eae63..6ac4075c 100644 --- a/src/tools/lambert.rs +++ b/src/tools/lambert.rs @@ -138,6 +138,7 @@ pub fn standard( let dm = calculate_dm(&kind, &r_final, &r_init)?; + // Compute the direction of motion let nu_init = r_init[1].atan2(r_init[0]); let nu_final = r_final[1].atan2(r_final[0]); @@ -147,6 +148,7 @@ pub fn standard( return Err(NyxError::TargetsTooClose); } + // Define the search space (note that we do not support multirevs in this algorithm) let mut phi_upper = 4.0 * PI.powi(2); let mut phi_lower = -4.0 * PI.powi(2); let mut phi = 0.0; @@ -168,37 +170,44 @@ pub fn standard( y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt(); if a > 0.0 && y < 0.0 { + // Try to increase phi for _ in 0..500 { phi += 0.1; + // Recompute y y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt(); if y >= 0.0 { break; } } + // If y is still negative, then our attempts have failed. if y < 0.0 { return Err(NyxError::LambertNotReasonablePhi); } } let chi = (y / c2).sqrt(); + // Compute the current time of flight cur_tof = (chi.powi(3) * c3 + a * y.sqrt()) / gm.sqrt(); - + // Update the next TOF we should use if cur_tof < tof { phi_lower = phi; } else { phi_upper = phi; } + // Compute the next phi phi = (phi_upper + phi_lower) / 2.0; let (_phi, c2_new, c3_new) = calculate_phi_and_c2_c3(phi); c2 = c2_new; c3 = c3_new; } + // Time of flight matches! let f = 1.0 - y / r_init_norm; let g_dot = 1.0 - y / r_final_norm; let g = a * (y / gm).sqrt(); + // Compute velocities Ok(LambertSolution { v_init: (r_final - f * r_init) / g, v_final: (1.0 / g) * (g_dot * r_final - r_init), From b98dcbd591d7400378644e5b476b721522d8b894 Mon Sep 17 00:00:00 2001 From: ThibFrgsGmz Date: Mon, 12 Jun 2023 22:03:30 +1200 Subject: [PATCH 3/4] (feat) refactor according to code review --- src/tools/lambert.rs | 105 ++++++++++++++++++++++--------------------- 1 file changed, 54 insertions(+), 51 deletions(-) diff --git a/src/tools/lambert.rs b/src/tools/lambert.rs index 6ac4075c..c6bf6781 100644 --- a/src/tools/lambert.rs +++ b/src/tools/lambert.rs @@ -36,6 +36,44 @@ pub enum TransferKind { NRevs(u8), } +impl TransferKind { + /// Calculate the direction multiplier based on the transfer kind. + /// + /// # Arguments + /// + /// * `r_final` - The final radius vector. + /// * `r_init` - The initial radius vector. + /// + /// # Returns + /// + /// * `Result` - The direction multiplier or an error if the transfer kind is not supported. + fn direction_of_motion( + self, + r_final: &Vector3, + r_init: &Vector3, + ) -> Result { + match self { + TransferKind::Auto => { + let mut dnu = r_final[1].atan2(r_final[0]) - r_init[1].atan2(r_final[1]); + if dnu > TAU { + dnu -= TAU; + } else if dnu < 0.0 { + dnu += TAU; + } + + if dnu > std::f64::consts::PI { + Ok(-1.0) + } else { + Ok(1.0) + } + } + TransferKind::ShortWay => Ok(1.0), + TransferKind::LongWay => Ok(-1.0), + _ => Err(NyxError::LambertMultiRevNotSupported), + } + } +} + #[derive(Debug)] pub struct LambertSolution { pub v_init: Vector3, @@ -43,43 +81,6 @@ pub struct LambertSolution { pub phi: f64, } -/// Calculate the direction multiplier based on the transfer kind. -/// -/// # Arguments -/// -/// * `kind` - The kind of transfer (auto, short way, long way, or number of revolutions). -/// * `r_final` - The final radius vector. -/// * `r_init` - The initial radius vector. -/// -/// # Returns -/// -/// `Result` - The direction multiplier or an error if the transfer kind is not supported. -fn calculate_dm( - kind: &TransferKind, - r_final: &Vector3, - r_init: &Vector3, -) -> Result { - match kind { - TransferKind::Auto => { - let mut dnu = r_final[1].atan2(r_final[0]) - r_init[1].atan2(r_final[1]); - if dnu > TAU { - dnu -= TAU; - } else if dnu < 0.0 { - dnu += TAU; - } - - if dnu > std::f64::consts::PI { - Ok(-1.0) - } else { - Ok(1.0) - } - } - TransferKind::ShortWay => Ok(1.0), - TransferKind::LongWay => Ok(-1.0), - _ => Err(NyxError::LambertMultiRevNotSupported), - } -} - /// Calculate the new values of phi, c2, and c3 based on the current value of phi. /// /// # Arguments @@ -136,9 +137,8 @@ pub fn standard( let r_norm_product = r_init_norm * r_final_norm; let cos_dnu = r_init.dot(&r_final) / r_norm_product; - let dm = calculate_dm(&kind, &r_final, &r_init)?; + let dm = kind.direction_of_motion(&r_final, &r_init)?; - // Compute the direction of motion let nu_init = r_init[1].atan2(r_init[0]); let nu_final = r_final[1].atan2(r_final[0]); @@ -148,7 +148,6 @@ pub fn standard( return Err(NyxError::TargetsTooClose); } - // Define the search space (note that we do not support multirevs in this algorithm) let mut phi_upper = 4.0 * PI.powi(2); let mut phi_lower = -4.0 * PI.powi(2); let mut phi = 0.0; @@ -170,44 +169,48 @@ pub fn standard( y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt(); if a > 0.0 && y < 0.0 { - // Try to increase phi for _ in 0..500 { phi += 0.1; - // Recompute y y = r_init_norm + r_final_norm + a * (phi * c3 - 1.0) / c2.sqrt(); if y >= 0.0 { break; } } - // If y is still negative, then our attempts have failed. if y < 0.0 { return Err(NyxError::LambertNotReasonablePhi); } } let chi = (y / c2).sqrt(); - // Compute the current time of flight cur_tof = (chi.powi(3) * c3 + a * y.sqrt()) / gm.sqrt(); - // Update the next TOF we should use + if cur_tof < tof { phi_lower = phi; } else { phi_upper = phi; } - // Compute the next phi phi = (phi_upper + phi_lower) / 2.0; - let (_phi, c2_new, c3_new) = calculate_phi_and_c2_c3(phi); - c2 = c2_new; - c3 = c3_new; + + if phi > LAMBERT_EPSILON { + let sqrt_phi = phi.sqrt(); + let (s_sphi, c_sphi) = sqrt_phi.sin_cos(); + c2 = (1.0 - c_sphi) / phi; + c3 = (sqrt_phi - s_sphi) / phi.powi(3).sqrt(); + } else if phi < -LAMBERT_EPSILON { + let sqrt_phi = (-phi).sqrt(); + c2 = (1.0 - sqrt_phi.cosh()) / phi; + c3 = (sqrt_phi.sinh() - sqrt_phi) / (-phi).powi(3).sqrt(); + } else { + c2 = 0.5; + c3 = 1.0 / 6.0; + } } - // Time of flight matches! let f = 1.0 - y / r_init_norm; let g_dot = 1.0 - y / r_final_norm; let g = a * (y / gm).sqrt(); - // Compute velocities Ok(LambertSolution { v_init: (r_final - f * r_init) / g, v_final: (1.0 / g) * (g_dot * r_final - r_init), From 3443b220f687d7df482c7e809ab167eb8fbfba96 Mon Sep 17 00:00:00 2001 From: ThibFrgsGmz Date: Tue, 13 Jun 2023 20:50:05 +1200 Subject: [PATCH 4/4] (fix) remove unused function --- src/tools/lambert.rs | 27 --------------------------- 1 file changed, 27 deletions(-) diff --git a/src/tools/lambert.rs b/src/tools/lambert.rs index c6bf6781..08179bfc 100644 --- a/src/tools/lambert.rs +++ b/src/tools/lambert.rs @@ -81,33 +81,6 @@ pub struct LambertSolution { pub phi: f64, } -/// Calculate the new values of phi, c2, and c3 based on the current value of phi. -/// -/// # Arguments -/// -/// `phi` - The current value of phi. -/// -/// # Returns -/// -/// `(f64, f64, f64)` - The new values of phi, c2, and c3. -fn calculate_phi_and_c2_c3(phi: f64) -> (f64, f64, f64) { - let mut c2 = 0.5; - let mut c3 = 1.0 / 6.0; - - if phi > LAMBERT_EPSILON { - let sqrt_phi = phi.sqrt(); - let (s_sphi, c_sphi) = sqrt_phi.sin_cos(); - c2 = (1.0 - c_sphi) / phi; - c3 = (sqrt_phi - s_sphi) / phi.powi(3).sqrt(); - } else if phi < -LAMBERT_EPSILON { - let sqrt_phi = (-phi).sqrt(); - c2 = (1.0 - sqrt_phi.cosh()) / phi; - c3 = (sqrt_phi.sinh() - sqrt_phi) / (-phi).powi(3).sqrt(); - } - - (phi, c2, c3) -} - /// Solve the Lambert boundary problem using a standard secant method. /// /// Given the initial and final radii, a time of flight, and a gravitational parameters, it returns the needed initial and final velocities