astar: Probe neighbors before adding tem to queue

src/astar.rs

@@ -101,7 +101,7 @@ where
     G::NodeId: Eq + Hash,
 {
     fn is_goal(&mut self, node: G::NodeId, tracker: &PathTracker<G>) -> bool;
-    fn edge_cost(&mut self, edge: G::EdgeRef) -> K;
+    fn edge_cost(&mut self, edge: G::EdgeRef) -> Option<K>;
     fn estimate_cost(&mut self, node: G::NodeId) -> K;
 }
 
@@ -150,8 +150,9 @@ where
         }
 
         for edge in graph.edges(node) {
+            if let Some(edge_cost) = strategy.edge_cost(edge) {
                 let next = edge.target();
-            let next_score = node_score + strategy.edge_cost(edge);
+                let next_score = node_score + edge_cost;
 
                 match scores.entry(next) {
                     Occupied(mut entry) => {
@@ -172,6 +173,7 @@ where
                 visit_next.push(MinScored(next_estimate_score, next));
             }
         }
+    }
 
     None
 }

src/router.rs

@@ -50,9 +50,18 @@ impl<'a, RO: RouterObserver> AstarStrategy<&Mesh, u64> for RouterAstarStrategy<'
         self.tracer.finish(&mut self.trace, self.to, 5.0).is_ok()
     }
 
-    fn edge_cost(&mut self, edge: MeshEdgeReference) -> u64 {
+    fn edge_cost(&mut self, edge: MeshEdgeReference) -> Option<u64> {
+        if self
+            .tracer
+            .step(&mut self.trace, edge.target(), 5.0)
+            .is_ok()
+        {
             self.observer.on_probe(&self.tracer, edge);
-        1
+            self.tracer.undo_step(&mut self.trace);
+            Some(1)
+        } else {
+            None
+        }
     }
 
     fn estimate_cost(&mut self, vertex: VertexIndex) -> u64 {