navcore_route_find/

lib.rs

//! Automatic route generation: the shortest path [`enc_store::check_leg`]
//! calls safe between two positions.
//!
//! Safety decisions belong to [`enc_store::check_leg`], the same function
//! [`route_check`](../route_check) uses to validate a route a human drew.
//! This crate generates candidate waypoints and searches them:
//!
//! 1. `grid` lays a lattice of candidate positions over the area between
//!    `from` and `to`, and `hazards` marks which of them a vessel cannot
//!    stand on, from a single chart query rather than one per candidate.
//! 2. `search` finds the shortest safe path over that lattice with A*.
//! 3. `smooth` straightens the resulting zig-zag into a short waypoint
//!    list and calls `check_leg` on every straightened leg, so every leg
//!    this crate hands back is approved at the vessel's real corridor,
//!    not the lattice's approximation of it.
//!
//! The lattice, hazard test and search generalize to weather routing:
//! only the edge cost (distance here) would change to sailing time under
//! a wind forecast.

#![forbid(unsafe_code)]

mod gateway;
mod grid;
mod hazards;
mod land_cache;
mod search;
mod smooth;
mod visibility;

pub use gateway::{find_dock_to_dock_route, find_gateway, find_gateway_route};
pub use land_cache::{LandCacheError, bake as bake_land_cache};

use std::fmt;

use geo::{Coord, Rect};
use nav_math::{Position, rhumb};

use enc_store::{ChartStore, StoreError, Vessel};
use grid::Grid;
use hazards::Hazards;
use routes::Waypoint;

/// What a route is being planned for, and how far around the direct line
/// to search.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct FindOptions {
    /// The vessel's safety contour, in metres -- the same setting
    /// `enc_store::Vessel` takes, and the same one the chart is drawn with.
    pub safety_contour_m: f64,
    /// How far the vessel may stray to port of any leg the route ends up
    /// with, in nautical miles.
    pub port_xtd_nm: f64,
    /// The same to starboard.
    pub starboard_xtd_nm: f64,
    /// How far beyond the direct line between `from` and `to` the search
    /// is allowed to reach, in nautical miles. `Some` is tried exactly
    /// once, at the given value, and not escalated further. `None`
    /// auto-scales the margin from the direct distance and escalates it
    /// a few times before giving up if the search finds nothing (see
    /// `margin_schedule`).
    pub margin_nm: Option<f64>,
    /// Lattice spacing, in nautical miles. `None` auto-scales it from the
    /// direct distance, so a long passage does not blow up the lattice
    /// size and a short one is not searched more coarsely than it needs.
    pub cell_nm: Option<f64>,
    /// Whether a restricted area (`RESARE` -- a marine reserve, a firing
    /// range, or another area a mariner must stay out of, not merely be
    /// cautioned about) blocks the route outright rather than only being
    /// crossed and reported. Off by default, the same posture `check_leg`
    /// itself takes: most restricted areas restrict anchoring or require
    /// a licence rather than prohibiting transit, and only the mariner
    /// planning the route knows which applies.
    pub avoid_restricted_areas: bool,
}

/// Everything that can go wrong finding a route.
#[derive(Debug)]
pub enum FindError {
    /// The chart could not be read.
    Store(StoreError),
}

impl fmt::Display for FindError {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::Store(error) => write!(f, "chart: {error}"),
        }
    }
}

impl std::error::Error for FindError {
    fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
        match self {
            Self::Store(error) => Some(error),
        }
    }
}

impl From<StoreError> for FindError {
    fn from(error: StoreError) -> Self {
        Self::Store(error)
    }
}

/// The shortest route between `from` and `to` that `check_leg` calls safe
/// for the vessel described in `options`.
///
/// `Ok(None)` when no route was found even after `margin_schedule`'s
/// escalation (or, with an explicit `margin_nm`, after that one attempt).
/// This means either the route is genuinely blocked (an island fills the
/// whole search area) or the lattice is too coarse to find a way through
/// (see `smooth::straighten`'s own doc). A caller that supplied
/// `margin_nm` can retry with a larger value; auto-escalation has
/// already been tried when `margin_nm` was `None`.
///
/// The returned waypoints carry no identity: turning them into a
/// [`routes::Route`] needs a fresh id, and this layer has no source of
/// randomness, the same reason `nav-math` has no clock.
///
/// # Errors
///
/// If the chart cannot be read.
pub fn find_route(
    chart: &ChartStore,
    from: Position,
    to: Position,
    options: FindOptions,
) -> Result<Option<Vec<Waypoint>>, FindError> {
    let direct_distance_nm = rhumb::distance_nm(from, to);
    if let Some(route) = lattice_route(chart, from, to, &options, direct_distance_nm)? {
        return Ok(Some(route));
    }

    // Only in auto mode: an explicit `margin_nm` is a caller's own "is N
    // NM enough", and reaching for a materially more expensive search
    // past it would quietly answer a different question than the one
    // asked -- see `margin_schedule`'s own doc for the same rule applied
    // to the lattice's escalation.
    if options.margin_nm.is_none() {
        if let Some(route) = attempt_around(chart, from, to, &options, direct_distance_nm)? {
            return Ok(Some(route));
        }
    }

    Ok(None)
}

/// [`margin_schedule`]'s escalation, run on its own. Both `find_route`
/// and each macro-hop inside [`attempt_around`] call this directly,
/// rather than `attempt_around` calling itself: a macro-hop is already a
/// piece of a route `attempt_around` chose specifically to avoid a
/// detour this escalation cannot reach, so a nested `attempt_around`
/// call on a shorter leg would only turn one visibility-graph search
/// into a cascade of them, each fetching and solving its own graph over
/// a search area that does not shrink as fast as the leg does. A
/// macro-hop that cannot find a lattice route on its own is reported as
/// a failure of the fallback as a whole, not retried further.
fn lattice_route(
    chart: &ChartStore,
    from: Position,
    to: Position,
    options: &FindOptions,
    direct_distance_nm: f64,
) -> Result<Option<Vec<Waypoint>>, FindError> {
    for margin_nm in margin_schedule(direct_distance_nm, options.margin_nm) {
        if let Some(route) = attempt(chart, from, to, options, margin_nm)? {
            return Ok(Some(route));
        }
    }
    Ok(None)
}

/// One search at a fixed `margin_nm`, the unit [`find_route`]'s own
/// margin retry schedule calls repeatedly.
fn attempt(
    chart: &ChartStore,
    from: Position,
    to: Position,
    options: &FindOptions,
    margin_nm: f64,
) -> Result<Option<Vec<Waypoint>>, FindError> {
    let vessel = Vessel { safety_contour_m: options.safety_contour_m };
    let area = search_area(from, to, margin_nm);
    let cell_nm = options
        .cell_nm
        .unwrap_or_else(|| auto_cell_nm(rhumb::distance_nm(from, to), margin_nm));

    let grid = Grid::covering(area, cell_nm);
    let hazards = Hazards::fetch(chart, area, options.avoid_restricted_areas)?;
    // The corridor's own half-width, not the cell spacing: the coarse
    // search has to know how much room a real leg through a node would
    // need either side of it, or a hazard just off to one side clears
    // every point test and only turns up later, in smooth::straighten's
    // real check_leg calls, as a route that got this far and then found
    // no safe step at all.
    let radius_nm = options.port_xtd_nm.max(options.starboard_xtd_nm);

    // `from`/`to` themselves, tested directly rather than through
    // whichever lattice node they happen to snap to. A coarser cell size
    // (which a wider auto-escalated margin produces, since auto_cell_nm
    // scales with it) can snap an otherwise clear point onto a node close
    // enough to a real hazard to test blocked -- a rounding artefact of
    // the lattice, not a fact about the position the caller actually
    // asked for. This is the one place in the search that has to answer
    // for the exact position rather than the lattice's approximation of
    // it.
    if hazards.blocked(from, radius_nm, vessel) || hazards.blocked(to, radius_nm, vessel) {
        return Ok(None);
    }

    let start = grid.nearest(from);
    let goal = grid.nearest(to);
    // Beyond that direct check, the start/goal *nodes* are exempted from
    // their own lattice point test for the same reason: they stand in
    // for `from`/`to`, already found clear above, not for whatever
    // lies exactly at the node's own rounded position.
    //
    // The node test itself runs against a land-simplified copy, not
    // `hazards` -- see `Hazards::with_simplified_land`'s own doc for why
    // that is safe here: `smooth::straighten` below re-validates every
    // real leg of the final route against the exact chart regardless, so
    // this coarse test only ever decides which candidate nodes the A*
    // search bothers to consider.
    let coarse = hazards.with_simplified_land();
    let blocked = |node: (usize, usize)| {
        node != start
            && node != goal
            && coarse.blocked(grid.position(node.0, node.1), radius_nm, vessel)
    };
    let Some(raw_path) = search::shortest_path(&grid, start, goal, blocked) else {
        return Ok(None);
    };

    let mut positions: Vec<Position> =
        raw_path.iter().map(|&(row, col)| grid.position(row, col)).collect();
    // The promised route runs from `from` to `to`, not from whichever
    // lattice node happened to be nearest them; `smooth::straighten`
    // re-validates every leg it touches anyway, so substituting the real
    // endpoints costs nothing and the first and last legs it checks are
    // the ones that will actually be sailed.
    if let Some(first) = positions.first_mut() {
        *first = from;
    }
    if let Some(last) = positions.last_mut() {
        *last = to;
    }

    // `coarse`, not `hazards`: `Safety::is_clear`'s own cheap pre-filter
    // runs once per candidate leg `straighten` considers, and land is the
    // one part of it simplification cannot get meaningfully wrong for
    // this purpose -- see `Hazards::with_simplified_land`'s own doc.
    let safety = smooth::Safety { chart, hazards: &coarse };
    let straightened =
        smooth::straighten(&positions, options.port_xtd_nm, options.starboard_xtd_nm, vessel, &safety)?;
    Ok(straightened.map(|path| path.into_iter().map(Waypoint::at).collect()))
}

/// How far beyond the direct line [`attempt_around`] fetches hazards
/// from, as a multiple of the direct distance.
///
/// Set generous: this fallback exists for a detour the lattice's much
/// narrower escalation cannot reach (see `visibility`'s own doc), such
/// as a peninsula or a gulf whose far shore is the real way round.
/// [`visibility::MAX_VERTICES`] bounds the resulting search's cost, not
/// this constant.
const VISIBILITY_MARGIN_FRACTION: f64 = 1.0;

/// The fallback [`find_route`] uses once every attempt in
/// `margin_schedule` has failed: a visibility graph over land obstacles
/// found in a much wider area than the lattice searches, for a route
/// whose detour is significantly wider than the direct line.
fn attempt_around(
    chart: &ChartStore,
    from: Position,
    to: Position,
    options: &FindOptions,
    direct_distance_nm: f64,
) -> Result<Option<Vec<Waypoint>>, FindError> {
    let vessel = Vessel { safety_contour_m: options.safety_contour_m };
    let area = search_area(from, to, direct_distance_nm * VISIBILITY_MARGIN_FRACTION);
    let hazards = Hazards::fetch(chart, area, options.avoid_restricted_areas)?;

    // The same direct, exact-position check `attempt` already makes for
    // every margin it tries: if `from` or `to` itself is blocked, no
    // graph over any obstacle *between* them is going to change that, and
    // building one anyway would pay this fallback's own, much wider
    // fetch and search cost to confirm what this already knows for free.
    let radius_nm = options.port_xtd_nm.max(options.starboard_xtd_nm);
    if hazards.blocked(from, radius_nm, vessel) || hazards.blocked(to, radius_nm, vessel) {
        return Ok(None);
    }

    let Some(waypoints) =
        visibility::find_path(&hazards, from, to, vessel, options.port_xtd_nm, options.starboard_xtd_nm)
    else {
        return Ok(None);
    };

    // A visibility-graph edge only promises to clear the land and
    // no-entry areas this attempt's own fetch found along a straight
    // screening rectangle -- not everything a real `check_leg` would
    // catch (charted-shallow water among others; see
    // `Hazards::coarsened_for_screening`'s own doc for why depth is left
    // to the lattice search below rather than checked here). What it is
    // trusted for is the macro shape: which side of the obstacle to pass
    // on. Each hop between its waypoints is
    // handed back to the lattice search proper, the same margin
    // escalation and real, corridor-aware validation this crate already
    // trusts locally -- just run once per macro-leg instead of once for
    // a passage possibly a hundred miles long.
    let mut route: Vec<Waypoint> = Vec::new();
    for pair in waypoints.windows(2) {
        let hop_distance_nm = rhumb::distance_nm(pair[0], pair[1]);
        let Some(leg) = lattice_route(chart, pair[0], pair[1], options, hop_distance_nm)? else {
            return Ok(None);
        };
        if route.is_empty() {
            route.push(Waypoint::at(pair[0]));
        }
        route.extend(leg.into_iter().skip(1));
    }
    Ok(Some(route))
}

/// A baseline for [`margin_schedule`]'s auto mode, in nautical miles: 30%
/// of the direct distance, clamped so a short hop still gets enough room
/// to clear a headland and a long passage does not get an unreasonably
/// wide search area to start with.
const AUTO_MARGIN_FRACTION: f64 = 0.3;
const MIN_AUTO_MARGIN_NM: f64 = 3.0;
const MAX_AUTO_MARGIN_NM: f64 = 12.0;

/// How far [`margin_schedule`]'s auto mode widens its baseline before
/// giving up -- three attempts, each doubling the last, the same
/// escalate-rather-than-guess-right-first-time idea
/// [`smooth::Safety::is_clear`] applies to a Coarse corridor.
const AUTO_MARGIN_MULTIPLIERS: [f64; 3] = [1.0, 2.0, 4.0];

/// The sequence of `margin_nm` values [`find_route`] tries, in order.
///
/// `explicit` (`FindOptions::margin_nm`) is used exactly once,
/// unmodified: escalating past a margin the caller specified would
/// silently change the meaning of "no route within 1 NM" from a
/// debugging answer into a different question. `None` builds a short
/// escalating schedule from `direct_distance_nm` instead.
fn margin_schedule(direct_distance_nm: f64, explicit: Option<f64>) -> Vec<f64> {
    let Some(margin_nm) = explicit else {
        let base = (direct_distance_nm * AUTO_MARGIN_FRACTION).clamp(MIN_AUTO_MARGIN_NM, MAX_AUTO_MARGIN_NM);
        return AUTO_MARGIN_MULTIPLIERS.iter().map(|multiplier| base * multiplier).collect();
    };
    vec![margin_nm]
}

/// How many lattice cells [`auto_cell_nm`] aims to span the longer of the
/// search area's two axes with. Coarse enough to keep a long ocean passage
/// tractable, fine enough that a short coastal hop is not searched with a
/// handful of giant cells.
const TARGET_LATTICE_SPAN: f64 = 150.0;

/// A floor under [`auto_cell_nm`], so a `from` and `to` that sit on top of
/// each other (or a tiny `margin_nm`) do not produce a cell size of zero.
const MIN_CELL_NM: f64 = 0.05;

/// The bounding box of `from` and `to`, expanded by `margin_nm` on every
/// side.
///
/// Expanded in true nautical miles via [`rhumb::destination`] from each
/// corner, not by adding degrees -- the same reason [`grid::Grid`] steps
/// its lattice that way.
pub(crate) fn search_area(from: Position, to: Position, margin_nm: f64) -> Rect<f64> {
    let south_west = Position::new(from.lat_deg.min(to.lat_deg), from.lon_deg.min(to.lon_deg));
    let north_east = Position::new(from.lat_deg.max(to.lat_deg), from.lon_deg.max(to.lon_deg));

    let south_west = rhumb::destination(
        rhumb::destination(south_west, 180.0, margin_nm),
        270.0,
        margin_nm,
    );
    let north_east =
        rhumb::destination(rhumb::destination(north_east, 0.0, margin_nm), 90.0, margin_nm);

    Rect::new(
        Coord { x: south_west.lon_deg, y: south_west.lat_deg },
        Coord { x: north_east.lon_deg, y: north_east.lat_deg },
    )
}

/// A lattice spacing that keeps the search area's longer axis to roughly
/// [`TARGET_LATTICE_SPAN`] cells, however long the passage is.
fn auto_cell_nm(direct_distance_nm: f64, margin_nm: f64) -> f64 {
    ((direct_distance_nm + 2.0 * margin_nm) / TARGET_LATTICE_SPAN).max(MIN_CELL_NM)
}

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

    #[test]
    fn the_search_area_covers_both_endpoints_with_margin_on_every_side() {
        let from = Position::new(45.50, 13.60);
        let to = Position::new(45.55, 13.70);
        let area = search_area(from, to, 2.0);

        assert!(area.min().y < from.lat_deg.min(to.lat_deg));
        assert!(area.max().y > from.lat_deg.max(to.lat_deg));
        assert!(area.min().x < from.lon_deg.min(to.lon_deg));
        assert!(area.max().x > from.lon_deg.max(to.lon_deg));
    }

    #[test]
    fn auto_cell_nm_grows_with_distance_and_never_falls_below_the_floor() {
        assert_eq!(auto_cell_nm(0.0, 0.0), MIN_CELL_NM);
        assert!(auto_cell_nm(1500.0, 5.0) > auto_cell_nm(15.0, 5.0));
    }

    #[test]
    fn an_explicit_margin_is_tried_exactly_once_and_unmodified() {
        assert_eq!(margin_schedule(15.0, Some(1.0)), vec![1.0]);
    }

    #[test]
    fn auto_margin_escalates_from_a_baseline_that_scales_with_distance() {
        let short = margin_schedule(1.0, None);
        let long = margin_schedule(40.0, None);

        assert_eq!(short.len(), 3, "an escalating schedule, not one guess");
        assert!(short.windows(2).all(|pair| pair[1] > pair[0]), "each step widens");
        assert!(short[0] >= MIN_AUTO_MARGIN_NM, "a short hop still gets room to clear a headland");
        assert!(long[0] > short[0], "a longer passage starts with a wider margin");
    }

    #[test]
    fn auto_margin_never_exceeds_the_clamp_before_escalating() {
        let schedule = margin_schedule(1000.0, None);
        assert_eq!(schedule[0], MAX_AUTO_MARGIN_NM);
    }
}