1use std::f64::consts::PI;
22
23const VALLADO_MU: f64 = 398600.4415;
26const TWOPI: f64 = 2.0 * PI;
27const SMALL: f64 = 1e-10;
28
29use crate::astro::math::vec3;
30
31fn cross(a: &[f64; 3], b: &[f64; 3]) -> [f64; 3] {
32 vec3::cross3_ref(a, b)
33}
34
35fn dot(a: &[f64; 3], b: &[f64; 3]) -> f64 {
36 vec3::dot3_ref(a, b)
37}
38
39fn mag(a: &[f64; 3]) -> f64 {
40 vec3::norm3_ref(a)
41}
42
43fn smul(s: f64, a: &[f64; 3]) -> [f64; 3] {
46 vec3::scale3(*a, s)
47}
48
49fn vadd(a: &[f64; 3], b: &[f64; 3]) -> [f64; 3] {
53 [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
54}
55
56fn seebatt(v: f64) -> f64 {
58 let c = [
59 9.0 / 7.0,
60 16.0 / 63.0,
61 25.0 / 99.0,
62 36.0 / 143.0,
63 49.0 / 195.0,
64 64.0 / 255.0,
65 81.0 / 323.0,
66 100.0 / 399.0,
67 121.0 / 483.0,
68 144.0 / 575.0,
69 169.0 / 675.0,
70 196.0 / 783.0,
71 225.0 / 899.0,
72 256.0 / 1023.0,
73 289.0 / 1155.0,
74 324.0 / 1295.0,
75 361.0 / 1443.0,
76 400.0 / 1599.0,
77 441.0 / 1763.0,
78 484.0 / 1935.0,
79 ];
80
81 let sqrtopv = (1.0 + v).sqrt();
82 let eta = v / (1.0 + sqrtopv).powi(2);
83
84 let mut term2 = 1.0 + c[19] * eta;
85 for j in (0..19).rev() {
86 term2 = 1.0 + c[j] * eta / term2;
87 }
88
89 8.0 * (1.0 + sqrtopv) / (3.0 + 1.0 / (5.0 + eta + (9.0 / 7.0) * eta / term2))
90}
91
92fn kbatt(v: f64) -> f64 {
94 let d = [
95 1.0 / 3.0,
96 4.0 / 27.0,
97 8.0 / 27.0,
98 2.0 / 9.0,
99 22.0 / 81.0,
100 208.0 / 891.0,
101 340.0 / 1287.0,
102 418.0 / 1755.0,
103 598.0 / 2295.0,
104 700.0 / 2907.0,
105 928.0 / 3591.0,
106 1054.0 / 4347.0,
107 1330.0 / 5175.0,
108 1480.0 / 6075.0,
109 1804.0 / 7047.0,
110 1978.0 / 8091.0,
111 2350.0 / 9207.0,
112 2548.0 / 10395.0,
113 2968.0 / 11655.0,
114 3190.0 / 12987.0,
115 3658.0 / 14391.0,
116 ];
117
118 let mut sum1: f64 = d[0];
120 let mut delold: f64 = 1.0;
121 let mut termold: f64 = d[0];
122 let ktr = 21;
123
124 for di in d.iter().take(ktr).skip(1) {
125 if termold.abs() <= 1e-8 {
126 break;
127 }
128 let del = 1.0 / (1.0 + di * v * delold);
129 let term = termold * (del - 1.0);
130 sum1 += term;
131 delold = del;
132 termold = term;
133 }
134 let _ = sum1; let mut term2 = 1.0 + d[ktr - 1] * v;
138 for i in 0..(ktr - 2) {
139 let sum2 = d[ktr - i - 2] * v / term2;
140 term2 = 1.0 + sum2;
141 }
142
143 d[0] / term2
144}
145
146fn hodograph(
148 r1: &[f64; 3],
149 r2: &[f64; 3],
150 v1: &[f64; 3],
151 p: f64,
152 ecc: f64,
153 dnu: f64,
154 dtsec: f64,
155) -> ([f64; 3], [f64; 3]) {
156 let magr1 = mag(r1);
157 let magr2 = mag(r2);
158
159 let a = VALLADO_MU * (1.0 / magr1 - 1.0 / p);
160 let b = (VALLADO_MU * ecc / p).powi(2) - a * a;
161 let x1_abs = if b <= 0.0 { 0.0 } else { b.sqrt() };
162 let mut x1 = -x1_abs;
163
164 let nvec;
165 if dnu.sin().abs() < SMALL {
166 let cp = cross(r1, v1);
168 let ncp = mag(&cp);
169 nvec = smul(1.0 / ncp, &cp);
170
171 if ecc < 1.0 {
172 let ptx = TWOPI * (p.powi(3) / (VALLADO_MU * (1.0 - ecc * ecc).powi(3))).sqrt();
173 if dtsec % ptx > ptx * 0.5 {
174 x1 = x1_abs;
175 }
176 }
177 } else {
178 let y2a = VALLADO_MU / p - x1 * dnu.sin() + a * dnu.cos();
180 let y2b = VALLADO_MU / p + x1 * dnu.sin() + a * dnu.cos();
181 if (VALLADO_MU / magr2 - y2b).abs() < (VALLADO_MU / magr2 - y2a).abs() {
182 x1 = x1_abs;
183 }
184
185 let cp = cross(r1, r2);
186 let ncp = mag(&cp);
187 nvec = if dnu % TWOPI > PI {
188 smul(-1.0 / ncp, &cp)
189 } else {
190 smul(1.0 / ncp, &cp)
191 };
192 }
193
194 let sqrtmup = (VALLADO_MU * p).sqrt();
195 let nr1 = cross(&nvec, r1);
196 let v1t = smul(
197 sqrtmup / magr1,
198 &vadd(&smul(x1 / VALLADO_MU, r1), &smul(1.0 / magr1, &nr1)),
199 );
200
201 let x2 = x1 * dnu.cos() + a * dnu.sin();
202 let nr2 = cross(&nvec, r2);
203 let v2t = smul(
204 sqrtmup / magr2,
205 &vadd(&smul(x2 / VALLADO_MU, r2), &smul(1.0 / magr2, &nr2)),
206 );
207
208 (v1t, v2t)
209}
210
211#[derive(Clone, Copy, PartialEq, Eq, Debug)]
213pub enum DirectionOfMotion {
214 Short,
216 Long,
218}
219
220#[derive(Clone, Copy, PartialEq, Eq, Debug)]
222pub enum DirectionOfEnergy {
223 Low,
225 High,
227}
228
229#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
231pub enum LambertError {
232 #[error("Lambert solver did not converge")]
234 NoConvergence,
235 #[error("position vector has near-zero magnitude")]
237 ZeroVector,
238 #[error("time of flight must be strictly positive")]
240 NonPositiveTimeOfFlight,
241 #[error("transfer-plane geometry is degenerate")]
247 DegenerateGeometry,
248 #[error("non-finite value encountered")]
250 NonFiniteValue,
251}
252
253fn all_finite(a: &[f64; 3]) -> bool {
255 a.iter().all(|x| x.is_finite())
256}
257
258pub fn battin(
267 r1: &[f64; 3],
268 r2: &[f64; 3],
269 v1: &[f64; 3],
270 dm: DirectionOfMotion,
271 de: DirectionOfEnergy,
272 nrev: i32,
273 dtsec: f64,
274) -> Result<([f64; 3], [f64; 3]), LambertError> {
275 if !all_finite(r1) || !all_finite(r2) || !all_finite(v1) {
278 return Err(LambertError::NonFiniteValue);
279 }
280
281 let magr1 = mag(r1);
282 let magr2 = mag(r2);
283 if !magr1.is_finite() || !magr2.is_finite() {
287 return Err(LambertError::NonFiniteValue);
288 }
289 if magr1 < SMALL || magr2 < SMALL {
290 return Err(LambertError::ZeroVector);
291 }
292
293 if !dtsec.is_finite() || dtsec <= 0.0 {
294 return Err(LambertError::NonPositiveTimeOfFlight);
295 }
296
297 let cosdeltanu = dot(r1, r2) / (magr1 * magr2);
299 let magrcrossr = mag(&cross(r1, r2));
300 if !magrcrossr.is_finite() {
303 return Err(LambertError::NonFiniteValue);
304 }
305 let sign = if dm == DirectionOfMotion::Short {
306 1.0
307 } else {
308 -1.0
309 };
310 let sindeltanu = sign * magrcrossr / (magr1 * magr2);
311 let mut dnu = sindeltanu.atan2(cosdeltanu);
312 if dnu < 0.0 {
313 dnu += TWOPI;
314 }
315
316 if dnu.sin().abs() < SMALL {
322 let n_v1 = mag(&cross(r1, v1));
323 let plane_from_v1 = cosdeltanu < 0.0 && n_v1.is_finite() && n_v1 >= SMALL;
324 if !plane_from_v1 {
325 return Err(LambertError::DegenerateGeometry);
326 }
327 }
328
329 let chord = (magr1 * magr1 + magr2 * magr2 - 2.0 * magr1 * magr2 * cosdeltanu).sqrt();
331 let s = (magr1 + magr2 + chord) * 0.5;
332 let eps = magr2 / magr1 - 1.0;
333
334 let lam = (magr1 * magr2).sqrt() / s * (dnu * 0.5).cos();
336 let l_ = ((1.0 - lam) / (1.0 + lam)).powi(2);
337 let m = 8.0 * VALLADO_MU * dtsec * dtsec / (s.powi(3) * (1.0 + lam).powi(6));
338
339 let mut xn = if nrev > 0 { 1.0 + 4.0 * l_ } else { l_ };
341
342 if de == DirectionOfEnergy::High && nrev > 0 {
343 xn = 1e-20;
345 let mut x = 10.0;
346 for _ in 0..20 {
347 if (xn - x).abs() < SMALL {
348 break;
349 }
350 x = xn;
351 let temp = 1.0 / (2.0 * (l_ - x * x));
352 let temp1 = x.sqrt();
353 let temp2 = (nrev as f64 * PI * 0.5 + temp1.atan()) / temp1;
354 let h1 = temp * (l_ + x) * (1.0 + 2.0 * x + l_);
355 let h2 = temp * m * temp1 * ((l_ - x * x) * temp2 - (l_ + x));
356
357 let b = 0.25 * 27.0 * h2 / (temp1 * (1.0 + h1)).powi(3);
358 let f = if b < 0.0 {
359 2.0 * ((b + 1.0).sqrt().acos() / 3.0).cos()
360 } else {
361 let a_ = (b.sqrt() + (b + 1.0).sqrt()).powf(1.0 / 3.0);
362 a_ + 1.0 / a_
363 };
364
365 let y = 2.0 / 3.0 * temp1 * (1.0 + h1) * ((b + 1.0).sqrt() / f + 1.0);
366 xn = 0.5
367 * ((m / (y * y) - (1.0 + l_))
368 - ((m / (y * y) - (1.0 + l_)).powi(2) - 4.0 * l_).sqrt());
369 }
370
371 let converged = (xn - x).abs() < SMALL;
376 if !converged {
377 return Err(LambertError::NoConvergence);
378 }
379
380 let a_orbit = s * (1.0 + lam).powi(2) * (1.0 + xn) * (l_ + xn) / (8.0 * xn);
381 let p = 2.0 * magr1 * magr2 * (1.0 + xn) * (dnu * 0.5).sin().powi(2)
382 / (s * (1.0 + lam).powi(2) * (l_ + xn));
383 let ecc = (1.0 - p / a_orbit).abs().sqrt();
384 finite_or_err(hodograph(r1, r2, v1, p, ecc, dnu, dtsec))
385 } else {
386 let mut x = 10.0;
388 let max_loops = 30;
389 let mut loops = 0;
390 let mut y = 0.0;
391
392 while (xn - x).abs() >= SMALL && loops < max_loops {
393 x = xn;
394 loops += 1;
395
396 let (h1, h2) = if nrev > 0 {
397 let temp = 1.0 / ((1.0 + 2.0 * x + l_) * (4.0 * x * x));
398 let temp1 = (nrev as f64 * PI * 0.5 + x.sqrt().atan()) / x.sqrt();
399 let h1 =
400 temp * (l_ + x).powi(2) * (3.0 * (1.0 + x).powi(2) * temp1 - (3.0 + 5.0 * x));
401 let h2 = temp * m * ((x * x - x * (1.0 + l_) - 3.0 * l_) * temp1 + (3.0 * l_ + x));
402 (h1, h2)
403 } else {
404 let tempx = seebatt(x);
405 let denom = 1.0 / ((1.0 + 2.0 * x + l_) * (4.0 * x + tempx * (3.0 + x)));
406 let h1 = (l_ + x).powi(2) * (1.0 + 3.0 * x + tempx) * denom;
407 let h2 = m * (x - l_ + tempx) * denom;
408 (h1, h2)
409 };
410
411 let b = 0.25 * 27.0 * h2 / (1.0 + h1).powi(3);
412 let u = 0.5 * b / (1.0 + (1.0 + b).sqrt());
413 let k2 = kbatt(u);
414 y = (1.0 + h1) / 3.0 * (2.0 + (1.0 + b).sqrt() / (1.0 + 2.0 * u * k2 * k2));
415 xn = (((1.0 - l_) * 0.5).powi(2) + m / (y * y)).sqrt() - (1.0 + l_) * 0.5;
416 }
417
418 let converged = (xn - x).abs() < SMALL;
422 if !converged {
423 return Err(LambertError::NoConvergence);
424 }
425
426 let p = 2.0 * magr1 * magr2 * y * y * (1.0 + x).powi(2) * (dnu * 0.5).sin().powi(2)
427 / (m * s * (1.0 + lam).powi(2));
428 let ecc = (eps * eps
429 + 4.0 * magr2 / magr1 * (dnu * 0.5).sin().powi(2) * ((l_ - x) / (l_ + x)).powi(2))
430 / (eps * eps + 4.0 * magr2 / magr1 * (dnu * 0.5).sin().powi(2));
431 let ecc = ecc.sqrt();
432
433 finite_or_err(hodograph(r1, r2, v1, p, ecc, dnu, dtsec))
434 }
435}
436
437fn finite_or_err(vels: ([f64; 3], [f64; 3])) -> Result<([f64; 3], [f64; 3]), LambertError> {
440 let (v1t, v2t) = vels;
441 if all_finite(&v1t) && all_finite(&v2t) {
442 Ok((v1t, v2t))
443 } else {
444 Err(LambertError::NonFiniteValue)
445 }
446}
447
448#[cfg(test)]
449mod tests {
450 use super::*;
451
452 const RE: f64 = 6378.1363;
454 const DTSEC: f64 = 92854.234;
455 const NREV: i32 = 1;
456
457 fn inputs() -> ([f64; 3], [f64; 3], [f64; 3]) {
458 let r1 = [2.5 * RE, 0.0, 0.0];
459 let r2 = [1.9151111 * RE, 1.6069690 * RE, 0.0];
460 let v1 = [0.0, 4.999792554221911, 0.0];
461 (r1, r2, v1)
462 }
463
464 fn assert_close(actual: f64, expected: f64, label: &str) {
465 if expected == 0.0 {
466 assert!(actual.abs() < 1e-10, "{label}: expected ~0, got {actual}");
467 } else {
468 let rel = ((actual - expected) / expected).abs();
469 assert!(rel < 1e-12, "{label}: relative error {rel:e} exceeds 1e-12");
470 }
471 }
472
473 #[test]
474 fn battin_short_high() {
475 let (r1, r2, v1) = inputs();
476 let (v1t, v2t) = battin(
477 &r1,
478 &r2,
479 &v1,
480 DirectionOfMotion::Short,
481 DirectionOfEnergy::High,
482 NREV,
483 DTSEC,
484 )
485 .unwrap();
486 assert_close(v1t[0], -0.8696153795282852, "v1t_x");
487 assert_close(v1t[1], 6.3351545812502374, "v1t_y");
488 assert_close(v1t[2], 0.0, "v1t_z");
489 assert_close(v2t[0], -3.405994961791248, "v2t_x");
490 assert_close(v2t[1], 5.41198791828363, "v2t_y");
491 assert_close(v2t[2], 0.0, "v2t_z");
492 }
493
494 #[test]
495 fn battin_short_low() {
496 let (r1, r2, v1) = inputs();
497 let (v1t, v2t) = battin(
498 &r1,
499 &r2,
500 &v1,
501 DirectionOfMotion::Short,
502 DirectionOfEnergy::Low,
503 NREV,
504 DTSEC,
505 )
506 .unwrap();
507 assert_close(v1t[0], 5.832522716212579, "v1t_x");
508 assert_close(v1t[1], 1.4319944881331306, "v1t_y");
509 assert_close(v2t[0], -5.388439978490882, "v2t_x");
510 assert_close(v2t[1], -2.652101898141935, "v2t_y");
511 }
512
513 #[test]
514 fn battin_long_high() {
515 let (r1, r2, v1) = inputs();
516 let (v1t, v2t) = battin(
517 &r1,
518 &r2,
519 &v1,
520 DirectionOfMotion::Long,
521 DirectionOfEnergy::High,
522 NREV,
523 DTSEC,
524 )
525 .unwrap();
526 assert_close(v1t[0], -6.241103309400493, "v1t_x");
527 assert_close(v1t[1], -1.351339299630816, "v1t_y");
528 assert_close(v2t[0], 5.649586715490154, "v2t_x");
529 assert_close(v2t[1], 2.976517897853268, "v2t_y");
530 }
531
532 #[test]
533 fn battin_long_low() {
534 let (r1, r2, v1) = inputs();
535 let (v1t, v2t) = battin(
536 &r1,
537 &r2,
538 &v1,
539 DirectionOfMotion::Long,
540 DirectionOfEnergy::Low,
541 NREV,
542 DTSEC,
543 )
544 .unwrap();
545 assert_close(v1t[0], 0.641119158614630, "v1t_x");
546 assert_close(v1t[1], -5.957501823796459, "v1t_y");
547 assert_close(v2t[0], 3.33828270226307, "v2t_x");
548 assert_close(v2t[1], -4.975814585231199, "v2t_y");
549 }
550
551 #[test]
552 fn battin_rejects_zero_position() {
553 let (_, r2, v1) = inputs();
554 let r1 = [0.0, 0.0, 0.0];
555 let err = battin(
556 &r1,
557 &r2,
558 &v1,
559 DirectionOfMotion::Short,
560 DirectionOfEnergy::Low,
561 NREV,
562 DTSEC,
563 )
564 .unwrap_err();
565 assert_eq!(err, LambertError::ZeroVector);
566 }
567
568 #[test]
569 fn battin_rejects_nonpositive_dtsec() {
570 let (r1, r2, v1) = inputs();
571 for bad in [0.0, -1.0] {
572 let err = battin(
573 &r1,
574 &r2,
575 &v1,
576 DirectionOfMotion::Short,
577 DirectionOfEnergy::Low,
578 NREV,
579 bad,
580 )
581 .unwrap_err();
582 assert_eq!(err, LambertError::NonPositiveTimeOfFlight);
583 }
584 }
585
586 #[test]
587 fn battin_rejects_overflowing_magnitude() {
588 let r1 = [1e160, 0.0, 0.0];
591 let r2 = [0.0, 1e160, 0.0];
592 let v1 = [0.0, 5.0, 0.0];
593 let err = battin(
594 &r1,
595 &r2,
596 &v1,
597 DirectionOfMotion::Short,
598 DirectionOfEnergy::Low,
599 NREV,
600 DTSEC,
601 )
602 .unwrap_err();
603 assert_eq!(err, LambertError::NonFiniteValue);
604 }
605
606 #[test]
607 fn battin_rejects_collinear_endpoints() {
608 let r1 = [2.5 * RE, 0.0, 0.0];
610 let r2 = [5.0 * RE, 0.0, 0.0];
611 let v1 = [0.0, 4.999792554221911, 0.0];
612 let err = battin(
613 &r1,
614 &r2,
615 &v1,
616 DirectionOfMotion::Short,
617 DirectionOfEnergy::Low,
618 NREV,
619 DTSEC,
620 )
621 .unwrap_err();
622 assert_eq!(err, LambertError::DegenerateGeometry);
623 }
624
625 #[test]
626 fn battin_rejects_bad_180_transfer() {
627 let r1 = [2.5 * RE, 0.0, 0.0];
630 let r2 = [-5.0 * RE, 0.0, 0.0];
631 let v1 = [3.0, 0.0, 0.0];
632 let err = battin(
633 &r1,
634 &r2,
635 &v1,
636 DirectionOfMotion::Short,
637 DirectionOfEnergy::Low,
638 NREV,
639 DTSEC,
640 )
641 .unwrap_err();
642 assert_eq!(err, LambertError::DegenerateGeometry);
643 }
644
645 #[test]
646 fn battin_rejects_nonfinite_input() {
647 let (_, r2, v1) = inputs();
648 let r1 = [f64::NAN, 0.0, 0.0];
649 let err = battin(
650 &r1,
651 &r2,
652 &v1,
653 DirectionOfMotion::Short,
654 DirectionOfEnergy::Low,
655 NREV,
656 DTSEC,
657 )
658 .unwrap_err();
659 assert_eq!(err, LambertError::NonFiniteValue);
660 }
661
662 #[test]
663 fn battin_high_energy_reports_nonconvergence() {
664 let (r1, r2, v1) = inputs();
668 let err = battin(
669 &r1,
670 &r2,
671 &v1,
672 DirectionOfMotion::Short,
673 DirectionOfEnergy::High,
674 5,
675 1.0,
676 )
677 .unwrap_err();
678 assert_eq!(err, LambertError::NoConvergence);
679 }
680}