router: step on successful probes

src/router/astar.rs

@@ -104,11 +104,12 @@ where
     K: Measure + Copy,
 {
     fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker<G>) -> Option<R>;
-    fn probe<'a>(
+    fn place_probe<'a>(
         &mut self,
         graph: &'a G,
         edge: <&'a G as IntoEdgeReferences>::EdgeRef,
     ) -> Option<K>;
+    fn remove_probe<'a>(&mut self, graph: &'a G);
     fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K;
 }
 
@@ -131,6 +132,8 @@ where
     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>,
+    // TODO: Rewrite this to be a well-designed state machine.
+    pub is_probing: bool,
 }
 
 #[derive(Error, Debug, Clone)]
@@ -147,6 +150,7 @@ where
     for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
+    Probing,
     Probed,
     Visited,
     Finished(K, Vec<G::NodeId>, R),
@@ -168,6 +172,7 @@ where
             path_tracker: PathTracker::<G>::new(),
             maybe_curr_node: None,
             edge_ids: VecDeque::new(),
+            is_probing: false,
         };
 
         let zero_score = K::default();
@@ -184,13 +189,18 @@ where
         strategy: &mut impl AstarStrategy<G, K, R>,
     ) -> Result<AstarStatus<G, K, R>, AstarError> {
         if let Some(curr_node) = self.maybe_curr_node {
+            if self.is_probing {
+                strategy.remove_probe(&self.graph);
+                self.is_probing = false;
+            }
+
             if let Some(edge_id) = self.edge_ids.pop_front() {
                 // 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[&curr_node];
                 let edge = (&self.graph).edge_ref(edge_id);
 
-                if let Some(edge_cost) = strategy.probe(&self.graph, edge) {
+                if let Some(edge_cost) = strategy.place_probe(&self.graph, edge) {
                     let next = edge.target();
                     let next_score = node_score + edge_cost;
 
@@ -212,6 +222,9 @@ where
                     let next_estimate_score =
                         next_score + strategy.estimate_cost(&self.graph, next);
                     self.visit_next.push(MinScored(next_estimate_score, next));
+
+                    self.is_probing = true;
+                    return Ok(AstarStatus::Probing);
                 }
 
                 return Ok(AstarStatus::Probed);

src/router/route.rs

@@ -85,12 +85,14 @@ impl Route {
         let mut strategy = RouterAstarStrategy::new(tracer, &mut self.trace, target);
 
         let result = match self.astar.step(&mut strategy)? {
-            AstarStatus::Probed | AstarStatus::Visited => Ok(RouterStatus::Running),
+            AstarStatus::Probing | AstarStatus::Probed | AstarStatus::Visited => {
+                Ok(RouterStatus::Running)
+            }
             AstarStatus::Finished(_cost, _path, band) => Ok(RouterStatus::Finished(band)),
         };
 
-        self.ghosts = strategy.last_ghosts;
+        self.ghosts = strategy.probe_ghosts;
-        self.obstacles = strategy.last_obstacles;
+        self.obstacles = strategy.probe_obstacles;
         result
     }
 

src/router/router.rs

@@ -47,8 +47,8 @@ pub struct RouterAstarStrategy<'a, R: AccessRules> {
     pub tracer: Tracer<'a, R>,
     pub trace: &'a mut Trace,
     pub target: FixedDotIndex,
-    pub last_ghosts: Vec<PrimitiveShape>,
+    pub probe_ghosts: Vec<PrimitiveShape>,
-    pub last_obstacles: Vec<PrimitiveIndex>,
+    pub probe_obstacles: Vec<PrimitiveIndex>,
 }
 
 impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> {
@@ -57,8 +57,8 @@ impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> {
             tracer,
             trace,
             target,
-            last_ghosts: vec![],
+            probe_ghosts: vec![],
-            last_obstacles: vec![],
+            probe_obstacles: vec![],
         }
     }
 
@@ -99,7 +99,7 @@ impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
             .ok()
     }
 
-    fn probe(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option<f64> {
+    fn place_probe(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option<f64> {
         if edge.target().petgraph_index() == self.target.petgraph_index() {
             return None;
         }
@@ -114,10 +114,7 @@ impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
         let probe_length = self.bihead_length() - prev_bihead_length;
 
         match result {
-            Ok(..) => {
+            Ok(..) => Some(probe_length),
-                self.trace.undo_step(&mut self.tracer);
-                Some(probe_length)
-            }
             Err(err) => {
                 if let TracerException::CannotDraw(draw_err) = err {
                     let layout_err = match draw_err {
@@ -135,14 +132,18 @@ impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
                         LayoutException::AlreadyConnected(..) => return None,
                     };
 
-                    self.last_ghosts = vec![ghost];
+                    self.probe_ghosts = vec![ghost];
-                    self.last_obstacles = vec![obstacle];
+                    self.probe_obstacles = vec![obstacle];
                 }
                 None
             }
         }
     }
 
+    fn remove_probe(&mut self, _navmesh: &Navmesh) {
+        self.trace.undo_step(&mut self.tracer);
+    }
+
     fn estimate_cost(&mut self, navmesh: &Navmesh, vertex: NavvertexIndex) -> f64 {
         let start_point = PrimitiveIndex::from(navmesh.node_weight(vertex).unwrap().node)
             .primitive(self.tracer.layout.drawing())