router: have probing and visiting as separate states

This feels overengineered, but I need something like this for better
debuginfo.

src/router/astar.rs

@@ -96,7 +96,7 @@ where
     }
 }
 
-pub trait AstarStrategy<G, K, R>
+pub trait AstarStrategy<G, K, PS, PE, R>
 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<G>) -> Option<R>;
-    fn edge_cost<'a>(
+    fn probe<'a>(
         &mut self,
         graph: &'a G,
         edge: <&'a G as IntoEdgeReferences>::EdgeRef,
-    ) -> Option<K>;
+    ) -> 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<G, K, R>
+pub enum AstarStatus<G, K, PS, PE, R>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
     for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
-    Running,
+    Probed(Result<PS, PE>),
+    Visited,
     Finished(K, Vec<G::NodeId>, R),
 }
 
@@ -158,7 +159,11 @@ where
     for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
-    pub fn new<R>(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy<G, K, R>) -> Self {
+    pub fn new<PS, PE, R>(
+        graph: G,
+        start: G::NodeId,
+        strategy: &mut impl AstarStrategy<G, K, PS, PE, R>,
+    ) -> Self {
         let mut this = Self {
             graph,
             visit_next: BinaryHeap::new(),
@@ -178,10 +183,10 @@ where
         this
     }
 
-    pub fn step<R>(
+    pub fn step<PS, PE, R>(
         &mut self,
-        strategy: &mut impl AstarStrategy<G, K, R>,
+        strategy: &mut impl AstarStrategy<G, K, PS, PE, R>,
-    ) -> Result<AstarStatus<G, K, R>, AstarError> {
+    ) -> Result<AstarStatus<G, K, PS, PE, R>, 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) {
+                match strategy.probe(&self.graph, edge) {
-                    let next = edge.target();
+                    Ok((edge_cost, probe_status)) => {
-                    let next_score = node_score + edge_cost;
+                        let next = edge.target();
+                        let next_score = node_score + edge_cost;
 
-                    match self.scores.entry(next) {
+                        match self.scores.entry(next) {
-                        Occupied(mut entry) => {
+                            Occupied(mut entry) => {
-                            // No need to add neighbors that we have already reached through a
+                                // No need to add neighbors that we have already reached through a
-                            // shorter path than now.
+                                // shorter path than now.
-                            if *entry.get() <= next_score {
+                                if *entry.get() <= next_score {
-                                return Ok(AstarStatus::Running);
+                                    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);
+                        self.path_tracker.set_predecessor(next, curr_node);
-                    let next_estimate_score =
+                        let next_estimate_score =
-                        next_score + strategy.estimate_cost(&self.graph, next);
+                            next_score + strategy.estimate_cost(&self.graph, next);
-                    self.visit_next.push(MinScored(next_estimate_score, 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<G, K, R>(
+pub fn astar<G, K, PS, PE, R>(
     graph: G,
     start: G::NodeId,
-    strategy: &mut impl AstarStrategy<G, K, R>,
+    strategy: &mut impl AstarStrategy<G, K, PS, PE, R>,
 ) -> Result<(K, Vec<G::NodeId>, R), AstarError>
 where
     G: GraphBase,

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)),
         }
     }

src/router/router.rs

@@ -71,7 +71,7 @@ impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> {
     }
 }
 
-impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
+impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, (), (), BandFirstSegIndex>
     for RouterAstarStrategy<'a, R>
 {
     fn is_goal(
@@ -92,9 +92,9 @@ impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
             .ok()
     }
 
-    fn edge_cost(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option<f64> {
+    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<Navmesh, f64, BandFirstSegIndex>
 
         if result.is_ok() {
             self.trace.undo_step(&mut self.tracer);
-            Some(probe_length)
+            Ok((probe_length, ()))
         } else {
-            None
+            Err(())
         }
     }