From 4e8e9298e6a26aadc9c24f933c586e9d26db27d3 Mon Sep 17 00:00:00 2001
From: Mikolaj Wielgus <wielgusmikolaj@gmail.com>
Date: Thu, 12 Oct 2023 00:08:54 +0000
Subject: [PATCH] astar: Probe neighbors before adding tem to queue

---
 src/astar.rs  | 36 +++++++++++++++++++-----------------
 src/router.rs | 15 ++++++++++++---
 2 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/astar.rs b/src/astar.rs
index e80b16d..d25be61 100644
--- a/src/astar.rs
+++ b/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,26 +150,28 @@ where
         }
 
         for edge in graph.edges(node) {
-            let next = edge.target();
-            let next_score = node_score + strategy.edge_cost(edge);
+            if let Some(edge_cost) = strategy.edge_cost(edge) {
+                let next = edge.target();
+                let next_score = node_score + edge_cost;
 
-            match 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 {
-                        continue;
+                match 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 {
+                            continue;
+                        }
+                        entry.insert(next_score);
+                    }
+                    Vacant(entry) => {
+                        entry.insert(next_score);
                     }
-                    entry.insert(next_score);
                 }
-                Vacant(entry) => {
-                    entry.insert(next_score);
-                }
-            }
 
-            path_tracker.set_predecessor(next, node);
-            let next_estimate_score = next_score + strategy.estimate_cost(next);
-            visit_next.push(MinScored(next_estimate_score, next));
+                path_tracker.set_predecessor(next, node);
+                let next_estimate_score = next_score + strategy.estimate_cost(next);
+                visit_next.push(MinScored(next_estimate_score, next));
+            }
         }
     }
 
diff --git a/src/router.rs b/src/router.rs
index bc7679b..ce800c9 100644
--- a/src/router.rs
+++ b/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 {
-        self.observer.on_probe(&self.tracer, edge);
-        1
+    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);
+            self.tracer.undo_step(&mut self.trace);
+            Some(1)
+        } else {
+            None
+        }
     }
 
     fn estimate_cost(&mut self, vertex: VertexIndex) -> u64 {