From 727eb37c6e34c73062b7522a211d7f472ec0126d Mon Sep 17 00:00:00 2001 From: Mikolaj Wielgus Date: Wed, 3 Jul 2024 18:12:07 +0200 Subject: [PATCH] router: have probing and visiting as separate states This feels overengineered, but I need something like this for better debuginfo. --- src/router/astar.rs | 124 +++++++++++++++++++++++-------------------- src/router/route.rs | 2 +- src/router/router.rs | 10 ++-- 3 files changed, 73 insertions(+), 63 deletions(-) diff --git a/src/router/astar.rs b/src/router/astar.rs index 25b73e2..eca12bf 100644 --- a/src/router/astar.rs +++ b/src/router/astar.rs @@ -96,7 +96,7 @@ where } } -pub trait AstarStrategy +pub trait AstarStrategy where G: GraphBase, G::NodeId: Eq + Hash, @@ -104,11 +104,11 @@ where K: Measure + Copy, { fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker) -> Option; - fn edge_cost<'a>( + fn probe<'a>( &mut self, graph: &'a G, edge: <&'a G as IntoEdgeReferences>::EdgeRef, - ) -> Option; + ) -> Result<(K, PS), PE>; fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K; } @@ -140,14 +140,15 @@ pub enum AstarError { } #[derive(Debug)] -pub enum AstarStatus +pub enum AstarStatus where G: GraphBase, G::NodeId: Eq + Hash, for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { - Running, + Probed(Result), + Visited, Finished(K, Vec, R), } @@ -158,7 +159,11 @@ where for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { - pub fn new(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy) -> Self { + pub fn new( + graph: G, + start: G::NodeId, + strategy: &mut impl AstarStrategy, + ) -> Self { let mut this = Self { graph, visit_next: BinaryHeap::new(), @@ -178,10 +183,10 @@ where this } - pub fn step( + pub fn step( &mut self, - strategy: &mut impl AstarStrategy, - ) -> Result, AstarError> { + strategy: &mut impl AstarStrategy, + ) -> Result, AstarError> { 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 @@ -189,69 +194,74 @@ where let node_score = self.scores[&curr_node]; let edge = (&self.graph).edge_ref(edge_id); - if let Some(edge_cost) = strategy.edge_cost(&self.graph, edge) { - let next = edge.target(); - let next_score = node_score + edge_cost; + match strategy.probe(&self.graph, edge) { + Ok((edge_cost, probe_status)) => { + 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::Probed(Ok(probe_status))); + } + 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, curr_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)); + + return Ok(AstarStatus::Probed(Ok(probe_status))); + } + Err(probe_err) => return Ok(AstarStatus::Probed(Err(probe_err))), } } 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) + 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::Visited); + } + 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::Visited) } } -pub fn astar( +pub fn astar( graph: G, start: G::NodeId, - strategy: &mut impl AstarStrategy, + strategy: &mut impl AstarStrategy, ) -> Result<(K, Vec, R), AstarError> where G: GraphBase, diff --git a/src/router/route.rs b/src/router/route.rs index 802b816..7490d94 100644 --- a/src/router/route.rs +++ b/src/router/route.rs @@ -73,7 +73,7 @@ impl Route { let mut strategy = RouterAstarStrategy::new(tracer, &mut self.trace, target); match self.astar.step(&mut strategy)? { - AstarStatus::Running => Ok(RouterStatus::Running), + AstarStatus::Probed(..) | AstarStatus::Visited => Ok(RouterStatus::Running), AstarStatus::Finished(_cost, _path, band) => Ok(RouterStatus::Finished(band)), } } diff --git a/src/router/router.rs b/src/router/router.rs index 890502b..d77158f 100644 --- a/src/router/router.rs +++ b/src/router/router.rs @@ -71,7 +71,7 @@ impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> { } } -impl<'a, R: AccessRules> AstarStrategy +impl<'a, R: AccessRules> AstarStrategy for RouterAstarStrategy<'a, R> { fn is_goal( @@ -92,9 +92,9 @@ impl<'a, R: AccessRules> AstarStrategy .ok() } - fn edge_cost(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option { + fn probe(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Result<(f64, ()), ()> { if edge.target().petgraph_index() == self.target.petgraph_index() { - return None; + return Err(()); } let prev_bihead_length = self.bihead_length(); @@ -108,9 +108,9 @@ impl<'a, R: AccessRules> AstarStrategy if result.is_ok() { self.trace.undo_step(&mut self.tracer); - Some(probe_length) + Ok((probe_length, ())) } else { - None + Err(()) } }