refactor(router/thetastar): Backtrack not once, but repeatedly, if condition is met

src/router/router.rs

@@ -2,6 +2,8 @@
 //
 // SPDX-License-Identifier: MIT
 
+use std::ops::ControlFlow;
+
 use derive_getters::Getters;
 use geo::algorithm::line_measures::{Distance, Euclidean};
 use petgraph::data::DataMap;
@@ -93,26 +95,28 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
         &mut self,
         navmesh: &Navmesh,
         probed_navnode: NavnodeIndex,
-    ) -> Option<f64> {
+    ) -> ControlFlow<Option<f64>> {
         let result = self.navcord.step_to(self.layout, navmesh, probed_navnode);
 
-        match result {
+        ControlFlow::Break(match result {
             Ok(probe_length) => Some(probe_length),
             Err(err) => {
                 if let NavcorderException::CannotDraw(draw_err) = err {
                     let layout_err = match draw_err {
-                        DrawException::NoTangents(..) => return None,
+                        DrawException::NoTangents(..) => return ControlFlow::Break(None),
                         DrawException::CannotFinishIn(.., layout_err) => layout_err,
                         DrawException::CannotWrapAround(.., layout_err) => layout_err,
                     };
 
-                    let (ghost, obstacle) = layout_err.maybe_ghost_and_obstacle()?;
+                    let Some((ghost, obstacle)) = layout_err.maybe_ghost_and_obstacle() else {
+                        return ControlFlow::Break(None);
+                    };
                     self.probe_ghosts = vec![*ghost];
                     self.probe_obstacles = vec![obstacle];
                 }
                 None
             }
-        }
+        })
     }
 
     fn remove_probe(&mut self, _navmesh: &Navmesh) {

src/router/thetastar.rs

@@ -120,7 +120,11 @@ where
         navnode: G::NodeId,
         tracker: &PathTracker<G>,
     ) -> Result<Option<R>, ()>;
-    fn place_probe_to_navnode(&mut self, graph: &G, probed_navnode: G::NodeId) -> Option<K>;
+    fn place_probe_to_navnode(
+        &mut self,
+        graph: &G,
+        probed_navnode: G::NodeId,
+    ) -> ControlFlow<Option<K>>;
     fn remove_probe(&mut self, graph: &G);
     fn estimate_cost_to_goal(&mut self, graph: &G, navnode: G::NodeId) -> K;
 }
@@ -145,10 +149,17 @@ pub enum ThetastarState<N: Copy, E: Copy> {
 }
 
 /// The pathfinding algorithm Topola uses to find the shortest path to route
-/// is Theta*. Theta* is just A* with an improvement: every time an navedge is
-/// scanned, the algorithm first tries to draw directly to its target navnode
-/// from the predecessor of the currently visited navnode. Note that this
-/// creates paths with edges that do not all lie on the navmesh.
+/// is Theta* with conditional repeated backtracking. Theta* is just A* with an
+/// improvement: every time an navedge is scanned, the algorithm first tries to
+/// draw directly to its target navnode from the predecessor of the currently
+/// visited navnode. Note that this creates paths with edges that do not all lie
+/// on the navmesh.
+///
+/// Conditional repeated backtracking is our improvement to Theta*: if
+/// line-of-sight routing fails if a condition is met, continue trying to draw
+/// from parent of the parent navnode, and so on. This is different from Theta*
+/// because in Theta* there is only one backtracking step -- only one attempt to
+/// do line-of-sight routing.
 #[derive(Getters)]
 pub struct ThetastarStepper<G, K>
 where
@@ -301,16 +312,27 @@ where
                     // necessarily scored before adding it to `.visit_next`.
                     //let node_score = self.scores[&visited_navnode];
                     let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
+                    let mut curr_navnode = visited_navnode;
+
+                    // 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);
+                        };
 
-                    if let Some(parent_navnode) = self.path_tracker.predecessor(visited_navnode) {
                         // Visit parent node.
                         strategy.visit_navnode(&self.graph, parent_navnode, &self.path_tracker);
 
-                        let parent_score = self.scores[&parent_navnode];
-
-                        if let Some(los_cost) =
+                        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;
 
@@ -344,18 +366,7 @@ where
                         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 {
+                        strategy.visit_navnode(&self.graph, visited_navnode, &self.path_tracker);
                         self.state =
                             ThetastarState::VisitingProbeOnNavedge(visited_navnode, curr_navedge);
                         Ok(ControlFlow::Continue(self.state))
@@ -369,7 +380,8 @@ where
                 let visited_score = self.scores[&visited_navnode];
                 let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
 
-                if let Some(navedge_cost) = strategy.place_probe_to_navnode(&self.graph, to_navnode)
+                if let ControlFlow::Break(Some(navedge_cost)) =
+                    strategy.place_probe_to_navnode(&self.graph, to_navnode)
                 {
                     let next = to_navnode;
                     let next_score = visited_score + navedge_cost;