rustsim_mobility/

router.rs

//! Mode-tagged routing graph and Dijkstra router.
//!
//! This is a small multi-modal shortest-path engine. Nodes are opaque
//! `u64` identifiers; edges carry a traversal `cost` and an
//! [`rustsim_modes::AllowedModes`] permission mask. The router only
//! relaxes edges whose permission mask intersects the traveller's
//! available mode set.

use std::cmp::Ordering;
use std::collections::BinaryHeap;

use rustsim_modes::{AllowedModes, TravelMode};
use rustsim_traffic::{TransportLinkMetadata, TransportLinkOps, TransportLinkSpace};
use thiserror::Error;

/// Opaque node identifier in the routing graph.
pub type NodeId = u64;

/// A directed, mode-tagged edge.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ModalEdge {
    /// Source node.
    pub from: NodeId,
    /// Destination node.
    pub to: NodeId,
    /// Traversal cost (typically seconds of travel time).
    pub cost: f64,
    /// Which modes are allowed on this edge.
    pub modes: AllowedModes,
}

/// Mode-tagged routing graph.
#[derive(Debug, Clone, Default)]
pub struct ModalGraph {
    /// Sorted adjacency list: `edges[from] = [edge, ...]`.
    adjacency: hashbrown::HashMap<NodeId, Vec<ModalEdge>>,
}

/// Errors returned when building a modal graph from transport link metadata.
#[derive(Debug, Clone, PartialEq, Error)]
pub enum ModalGraphBuildError {
    /// Link metadata referenced a link that does not exist in the link space.
    #[error("transport metadata references missing link {0}")]
    MissingLink(u64),
    /// Link metadata endpoints disagree with the topology held by the link space.
    #[error("transport metadata endpoints for link {link_id} are ({metadata_from}, {metadata_to}) but link space has ({space_from}, {space_to})")]
    EndpointMismatch {
        /// Link identifier.
        link_id: u64,
        /// Source node from metadata.
        metadata_from: u64,
        /// Destination node from metadata.
        metadata_to: u64,
        /// Source node from link space.
        space_from: u64,
        /// Destination node from link space.
        space_to: u64,
    },
    /// A link's current travel time could not be used as a routing cost.
    #[error("transport link {link_id} has invalid routing cost {cost}")]
    InvalidCost {
        /// Link identifier.
        link_id: u64,
        /// Computed cost.
        cost: f64,
    },
}

impl ModalGraph {
    /// Empty graph.
    pub fn new() -> Self {
        Self::default()
    }

    /// Append an edge.
    pub fn add_edge(&mut self, edge: ModalEdge) {
        self.adjacency.entry(edge.from).or_default().push(edge);
    }

    /// Neighbours of `node`.
    pub fn neighbours(&self, node: NodeId) -> &[ModalEdge] {
        self.adjacency
            .get(&node)
            .map(|v| v.as_slice())
            .unwrap_or(&[])
    }

    /// Build a mode-tagged graph from a `rustsim-traffic` link space and
    /// matching transport metadata.
    ///
    /// Each metadata record becomes one directed [`ModalEdge`]. The edge cost
    /// is the link's current snapshot travel time from
    /// [`TransportLinkOps::link_travel_time`], so density and blocking effects
    /// already represented in the link space are reflected in routing.
    pub fn from_transport_links(
        space: &TransportLinkSpace,
        metadata: impl IntoIterator<Item = TransportLinkMetadata>,
    ) -> Result<Self, ModalGraphBuildError> {
        let mut graph = Self::new();
        for link in metadata {
            let link_id = link.edge_id;
            let (space_from, space_to) = space
                .link_endpoints(link_id)
                .ok_or(ModalGraphBuildError::MissingLink(link_id as u64))?;
            if (link.from_node, link.to_node) != (space_from, space_to) {
                return Err(ModalGraphBuildError::EndpointMismatch {
                    link_id: link_id as u64,
                    metadata_from: link.from_node as u64,
                    metadata_to: link.to_node as u64,
                    space_from: space_from as u64,
                    space_to: space_to as u64,
                });
            }

            let cost = space.link_travel_time(link_id);
            if !cost.is_finite() || cost < 0.0 {
                return Err(ModalGraphBuildError::InvalidCost {
                    link_id: link_id as u64,
                    cost,
                });
            }

            graph.add_edge(ModalEdge {
                from: space_from as NodeId,
                to: space_to as NodeId,
                cost,
                modes: link.allowed_modes,
            });
        }
        Ok(graph)
    }
}

/// Resulting route from [`shortest_path`].
#[derive(Debug, Clone, PartialEq)]
pub struct ModalRoute {
    /// Ordered nodes from `start` to `goal`.
    pub nodes: Vec<NodeId>,
    /// Total cost along the route.
    pub total_cost: f64,
}

/// Compute the lowest-cost path from `start` to `goal` using only edges
/// that permit at least one mode in `available_modes`. Returns `None` if
/// unreachable.
pub fn shortest_path(
    graph: &ModalGraph,
    start: NodeId,
    goal: NodeId,
    available_modes: AllowedModes,
) -> Option<ModalRoute> {
    #[derive(Copy, Clone, PartialEq)]
    struct State {
        cost: f64,
        node: NodeId,
    }
    impl Eq for State {}
    impl Ord for State {
        fn cmp(&self, other: &Self) -> Ordering {
            other
                .cost
                .partial_cmp(&self.cost)
                .unwrap_or(Ordering::Equal)
        }
    }
    impl PartialOrd for State {
        fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
            Some(self.cmp(other))
        }
    }

    let mut dist: hashbrown::HashMap<NodeId, f64> = hashbrown::HashMap::new();
    let mut came_from: hashbrown::HashMap<NodeId, NodeId> = hashbrown::HashMap::new();
    let mut heap = BinaryHeap::new();

    dist.insert(start, 0.0);
    heap.push(State {
        cost: 0.0,
        node: start,
    });

    while let Some(State { cost, node }) = heap.pop() {
        if node == goal {
            return Some(reconstruct(&came_from, start, goal, cost));
        }
        if let Some(&best) = dist.get(&node) {
            if cost > best + 1e-9 {
                continue;
            }
        }
        for edge in graph.neighbours(node) {
            if !modes_intersect(edge.modes, available_modes) {
                continue;
            }
            let next_cost = cost + edge.cost;
            let better = dist
                .get(&edge.to)
                .map(|&d| next_cost < d - 1e-12)
                .unwrap_or(true);
            if better {
                dist.insert(edge.to, next_cost);
                came_from.insert(edge.to, node);
                heap.push(State {
                    cost: next_cost,
                    node: edge.to,
                });
            }
        }
    }

    None
}

fn reconstruct(
    came_from: &hashbrown::HashMap<NodeId, NodeId>,
    start: NodeId,
    goal: NodeId,
    cost: f64,
) -> ModalRoute {
    let mut nodes = vec![goal];
    let mut cur = goal;
    while cur != start {
        if let Some(&prev) = came_from.get(&cur) {
            nodes.push(prev);
            cur = prev;
        } else {
            break;
        }
    }
    nodes.reverse();
    ModalRoute {
        nodes,
        total_cost: cost,
    }
}

/// Permission helper for single-mode callers.
pub fn only(mode: TravelMode) -> AllowedModes {
    AllowedModes::none().with_mode(mode)
}

/// Do two permission masks have at least one mode in common?
fn modes_intersect(a: AllowedModes, b: AllowedModes) -> bool {
    [
        TravelMode::Pedestrian,
        TravelMode::Vehicle,
        TravelMode::Bicycle,
        TravelMode::Transit,
    ]
    .iter()
    .any(|m| a.allows(*m) && b.allows(*m))
}

#[cfg(test)]
mod tests {
    use super::*;
    use rustsim_traffic::{LinkClass, LinkProperties};
    use rustsim_traffic::{TrafficControlType, TransportLinkMetadata, TransportLinkSpace};

    fn walk_plus_transit_graph() -> ModalGraph {
        let mut g = ModalGraph::new();
        // Walk network: 1 - 2 - 3 - 4 (slow).
        let walk_only = only(TravelMode::Pedestrian);
        g.add_edge(ModalEdge {
            from: 1,
            to: 2,
            cost: 600.0,
            modes: walk_only,
        });
        g.add_edge(ModalEdge {
            from: 2,
            to: 3,
            cost: 600.0,
            modes: walk_only,
        });
        g.add_edge(ModalEdge {
            from: 3,
            to: 4,
            cost: 300.0,
            modes: walk_only,
        });
        // Transit shortcut 2 -> 3 (fast).
        let transit_only = only(TravelMode::Transit);
        g.add_edge(ModalEdge {
            from: 2,
            to: 3,
            cost: 120.0,
            modes: transit_only,
        });
        g
    }

    #[test]
    fn walk_only_takes_slow_path() {
        let g = walk_plus_transit_graph();
        let r = shortest_path(&g, 1, 4, only(TravelMode::Pedestrian)).unwrap();
        assert_eq!(r.nodes, vec![1, 2, 3, 4]);
        assert!((r.total_cost - 1500.0).abs() < 1e-9);
    }

    #[test]
    fn walk_plus_transit_takes_transit_shortcut() {
        let g = walk_plus_transit_graph();
        let modes = AllowedModes::none()
            .with_mode(TravelMode::Pedestrian)
            .with_mode(TravelMode::Transit);
        let r = shortest_path(&g, 1, 4, modes).unwrap();
        assert_eq!(r.nodes, vec![1, 2, 3, 4]);
        // Walk leg 1->2 (600) + transit 2->3 (120) + walk 3->4 (300) = 1020
        assert!((r.total_cost - 1020.0).abs() < 1e-9);
    }

    #[test]
    fn disconnected_returns_none() {
        let g = walk_plus_transit_graph();
        let r = shortest_path(&g, 1, 99, only(TravelMode::Pedestrian));
        assert!(r.is_none());
    }

    #[test]
    fn builds_graph_from_transport_link_space() {
        let mut space = TransportLinkSpace::new();
        let a = space.add_node();
        let b = space.add_node();
        let c = space.add_node();
        let (walk_geom, walk_props) = LinkProperties::pedestrian(60.0, 3.0).unwrap();
        let walk = space.add_link(a, b, walk_geom, walk_props).unwrap();
        let (road_geom, road_props) = LinkProperties::urban(500.0, 40.0, 1).unwrap();
        let road = space.add_link(b, c, road_geom, road_props).unwrap();

        let metadata = vec![
            TransportLinkMetadata::new(
                walk,
                a,
                b,
                LinkClass::Walkway,
                AllowedModes::pedestrian_only(),
            ),
            TransportLinkMetadata::new(
                road,
                b,
                c,
                LinkClass::Transitway,
                AllowedModes::vehicular(),
            )
            .with_traffic_control(TrafficControlType::Signal),
        ];

        let graph = ModalGraph::from_transport_links(&space, metadata).unwrap();
        assert_eq!(graph.neighbours(a as NodeId).len(), 1);
        assert_eq!(graph.neighbours(b as NodeId).len(), 1);
        assert!(shortest_path(
            &graph,
            a as NodeId,
            c as NodeId,
            AllowedModes::none()
                .with_mode(TravelMode::Pedestrian)
                .with_mode(TravelMode::Transit),
        )
        .is_some());
    }
}