Skip to content

routing: add a real heuristic to public route search #19

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
blackopsrepl merged 1 commit into from
Mar 21, 2026
Merged

routing: add a real heuristic to public route search #19

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
132 changes: 118 additions & 14 deletions src/routing/network.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ pub struct RoadNetwork {
pub(super) coord_to_node: HashMap<(i64, i64), NodeIdx>,
spatial_index: Option<KdTree<SpatialPoint>>,
edge_spatial_index: Option<SegmentIndex<SpatialSegment>>,
max_speed_mps: f64,
}

impl RoadNetwork {
Expand All @@ -106,6 +107,7 @@ impl RoadNetwork {
coord_to_node: HashMap::new(),
spatial_index: None,
edge_spatial_index: None,
max_speed_mps: 0.0,
}
}

Expand All @@ -128,6 +130,7 @@ impl RoadNetwork {
}

pub(super) fn add_edge(&mut self, from: NodeIdx, to: NodeIdx, data: EdgeData) {
self.record_edge_speed(&data);
self.graph.add_edge(from, to, data);
}

Expand All @@ -140,14 +143,12 @@ impl RoadNetwork {
) {
let from_idx = NodeIdx::new(from);
let to_idx = NodeIdx::new(to);
self.graph.add_edge(
from_idx,
to_idx,
EdgeData {
travel_time_s,
distance_m,
},
);
let data = EdgeData {
travel_time_s,
distance_m,
};
self.record_edge_speed(&data);
self.graph.add_edge(from_idx, to_idx, data);
}

pub(super) fn build_spatial_index(&mut self) {
Expand Down Expand Up @@ -193,6 +194,35 @@ impl RoadNetwork {
self.edge_spatial_index = Some(SegmentIndex::bulk_load(segments));
}

fn record_edge_speed(&mut self, edge: &EdgeData) {
if edge.travel_time_s <= 0.0 || edge.distance_m <= 0.0 {
return;
}

let speed_mps = edge.distance_m / edge.travel_time_s;
if speed_mps.is_finite() {
self.max_speed_mps = self.max_speed_mps.max(speed_mps);
}
}

fn distance_lower_bound_between(&self, from: NodeIdx, to: NodeIdx) -> f64 {
let Some(from_node) = self.graph.node_weight(from) else {
return 0.0;
};
let Some(to_node) = self.graph.node_weight(to) else {
return 0.0;
};
haversine_distance(from_node.coord(), to_node.coord())
Copy link
Copy Markdown

@chatgpt-codex-connector chatgpt-codex-connector bot Mar 21, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 Badge Avoid non-admissible heuristics on from_test_data graphs

When a graph comes from from_test_data or any other synthetic source where distance_m / travel_time_s are not derived from the node coordinates, haversine_distance is not a safe lower bound. from_test_data explicitly accepts arbitrary weights (the example in this file uses nodes ~28 km apart with a 1 km edge), so distance_lower_bound_between and time_lower_bound_between can exceed the true remaining cost. Because src/routing/algo.rs::astar returns on the first goal pop, these new heuristics can now make route* return a non-optimal path on custom/test networks, whereas the previous zero-heuristic behavior stayed correct.

Useful? React with 👍 / 👎.

}

fn time_lower_bound_between(&self, from: NodeIdx, to: NodeIdx) -> f64 {
if !self.max_speed_mps.is_finite() || self.max_speed_mps <= 0.0 {
return 0.0;
}

self.distance_lower_bound_between(from, to) / self.max_speed_mps
}

/// Iterate over all nodes as (lat, lng) pairs.
pub fn nodes_iter(&self) -> impl Iterator<Item = (f64, f64)> + '_ {
self.graph
Expand Down Expand Up @@ -378,7 +408,7 @@ impl RoadNetwork {
start_exit.node,
|n| n == end_entry.node,
|e| e.travel_time_s,
|_| 0.0,
|n| self.time_lower_bound_between(n, end_entry.node),
)
.map(|(path_cost, path)| (start_exit.time_s + path_cost + end_entry.time_s, path))
};
Expand Down Expand Up @@ -444,7 +474,7 @@ impl RoadNetwork {
from.node_index,
|n| n == to.node_index,
|e| e.travel_time_s,
|_| 0.0,
|n| self.time_lower_bound_between(n, to.node_index),
);

match result {
Expand Down Expand Up @@ -478,8 +508,8 @@ impl RoadNetwork {

/// Find a route between two coordinates with an explicit optimization objective.
///
/// Like `route`, this method snaps to the nearest graph nodes first. The
/// current public search still uses a zero heuristic for both objectives.
/// Like `route`, this method snaps to the nearest graph nodes first and
/// uses admissible straight-line lower bounds for both objectives.
pub fn route_with(
&self,
from: Coord,
Expand All @@ -503,14 +533,14 @@ impl RoadNetwork {
start_snap.node_index,
|n| n == end_snap.node_index,
|e| e.travel_time_s,
|_| 0.0,
|n| self.time_lower_bound_between(n, end_snap.node_index),
),
Objective::Distance => astar(
&self.graph,
start_snap.node_index,
|n| n == end_snap.node_index,
|e| e.distance_m,
|_| 0.0,
|n| self.distance_lower_bound_between(n, end_snap.node_index),
),
};

Expand Down Expand Up @@ -649,6 +679,80 @@ impl RoadNetwork {
}
}

#[cfg(test)]
mod tests {
use super::{NodeIdx, Objective, RoadNetwork};
use crate::routing::Coord;

#[test]
fn time_routing_uses_non_zero_admissible_heuristic() {
let nodes = &[(0.0, 0.0), (0.0, 0.01), (0.01, 0.0), (0.01, 0.01)];
let edges = &[
(0, 1, 200.0, 1_200.0),
(1, 3, 200.0, 1_200.0),
(0, 2, 50.0, 1_200.0),
(2, 3, 50.0, 1_200.0),
(1, 0, 200.0, 1_200.0),
(3, 1, 200.0, 1_200.0),
(2, 0, 50.0, 1_200.0),
(3, 2, 50.0, 1_200.0),
];
let network = RoadNetwork::from_test_data(nodes, edges);

let result = network
.route(Coord::new(0.0, 0.0), Coord::new(0.01, 0.01))
.expect("time route should exist");

assert_eq!(result.duration_seconds, 100);
assert_eq!(result.distance_meters, 2_400.0);
assert_eq!(
result.geometry,
vec![
Coord::new(0.0, 0.0),
Coord::new(0.01, 0.0),
Coord::new(0.01, 0.01),
]
);
assert!(network.time_lower_bound_between(NodeIdx(0), NodeIdx(3)) > 0.0);
}

#[test]
fn distance_routing_uses_non_zero_admissible_heuristic() {
let nodes = &[(0.0, 0.0), (0.0, 0.02), (0.01, 0.0), (0.01, 0.02)];
let edges = &[
(0, 1, 40.0, 3_000.0),
(1, 3, 40.0, 3_000.0),
(0, 2, 90.0, 900.0),
(2, 3, 90.0, 900.0),
(1, 0, 40.0, 3_000.0),
(3, 1, 40.0, 3_000.0),
(2, 0, 90.0, 900.0),
(3, 2, 90.0, 900.0),
];
let network = RoadNetwork::from_test_data(nodes, edges);

let result = network
.route_with(
Coord::new(0.0, 0.0),
Coord::new(0.01, 0.02),
Objective::Distance,
)
.expect("distance route should exist");

assert_eq!(result.distance_meters, 1_800.0);
assert_eq!(result.duration_seconds, 180);
assert_eq!(
result.geometry,
vec![
Coord::new(0.0, 0.0),
Coord::new(0.01, 0.0),
Coord::new(0.01, 0.02),
]
);
assert!(network.distance_lower_bound_between(NodeIdx(0), NodeIdx(3)) > 0.0);
}
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Objective {
Time,
Expand Down
Loading