From 27d1c9e7120d09f437bcddcf48ccf186cb72db68 Mon Sep 17 00:00:00 2001 From: Mikolaj Wielgus Date: Tue, 2 Jul 2024 02:36:42 +0200 Subject: [PATCH] router: step on every probe, not only navvertices --- src/router/astar.rs | 121 ++++++++++++++++++++++++------------------ src/router/navmesh.rs | 13 +++++ src/router/route.rs | 1 - 3 files changed, 82 insertions(+), 53 deletions(-) diff --git a/src/router/astar.rs b/src/router/astar.rs index e9df77d..25b73e2 100644 --- a/src/router/astar.rs +++ b/src/router/astar.rs @@ -5,7 +5,7 @@ * Copyright (c) 2015 **/ use std::collections::hash_map::Entry::{Occupied, Vacant}; -use std::collections::{BinaryHeap, HashMap}; +use std::collections::{BinaryHeap, HashMap, VecDeque}; use std::hash::Hash; @@ -100,7 +100,7 @@ pub trait AstarStrategy where G: GraphBase, G::NodeId: Eq + Hash, - for<'a> &'a G: IntoEdges, + for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker) -> Option; @@ -112,12 +112,15 @@ where fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K; } -#[derive(Debug)] +pub trait MakeEdgeRef: IntoEdgeReferences { + fn edge_ref(&self, edge_id: Self::EdgeId) -> Self::EdgeRef; +} + pub struct Astar where G: GraphBase, G::NodeId: Eq + Hash, - for<'a> &'a G: IntoEdges, + for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { pub graph: G, @@ -125,6 +128,9 @@ where pub scores: HashMap, pub estimate_scores: HashMap, pub path_tracker: PathTracker, + pub maybe_curr_node: Option, + // FIXME: To work around edge references borrowing from the graph we collect then reiterate over tem. + pub edge_ids: VecDeque, } #[derive(Error, Debug, Clone)] @@ -138,7 +144,7 @@ pub enum AstarStatus where G: GraphBase, G::NodeId: Eq + Hash, - for<'a> &'a G: IntoEdges, + for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { Running, @@ -149,7 +155,7 @@ impl Astar where G: GraphBase, G::NodeId: Eq + Hash, - for<'a> &'a G: IntoEdges, + for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { pub fn new(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy) -> Self { @@ -159,6 +165,8 @@ where scores: HashMap::new(), estimate_scores: HashMap::new(), path_tracker: PathTracker::::new(), + maybe_curr_node: None, + edge_ids: VecDeque::new(), }; let zero_score = K::default(); @@ -174,57 +182,66 @@ where &mut self, strategy: &mut impl AstarStrategy, ) -> Result, AstarError> { - let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else { - return Err(AstarError::NotFound); - }; + if let Some(curr_node) = self.maybe_curr_node { + if let Some(edge_id) = self.edge_ids.pop_front() { + // This lookup can be unwrapped without fear of panic since the node was + // necessarily scored before adding it to `visit_next`. + let node_score = self.scores[&curr_node]; + let edge = (&self.graph).edge_ref(edge_id); - if let Some(result) = strategy.is_goal(&self.graph, node, &self.path_tracker) { - let path = self.path_tracker.reconstruct_path_to(node); - let cost = self.scores[&node]; - return Ok(AstarStatus::Finished(cost, path, result)); - } + if let Some(edge_cost) = strategy.edge_cost(&self.graph, edge) { + let next = edge.target(); + let next_score = node_score + edge_cost; - // This lookup can be unwrapped without fear of panic since the node was - // necessarily scored before adding it to `visit_next`. - let node_score = self.scores[&node]; - - match self.estimate_scores.entry(node) { - Occupied(mut entry) => { - // If the node has already been visited with an equal or lower score than - // now, then we do not need to re-visit it. - if *entry.get() <= estimate_score { - return Ok(AstarStatus::Running); - } - entry.insert(estimate_score); - } - Vacant(entry) => { - entry.insert(estimate_score); - } - } - - for edge in self.graph.edges(node) { - if let Some(edge_cost) = strategy.edge_cost(&self.graph, edge) { - let next = edge.target(); - let next_score = node_score + edge_cost; - - match self.scores.entry(next) { - Occupied(mut entry) => { - // No need to add neighbors that we have already reached through a - // shorter path than now. - if *entry.get() <= next_score { - return Ok(AstarStatus::Running); + match self.scores.entry(next) { + Occupied(mut entry) => { + // No need to add neighbors that we have already reached through a + // shorter path than now. + if *entry.get() <= next_score { + return Ok(AstarStatus::Running); + } + entry.insert(next_score); + } + Vacant(entry) => { + entry.insert(next_score); } - entry.insert(next_score); } - Vacant(entry) => { - entry.insert(next_score); - } - } - self.path_tracker.set_predecessor(next, node); - let next_estimate_score = next_score + strategy.estimate_cost(&self.graph, next); - self.visit_next.push(MinScored(next_estimate_score, next)); + self.path_tracker.set_predecessor(next, curr_node); + let next_estimate_score = + next_score + strategy.estimate_cost(&self.graph, next); + self.visit_next.push(MinScored(next_estimate_score, next)); + } + } else { + self.maybe_curr_node = None; } + } else { + let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else { + return Err(AstarError::NotFound); + }; + + if let Some(result) = strategy.is_goal(&self.graph, node, &self.path_tracker) { + let path = self.path_tracker.reconstruct_path_to(node); + let cost = self.scores[&node]; + return Ok(AstarStatus::Finished(cost, path, result)); + } + + match self.estimate_scores.entry(node) { + Occupied(mut entry) => { + // If the node has already been visited with an equal or lower score than + // now, then we do not need to re-visit it. + if *entry.get() <= estimate_score { + return Ok(AstarStatus::Running); + } + entry.insert(estimate_score); + } + Vacant(entry) => { + entry.insert(estimate_score); + } + } + + self.maybe_curr_node = Some(node); + self.edge_ids = self.graph.edges(node).map(|edge| edge.id()).collect(); } Ok(AstarStatus::Running) @@ -239,7 +256,7 @@ pub fn astar( where G: GraphBase, G::NodeId: Eq + Hash, - for<'a> &'a G: IntoEdges, + for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { let mut astar = Astar::new(graph, start, strategy); diff --git a/src/router/navmesh.rs b/src/router/navmesh.rs index 41c5fc0..f1146e1 100644 --- a/src/router/navmesh.rs +++ b/src/router/navmesh.rs @@ -26,6 +26,7 @@ use crate::{ geometry::shape::ShapeTrait, graph::GetPetgraphIndex, layout::Layout, + router::astar::MakeEdgeRef, triangulation::{GetTrianvertexNodeIndex, Triangulation}, }; @@ -322,3 +323,15 @@ impl<'a> IntoEdges for &'a Navmesh { ) } } + +impl<'a> MakeEdgeRef for &'a Navmesh { + fn edge_ref( + &self, + edge_id: <&'a Navmesh as GraphBase>::EdgeId, + ) -> <&'a Navmesh as IntoEdgeReferences>::EdgeRef { + NavmeshEdgeReference { + from: edge_id.0, + to: edge_id.1, + } + } +} diff --git a/src/router/route.rs b/src/router/route.rs index d32e44a..c8a00c3 100644 --- a/src/router/route.rs +++ b/src/router/route.rs @@ -31,7 +31,6 @@ use crate::{ }, }; -#[derive(Debug)] pub struct Route { astar: Astar, trace: Trace,