use crate::{EdgeId, EdgeIndex, RoutingLaw, VertexId, WalkGraph};
use std::{
cmp::Ordering,
collections::{BTreeSet, BinaryHeap},
};
#[derive(Clone, Debug)]
pub struct WalkRouter {
forbidden_turns: BTreeSet<(EdgeId, VertexId, EdgeId)>,
}
#[derive(Clone)]
pub struct WalkRealmIndex {
allowed: Vec<bool>,
adjacency: Vec<Vec<EdgeId>>,
edges: EdgeIndex,
}
impl WalkRealmIndex {
#[must_use]
pub fn finder(graph: &WalkGraph) -> Self {
Self::forge(
graph,
graph
.edges
.iter()
.map(|edge| edge.attr.realm.admitted_by_finder())
.collect(),
)
}
fn forge(graph: &WalkGraph, allowed: Vec<bool>) -> Self {
let mut adjacency = vec![Vec::new(); graph.vertices.len()];
for (vertex, fanout) in graph.adjacency.iter().enumerate() {
adjacency[vertex].extend(fanout.iter().copied().filter(|edge| allowed[edge.0]));
}
let edges = EdgeIndex::forge_allowed(graph, &allowed);
Self {
allowed,
adjacency,
edges,
}
}
#[must_use]
pub fn allowed(&self) -> &[bool] {
&self.allowed
}
#[must_use]
pub fn adjacency(&self) -> &[Vec<EdgeId>] {
&self.adjacency
}
#[must_use]
pub const fn edges(&self) -> &EdgeIndex {
&self.edges
}
}
impl WalkRouter {
#[must_use]
pub fn forge(graph: &WalkGraph) -> Self {
Self {
forbidden_turns: graph
.turn_bans
.iter()
.map(|ban| (ban.from, ban.via, ban.to))
.collect(),
}
}
#[must_use]
pub fn workspace(&self, graph: &WalkGraph) -> RoutingWorkspace {
RoutingWorkspace::forge(graph.edges.len())
}
#[must_use]
pub fn shortest_path(
&self,
graph: &WalkGraph,
workspace: &mut RoutingWorkspace,
request: RouteRequest<'_>,
) -> Option<Vec<EdgeId>> {
if request.from == request.target {
return Some(Vec::new());
}
workspace.begin(graph.edges.len());
let origin = workspace.origin_slot();
workspace.set(origin, 0.0, None);
let mut frontier = BinaryHeap::from([Frontier {
estimate: heuristic(graph, request.from, request.target),
cost: 0.0,
slot: origin,
at: request.from,
previous: request.previous,
}]);
while let Some(here) = frontier.pop() {
if request
.cost_ceiling
.is_some_and(|ceiling| here.cost > ceiling)
|| workspace
.cost(here.slot)
.is_none_or(|best| here.cost.total_cmp(&best).is_gt())
{
continue;
}
if here.at == request.target {
return workspace.reconstruct(origin, here.slot);
}
for edge_id in graph.adjacency[here.at.0].iter().copied() {
if request
.forbidden_edges
.is_some_and(|edges| edges.contains(&edge_id))
|| !self.turn_allowed(here.previous, here.at, edge_id)
{
continue;
}
let Some(step_cost) = request.law.edge_cost(graph, edge_id) else {
continue;
};
let cost = here.cost + step_cost;
if request.cost_ceiling.is_some_and(|ceiling| cost > ceiling) {
continue;
}
let edge = &graph.edges[edge_id.0];
let Some(at) = edge.traverse(here.at) else {
continue;
};
if at != request.target
&& request
.forbidden_vertices
.is_some_and(|vertices| vertices.contains(&at))
{
continue;
}
let slot = state_slot(edge_id, at, edge.a);
if workspace
.cost(slot)
.is_some_and(|best| !cost.total_cmp(&best).is_lt())
{
continue;
}
workspace.set(
slot,
cost,
Some(Predecessor {
slot: here.slot,
edge: edge_id,
}),
);
frontier.push(Frontier {
estimate: cost + heuristic(graph, at, request.target),
cost,
slot,
at,
previous: Some(edge_id),
});
}
}
None
}
fn turn_allowed(&self, from: Option<EdgeId>, via: VertexId, to: EdgeId) -> bool {
from.is_none_or(|from| !self.forbidden_turns.contains(&(from, via, to)))
}
}
#[derive(Clone, Copy)]
pub struct RouteRequest<'a> {
pub from: VertexId,
pub target: VertexId,
pub previous: Option<EdgeId>,
pub law: RoutingLaw,
pub cost_ceiling: Option<f64>,
pub forbidden_edges: Option<&'a BTreeSet<EdgeId>>,
pub forbidden_vertices: Option<&'a BTreeSet<VertexId>>,
}
#[derive(Clone, Debug)]
pub struct RoutingWorkspace {
generation: u32,
labels: Vec<Label>,
}
impl RoutingWorkspace {
fn forge(edge_count: usize) -> Self {
Self {
generation: 0,
labels: vec![Label::default(); edge_count.saturating_mul(2).saturating_add(1)],
}
}
fn begin(&mut self, edge_count: usize) {
let slots = edge_count.saturating_mul(2).saturating_add(1);
if self.labels.len() != slots {
self.labels.resize(slots, Label::default());
}
self.generation = self.generation.wrapping_add(1);
if self.generation == 0 {
self.labels.fill(Label::default());
self.generation = 1;
}
}
const fn origin_slot(&self) -> usize {
self.labels.len() - 1
}
fn cost(&self, slot: usize) -> Option<f64> {
let label = &self.labels[slot];
(label.generation == self.generation).then_some(label.cost)
}
fn predecessor(&self, slot: usize) -> Option<Predecessor> {
let label = &self.labels[slot];
(label.generation == self.generation)
.then_some(label.predecessor)
.flatten()
}
fn set(&mut self, slot: usize, cost: f64, predecessor: Option<Predecessor>) {
self.labels[slot] = Label {
generation: self.generation,
cost,
predecessor,
};
}
fn reconstruct(&self, origin: usize, mut cursor: usize) -> Option<Vec<EdgeId>> {
let mut edges = Vec::new();
while cursor != origin {
let predecessor = self.predecessor(cursor)?;
edges.push(predecessor.edge);
cursor = predecessor.slot;
}
edges.reverse();
Some(edges)
}
}
#[derive(Clone, Copy, Debug, Default)]
struct Label {
generation: u32,
cost: f64,
predecessor: Option<Predecessor>,
}
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
struct Predecessor {
slot: usize,
edge: EdgeId,
}
#[derive(Clone, Copy, Debug, PartialEq)]
struct Frontier {
estimate: f64,
cost: f64,
slot: usize,
at: VertexId,
previous: Option<EdgeId>,
}
impl Eq for Frontier {}
impl Ord for Frontier {
fn cmp(&self, other: &Self) -> Ordering {
other
.estimate
.total_cmp(&self.estimate)
.then_with(|| other.cost.total_cmp(&self.cost))
.then_with(|| other.slot.cmp(&self.slot))
}
}
impl PartialOrd for Frontier {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
fn state_slot(edge: EdgeId, at: VertexId, edge_a: VertexId) -> usize {
edge.0 * 2 + usize::from(at != edge_a)
}
fn heuristic(graph: &WalkGraph, from: VertexId, target: VertexId) -> f64 {
graph.vertices[from.0]
.coord
.haversine_m(graph.vertices[target.0].coord)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::{GraphBuilder, io::geojson};
use std::collections::BTreeMap;
#[test]
fn astar_matches_reference_dijkstra_across_vetoes() {
let drafts =
geojson::network_from_str(include_str!("../tests/fixtures/mini_network.geojson"))
.unwrap();
let graph = GraphBuilder::default().build(&drafts).unwrap();
let router = WalkRouter::forge(&graph);
let mut workspace = router.workspace(&graph);
let mut entropy = 0x9e37_79b9_7f4a_7c15_u64;
for trial in 0..256 {
entropy = entropy
.wrapping_mul(6_364_136_223_846_793_005)
.wrapping_add(1_442_695_040_888_963_407);
let from = VertexId(
usize::from(u16::try_from(entropy & u64::from(u16::MAX)).expect("masked to u16"))
% graph.vertices.len(),
);
let target = VertexId(
usize::from(
u16::try_from((entropy >> 16) & u64::from(u16::MAX)).expect("masked to u16"),
) % graph.vertices.len(),
);
let law = RoutingLaw {
road_aversion: [0.0, 1.0, 3.0][trial % 3],
};
let forbidden_edges = graph
.edges
.iter()
.filter_map(|edge| {
let rotation = u32::try_from(edge.id.0 % 64).expect("rotation is below 64");
(entropy.rotate_left(rotation).trailing_zeros() >= 5).then_some(edge.id)
})
.collect::<BTreeSet<_>>();
let forbidden_vertices = graph
.vertices
.iter()
.filter_map(|vertex| {
(vertex.id != from
&& vertex.id != target
&& entropy
.rotate_right(
u32::try_from(vertex.id.0 % 64).expect("rotation is below 64"),
)
.trailing_zeros()
>= 6)
.then_some(vertex.id)
})
.collect::<BTreeSet<_>>();
let request = RouteRequest {
from,
target,
previous: None,
law,
cost_ceiling: (trial % 5 == 0).then_some(2_000.0),
forbidden_edges: Some(&forbidden_edges),
forbidden_vertices: Some(&forbidden_vertices),
};
let expected = reference_cost(&graph, request);
let actual = router
.shortest_path(&graph, &mut workspace, request)
.map(|path| path_cost(&graph, request, &path));
assert_eq!(actual.is_some(), expected.is_some(), "trial {trial}");
if let (Some(actual), Some(expected)) = (actual, expected) {
assert!((actual - expected).abs() < 1.0e-9, "trial {trial}");
}
}
}
fn path_cost(graph: &WalkGraph, request: RouteRequest<'_>, path: &[EdgeId]) -> f64 {
path.iter()
.map(|edge| request.law.edge_cost(graph, *edge).unwrap())
.sum()
}
fn reference_cost(graph: &WalkGraph, request: RouteRequest<'_>) -> Option<f64> {
if request.from == request.target {
return Some(0.0);
}
let origin = (request.from, request.previous);
let mut labels = BTreeMap::<(VertexId, Option<EdgeId>), f64>::from([(origin, 0.0)]);
let mut settled = BTreeSet::new();
loop {
let (&state, &cost) = labels
.iter()
.filter(|(state, _)| !settled.contains(*state))
.min_by(|(left_state, left), (right_state, right)| {
(**left)
.total_cmp(right)
.then_with(|| left_state.cmp(right_state))
})?;
settled.insert(state);
let (at, previous) = state;
if at == request.target {
return Some(cost);
}
for edge in graph.adjacency[at.0].iter().copied() {
if request
.forbidden_edges
.is_some_and(|forbidden| forbidden.contains(&edge))
|| !graph.turn_allowed(previous, at, edge)
{
continue;
}
let Some(next) = graph.edges[edge.0].traverse(at) else {
continue;
};
if next != request.target
&& request
.forbidden_vertices
.is_some_and(|forbidden| forbidden.contains(&next))
{
continue;
}
let Some(step_cost) = request.law.edge_cost(graph, edge) else {
continue;
};
let next_cost = cost + step_cost;
if request
.cost_ceiling
.is_some_and(|ceiling| next_cost > ceiling)
{
continue;
}
let next_state = (next, Some(edge));
if labels
.get(&next_state)
.is_none_or(|known| next_cost < *known)
{
labels.insert(next_state, next_cost);
}
}
}
}
}