feat(router/thetastar): Transition state for each backtrack step for better debugging

src/router/thetastar.rs

@@ -138,12 +138,14 @@ pub trait MakeEdgeRef: IntoEdgeReferences {
 pub enum ThetastarState<N: Copy, E: Copy> {
     /// Visit a new navnode and copy all navedges going out of it.
     Scanning,
-    /// Load a new navedge, try to probe it from current navnode's predecessor.
+    /// Load a new navedge.
-    /// If this fails, transition to `VisitingProbeOnNavedge`. If there is no
+    VisitFrontierNavedge(N),
-    /// more navedges, transition back to `Scanning`.
+    /// Backtrack by one navnode, then line-of-sight probe to target of the
-    VisitingProbeOnLineOfSight(N),
+    /// currently visited navedge. Transition to this state again until a
-    /// Probe the currently loaded navedge from the current navnode.
+    /// condition is met, then continue the algorithm by transitioning to the
-    VisitingProbeOnNavedge(N, E),
+    /// `ProbeOnNavedge(...)` state.
+    BacktrackAndProbeOnLineOfSight(N, E),
+    ProbeOnNavedge(N, E),
     /// The probe is in place, retract it and continue to the next navedge.
     Probing(N),
 }
@@ -303,82 +305,99 @@ where
 
                 self.edge_ids = self.graph.edges(navnode).map(|edge| edge.id()).collect();
 
-                self.state = ThetastarState::VisitingProbeOnLineOfSight(navnode);
+                self.state = ThetastarState::VisitFrontierNavedge(navnode);
                 Ok(ControlFlow::Continue(self.state))
             }
-            ThetastarState::VisitingProbeOnLineOfSight(visited_navnode) => {
+            ThetastarState::VisitFrontierNavedge(visited_navnode) => {
-                if let Some(curr_navedge) = self.edge_ids.pop() {
+                if let Some(visited_navedge) = self.edge_ids.pop() {
-                    // This lookup can be unwrapped without fear of panic since the node was
+                    self.state = ThetastarState::BacktrackAndProbeOnLineOfSight(
-                    // necessarily scored before adding it to `.visit_next`.
+                        visited_navnode,
-                    //let node_score = self.scores[&visited_navnode];
+                        visited_navedge,
-                    let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
+                    );
-                    let mut curr_navnode = visited_navnode;
+                    Ok(ControlFlow::Continue(self.state))
-
-                    // Loop to repeatedly backtrack.
-                    if let (Some(parent_navnode), Some(los_cost)) = loop {
-                        let Some(parent_navnode) = self.path_tracker.predecessor(curr_navnode)
-                        else {
-                            break (None, None);
-                        };
-
-                        // Visit parent node.
-                        strategy.visit_navnode(&self.graph, parent_navnode, &self.path_tracker);
-
-                        if let ControlFlow::Break(result) =
-                            strategy.place_probe_to_navnode(&self.graph, to_navnode)
-                        {
-                            break (Some(parent_navnode), result);
-                        }
-
-                        curr_navnode = parent_navnode;
-                    } {
-                        let parent_score = self.scores[&parent_navnode];
-                        let next = to_navnode;
-                        let next_score = parent_score + los_cost;
-
-                        match self.scores.entry(next) {
-                            Entry::Occupied(mut entry) => {
-                                // No need to add neighbors that we have already reached through a
-                                // shorter path than now.
-                                if *entry.get() <= next_score {
-                                    // We just remove the probe instantly
-                                    // here instead of doing it in
-                                    // ThetastarState::Probing or a new
-                                    // state to avoid complicating.
-                                    strategy.remove_probe(&self.graph);
-
-                                    self.state = ThetastarState::VisitingProbeOnNavedge(
-                                        visited_navnode,
-                                        curr_navedge,
-                                    );
-                                    return Ok(ControlFlow::Continue(self.state));
-                                }
-                                entry.insert(next_score);
-                            }
-                            Entry::Vacant(entry) => {
-                                entry.insert(next_score);
-                            }
-                        }
-
-                        self.push_to_frontier(next, next_score, parent_navnode, strategy);
-
-                        self.state = ThetastarState::Probing(visited_navnode);
-                        Ok(ControlFlow::Continue(self.state))
-                    } else {
-                        // Come back from parent node if drawing from it failed.
-                        strategy.visit_navnode(&self.graph, visited_navnode, &self.path_tracker);
-                        self.state =
-                            ThetastarState::VisitingProbeOnNavedge(visited_navnode, curr_navedge);
-                        Ok(ControlFlow::Continue(self.state))
-                    }
                 } else {
                     self.state = ThetastarState::Scanning;
                     Ok(ControlFlow::Continue(self.state))
                 }
             }
-            ThetastarState::VisitingProbeOnNavedge(visited_navnode, curr_navedge) => {
+            ThetastarState::BacktrackAndProbeOnLineOfSight(visited_navnode, visited_navedge) => {
+                // 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[&visited_navnode];
+                let initial_from_navnode = (&self.graph).edge_ref(visited_navedge).source();
+                let to_navnode = (&self.graph).edge_ref(visited_navedge).target();
+
+                if let Some(parent_navnode) = self.path_tracker.predecessor(visited_navnode) {
+                    strategy.visit_navnode(&self.graph, parent_navnode, &self.path_tracker);
+                    let parent_score = self.scores[&parent_navnode];
+
+                    match strategy.place_probe_to_navnode(&self.graph, to_navnode) {
+                        ControlFlow::Continue(()) => {
+                            // Transition to self to repeatedly backtrack.
+                            self.state = ThetastarState::BacktrackAndProbeOnLineOfSight(
+                                parent_navnode,
+                                visited_navedge,
+                            );
+                            Ok(ControlFlow::Continue(self.state))
+                        }
+                        ControlFlow::Break(Some(los_cost)) => {
+                            let next = to_navnode;
+                            let next_score = parent_score + los_cost;
+
+                            match self.scores.entry(next) {
+                                Entry::Occupied(mut entry) => {
+                                    // No need to add neighbors that we have already reached through a
+                                    // shorter path than now.
+                                    if *entry.get() <= next_score {
+                                        // We just remove the probe instantly
+                                        // here instead of doing it in
+                                        // ThetastarState::Probing or a new
+                                        // state to avoid complicating.
+                                        strategy.remove_probe(&self.graph);
+
+                                        self.state = ThetastarState::ProbeOnNavedge(
+                                            visited_navnode,
+                                            visited_navedge,
+                                        );
+                                        return Ok(ControlFlow::Continue(self.state));
+                                    }
+                                    entry.insert(next_score);
+                                }
+                                Entry::Vacant(entry) => {
+                                    entry.insert(next_score);
+                                }
+                            }
+
+                            self.push_to_frontier(next, next_score, parent_navnode, strategy);
+
+                            self.state = ThetastarState::Probing(visited_navnode);
+                            Ok(ControlFlow::Continue(self.state))
+                        }
+                        ControlFlow::Break(None) => {
+                            // Come back to initial navnode if drawing failed
+                            // and the backtracking condition is not met.
+                            strategy.visit_navnode(
+                                &self.graph,
+                                visited_navnode,
+                                &self.path_tracker,
+                            );
+                            self.state = ThetastarState::ProbeOnNavedge(
+                                initial_from_navnode,
+                                visited_navedge,
+                            );
+                            Ok(ControlFlow::Continue(self.state))
+                        }
+                    }
+                } else {
+                    // Come back from parent node if drawing from it failed.
+                    strategy.visit_navnode(&self.graph, visited_navnode, &self.path_tracker);
+                    self.state = ThetastarState::ProbeOnNavedge(visited_navnode, visited_navedge);
+                    Ok(ControlFlow::Continue(self.state))
+                }
+            }
+            ThetastarState::ProbeOnNavedge(visited_navnode, visited_navedge) => {
                 let visited_score = self.scores[&visited_navnode];
-                let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
+                let to_navnode = (&self.graph).edge_ref(visited_navedge).target();
 
                 if let ControlFlow::Break(Some(navedge_cost)) =
                     strategy.place_probe_to_navnode(&self.graph, to_navnode)
@@ -406,14 +425,14 @@ where
                     self.state = ThetastarState::Probing(visited_navnode);
                     Ok(ControlFlow::Continue(self.state))
                 } else {
-                    self.state = ThetastarState::VisitingProbeOnLineOfSight(visited_navnode);
+                    self.state = ThetastarState::VisitFrontierNavedge(visited_navnode);
                     Ok(ControlFlow::Continue(self.state))
                 }
             }
             ThetastarState::Probing(visited_navnode) => {
                 strategy.remove_probe(&self.graph);
 
-                self.state = ThetastarState::VisitingProbeOnLineOfSight(visited_navnode);
+                self.state = ThetastarState::VisitFrontierNavedge(visited_navnode);
                 Ok(ControlFlow::Continue(self.state))
             }
         }