navcore_math/
track.rs

1//! Position relative to a route leg: cross-track error and along-track
2//! distance.
3//!
4//! This is what drives the steering display and the "waypoint reached"
5//! decision, so the sign conventions here are load-bearing and stated
6//! explicitly on every field.
7
8use crate::{EARTH_RADIUS_NM, Position, angle, great_circle};
9
10/// Where a vessel sits relative to the leg `from` -> `to`.
11#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct TrackError {
13    /// Perpendicular distance off the leg, nautical miles, **positive when
14    /// the vessel is to starboard of the track**.
15    ///
16    /// The sign is the one a helm display needs inverted: positive here
17    /// means "steer to port to get back". NMEA's XTE sentence instead
18    /// carries a magnitude plus an L/R letter naming the direction to
19    /// steer, so an adapter emitting XTE must flip this, not copy it.
20    pub cross_track_nm: f64,
21    /// Distance from `from` measured along the leg, nautical miles.
22    ///
23    /// Negative when the vessel has not yet reached `from` (it lies behind
24    /// the start of the leg), and greater than the leg length once past
25    /// `to`. Both cases are normal and are how leg advance is detected.
26    pub along_track_nm: f64,
27}
28
29impl TrackError {
30    /// Distance still to run along the leg, negative once past `to`.
31    #[must_use]
32    pub fn remaining_nm(&self, leg_length_nm: f64) -> f64 {
33        leg_length_nm - self.along_track_nm
34    }
35}
36
37/// Cross-track and along-track distance of `vessel` relative to the great
38/// circle leg from `from` to `to`.
39///
40/// Returns `None` when the leg has no direction to be off -- `from` and
41/// `to` are the same position, so "left or right of it" is meaningless.
42/// Callers should treat that as "waypoint reached", not as an error.
43#[must_use]
44pub fn track_error(from: Position, to: Position, vessel: Position) -> Option<TrackError> {
45    let leg_rad = great_circle::central_angle_rad(from, to);
46    if leg_rad < f64::EPSILON {
47        return None;
48    }
49
50    let d13 = great_circle::central_angle_rad(from, vessel);
51    let brg13 = great_circle::initial_bearing_deg(from, vessel);
52    let brg12 = great_circle::initial_bearing_deg(from, to);
53    let delta = angle::diff(brg13, brg12).to_radians();
54
55    let xt_rad = (d13.sin() * delta.sin()).asin();
56
57    // acos of a ratio that rounding can push just past 1.0 when the vessel
58    // is exactly on track; clamp rather than return NaN for the one case
59    // that is not an error at all.
60    let ratio = (d13.cos() / xt_rad.cos()).clamp(-1.0, 1.0);
61    let mut at_rad = ratio.acos();
62
63    // acos only ever yields 0..pi, which cannot express "behind the start".
64    // The bearing difference does: more than a right angle means the vessel
65    // is abaft the beam of the leg's origin.
66    if delta.abs() > std::f64::consts::FRAC_PI_2 {
67        at_rad = -at_rad;
68    }
69
70    Some(TrackError {
71        cross_track_nm: xt_rad * EARTH_RADIUS_NM,
72        along_track_nm: at_rad * EARTH_RADIUS_NM,
73    })
74}
75
76#[cfg(test)]
77mod tests {
78    use super::*;
79
80    fn close(a: f64, b: f64, tol: f64) -> bool {
81        (a - b).abs() < tol
82    }
83
84    /// A leg running due north up a meridian, which makes every expected
85    /// value checkable by hand.
86    fn leg() -> (Position, Position) {
87        (Position::new(45.0, 13.0), Position::new(46.0, 13.0))
88    }
89
90    #[test]
91    fn dead_on_track_is_zero_off() {
92        let (a, b) = leg();
93        let te = track_error(a, b, Position::new(45.5, 13.0)).unwrap();
94        assert!(close(te.cross_track_nm, 0.0, 1e-9), "{te:?}");
95        assert!(close(te.along_track_nm, 30.02, 1e-2), "{te:?}");
96    }
97
98    #[test]
99    fn east_of_a_northbound_leg_is_starboard_and_positive() {
100        let (a, b) = leg();
101        let te = track_error(a, b, Position::new(45.5, 13.1)).unwrap();
102        assert!(te.cross_track_nm > 0.0, "{te:?}");
103        // 0.1 deg of longitude at 45.5N ~= 0.1 * 60.04 * cos(45.5) = 4.20 NM
104        assert!(close(te.cross_track_nm, 4.20, 0.02), "{te:?}");
105    }
106
107    #[test]
108    fn west_of_a_northbound_leg_is_port_and_negative() {
109        let (a, b) = leg();
110        let te = track_error(a, b, Position::new(45.5, 12.9)).unwrap();
111        assert!(te.cross_track_nm < 0.0, "{te:?}");
112    }
113
114    #[test]
115    fn sign_follows_the_leg_direction_not_the_compass() {
116        // Same vessel, leg reversed: what was starboard is now port.
117        let (a, b) = leg();
118        let vessel = Position::new(45.5, 13.1);
119        let fwd = track_error(a, b, vessel).unwrap();
120        let rev = track_error(b, a, vessel).unwrap();
121        assert!(close(fwd.cross_track_nm, -rev.cross_track_nm, 1e-6));
122    }
123
124    #[test]
125    fn before_the_start_the_along_track_goes_negative() {
126        let (a, b) = leg();
127        let te = track_error(a, b, Position::new(44.5, 13.0)).unwrap();
128        assert!(te.along_track_nm < 0.0, "{te:?}");
129        assert!(close(te.along_track_nm, -30.02, 1e-2), "{te:?}");
130    }
131
132    #[test]
133    fn past_the_end_it_exceeds_the_leg_length() {
134        let (a, b) = leg();
135        let leg_len = great_circle::distance_nm(a, b);
136        let te = track_error(a, b, Position::new(46.5, 13.0)).unwrap();
137        assert!(te.along_track_nm > leg_len, "{te:?}");
138        assert!(te.remaining_nm(leg_len) < 0.0, "{te:?}");
139    }
140
141    #[test]
142    fn a_zero_length_leg_has_no_side_to_be_off() {
143        let a = Position::new(45.0, 13.0);
144        assert!(track_error(a, a, Position::new(45.1, 13.1)).is_none());
145    }
146}
navcore_math/track.rs

navcore_math/
track.rs
