geo3d 0.1.0

3D geodesy in pure f64, zero dependencies: geodetic/ECEF/ENU/NED frames on a reference ellipsoid, look angles, ray–ellipsoid intersection, volumes, closest approach, Helmert.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

//! First-order kinematics.

use crate::types::{Ecef, Vec3};

/// The closest point of approach of two objects in linear motion.
#[derive(Clone, Copy, Debug, Default, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Cpa {
    /// Seconds from now until the closest approach; negative when it is already past. Zero when the
    /// relative velocity is zero.
    pub time_s: f64,
    /// The separation at that moment, metres.
    pub distance_m: f64,
}

/// Closest point of approach of two objects — ECEF positions (metres) and velocities (metres per
/// second) — assuming constant velocity. A first-order conflict/collision estimate: compare
/// `distance_m` with a separation threshold (aircraft separation, vessel CPA, rendezvous).
///
/// ```
/// use geo3d::{closest_approach, Ecef, Vec3};
/// // Two objects converging along x, offset 5 m in y: closest in 5 s, 5 m apart.
/// let cpa = closest_approach(
///     Ecef::new(0.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0),
///     Ecef::new(10.0, 5.0, 0.0), Vec3::new(-1.0, 0.0, 0.0),
/// );
/// assert!((cpa.time_s - 5.0).abs() < 1e-9 && (cpa.distance_m - 5.0).abs() < 1e-9);
/// ```
pub fn closest_approach(p1: Ecef, v1: Vec3, p2: Ecef, v2: Vec3) -> Cpa {
    let prel = p1.vec() - p2.vec();
    let vrel = v1 - v2;
    let vv = vrel.dot(vrel);
    let time_s = if vv == 0.0 { 0.0 } else { -prel.dot(vrel) / vv };
    Cpa {
        time_s,
        distance_m: (prel + vrel * time_s).norm(),
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn parallel_non_closing_is_now() {
        let c = closest_approach(
            Ecef::new(0.0, 0.0, 0.0),
            Vec3::new(1.0, 0.0, 0.0),
            Ecef::new(0.0, 3.0, 0.0),
            Vec3::new(1.0, 0.0, 0.0),
        );
        assert_eq!(c.time_s, 0.0);
        assert!((c.distance_m - 3.0).abs() < 1e-9);
    }

    #[test]
    fn diverging_reports_a_past_time() {
        let c = closest_approach(
            Ecef::new(0.0, 0.0, 0.0),
            Vec3::new(1.0, 0.0, 0.0),
            Ecef::new(-10.0, 0.0, 0.0),
            Vec3::new(-1.0, 0.0, 0.0),
        );
        assert!(c.time_s < 0.0 && c.distance_m < 1e-9);
    }
}