feat(router/thetastar): Implement Theta* search algorithm

Closes https://codeberg.org/topola/topola/issues/121

committed.toml

@@ -60,13 +60,13 @@ allowed_scopes = [
   "math/cyclic_search",
   "math/polygon_tangents",
   "math/tangents",
-  "router/astar",
   "router/draw",
   "router/navcord",
   "router/navcorder",
   "router/navmesh",
   "router/route",
   "router/router",
+  "router/thetastar",
   "specctra/design",
   "stepper",
   "triangulation",

crates/topola-egui/src/viewport.rs

@@ -12,7 +12,8 @@ use topola::{
     autorouter::{
         execution::Command,
         invoker::{
-            GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+            GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts,
+            GetObstacles,
         },
     },
     board::AccessMesadata,
@@ -250,7 +251,7 @@ impl Viewport {
 
                         if menu_bar.show_navmesh {
                             if let Some(activity) = workspace.interactor.maybe_activity() {
-                                if let Some(astar) = activity.maybe_astar() {
+                                if let Some(astar) = activity.maybe_thetastar() {
                                     let navmesh = astar.graph();
 
                                     for edge in navmesh.edge_references() {
@@ -460,7 +461,7 @@ impl Viewport {
                             }
 
                             if let Some(ref navmesh) =
-                                activity.maybe_astar().map(|astar| astar.graph())
+                                activity.maybe_thetastar().map(|astar| astar.graph())
                             {
                                 if menu_bar.show_origin_destination {
                                     let (origin, destination) =

src/autorouter/autoroute.rs

@@ -14,13 +14,15 @@ use crate::{
     drawing::{band::BandTermsegIndex, graph::PrimitiveIndex, Collect},
     geometry::primitive::PrimitiveShape,
     layout::LayoutEdit,
-    router::{astar::AstarStepper, navcord::Navcord, navmesh::Navmesh, RouteStepper, Router},
+    router::{
+        navcord::Navcord, navmesh::Navmesh, thetastar::ThetastarStepper, RouteStepper, Router,
+    },
     stepper::Step,
 };
 
 use super::{
     invoker::{
-        GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+        GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, GetObstacles,
     },
     Autorouter, AutorouterError, AutorouterOptions,
 };
@@ -92,7 +94,7 @@ impl<M: AccessMesadata> Step<Autorouter<M>, Option<LayoutEdit>, AutorouteContinu
     ) -> Result<ControlFlow<Option<LayoutEdit>, AutorouteContinueStatus>, AutorouterError> {
         let Some(curr_ratline) = self.curr_ratline else {
             let recorder = if let Some(taken_route) = self.route.take() {
-                let (_astar, navcord, ..) = taken_route.dissolve();
+                let (_thetastar, navcord, ..) = taken_route.dissolve();
                 navcord.recorder
             } else {
                 LayoutEdit::new()
@@ -147,7 +149,7 @@ impl<M: AccessMesadata> Step<Autorouter<M>, Option<LayoutEdit>, AutorouteContinu
             self.curr_ratline = Some(new_ratline);
 
             let recorder = if let Some(taken_route) = self.route.take() {
-                let (_astar, navcord, ..) = taken_route.dissolve();
+                let (_thetastar, navcord, ..) = taken_route.dissolve();
                 navcord.recorder
             } else {
                 LayoutEdit::new()
@@ -167,9 +169,9 @@ impl<M: AccessMesadata> Step<Autorouter<M>, Option<LayoutEdit>, AutorouteContinu
     }
 }
 
-impl GetMaybeAstarStepper for AutorouteExecutionStepper {
+impl GetMaybeThetastarStepper for AutorouteExecutionStepper {
-    fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
+    fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
-        self.route.as_ref().map(|route| route.astar())
+        self.route.as_ref().map(|route| route.thetastar())
     }
 }
 

src/autorouter/autorouter.rs

@@ -14,7 +14,7 @@ use crate::{
     drawing::{band::BandTermsegIndex, dot::FixedDotIndex, Infringement},
     graph::MakeRef,
     layout::{via::ViaWeight, LayoutEdit},
-    router::{astar::AstarError, navmesh::NavmeshError, RouterOptions},
+    router::{navmesh::NavmeshError, thetastar::ThetastarError, RouterOptions},
     triangulation::GetTrianvertexNodeIndex,
 };
 
@@ -42,7 +42,7 @@ pub enum AutorouterError {
     #[error(transparent)]
     Navmesh(#[from] NavmeshError),
     #[error("routing failed: {0}")]
-    Astar(#[from] AstarError),
+    Thetastar(#[from] ThetastarError),
     #[error("could not place via")]
     CouldNotPlaceVia(#[from] Infringement),
     #[error("could not remove band")]

src/autorouter/compare_detours.rs

@@ -14,14 +14,14 @@ use crate::{
     drawing::graph::PrimitiveIndex,
     geometry::{primitive::PrimitiveShape, shape::MeasureLength},
     graph::MakeRef,
-    router::{astar::AstarStepper, navcord::Navcord, navmesh::Navmesh},
+    router::{navcord::Navcord, navmesh::Navmesh, thetastar::ThetastarStepper},
     stepper::Step,
 };
 
 use super::{
     autoroute::{AutorouteContinueStatus, AutorouteExecutionStepper},
     invoker::{
-        GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+        GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, GetObstacles,
     },
     Autorouter, AutorouterError, AutorouterOptions,
 };
@@ -102,9 +102,9 @@ impl<M: AccessMesadata> Step<Autorouter<M>, (f64, f64)> for CompareDetoursExecut
     }
 }
 
-impl GetMaybeAstarStepper for CompareDetoursExecutionStepper {
+impl GetMaybeThetastarStepper for CompareDetoursExecutionStepper {
-    fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
+    fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
-        self.autoroute.maybe_astar()
+        self.autoroute.maybe_thetastar()
     }
 }
 

src/autorouter/execution.rs

@@ -36,7 +36,7 @@ pub enum Command {
 }
 
 #[enum_dispatch(
-    GetMaybeAstarStepper,
+    GetMaybeThetastarStepper,
     GetMaybeNavcord,
     GetGhosts,
     GetObstacles,

src/autorouter/invoker.rs

@@ -16,9 +16,9 @@ use crate::{
     drawing::graph::PrimitiveIndex,
     geometry::{edit::ApplyGeometryEdit, primitive::PrimitiveShape},
     router::{
-        astar::AstarStepper,
         navcord::Navcord,
         navmesh::{Navmesh, NavnodeIndex},
+        thetastar::ThetastarStepper,
     },
     stepper::Step,
 };
@@ -37,8 +37,8 @@ use super::{
 /// Trait for getting the A* stepper to display its data on the debug overlay,
 /// most importantly the navmesh which is owned by the A* stepper.
 #[enum_dispatch]
-pub trait GetMaybeAstarStepper {
+pub trait GetMaybeThetastarStepper {
-    fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
+    fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
         None
     }
 }

src/autorouter/measure_length.rs

@@ -12,7 +12,7 @@ use crate::{
 
 use super::{
     invoker::{
-        GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+        GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, GetObstacles,
     },
     selection::BandSelection,
     Autorouter, AutorouterError,
@@ -53,7 +53,7 @@ impl MeasureLengthExecutionStepper {
     }
 }
 
-impl GetMaybeAstarStepper for MeasureLengthExecutionStepper {}
+impl GetMaybeThetastarStepper for MeasureLengthExecutionStepper {}
 impl GetMaybeNavcord for MeasureLengthExecutionStepper {}
 impl GetGhosts for MeasureLengthExecutionStepper {}
 impl GetObstacles for MeasureLengthExecutionStepper {}

src/autorouter/place_via.rs

@@ -13,7 +13,7 @@ use crate::{
 
 use super::{
     invoker::{
-        GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+        GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, GetObstacles,
     },
     Autorouter, AutorouterError,
 };
@@ -51,7 +51,7 @@ impl PlaceViaExecutionStepper {
     }
 }
 
-impl GetMaybeAstarStepper for PlaceViaExecutionStepper {}
+impl GetMaybeThetastarStepper for PlaceViaExecutionStepper {}
 impl GetMaybeNavcord for PlaceViaExecutionStepper {}
 impl GetGhosts for PlaceViaExecutionStepper {}
 impl GetObstacles for PlaceViaExecutionStepper {}

src/autorouter/remove_bands.rs

@@ -8,7 +8,7 @@ use crate::{board::AccessMesadata, layout::LayoutEdit};
 
 use super::{
     invoker::{
-        GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+        GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, GetObstacles,
     },
     selection::BandSelection,
     Autorouter, AutorouterError,
@@ -47,7 +47,7 @@ impl RemoveBandsExecutionStepper {
     }
 }
 
-impl GetMaybeAstarStepper for RemoveBandsExecutionStepper {}
+impl GetMaybeThetastarStepper for RemoveBandsExecutionStepper {}
 impl GetMaybeNavcord for RemoveBandsExecutionStepper {}
 impl GetGhosts for RemoveBandsExecutionStepper {}
 impl GetObstacles for RemoveBandsExecutionStepper {}

src/interactor/activity.rs

@@ -12,8 +12,8 @@ use crate::{
     autorouter::{
         execution::ExecutionStepper,
         invoker::{
-            GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+            GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts,
-            Invoker, InvokerError,
+            GetObstacles, Invoker, InvokerError,
         },
     },
     board::AccessMesadata,
@@ -21,9 +21,9 @@ use crate::{
     geometry::primitive::PrimitiveShape,
     interactor::interaction::{InteractionError, InteractionStepper},
     router::{
-        astar::AstarStepper,
         navcord::Navcord,
         navmesh::{Navmesh, NavnodeIndex},
+        thetastar::ThetastarStepper,
     },
     stepper::{Abort, Step},
 };
@@ -50,7 +50,7 @@ pub enum ActivityError {
 
 /// An activity is either an interaction or an execution
 #[enum_dispatch(
-    GetMaybeAstarStepper,
+    GetMaybeThetastarStepper,
     GetMaybeNavcord,
     GetGhosts,
     GetObstacles,
@@ -125,9 +125,9 @@ impl<M: AccessMesadata> Abort<ActivityContext<'_, M>> for ActivityStepperWithSta
     }
 }
 
-impl GetMaybeAstarStepper for ActivityStepperWithStatus {
+impl GetMaybeThetastarStepper for ActivityStepperWithStatus {
-    fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
+    fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
-        self.activity.maybe_astar()
+        self.activity.maybe_thetastar()
     }
 }
 

src/interactor/interaction.rs

@@ -8,15 +8,15 @@ use thiserror::Error;
 
 use crate::{
     autorouter::invoker::{
-        GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles,
+        GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, GetObstacles,
     },
     board::AccessMesadata,
     drawing::graph::PrimitiveIndex,
     geometry::primitive::PrimitiveShape,
     router::{
-        astar::AstarStepper,
         navcord::Navcord,
         navmesh::{Navmesh, NavnodeIndex},
+        thetastar::ThetastarStepper,
     },
     stepper::{Abort, Step},
 };
@@ -53,8 +53,8 @@ impl<M: AccessMesadata> Abort<ActivityContext<'_, M>> for InteractionStepper {
     }
 }
 
-impl GetMaybeAstarStepper for InteractionStepper {
+impl GetMaybeThetastarStepper for InteractionStepper {
-    fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
+    fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
         todo!()
     }
 }

src/router/astar.rs

@@ -1,343 +0,0 @@
-// Copyright (c) 2015
-// SPDX-FileCopyrightText: 2024 Topola contributors
-//
-// SPDX-License-Identifier: MIT
-
-use std::collections::btree_map::Entry;
-use std::collections::{BTreeMap, BinaryHeap, VecDeque};
-
-use std::ops::ControlFlow;
-
-use derive_getters::Getters;
-use petgraph::algo::Measure;
-use petgraph::visit::{EdgeRef, GraphBase, IntoEdgeReferences, IntoEdges};
-use thiserror::Error;
-
-use std::cmp::Ordering;
-
-use crate::stepper::Step;
-
-#[derive(Copy, Clone, Debug)]
-pub struct MinScored<K, T>(pub K, pub T);
-
-impl<K: PartialOrd, T> PartialEq for MinScored<K, T> {
-    #[inline]
-    fn eq(&self, other: &MinScored<K, T>) -> bool {
-        self.cmp(other) == Ordering::Equal
-    }
-}
-
-impl<K: PartialOrd, T> Eq for MinScored<K, T> {}
-
-impl<K: PartialOrd, T> PartialOrd for MinScored<K, T> {
-    #[inline]
-    fn partial_cmp(&self, other: &MinScored<K, T>) -> Option<Ordering> {
-        Some(self.cmp(other))
-    }
-}
-
-impl<K: PartialOrd, T> Ord for MinScored<K, T> {
-    #[inline]
-    fn cmp(&self, other: &MinScored<K, T>) -> Ordering {
-        let a = &self.0;
-        let b = &other.0;
-        if a == b {
-            Ordering::Equal
-        } else if a < b {
-            Ordering::Greater
-        } else if a > b {
-            Ordering::Less
-        } else if a.ne(a) && b.ne(b) {
-            // these are the NaN cases
-            Ordering::Equal
-        } else if a.ne(a) {
-            // Order NaN less, so that it is last in the MinScore order
-            Ordering::Less
-        } else {
-            Ordering::Greater
-        }
-    }
-}
-
-#[derive(Debug)]
-pub struct PathTracker<G>
-where
-    G: GraphBase,
-    G::NodeId: Eq + Ord,
-{
-    came_from: BTreeMap<G::NodeId, G::NodeId>,
-}
-
-impl<G> PathTracker<G>
-where
-    G: GraphBase,
-    G::NodeId: Eq + Ord,
-{
-    fn new() -> PathTracker<G> {
-        PathTracker {
-            came_from: BTreeMap::new(),
-        }
-    }
-
-    fn set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId) {
-        self.came_from.insert(node, previous);
-    }
-
-    pub fn reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId> {
-        let mut path = vec![last];
-
-        let mut current = last;
-        while let Some(&previous) = self.came_from.get(&current) {
-            path.push(previous);
-            current = previous;
-        }
-
-        path.reverse();
-
-        path
-    }
-}
-
-pub trait AstarStrategy<G, K, R>
-where
-    G: GraphBase,
-    G::NodeId: Eq + Ord,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
-    K: Measure + Copy,
-{
-    fn visit_navnode(
-        &mut self,
-        graph: &G,
-        node: G::NodeId,
-        tracker: &PathTracker<G>,
-    ) -> Result<Option<R>, ()>;
-    fn place_probe_at_navedge<'a>(
-        &mut self,
-        graph: &'a G,
-        edge: <&'a G as IntoEdgeReferences>::EdgeRef,
-    ) -> Option<K>;
-    fn remove_probe(&mut self, graph: &G);
-    fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K;
-}
-
-pub trait MakeEdgeRef: IntoEdgeReferences {
-    fn edge_ref(&self, edge_id: Self::EdgeId) -> Self::EdgeRef;
-}
-
-enum AstarState<G>
-where
-    G: GraphBase,
-{
-    Scanning,
-    Visiting(G::NodeId),
-    Probing(G::NodeId, G::EdgeId),
-}
-
-#[derive(Getters)]
-pub struct AstarStepper<G, K>
-where
-    G: GraphBase,
-    G::NodeId: Eq + Ord,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
-    K: Measure + Copy,
-{
-    state: AstarState<G>,
-    graph: G,
-    #[getter(skip)]
-    visit_next: BinaryHeap<MinScored<K, G::NodeId>>,
-    /// Also known as the g-scores, or just g.
-    scores: BTreeMap<G::NodeId, K>,
-    /// Also known as the f-scores, or just f.
-    estimate_scores: BTreeMap<G::NodeId, K>,
-    #[getter(skip)]
-    path_tracker: PathTracker<G>,
-    // FIXME: To work around edge references borrowing from the graph we collect then reiterate over them.
-    #[getter(skip)]
-    edge_ids: VecDeque<G::EdgeId>,
-}
-
-/// The status enum of the A* stepper returned when there is no failure or
-/// break.
-///
-/// Note that, when thinking of the A* stepper as of a state machine, the
-/// variants of the status actually correspond to state transitions, not to
-/// states themselves, since `Probing` and `ProbingButDiscarded`, and likewise
-/// `VisitSkipped` and `Visited`, would correspond to the same state.
-#[derive(Debug)]
-pub enum AstarContinueStatus {
-    /// A* has now attempted to visit a new navnode, but it turned out that
-    /// it has been previously reached through a path with an equal or lower
-    /// estimated score, so the visit to that navnode has been skipped.
-    ScanningVisitSkipped,
-    /// A* has failed to visit a new navnode. Happens, so A* will just proceed
-    /// to the next node in the priority queue.
-    ScanningVisitFailed,
-    /// A* is now visiting a new navnode.
-    ///
-    /// Quick recap if you have been trying to remember what is the difference
-    /// between probing and visiting: probing is done as part of a scan of
-    /// neighboring navnodes around the currently visited navnode to add them to
-    /// the priority queue, whereas when a navnode is visited it is taken from
-    /// the priority queue to actually become the currently visited navnode.
-    Visiting,
-    /// A* has now placed a probe to measure the cost of the edge to a
-    /// neighboring navnode from the current position. The probed navnode has
-    /// been added to the priority queue, and the newly measured edge cost has
-    /// been stored in a map.
-    Probing,
-    /// A* has now placed a probe, but it turned out that the probed navnode has
-    /// been previously reached through a path with equal or lower score, so the
-    /// probe's measurement has been discarded. The probe, however, will be only
-    /// removed in the next state just as if it was after the normal `Probing`
-    /// status.
-    ProbingButDiscarded,
-    /// The probe that had been placed in the previous state has now been
-    /// removed.
-    ///
-    /// The probe is only removed in this separate state to make it possible
-    /// to pause the A* while the placed probe exists, which is very useful
-    /// for debugging.
-    Probed,
-}
-
-#[derive(Error, Debug, Clone)]
-pub enum AstarError {
-    #[error("A* search found no path")]
-    NotFound,
-}
-
-impl<G, K> AstarStepper<G, K>
-where
-    G: GraphBase,
-    G::NodeId: Eq + Ord,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
-    K: Measure + Copy,
-{
-    pub fn new<R>(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy<G, K, R>) -> Self {
-        let mut this = Self {
-            state: AstarState::Scanning,
-            graph,
-            visit_next: BinaryHeap::new(),
-            scores: BTreeMap::new(),
-            estimate_scores: BTreeMap::new(),
-            path_tracker: PathTracker::<G>::new(),
-            edge_ids: VecDeque::new(),
-        };
-
-        let zero_score = K::default();
-        this.scores.insert(start, zero_score);
-        this.visit_next
-            .push(MinScored(strategy.estimate_cost(&this.graph, start), start));
-        this
-    }
-}
-
-impl<G, K, R, S: AstarStrategy<G, K, R>> Step<S, (K, Vec<G::NodeId>, R), AstarContinueStatus>
-    for AstarStepper<G, K>
-where
-    G: GraphBase,
-    G::NodeId: Eq + Ord,
-    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
-    K: Measure + Copy,
-{
-    type Error = AstarError;
-
-    fn step(
-        &mut self,
-        strategy: &mut S,
-    ) -> Result<ControlFlow<(K, Vec<G::NodeId>, R), AstarContinueStatus>, AstarError> {
-        match self.state {
-            AstarState::Scanning => {
-                let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else {
-                    return Err(AstarError::NotFound);
-                };
-
-                let Ok(maybe_result) =
-                    strategy.visit_navnode(&self.graph, node, &self.path_tracker)
-                else {
-                    return Ok(ControlFlow::Continue(
-                        AstarContinueStatus::ScanningVisitFailed,
-                    ));
-                };
-
-                if let Some(result) = maybe_result {
-                    let path = self.path_tracker.reconstruct_path_to(node);
-                    let cost = self.scores[&node];
-                    return Ok(ControlFlow::Break((cost, path, result)));
-                }
-
-                match self.estimate_scores.entry(node) {
-                    Entry::Occupied(mut entry) => {
-                        // If the node has already been visited with an equal or lower
-                        // estimated score than now, then we do not need to re-visit it.
-                        if *entry.get() <= estimate_score {
-                            return Ok(ControlFlow::Continue(
-                                AstarContinueStatus::ScanningVisitSkipped,
-                            ));
-                        }
-                        entry.insert(estimate_score);
-                    }
-                    Entry::Vacant(entry) => {
-                        entry.insert(estimate_score);
-                    }
-                }
-
-                self.edge_ids = self.graph.edges(node).map(|edge| edge.id()).collect();
-
-                self.state = AstarState::Visiting(node);
-                Ok(ControlFlow::Continue(AstarContinueStatus::Visiting))
-            }
-            AstarState::Visiting(curr_visited_navnode) => {
-                if let Some(curr_probed_navedge) = 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_visited_navnode];
-                    let curr_probed_navedge_ref = (&self.graph).edge_ref(curr_probed_navedge);
-
-                    if let Some(edge_cost) =
-                        strategy.place_probe_at_navedge(&self.graph, curr_probed_navedge_ref)
-                    {
-                        let next = curr_probed_navedge_ref.target();
-                        let next_score = node_score + edge_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 {
-                                    return Ok(ControlFlow::Continue(
-                                        AstarContinueStatus::ProbingButDiscarded,
-                                    ));
-                                }
-                                entry.insert(next_score);
-                            }
-                            Entry::Vacant(entry) => {
-                                entry.insert(next_score);
-                            }
-                        }
-
-                        self.path_tracker
-                            .set_predecessor(next, curr_visited_navnode);
-                        let next_estimate_score =
-                            next_score + strategy.estimate_cost(&self.graph, next);
-                        self.visit_next.push(MinScored(next_estimate_score, next));
-
-                        self.state = AstarState::Probing(curr_visited_navnode, curr_probed_navedge);
-                        Ok(ControlFlow::Continue(AstarContinueStatus::Probing))
-                    } else {
-                        Ok(ControlFlow::Continue(AstarContinueStatus::Probed))
-                    }
-                } else {
-                    self.state = AstarState::Scanning;
-                    Ok(ControlFlow::Continue(AstarContinueStatus::Probed))
-                }
-            }
-            AstarState::Probing(curr_visited_navnode, _curr_probed_navedge) => {
-                strategy.remove_probe(&self.graph);
-
-                self.state = AstarState::Visiting(curr_visited_navnode);
-                Ok(ControlFlow::Continue(AstarContinueStatus::Probed))
-            }
-        }
-    }
-}

src/router/mod.rs

@@ -2,13 +2,13 @@
 //
 // SPDX-License-Identifier: MIT
 
-pub mod astar;
 pub mod draw;
 pub mod navcord;
 pub mod navcorder;
 pub mod navmesh;
 mod route;
 mod router;
+pub mod thetastar;
 
 pub use route::RouteStepper;
 pub use router::*;

src/router/navcord.rs

@@ -112,7 +112,7 @@ impl Navcord {
                 unreachable!();
             };
 
-            self.final_termseg = Some(layout.finish(navmesh, self, to_dot).unwrap());
+            self.final_termseg = Some(layout.finish(navmesh, self, to_dot)?);
 
             // NOTE: We don't update the head here because there is currently
             // no head variant that consists only of a seg, and I'm not sure if

src/router/navmesh.rs

@@ -32,7 +32,7 @@ use crate::{
     graph::{GetPetgraphIndex, MakeRef},
     layout::Layout,
     math::RotationSense,
-    router::astar::MakeEdgeRef,
+    router::thetastar::MakeEdgeRef,
     triangulation::{GetTrianvertexNodeIndex, Triangulation},
 };
 

src/router/route.rs

@@ -13,18 +13,18 @@ use crate::{
     geometry::primitive::PrimitiveShape,
     layout::LayoutEdit,
     router::{
-        astar::{AstarError, AstarStepper},
         navcord::Navcord,
         navcorder::Navcorder,
         navmesh::{Navmesh, NavmeshError},
-        Router, RouterAstarStrategy,
+        thetastar::{ThetastarError, ThetastarStepper},
+        Router, RouterThetastarStrategy,
     },
     stepper::Step,
 };
 
 #[derive(Getters, Dissolve)]
 pub struct RouteStepper {
-    astar: AstarStepper<Navmesh, f64>,
+    thetastar: ThetastarStepper<Navmesh, f64>,
     navcord: Navcord,
     ghosts: Vec<PrimitiveShape>,
     obstacles: Vec<PrimitiveIndex>,
@@ -55,13 +55,13 @@ impl RouteStepper {
         let layout = router.layout_mut();
         let mut navcord = layout.start(recorder, source, source_navnode, width);
 
-        let mut strategy = RouterAstarStrategy::new(layout, &mut navcord, target);
+        let mut strategy = RouterThetastarStrategy::new(layout, &mut navcord, target);
-        let astar = AstarStepper::new(navmesh, source_navnode, &mut strategy);
+        let thetastar = ThetastarStepper::new(navmesh, source_navnode, &mut strategy);
         let ghosts = vec![];
         let obstacles = vec![];
 
         Self {
-            astar,
+            thetastar,
             navcord,
             ghosts,
             obstacles,
@@ -70,16 +70,16 @@ impl RouteStepper {
 }
 
 impl<R: AccessRules> Step<Router<'_, R>, BandTermsegIndex> for RouteStepper {
-    type Error = AstarError;
+    type Error = ThetastarError;
 
     fn step(
         &mut self,
         router: &mut Router<R>,
-    ) -> Result<ControlFlow<BandTermsegIndex>, AstarError> {
+    ) -> Result<ControlFlow<BandTermsegIndex>, ThetastarError> {
         let layout = router.layout_mut();
-        let target = self.astar.graph().destination();
+        let target = self.thetastar.graph().destination();
-        let mut strategy = RouterAstarStrategy::new(layout, &mut self.navcord, target);
+        let mut strategy = RouterThetastarStrategy::new(layout, &mut self.navcord, target);
-        let result = self.astar.step(&mut strategy);
+        let result = self.thetastar.step(&mut strategy);
         self.ghosts = strategy.probe_ghosts;
         self.obstacles = strategy.probe_obstacles;
 

src/router/router.rs

@@ -4,7 +4,7 @@
 
 use derive_getters::Getters;
 use geo::algorithm::line_measures::{Distance, Euclidean};
-use petgraph::{data::DataMap, visit::EdgeRef};
+use petgraph::data::DataMap;
 use serde::{Deserialize, Serialize};
 
 use crate::{
@@ -25,12 +25,12 @@ use crate::{
 };
 
 use super::{
-    astar::{AstarStrategy, PathTracker},
     draw::DrawException,
     navcord::Navcord,
     navcorder::{Navcorder, NavcorderException},
-    navmesh::{Navmesh, NavmeshEdgeReference, NavmeshError, NavnodeIndex},
+    navmesh::{Navmesh, NavmeshError, NavnodeIndex},
     route::RouteStepper,
+    thetastar::{PathTracker, ThetastarStrategy},
 };
 
 #[derive(Debug, Clone, Copy, Serialize, Deserialize)]
@@ -41,7 +41,7 @@ pub struct RouterOptions {
 }
 
 #[derive(Debug)]
-pub struct RouterAstarStrategy<'a, R> {
+pub struct RouterThetastarStrategy<'a, R> {
     pub layout: &'a mut Layout<R>,
     pub navcord: &'a mut Navcord,
     pub target: FixedDotIndex,
@@ -49,7 +49,7 @@ pub struct RouterAstarStrategy<'a, R> {
     pub probe_obstacles: Vec<PrimitiveIndex>,
 }
 
-impl<'a, R> RouterAstarStrategy<'a, R> {
+impl<'a, R> RouterThetastarStrategy<'a, R> {
     pub fn new(layout: &'a mut Layout<R>, navcord: &'a mut Navcord, target: FixedDotIndex) -> Self {
         Self {
             layout,
@@ -61,28 +61,30 @@ impl<'a, R> RouterAstarStrategy<'a, R> {
     }
 }
 
-impl<R: AccessRules> AstarStrategy<Navmesh, f64, BandTermsegIndex> for RouterAstarStrategy<'_, R> {
+impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
+    for RouterThetastarStrategy<'_, R>
+{
     fn visit_navnode(
         &mut self,
         navmesh: &Navmesh,
-        vertex: NavnodeIndex,
+        navnode: NavnodeIndex,
         tracker: &PathTracker<Navmesh>,
     ) -> Result<Option<BandTermsegIndex>, ()> {
-        let new_path = tracker.reconstruct_path_to(vertex);
+        let new_path = tracker.reconstruct_path_to(navnode);
 
-        if vertex == navmesh.destination_navnode() {
+        if navnode == navmesh.destination_navnode() {
             self.layout
                 .rework_path(navmesh, self.navcord, &new_path[..new_path.len() - 1])
-                .unwrap();
+                .map_err(|_| ())?;
 
             // Set navcord members for consistency. The code would probably work
             // without this, since A* will terminate now anyway.
             self.navcord.final_termseg = Some(
                 self.layout
                     .finish(navmesh, self.navcord, self.target)
-                    .unwrap(),
+                    .map_err(|_| ())?,
             );
-            self.navcord.path.push(vertex);
+            self.navcord.path.push(navnode);
 
             Ok(self.navcord.final_termseg)
         } else {
@@ -92,13 +94,13 @@ impl<R: AccessRules> AstarStrategy<Navmesh, f64, BandTermsegIndex> for RouterAst
         }
     }
 
-    fn place_probe_at_navedge(
+    fn place_probe_to_navnode(
         &mut self,
         navmesh: &Navmesh,
-        edge: NavmeshEdgeReference,
+        probed_navnode: NavnodeIndex,
     ) -> Option<f64> {
         let old_head = self.navcord.head;
-        let result = self.navcord.step_to(self.layout, navmesh, edge.target());
+        let result = self.navcord.step_to(self.layout, navmesh, probed_navnode);
 
         let prev_bend_length = match old_head {
             Head::Cane(old_cane_head) => self

src/router/thetastar.rs

@@ -0,0 +1,359 @@
+// Copyright (c) 2015
+// SPDX-FileCopyrightText: 2024 Topola contributors
+//
+// SPDX-License-Identifier: MIT
+
+use std::collections::btree_map::Entry;
+use std::collections::{BTreeMap, BinaryHeap, VecDeque};
+
+use std::ops::ControlFlow;
+
+use derive_getters::Getters;
+use petgraph::algo::Measure;
+use petgraph::visit::{EdgeRef, GraphBase, IntoEdgeReferences, IntoEdges};
+use thiserror::Error;
+
+use std::cmp::Ordering;
+
+use crate::stepper::Step;
+
+#[derive(Copy, Clone, Debug)]
+pub struct MinScored<K, T>(pub K, pub T);
+
+impl<K: PartialOrd, T> PartialEq for MinScored<K, T> {
+    #[inline]
+    fn eq(&self, other: &MinScored<K, T>) -> bool {
+        self.cmp(other) == Ordering::Equal
+    }
+}
+
+impl<K: PartialOrd, T> Eq for MinScored<K, T> {}
+
+impl<K: PartialOrd, T> PartialOrd for MinScored<K, T> {
+    #[inline]
+    fn partial_cmp(&self, other: &MinScored<K, T>) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl<K: PartialOrd, T> Ord for MinScored<K, T> {
+    #[inline]
+    fn cmp(&self, other: &MinScored<K, T>) -> Ordering {
+        let a = &self.0;
+        let b = &other.0;
+        if a == b {
+            Ordering::Equal
+        } else if a < b {
+            Ordering::Greater
+        } else if a > b {
+            Ordering::Less
+        } else if a.ne(a) && b.ne(b) {
+            // these are the NaN cases
+            Ordering::Equal
+        } else if a.ne(a) {
+            // Order NaN less, so that it is last in the MinScore order
+            Ordering::Less
+        } else {
+            Ordering::Greater
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct PathTracker<G>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Ord,
+{
+    predecessors: BTreeMap<G::NodeId, G::NodeId>,
+}
+
+impl<G> PathTracker<G>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Ord,
+{
+    fn new() -> PathTracker<G> {
+        PathTracker {
+            predecessors: BTreeMap::new(),
+        }
+    }
+
+    fn predecessor(&self, node: G::NodeId) -> Option<G::NodeId> {
+        self.predecessors.get(&node).copied()
+    }
+
+    fn set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId) {
+        self.predecessors.insert(node, previous);
+    }
+
+    pub fn reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId> {
+        let mut path = vec![last];
+
+        let mut current = last;
+        while let Some(&previous) = self.predecessors.get(&current) {
+            path.push(previous);
+            current = previous;
+        }
+
+        path.reverse();
+
+        path
+    }
+}
+
+pub trait ThetastarStrategy<G, K, R>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Ord,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
+    K: Measure + Copy,
+{
+    fn visit_navnode(
+        &mut self,
+        graph: &G,
+        navnode: G::NodeId,
+        tracker: &PathTracker<G>,
+    ) -> Result<Option<R>, ()>;
+    fn place_probe_to_navnode<'a>(&mut self, graph: &'a G, probed_navnode: G::NodeId) -> Option<K>;
+    fn remove_probe(&mut self, graph: &G);
+    fn estimate_cost(&mut self, graph: &G, navnode: G::NodeId) -> K;
+}
+
+pub trait MakeEdgeRef: IntoEdgeReferences {
+    fn edge_ref(&self, edge_id: Self::EdgeId) -> Self::EdgeRef;
+}
+
+#[derive(Clone, Copy, Debug)]
+pub enum ThetastarState<N: Copy, E: Copy> {
+    Scanning,
+    VisitingProbeOnLineOfSight(N),
+    VisitingProbeOnNavedge(N, E),
+    Probing(N),
+}
+
+#[derive(Getters)]
+pub struct ThetastarStepper<G, K>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Ord,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
+    K: Measure + Copy,
+{
+    state: ThetastarState<G::NodeId, G::EdgeId>,
+    graph: G,
+    #[getter(skip)]
+    visit_next: BinaryHeap<MinScored<K, G::NodeId>>,
+    /// Also known as the g-scores, or just g.
+    scores: BTreeMap<G::NodeId, K>,
+    /// Also known as the f-scores, or just f.
+    estimate_scores: BTreeMap<G::NodeId, K>,
+    #[getter(skip)]
+    path_tracker: PathTracker<G>,
+    // FIXME: To work around edge references borrowing from the graph we collect then reiterate over them.
+    #[getter(skip)]
+    edge_ids: VecDeque<G::EdgeId>,
+}
+
+#[derive(Error, Debug, Clone)]
+pub enum ThetastarError {
+    #[error("A* search found no path")]
+    NotFound,
+}
+
+impl<G, K> ThetastarStepper<G, K>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Ord,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
+    K: Measure + Copy,
+{
+    pub fn new<R>(
+        graph: G,
+        start: G::NodeId,
+        strategy: &mut impl ThetastarStrategy<G, K, R>,
+    ) -> Self {
+        let mut this = Self {
+            state: ThetastarState::Scanning,
+            graph,
+            visit_next: BinaryHeap::new(),
+            scores: BTreeMap::new(),
+            estimate_scores: BTreeMap::new(),
+            path_tracker: PathTracker::<G>::new(),
+            edge_ids: VecDeque::new(),
+        };
+
+        let zero_score = K::default();
+        this.scores.insert(start, zero_score);
+        this.visit_next
+            .push(MinScored(strategy.estimate_cost(&this.graph, start), start));
+        this
+    }
+}
+
+impl<G, K, R, S: ThetastarStrategy<G, K, R>>
+    Step<S, (K, Vec<G::NodeId>, R), ThetastarState<G::NodeId, G::EdgeId>> for ThetastarStepper<G, K>
+where
+    G: GraphBase,
+    G::NodeId: Eq + Ord,
+    for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
+    K: Measure + Copy,
+{
+    type Error = ThetastarError;
+
+    fn step(
+        &mut self,
+        strategy: &mut S,
+    ) -> Result<
+        ControlFlow<(K, Vec<G::NodeId>, R), ThetastarState<G::NodeId, G::EdgeId>>,
+        ThetastarError,
+    > {
+        match self.state {
+            ThetastarState::Scanning => {
+                let Some(MinScored(estimate_score, navnode)) = self.visit_next.pop() else {
+                    return Err(ThetastarError::NotFound);
+                };
+
+                let Ok(maybe_result) =
+                    strategy.visit_navnode(&self.graph, navnode, &self.path_tracker)
+                else {
+                    return Ok(ControlFlow::Continue(self.state));
+                };
+
+                if let Some(result) = maybe_result {
+                    let path = self.path_tracker.reconstruct_path_to(navnode);
+                    let cost = self.scores[&navnode];
+                    return Ok(ControlFlow::Break((cost, path, result)));
+                }
+
+                match self.estimate_scores.entry(navnode) {
+                    Entry::Occupied(mut entry) => {
+                        // If the node has already been visited with an equal or lower
+                        // estimated score than now, then we do not need to re-visit it.
+                        if *entry.get() <= estimate_score {
+                            return Ok(ControlFlow::Continue(self.state));
+                        }
+                        entry.insert(estimate_score);
+                    }
+                    Entry::Vacant(entry) => {
+                        entry.insert(estimate_score);
+                    }
+                }
+
+                self.edge_ids = self.graph.edges(navnode).map(|edge| edge.id()).collect();
+
+                self.state = ThetastarState::VisitingProbeOnLineOfSight(navnode);
+                Ok(ControlFlow::Continue(self.state))
+            }
+            ThetastarState::VisitingProbeOnLineOfSight(visited_navnode) => {
+                if let Some(curr_navedge) = 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[&visited_navnode];
+                    let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
+
+                    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) =
+                            strategy.place_probe_to_navnode(&self.graph, to_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 {
+                                        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.path_tracker.set_predecessor(next, parent_navnode);
+                            let next_estimate_score =
+                                next_score + strategy.estimate_cost(&self.graph, next);
+                            self.visit_next.push(MinScored(next_estimate_score, next));
+
+                            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::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) => {
+                let node_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)
+                {
+                    let next = to_navnode;
+                    let next_score = node_score + navedge_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 {
+                                self.state = ThetastarState::Probing(visited_navnode);
+                                return Ok(ControlFlow::Continue(self.state));
+                            }
+                            entry.insert(next_score);
+                        }
+                        Entry::Vacant(entry) => {
+                            entry.insert(next_score);
+                        }
+                    }
+
+                    self.path_tracker.set_predecessor(next, visited_navnode);
+                    let next_estimate_score =
+                        next_score + strategy.estimate_cost(&self.graph, next);
+                    self.visit_next.push(MinScored(next_estimate_score, next));
+
+                    self.state = ThetastarState::Probing(visited_navnode);
+                    Ok(ControlFlow::Continue(self.state))
+                } else {
+                    self.state = ThetastarState::VisitingProbeOnLineOfSight(visited_navnode);
+                    Ok(ControlFlow::Continue(self.state))
+                }
+            }
+            ThetastarState::Probing(visited_navnode) => {
+                strategy.remove_probe(&self.graph);
+
+                self.state = ThetastarState::VisitingProbeOnLineOfSight(visited_navnode);
+                Ok(ControlFlow::Continue(self.state))
+            }
+        }
+    }
+}