navcore_math/

great_circle.rs

//! Great circle sailing -- the shortest path between two positions.
//!
//! This is the right sailing for "how far is it" and for legs long enough
//! that the saving matters. It is the wrong one for steering: the bearing
//! changes continuously along the track, which is why [`crate::rhumb`]
//! exists alongside it.

use crate::{EARTH_RADIUS_NM, Position, angle};

/// Distance along the great circle between two positions, in nautical
/// miles.
///
/// Haversine rather than the spherical law of cosines: the latter loses
/// precision badly for short distances, which are exactly the distances
/// computed most often (metres to the next waypoint in a harbour).
#[must_use]
pub fn distance_nm(from: Position, to: Position) -> f64 {
    central_angle_rad(from, to) * EARTH_RADIUS_NM
}

/// The angle subtended at the centre of the earth, in radians. Shared by
/// distance and cross-track, which both need it.
pub(crate) fn central_angle_rad(from: Position, to: Position) -> f64 {
    let (lat1, lat2) = (from.lat_rad(), to.lat_rad());
    let d_lat = lat2 - lat1;
    let d_lon = to.lon_rad() - from.lon_rad();

    let a = (d_lat / 2.0).sin().powi(2)
        + lat1.cos() * lat2.cos() * (d_lon / 2.0).sin().powi(2);
    2.0 * a.sqrt().atan2((1.0 - a).sqrt())
}

/// The bearing to leave `from` on to reach `to` by the great circle, in
/// degrees true.
///
/// Only the *initial* bearing: on any leg that is not along a meridian or
/// the equator, holding this bearing does not arrive at `to`. See
/// [`final_bearing_deg`] for the other end, and [`crate::rhumb`] for the
/// constant-bearing alternative.
#[must_use]
pub fn initial_bearing_deg(from: Position, to: Position) -> f64 {
    let (lat1, lat2) = (from.lat_rad(), to.lat_rad());
    let d_lon = to.lon_rad() - from.lon_rad();

    let y = d_lon.sin() * lat2.cos();
    let x = lat1.cos() * lat2.sin() - lat1.sin() * lat2.cos() * d_lon.cos();
    angle::norm_360(y.atan2(x).to_degrees())
}

/// The bearing the great circle arrives at `to` on, in degrees true.
///
/// The initial bearing of the reverse leg, turned around.
#[must_use]
pub fn final_bearing_deg(from: Position, to: Position) -> f64 {
    angle::reciprocal(initial_bearing_deg(to, from))
}

/// The position reached by sailing `distance_nm` from `from` along the
/// great circle that leaves on `bearing_deg`.
#[must_use]
pub fn destination(from: Position, bearing_deg: f64, distance_nm: f64) -> Position {
    let angular = distance_nm / EARTH_RADIUS_NM;
    let brg = bearing_deg.to_radians();
    let (lat1, lon1) = (from.lat_rad(), from.lon_rad());

    let lat2 =
        (lat1.sin() * angular.cos() + lat1.cos() * angular.sin() * brg.cos()).asin();
    let lon2 = lon1
        + (brg.sin() * angular.sin() * lat1.cos())
            .atan2(angular.cos() - lat1.sin() * lat2.sin());

    Position::new(lat2.to_degrees(), angle::norm_180(lon2.to_degrees()))
}

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

    const EPS: f64 = 1e-6;

    fn close(a: f64, b: f64, tol: f64) -> bool {
        (a - b).abs() < tol
    }

    #[test]
    fn one_degree_of_latitude_is_sixty_and_a_bit() {
        // 60.04, not 60 -- see the crate docs on the earth model. If this
        // ever reads exactly 60.0 someone has swapped the radius for the
        // "rounded earth" 3437.75 NM, which is a defensible choice but a
        // different one, and every computed distance shifts by 0.07%.
        let d = distance_nm(Position::new(45.0, 13.0), Position::new(46.0, 13.0));
        assert!(close(d, 60.0405, 1e-3), "got {d}");
    }

    #[test]
    fn a_degree_of_longitude_shrinks_with_latitude() {
        let at_equator = distance_nm(Position::new(0.0, 0.0), Position::new(0.0, 1.0));
        let at_sixty = distance_nm(Position::new(60.0, 0.0), Position::new(60.0, 1.0));
        assert!(close(at_equator, 60.0405, 1e-3), "got {at_equator}");
        // cos(60) = 0.5 exactly, so this one is analytically checkable.
        assert!(close(at_sixty, at_equator * 0.5, 1e-3), "got {at_sixty}");
    }

    #[test]
    fn distance_is_symmetric_and_zero_for_a_point() {
        let a = Position::new(45.55, 13.73);
        let b = Position::new(45.65, 13.60);
        assert!(close(distance_nm(a, b), distance_nm(b, a), EPS));
        assert!(close(distance_nm(a, a), 0.0, EPS));
    }

    #[test]
    fn cardinal_bearings_come_out_cardinal() {
        let origin = Position::new(45.0, 13.0);
        assert!(close(
            initial_bearing_deg(origin, Position::new(46.0, 13.0)),
            0.0,
            EPS
        ));
        assert!(close(
            initial_bearing_deg(origin, Position::new(44.0, 13.0)),
            180.0,
            EPS
        ));
        // Due east along a parallel is 090 only at the moment of leaving;
        // the great circle then bends poleward, so check just the start.
        assert!(close(
            initial_bearing_deg(origin, Position::new(45.0, 14.0)),
            90.0,
            0.5
        ));
    }

    #[test]
    fn destination_inverts_distance_and_bearing() {
        let start = Position::new(45.55, 13.73);
        for bearing in [0.0, 37.5, 90.0, 180.0, 271.3, 359.0] {
            for dist in [0.5, 12.0, 300.0] {
                let end = destination(start, bearing, dist);
                assert!(
                    close(distance_nm(start, end), dist, 1e-6),
                    "distance back differs for {bearing}/{dist}"
                );
                assert!(
                    close(initial_bearing_deg(start, end), bearing, 1e-6),
                    "bearing back differs for {bearing}/{dist}"
                );
            }
        }
    }

    #[test]
    fn final_bearing_differs_from_initial_on_a_long_leg() {
        // The whole reason rhumb lines exist. A long east-west leg at high
        // latitude arrives on a noticeably different bearing than it left.
        let a = Position::new(60.0, -10.0);
        let b = Position::new(60.0, 10.0);
        let start = initial_bearing_deg(a, b);
        let end = final_bearing_deg(a, b);
        assert!(start < 90.0, "great circle leaves poleward of east: {start}");
        assert!(end > 90.0, "and arrives equatorward of east: {end}");
    }

    #[test]
    fn crossing_the_antimeridian_is_the_short_way() {
        let a = Position::new(0.0, 179.0);
        let b = Position::new(0.0, -179.0);
        assert!(close(distance_nm(a, b), 2.0 * 60.0405, 1e-3));
    }

    #[test]
    fn destination_normalises_longitude_across_the_antimeridian() {
        let p = destination(Position::new(0.0, 179.5), 90.0, 120.0);
        assert!(p.lon_deg < 0.0, "should have wrapped to negative: {p:?}");
        assert!(p.is_valid());
    }
}