router: step on every probe, not only navvertices

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<G, K, R>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
     fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker<G>) -> Option<R>;
@@ -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<G, K>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
     pub graph: G,
@@ -125,6 +128,9 @@ where
     pub scores: HashMap<G::NodeId, K>,
     pub estimate_scores: HashMap<G::NodeId, K>,
     pub path_tracker: PathTracker<G>,
+    pub maybe_curr_node: Option<G::NodeId>,
+    // FIXME: To work around edge references borrowing from the graph we collect then reiterate over tem.
+    pub edge_ids: VecDeque<G::EdgeId>,
 }
 
 #[derive(Error, Debug, Clone)]
@@ -138,7 +144,7 @@ pub enum AstarStatus<G, K, R>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
     Running,
@@ -149,7 +155,7 @@ impl<G, K> Astar<G, K>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
+    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 {
@@ -159,6 +165,8 @@ where
             scores: HashMap::new(),
             estimate_scores: HashMap::new(),
             path_tracker: PathTracker::<G>::new(),
+            maybe_curr_node: None,
+            edge_ids: VecDeque::new(),
         };
 
         let zero_score = K::default();
@@ -174,35 +182,13 @@ where
         &mut self,
         strategy: &mut impl AstarStrategy<G, K, R>,
     ) -> Result<AstarStatus<G, K, R>, AstarError> {
-        let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else {
+        if let Some(curr_node) = self.maybe_curr_node {
-            return Err(AstarError::NotFound);
+            if let Some(edge_id) = self.edge_ids.pop_front() {
-        };
-
-        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));
-        }
-
                 // 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];
+                let node_score = self.scores[&curr_node];
+                let edge = (&self.graph).edge_ref(edge_id);
 
-        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;
@@ -221,10 +207,41 @@ where
                         }
                     }
 
-                self.path_tracker.set_predecessor(next, node);
+                    self.path_tracker.set_predecessor(next, curr_node);
-                let next_estimate_score = next_score + strategy.estimate_cost(&self.graph, next);
+                    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<G, K, R>(
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId>,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
     let mut astar = Astar::new(graph, start, strategy);

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

src/router/route.rs

@@ -31,7 +31,6 @@ use crate::{
     },
 };
 
-#[derive(Debug)]
 pub struct Route {
     astar: Astar<Navmesh, f64>,
     trace: Trace,