pykep_core/astro/
lambert.rs

1// Copyright (c) 2023-2026 Dario Izzo (dario.izzo@gmail.com)
2// Copyright (c) 2026 pykep-rust contributors
3// SPDX-License-Identifier: MPL-2.0
4//
5// Adapted from src/lambert_problem.cpp at pykep commit
6// 53b1ca3ce5f8c223f96819b2ea9ba16c3719e63e.
7
8//! Izzo single- and multi-revolution Lambert solver.
9//!
10//! Iteration exhaustion is an explicit [`crate::PykepError::ConvergenceFailure`]
11//! instead of returning the pinned C++ implementation's last unconverged
12//! iterate. At the measure-zero `|x - 1| = 0.01` boundary, Rust selects the
13//! Lagrange time-of-flight expression.
14
15use core::f64::consts::PI;
16
17use crate::error::{ensure_finite, ensure_finite_output};
18use crate::math::linalg::{cross, norm, normalize};
19use crate::{PykepError, Result, Vector3};
20
21/// Deterministic position of a solution within its revolution family.
22#[derive(Clone, Copy, Debug, Eq, PartialEq)]
23pub enum LambertPath {
24    /// The unique zero-revolution solution.
25    ZeroRevolution,
26    /// Multi-revolution solution on the left side of minimum time.
27    Left,
28    /// Multi-revolution solution on the right side of minimum time.
29    Right,
30}
31
32/// One Lambert velocity solution.
33#[derive(Clone, Debug, PartialEq)]
34pub struct LambertSolution {
35    /// Departure velocity.
36    pub departure_velocity: Vector3,
37    /// Arrival velocity.
38    pub arrival_velocity: Vector3,
39    /// Izzo solver variable.
40    pub x: f64,
41    /// Householder iterations used.
42    pub iterations: usize,
43    /// Complete revolutions.
44    pub revolutions: usize,
45    /// Path within the revolution family.
46    pub path: LambertPath,
47}
48
49/// Inputs for one member of an ordered Lambert batch.
50#[derive(Clone, Copy, Debug, PartialEq)]
51pub struct LambertRequest {
52    /// Initial position.
53    pub initial_position: Vector3,
54    /// Final position.
55    pub final_position: Vector3,
56    /// Positive time of flight.
57    pub time: f64,
58    /// Positive gravitational parameter.
59    pub mu: f64,
60    /// Whether clockwise/retrograde motion is requested.
61    pub clockwise: bool,
62    /// Largest requested complete-revolution count.
63    pub maximum_revolutions: usize,
64}
65
66impl LambertRequest {
67    /// Creates one Lambert batch request.
68    #[must_use]
69    pub const fn new(
70        initial_position: Vector3,
71        final_position: Vector3,
72        time: f64,
73        mu: f64,
74        clockwise: bool,
75        maximum_revolutions: usize,
76    ) -> Self {
77        Self {
78            initial_position,
79            final_position,
80            time,
81            mu,
82            clockwise,
83            maximum_revolutions,
84        }
85    }
86
87    fn solve(&self) -> Result<LambertProblem> {
88        LambertProblem::new(
89            self.initial_position,
90            self.final_position,
91            self.time,
92            self.mu,
93            self.clockwise,
94            self.maximum_revolutions,
95        )
96    }
97}
98
99/// Solves an ordered batch of independent Lambert problems.
100///
101/// Zero workers uses Rayon's shared global pool, one executes serially, and
102/// larger values use exactly that many cached worker threads. Both problem
103/// order and each problem's branch order are deterministic.
104///
105/// # Errors
106///
107/// Returns an invalid worker count or the first Lambert error in input order.
108pub fn solve_lambert_batch(
109    requests: &[LambertRequest],
110    workers: usize,
111) -> Result<Vec<LambertProblem>> {
112    crate::batch::try_map(requests, workers, LambertRequest::solve)
113}
114
115/// Solved Lambert boundary-value problem.
116///
117/// ```
118/// use pykep_core::astro::lambert::{LambertPath, LambertProblem};
119///
120/// let problem = LambertProblem::new(
121///     [1.0, 0.0, 0.0],
122///     [0.2, 1.1, 0.3],
123///     20.0,
124///     1.0,
125///     false,
126///     2,
127/// )?;
128/// assert_eq!(problem.solutions()[0].path, LambertPath::ZeroRevolution);
129/// # Ok::<(), pykep_core::PykepError>(())
130/// ```
131#[derive(Clone, Debug, PartialEq)]
132pub struct LambertProblem {
133    initial_position: Vector3,
134    final_position: Vector3,
135    time: f64,
136    mu: f64,
137    clockwise: bool,
138    chord: f64,
139    semiperimeter: f64,
140    lambda: f64,
141    solutions: Vec<LambertSolution>,
142}
143
144impl LambertProblem {
145    /// Solves a Lambert problem and all feasible branches through
146    /// `maximum_revolutions`.
147    ///
148    /// Solution order is zero-revolution, then left/right for revolution
149    /// counts `1..=maximum_revolutions`.
150    ///
151    /// # Errors
152    ///
153    /// Returns an error for non-finite input, non-positive time or `mu`, zero
154    /// positions, coincident/collinear endpoints, or non-convergence.
155    pub fn new(
156        initial_position: Vector3,
157        final_position: Vector3,
158        time: f64,
159        mu: f64,
160        clockwise: bool,
161        maximum_revolutions: usize,
162    ) -> Result<Self> {
163        for &value in initial_position.iter().chain(final_position.iter()) {
164            ensure_finite("position", value)?;
165        }
166        ensure_finite("time", time)?;
167        ensure_finite("mu", mu)?;
168        if time <= 0.0 || mu <= 0.0 {
169            return Err(PykepError::InvalidInput {
170                parameter: "time/mu",
171                reason: "must be greater than zero".into(),
172            });
173        }
174        let initial_radius = norm(&initial_position)?;
175        let final_radius = norm(&final_position)?;
176        if initial_radius == 0.0 || final_radius == 0.0 {
177            return Err(PykepError::SingularGeometry {
178                operation: "LambertProblem::new",
179            });
180        }
181        let chord_vector = [
182            final_position[0] - initial_position[0],
183            final_position[1] - initial_position[1],
184            final_position[2] - initial_position[2],
185        ];
186        let chord = norm(&chord_vector)?;
187        if chord == 0.0 {
188            return Err(PykepError::SingularGeometry {
189                operation: "LambertProblem::new",
190            });
191        }
192        let initial_direction = normalize(&initial_position)?;
193        let final_direction = normalize(&final_position)?;
194        let angular = normalize(&cross(&initial_direction, &final_direction)?)?;
195        if angular[2] == 0.0 {
196            return Err(PykepError::SingularGeometry {
197                operation: "LambertProblem::automatic_direction",
198            });
199        }
200        let semiperimeter = (chord + initial_radius + final_radius) / 2.0;
201        let lambda_squared = 1.0 - chord / semiperimeter;
202        let mut lambda = lambda_squared.sqrt();
203        let mut initial_tangent = normalize(&cross(&angular, &initial_direction)?)?;
204        let mut final_tangent = normalize(&cross(&angular, &final_direction)?)?;
205        if angular[2] < 0.0 {
206            lambda = -lambda;
207            initial_tangent = initial_tangent.map(|value| -value);
208            final_tangent = final_tangent.map(|value| -value);
209        }
210        if clockwise {
211            lambda = -lambda;
212            initial_tangent = initial_tangent.map(|value| -value);
213            final_tangent = final_tangent.map(|value| -value);
214        }
215        let dimensionless_time = (2.0 * mu / semiperimeter.powi(3)).sqrt() * time;
216        let mut problem = Self {
217            initial_position,
218            final_position,
219            time,
220            mu,
221            clockwise,
222            chord,
223            semiperimeter,
224            lambda,
225            solutions: Vec::new(),
226        };
227        let mut maximum = ((dimensionless_time / PI) as usize).min(maximum_revolutions);
228        let time_at_zero = lambda.acos() + lambda * (1.0 - lambda_squared).sqrt();
229        let lambda_cubed = lambda * lambda_squared;
230        let time_at_one = 2.0 / 3.0 * (1.0 - lambda_cubed);
231        if maximum > 0 {
232            let mut minimum_time = time_at_zero + maximum as f64 * PI;
233            if dimensionless_time < minimum_time {
234                let mut old_x = 0.0;
235                for _ in 0..13 {
236                    let (first, second, third) = problem.time_derivatives(old_x, minimum_time);
237                    let new_x = if first != 0.0 {
238                        old_x - first * second / (second * second - first * third / 2.0)
239                    } else {
240                        old_x
241                    };
242                    if (old_x - new_x).abs() < 1e-13 {
243                        break;
244                    }
245                    minimum_time = problem.time_of_flight(new_x, maximum)?;
246                    old_x = new_x;
247                }
248                if minimum_time > dimensionless_time {
249                    maximum -= 1;
250                }
251            }
252            maximum = maximum.min(maximum_revolutions);
253        }
254
255        let initial_x = if dimensionless_time >= time_at_zero {
256            -(dimensionless_time - time_at_zero) / (dimensionless_time - time_at_zero + 4.0)
257        } else if dimensionless_time <= time_at_one {
258            time_at_one * (time_at_one - dimensionless_time)
259                / (0.4 * (1.0 - lambda_squared * lambda_cubed) * dimensionless_time)
260                + 1.0
261        } else {
262            (dimensionless_time / time_at_zero)
263                .powf(core::f64::consts::LN_2 / (time_at_one / time_at_zero).ln())
264                - 1.0
265        };
266        let (x, iterations) = problem.householder(dimensionless_time, initial_x, 0, 1e-5)?;
267        problem.push_solution(
268            x,
269            iterations,
270            0,
271            LambertPath::ZeroRevolution,
272            initial_radius,
273            final_radius,
274            &initial_direction,
275            &final_direction,
276            &initial_tangent,
277            &final_tangent,
278        )?;
279        for revolution in 1..=maximum {
280            let temporary =
281                (((revolution + 1) as f64 * PI) / (8.0 * dimensionless_time)).powf(2.0 / 3.0);
282            let initial_left = (temporary - 1.0) / (temporary + 1.0);
283            let (left, iterations) =
284                problem.householder(dimensionless_time, initial_left, revolution, 1e-8)?;
285            problem.push_solution(
286                left,
287                iterations,
288                revolution,
289                LambertPath::Left,
290                initial_radius,
291                final_radius,
292                &initial_direction,
293                &final_direction,
294                &initial_tangent,
295                &final_tangent,
296            )?;
297            let temporary = (8.0 * dimensionless_time / (revolution as f64 * PI)).powf(2.0 / 3.0);
298            let initial_right = (temporary - 1.0) / (temporary + 1.0);
299            let (right, iterations) =
300                problem.householder(dimensionless_time, initial_right, revolution, 1e-8)?;
301            problem.push_solution(
302                right,
303                iterations,
304                revolution,
305                LambertPath::Right,
306                initial_radius,
307                final_radius,
308                &initial_direction,
309                &final_direction,
310                &initial_tangent,
311                &final_tangent,
312            )?;
313        }
314        Ok(problem)
315    }
316
317    #[allow(clippy::too_many_arguments)]
318    fn push_solution(
319        &mut self,
320        x: f64,
321        iterations: usize,
322        revolutions: usize,
323        path: LambertPath,
324        initial_radius: f64,
325        final_radius: f64,
326        initial_direction: &Vector3,
327        final_direction: &Vector3,
328        initial_tangent: &Vector3,
329        final_tangent: &Vector3,
330    ) -> Result<()> {
331        let lambda_squared = self.lambda * self.lambda;
332        let y = (1.0 - lambda_squared + lambda_squared * x * x).sqrt();
333        let gamma = (self.mu * self.semiperimeter / 2.0).sqrt();
334        let rho = (initial_radius - final_radius) / self.chord;
335        let sigma = (1.0 - rho * rho).sqrt();
336        let radial_initial =
337            gamma * ((self.lambda * y - x) - rho * (self.lambda * y + x)) / initial_radius;
338        let radial_final =
339            -gamma * ((self.lambda * y - x) + rho * (self.lambda * y + x)) / final_radius;
340        let tangential = gamma * sigma * (y + self.lambda * x);
341        let tangential_initial = tangential / initial_radius;
342        let tangential_final = tangential / final_radius;
343        let departure_velocity = core::array::from_fn(|index| {
344            radial_initial * initial_direction[index] + tangential_initial * initial_tangent[index]
345        });
346        let arrival_velocity = core::array::from_fn(|index| {
347            radial_final * final_direction[index] + tangential_final * final_tangent[index]
348        });
349        for &value in departure_velocity.iter().chain(arrival_velocity.iter()) {
350            ensure_finite_output("LambertProblem::velocity_reconstruction", value)?;
351        }
352        self.solutions.push(LambertSolution {
353            departure_velocity,
354            arrival_velocity,
355            x,
356            iterations,
357            revolutions,
358            path,
359        });
360        Ok(())
361    }
362
363    fn householder(
364        &self,
365        target_time: f64,
366        mut x: f64,
367        revolutions: usize,
368        tolerance: f64,
369    ) -> Result<(f64, usize)> {
370        for iteration in 1..=15 {
371            let time = self.time_of_flight(x, revolutions)?;
372            let (first, second, third) = self.time_derivatives(x, time);
373            let difference = time - target_time;
374            let first_squared = first * first;
375            let next = x - difference * (first_squared - difference * second / 2.0)
376                / (first * (first_squared - difference * second)
377                    + third * difference * difference / 6.0);
378            if !next.is_finite() {
379                return Err(PykepError::NumericalOverflow {
380                    operation: "LambertProblem::householder",
381                });
382            }
383            let error = (x - next).abs();
384            x = next;
385            if error <= tolerance {
386                return Ok((x, iteration));
387            }
388        }
389        Err(PykepError::ConvergenceFailure {
390            operation: "LambertProblem::householder",
391            iterations: 15,
392        })
393    }
394
395    fn time_derivatives(&self, x: f64, time: f64) -> (f64, f64, f64) {
396        let lambda_squared = self.lambda * self.lambda;
397        let lambda_cubed = lambda_squared * self.lambda;
398        let one_minus_x_squared = 1.0 - x * x;
399        let y = (1.0 - lambda_squared * one_minus_x_squared).sqrt();
400        let first = (3.0 * time * x - 2.0 + 2.0 * lambda_cubed * x / y) / one_minus_x_squared;
401        let second = (3.0 * time
402            + 5.0 * x * first
403            + 2.0 * (1.0 - lambda_squared) * lambda_cubed / y.powi(3))
404            / one_minus_x_squared;
405        let third = (7.0 * x * second + 8.0 * first
406            - 6.0 * (1.0 - lambda_squared) * lambda_squared * lambda_cubed * x / y.powi(5))
407            / one_minus_x_squared;
408        (first, second, third)
409    }
410
411    fn time_of_flight(&self, x: f64, revolutions: usize) -> Result<f64> {
412        let distance = (x - 1.0).abs();
413        if (0.01..0.2).contains(&distance) {
414            return self.time_of_flight_lagrange(x, revolutions);
415        }
416        let lambda_squared = self.lambda * self.lambda;
417        let energy = x * x - 1.0;
418        let rho = energy.abs();
419        let z = (1.0 + lambda_squared * energy).sqrt();
420        let time = if distance < 0.01 {
421            let eta = z - self.lambda * x;
422            let series_argument = 0.5 * (1.0 - self.lambda - x * eta);
423            let q = 4.0 / 3.0 * hypergeometric(series_argument)?;
424            (eta.powi(3) * q + 4.0 * self.lambda * eta) / 2.0
425                + revolutions as f64 * PI / rho.powf(1.5)
426        } else {
427            let y = rho.sqrt();
428            let g = x * z - self.lambda * energy;
429            let d = if energy < 0.0 {
430                revolutions as f64 * PI + g.clamp(-1.0, 1.0).acos()
431            } else {
432                (y * (z - self.lambda * x) + g).ln()
433            };
434            (x - self.lambda * z - d / y) / energy
435        };
436        ensure_finite_output("LambertProblem::time_of_flight", time)
437    }
438
439    fn time_of_flight_lagrange(&self, x: f64, revolutions: usize) -> Result<f64> {
440        let semi_axis = 1.0 / (1.0 - x * x);
441        let time = if semi_axis > 0.0 {
442            let alpha = 2.0 * x.clamp(-1.0, 1.0).acos();
443            let mut beta = 2.0
444                * (self.lambda.abs() / semi_axis.sqrt())
445                    .clamp(-1.0, 1.0)
446                    .asin();
447            if self.lambda < 0.0 {
448                beta = -beta;
449            }
450            semi_axis.powf(1.5)
451                * ((alpha - alpha.sin()) - (beta - beta.sin()) + 2.0 * PI * revolutions as f64)
452                / 2.0
453        } else {
454            let alpha = 2.0 * x.acosh();
455            let mut beta = 2.0 * (-self.lambda * self.lambda / semi_axis).sqrt().asinh();
456            if self.lambda < 0.0 {
457                beta = -beta;
458            }
459            -semi_axis * (-semi_axis).sqrt() * ((beta - beta.sinh()) - (alpha - alpha.sinh())) / 2.0
460        };
461        ensure_finite_output("LambertProblem::time_of_flight_lagrange", time)
462    }
463
464    /// Initial position.
465    #[must_use]
466    pub const fn initial_position(&self) -> Vector3 {
467        self.initial_position
468    }
469
470    /// Final position.
471    #[must_use]
472    pub const fn final_position(&self) -> Vector3 {
473        self.final_position
474    }
475
476    /// Time of flight.
477    #[must_use]
478    pub const fn time(&self) -> f64 {
479        self.time
480    }
481
482    /// Gravitational parameter.
483    #[must_use]
484    pub const fn mu(&self) -> f64 {
485        self.mu
486    }
487
488    /// Whether clockwise/retrograde motion was requested.
489    #[must_use]
490    pub const fn clockwise(&self) -> bool {
491        self.clockwise
492    }
493
494    /// Maximum feasible revolution count returned.
495    #[must_use]
496    pub fn maximum_revolutions(&self) -> usize {
497        self.solutions
498            .last()
499            .map_or(0, |solution| solution.revolutions)
500    }
501
502    /// Ordered solution family.
503    #[must_use]
504    pub fn solutions(&self) -> &[LambertSolution] {
505        &self.solutions
506    }
507}
508
509fn hypergeometric(argument: f64) -> Result<f64> {
510    let mut sum = 1.0;
511    let mut coefficient = 1.0;
512    for index in 0..10_000 {
513        let index = index as f64;
514        coefficient *= (3.0 + index) * (1.0 + index) / (2.5 + index) * argument / (index + 1.0);
515        sum += coefficient;
516        if coefficient.abs() <= 1e-11 {
517            return ensure_finite_output("LambertProblem::hypergeometric", sum);
518        }
519    }
520    Err(PykepError::ConvergenceFailure {
521        operation: "LambertProblem::hypergeometric",
522        iterations: 10_000,
523    })
524}
525
526#[cfg(test)]
527mod tests {
528    use super::*;
529    use crate::astro::propagation::propagate_lagrangian;
530
531    #[test]
532    fn upstream_clockwise_reference_matches() {
533        let problem = LambertProblem::new(
534            [1.0, 0.0, 0.0],
535            [0.0, 1.0, 0.0],
536            3.0 * PI / 2.0,
537            1.0,
538            true,
539            5,
540        )
541        .unwrap();
542        assert_eq!(problem.maximum_revolutions(), 0);
543        assert_eq!(problem.solutions()[0].iterations, 3);
544        assert!((problem.solutions()[0].x + 0.382_683_432_365_089_6).abs() < 1e-14);
545        for (actual, expected) in problem.solutions()[0]
546            .departure_velocity
547            .iter()
548            .zip([0.0, -1.0, 0.0])
549        {
550            assert!((actual - expected).abs() < 3e-15);
551        }
552        for (actual, expected) in problem.solutions()[0]
553            .arrival_velocity
554            .iter()
555            .zip([1.0, 0.0, 0.0])
556        {
557            assert!((actual - expected).abs() < 3e-15);
558        }
559    }
560
561    #[test]
562    fn all_solution_branches_reconstruct_the_endpoint() {
563        let problem =
564            LambertProblem::new([1.0, 0.0, 0.0], [0.2, 1.1, 0.3], 20.0, 1.0, false, 4).unwrap();
565        assert!(problem.solutions().len() >= 3);
566        for solution in problem.solutions() {
567            let initial = [
568                1.0,
569                0.0,
570                0.0,
571                solution.departure_velocity[0],
572                solution.departure_velocity[1],
573                solution.departure_velocity[2],
574            ];
575            let propagated = propagate_lagrangian(&initial, 20.0, 1.0).unwrap();
576            for (actual, expected) in propagated[..3].iter().zip([0.2, 1.1, 0.3]) {
577                assert!((actual - expected).abs() < 2e-10);
578            }
579        }
580    }
581
582    #[test]
583    fn time_of_flight_dispatches_cover_each_expression_family() {
584        for clockwise in [false, true] {
585            let problem =
586                LambertProblem::new([1.0, 0.0, 0.0], [0.2, 1.1, 0.3], 20.0, 1.0, clockwise, 0)
587                    .unwrap();
588            for x in [0.995, 0.9, 0.5, 1.1, 2.0] {
589                assert!(problem.time_of_flight(x, 0).unwrap().is_finite());
590            }
591            let target = problem.time_of_flight(0.2, 0).unwrap();
592            assert!(matches!(
593                problem.householder(target, 0.2, 0, -1.0),
594                Err(PykepError::ConvergenceFailure {
595                    operation: "LambertProblem::householder",
596                    iterations: 15
597                })
598            ));
599        }
600        assert!(matches!(
601            hypergeometric(f64::NAN),
602            Err(PykepError::ConvergenceFailure {
603                operation: "LambertProblem::hypergeometric",
604                iterations: 10_000
605            })
606        ));
607    }
608
609    #[test]
610    fn invalid_geometries_and_scales_are_typed_errors() {
611        let valid = [1.0, 0.0, 0.0];
612        for result in [
613            LambertProblem::new([f64::NAN, 0.0, 0.0], [0.0, 1.0, 0.0], 1.0, 1.0, false, 0),
614            LambertProblem::new(valid, [0.0, 1.0, 0.0], 0.0, 1.0, false, 0),
615            LambertProblem::new(valid, [0.0, 1.0, 0.0], 1.0, 0.0, false, 0),
616            LambertProblem::new([0.0; 3], [0.0, 1.0, 0.0], 1.0, 1.0, false, 0),
617            LambertProblem::new(valid, valid, 1.0, 1.0, false, 0),
618            LambertProblem::new(valid, [0.0, 0.0, 1.0], 1.0, 1.0, false, 0),
619            LambertProblem::new([1e308; 3], [-1e308; 3], 1.0, 1.0, false, 0),
620        ] {
621            assert!(result.is_err());
622        }
623    }
624
625    #[test]
626    fn ordered_parallel_batch_matches_scalar_problems_and_error_order() {
627        let requests = [
628            LambertRequest::new([1.0, 0.0, 0.0], [0.2, 1.1, 0.3], 20.0, 1.0, false, 2),
629            LambertRequest::new([1.1, 0.1, 0.0], [0.1, 1.2, 0.2], 22.0, 1.0, true, 1),
630        ];
631        let batch = solve_lambert_batch(&requests, 2).unwrap();
632        assert_eq!(
633            batch,
634            requests
635                .iter()
636                .map(LambertRequest::solve)
637                .collect::<Result<Vec<_>>>()
638                .unwrap()
639        );
640
641        let invalid = [
642            LambertRequest::new([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], 0.0, 1.0, false, 0),
643            LambertRequest::new([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], 1.0, 0.0, false, 0),
644        ];
645        assert_eq!(
646            solve_lambert_batch(&invalid, 2).unwrap_err(),
647            invalid[0].solve().unwrap_err()
648        );
649    }
650}
pykep_core/astro/lambert.rs

pykep_core/astro/
lambert.rs
