From d0c304adbd23a292283fc9d19bb942738de06d59 Mon Sep 17 00:00:00 2001 From: Mikolaj Wielgus Date: Thu, 5 Jun 2025 21:16:26 +0200 Subject: [PATCH] feat(router/thetastar): Implement Theta* search algorithm Closes https://codeberg.org/topola/topola/issues/121 --- committed.toml | 2 +- crates/topola-egui/src/viewport.rs | 7 +- src/autorouter/autoroute.rs | 16 +- src/autorouter/autorouter.rs | 4 +- src/autorouter/compare_detours.rs | 10 +- src/autorouter/execution.rs | 2 +- src/autorouter/invoker.rs | 6 +- src/autorouter/measure_length.rs | 4 +- src/autorouter/place_via.rs | 4 +- src/autorouter/remove_bands.rs | 4 +- src/interactor/activity.rs | 14 +- src/interactor/interaction.rs | 8 +- src/router/astar.rs | 343 --------------------------- src/router/mod.rs | 2 +- src/router/navcord.rs | 2 +- src/router/navmesh.rs | 2 +- src/router/route.rs | 22 +- src/router/router.rs | 32 +-- src/router/thetastar.rs | 359 +++++++++++++++++++++++++++++ 19 files changed, 432 insertions(+), 411 deletions(-) delete mode 100644 src/router/astar.rs create mode 100644 src/router/thetastar.rs diff --git a/committed.toml b/committed.toml index 0dd6313..3d12836 100644 --- a/committed.toml +++ b/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", diff --git a/crates/topola-egui/src/viewport.rs b/crates/topola-egui/src/viewport.rs index 2275ba4..a4ee563 100644 --- a/crates/topola-egui/src/viewport.rs +++ b/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) = diff --git a/src/autorouter/autoroute.rs b/src/autorouter/autoroute.rs index f16c4d2..c093ed7 100644 --- a/src/autorouter/autoroute.rs +++ b/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 Step, Option, AutorouteContinu ) -> Result, 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 Step, Option, 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 Step, Option, AutorouteContinu } } -impl GetMaybeAstarStepper for AutorouteExecutionStepper { - fn maybe_astar(&self) -> Option<&AstarStepper> { - self.route.as_ref().map(|route| route.astar()) +impl GetMaybeThetastarStepper for AutorouteExecutionStepper { + fn maybe_thetastar(&self) -> Option<&ThetastarStepper> { + self.route.as_ref().map(|route| route.thetastar()) } } diff --git a/src/autorouter/autorouter.rs b/src/autorouter/autorouter.rs index d94b750..16503f4 100644 --- a/src/autorouter/autorouter.rs +++ b/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")] diff --git a/src/autorouter/compare_detours.rs b/src/autorouter/compare_detours.rs index a678a8b..fa01894 100644 --- a/src/autorouter/compare_detours.rs +++ b/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 Step, (f64, f64)> for CompareDetoursExecut } } -impl GetMaybeAstarStepper for CompareDetoursExecutionStepper { - fn maybe_astar(&self) -> Option<&AstarStepper> { - self.autoroute.maybe_astar() +impl GetMaybeThetastarStepper for CompareDetoursExecutionStepper { + fn maybe_thetastar(&self) -> Option<&ThetastarStepper> { + self.autoroute.maybe_thetastar() } } diff --git a/src/autorouter/execution.rs b/src/autorouter/execution.rs index b0763d7..9c43e2e 100644 --- a/src/autorouter/execution.rs +++ b/src/autorouter/execution.rs @@ -36,7 +36,7 @@ pub enum Command { } #[enum_dispatch( - GetMaybeAstarStepper, + GetMaybeThetastarStepper, GetMaybeNavcord, GetGhosts, GetObstacles, diff --git a/src/autorouter/invoker.rs b/src/autorouter/invoker.rs index b8414f9..0a3119d 100644 --- a/src/autorouter/invoker.rs +++ b/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 { - fn maybe_astar(&self) -> Option<&AstarStepper> { +pub trait GetMaybeThetastarStepper { + fn maybe_thetastar(&self) -> Option<&ThetastarStepper> { None } } diff --git a/src/autorouter/measure_length.rs b/src/autorouter/measure_length.rs index 06c37b4..36c32e3 100644 --- a/src/autorouter/measure_length.rs +++ b/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 {} diff --git a/src/autorouter/place_via.rs b/src/autorouter/place_via.rs index 25d7032..20b0a97 100644 --- a/src/autorouter/place_via.rs +++ b/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 {} diff --git a/src/autorouter/remove_bands.rs b/src/autorouter/remove_bands.rs index fa43957..081fdec 100644 --- a/src/autorouter/remove_bands.rs +++ b/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 {} diff --git a/src/interactor/activity.rs b/src/interactor/activity.rs index 08a1c4a..1d1f2cc 100644 --- a/src/interactor/activity.rs +++ b/src/interactor/activity.rs @@ -12,8 +12,8 @@ use crate::{ autorouter::{ execution::ExecutionStepper, invoker::{ - GetGhosts, GetMaybeAstarStepper, GetMaybeNavcord, GetNavmeshDebugTexts, GetObstacles, - Invoker, InvokerError, + GetGhosts, GetMaybeNavcord, GetMaybeThetastarStepper, GetNavmeshDebugTexts, + 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 Abort> for ActivityStepperWithSta } } -impl GetMaybeAstarStepper for ActivityStepperWithStatus { - fn maybe_astar(&self) -> Option<&AstarStepper> { - self.activity.maybe_astar() +impl GetMaybeThetastarStepper for ActivityStepperWithStatus { + fn maybe_thetastar(&self) -> Option<&ThetastarStepper> { + self.activity.maybe_thetastar() } } diff --git a/src/interactor/interaction.rs b/src/interactor/interaction.rs index 32e713c..c182367 100644 --- a/src/interactor/interaction.rs +++ b/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 Abort> for InteractionStepper { } } -impl GetMaybeAstarStepper for InteractionStepper { - fn maybe_astar(&self) -> Option<&AstarStepper> { +impl GetMaybeThetastarStepper for InteractionStepper { + fn maybe_thetastar(&self) -> Option<&ThetastarStepper> { todo!() } } diff --git a/src/router/astar.rs b/src/router/astar.rs deleted file mode 100644 index f6aef9a..0000000 --- a/src/router/astar.rs +++ /dev/null @@ -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(pub K, pub T); - -impl PartialEq for MinScored { - #[inline] - fn eq(&self, other: &MinScored) -> bool { - self.cmp(other) == Ordering::Equal - } -} - -impl Eq for MinScored {} - -impl PartialOrd for MinScored { - #[inline] - fn partial_cmp(&self, other: &MinScored) -> Option { - Some(self.cmp(other)) - } -} - -impl Ord for MinScored { - #[inline] - fn cmp(&self, other: &MinScored) -> 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 -where - G: GraphBase, - G::NodeId: Eq + Ord, -{ - came_from: BTreeMap, -} - -impl PathTracker -where - G: GraphBase, - G::NodeId: Eq + Ord, -{ - fn new() -> PathTracker { - 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 { - let mut path = vec![last]; - - let mut current = last; - while let Some(&previous) = self.came_from.get(¤t) { - path.push(previous); - current = previous; - } - - path.reverse(); - - path - } -} - -pub trait AstarStrategy -where - G: GraphBase, - G::NodeId: Eq + Ord, - for<'a> &'a G: IntoEdges + MakeEdgeRef, - K: Measure + Copy, -{ - fn visit_navnode( - &mut self, - graph: &G, - node: G::NodeId, - tracker: &PathTracker, - ) -> Result, ()>; - fn place_probe_at_navedge<'a>( - &mut self, - graph: &'a G, - edge: <&'a G as IntoEdgeReferences>::EdgeRef, - ) -> Option; - 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 -where - G: GraphBase, -{ - Scanning, - Visiting(G::NodeId), - Probing(G::NodeId, G::EdgeId), -} - -#[derive(Getters)] -pub struct AstarStepper -where - G: GraphBase, - G::NodeId: Eq + Ord, - for<'a> &'a G: IntoEdges + MakeEdgeRef, - K: Measure + Copy, -{ - state: AstarState, - graph: G, - #[getter(skip)] - visit_next: BinaryHeap>, - /// Also known as the g-scores, or just g. - scores: BTreeMap, - /// Also known as the f-scores, or just f. - estimate_scores: BTreeMap, - #[getter(skip)] - path_tracker: PathTracker, - // FIXME: To work around edge references borrowing from the graph we collect then reiterate over them. - #[getter(skip)] - edge_ids: VecDeque, -} - -/// 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 AstarStepper -where - G: GraphBase, - G::NodeId: Eq + Ord, - for<'a> &'a G: IntoEdges + MakeEdgeRef, - K: Measure + Copy, -{ - pub fn new(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy) -> Self { - let mut this = Self { - state: AstarState::Scanning, - graph, - visit_next: BinaryHeap::new(), - scores: BTreeMap::new(), - estimate_scores: BTreeMap::new(), - path_tracker: PathTracker::::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> Step, R), AstarContinueStatus> - for AstarStepper -where - G: GraphBase, - G::NodeId: Eq + Ord, - for<'a> &'a G: IntoEdges + MakeEdgeRef, - K: Measure + Copy, -{ - type Error = AstarError; - - fn step( - &mut self, - strategy: &mut S, - ) -> Result, 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)) - } - } - } -} diff --git a/src/router/mod.rs b/src/router/mod.rs index b864800..310642c 100644 --- a/src/router/mod.rs +++ b/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::*; diff --git a/src/router/navcord.rs b/src/router/navcord.rs index fd8ae90..f745d72 100644 --- a/src/router/navcord.rs +++ b/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 diff --git a/src/router/navmesh.rs b/src/router/navmesh.rs index 58e751a..963b6d8 100644 --- a/src/router/navmesh.rs +++ b/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}, }; diff --git a/src/router/route.rs b/src/router/route.rs index cd2d527..229ee1c 100644 --- a/src/router/route.rs +++ b/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, + thetastar: ThetastarStepper, navcord: Navcord, ghosts: Vec, obstacles: Vec, @@ -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 astar = AstarStepper::new(navmesh, source_navnode, &mut strategy); + let mut strategy = RouterThetastarStrategy::new(layout, &mut navcord, target); + 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 Step, BandTermsegIndex> for RouteStepper { - type Error = AstarError; + type Error = ThetastarError; fn step( &mut self, router: &mut Router, - ) -> Result, AstarError> { + ) -> Result, ThetastarError> { let layout = router.layout_mut(); - let target = self.astar.graph().destination(); - let mut strategy = RouterAstarStrategy::new(layout, &mut self.navcord, target); - let result = self.astar.step(&mut strategy); + let target = self.thetastar.graph().destination(); + let mut strategy = RouterThetastarStrategy::new(layout, &mut self.navcord, target); + let result = self.thetastar.step(&mut strategy); self.ghosts = strategy.probe_ghosts; self.obstacles = strategy.probe_obstacles; diff --git a/src/router/router.rs b/src/router/router.rs index be864ed..dc18504 100644 --- a/src/router/router.rs +++ b/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, pub navcord: &'a mut Navcord, pub target: FixedDotIndex, @@ -49,7 +49,7 @@ pub struct RouterAstarStrategy<'a, R> { pub probe_obstacles: Vec, } -impl<'a, R> RouterAstarStrategy<'a, R> { +impl<'a, R> RouterThetastarStrategy<'a, R> { pub fn new(layout: &'a mut Layout, navcord: &'a mut Navcord, target: FixedDotIndex) -> Self { Self { layout, @@ -61,28 +61,30 @@ impl<'a, R> RouterAstarStrategy<'a, R> { } } -impl AstarStrategy for RouterAstarStrategy<'_, R> { +impl ThetastarStrategy + for RouterThetastarStrategy<'_, R> +{ fn visit_navnode( &mut self, navmesh: &Navmesh, - vertex: NavnodeIndex, + navnode: NavnodeIndex, tracker: &PathTracker, ) -> Result, ()> { - 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 AstarStrategy for RouterAst } } - fn place_probe_at_navedge( + fn place_probe_to_navnode( &mut self, navmesh: &Navmesh, - edge: NavmeshEdgeReference, + probed_navnode: NavnodeIndex, ) -> Option { 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 diff --git a/src/router/thetastar.rs b/src/router/thetastar.rs new file mode 100644 index 0000000..f7c9236 --- /dev/null +++ b/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(pub K, pub T); + +impl PartialEq for MinScored { + #[inline] + fn eq(&self, other: &MinScored) -> bool { + self.cmp(other) == Ordering::Equal + } +} + +impl Eq for MinScored {} + +impl PartialOrd for MinScored { + #[inline] + fn partial_cmp(&self, other: &MinScored) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for MinScored { + #[inline] + fn cmp(&self, other: &MinScored) -> 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 +where + G: GraphBase, + G::NodeId: Eq + Ord, +{ + predecessors: BTreeMap, +} + +impl PathTracker +where + G: GraphBase, + G::NodeId: Eq + Ord, +{ + fn new() -> PathTracker { + PathTracker { + predecessors: BTreeMap::new(), + } + } + + fn predecessor(&self, node: G::NodeId) -> Option { + 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 { + let mut path = vec![last]; + + let mut current = last; + while let Some(&previous) = self.predecessors.get(¤t) { + path.push(previous); + current = previous; + } + + path.reverse(); + + path + } +} + +pub trait ThetastarStrategy +where + G: GraphBase, + G::NodeId: Eq + Ord, + for<'a> &'a G: IntoEdges + MakeEdgeRef, + K: Measure + Copy, +{ + fn visit_navnode( + &mut self, + graph: &G, + navnode: G::NodeId, + tracker: &PathTracker, + ) -> Result, ()>; + fn place_probe_to_navnode<'a>(&mut self, graph: &'a G, probed_navnode: G::NodeId) -> Option; + 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 { + Scanning, + VisitingProbeOnLineOfSight(N), + VisitingProbeOnNavedge(N, E), + Probing(N), +} + +#[derive(Getters)] +pub struct ThetastarStepper +where + G: GraphBase, + G::NodeId: Eq + Ord, + for<'a> &'a G: IntoEdges + MakeEdgeRef, + K: Measure + Copy, +{ + state: ThetastarState, + graph: G, + #[getter(skip)] + visit_next: BinaryHeap>, + /// Also known as the g-scores, or just g. + scores: BTreeMap, + /// Also known as the f-scores, or just f. + estimate_scores: BTreeMap, + #[getter(skip)] + path_tracker: PathTracker, + // FIXME: To work around edge references borrowing from the graph we collect then reiterate over them. + #[getter(skip)] + edge_ids: VecDeque, +} + +#[derive(Error, Debug, Clone)] +pub enum ThetastarError { + #[error("A* search found no path")] + NotFound, +} + +impl ThetastarStepper +where + G: GraphBase, + G::NodeId: Eq + Ord, + for<'a> &'a G: IntoEdges + MakeEdgeRef, + K: Measure + Copy, +{ + pub fn new( + graph: G, + start: G::NodeId, + strategy: &mut impl ThetastarStrategy, + ) -> Self { + let mut this = Self { + state: ThetastarState::Scanning, + graph, + visit_next: BinaryHeap::new(), + scores: BTreeMap::new(), + estimate_scores: BTreeMap::new(), + path_tracker: PathTracker::::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> + Step, R), ThetastarState> for ThetastarStepper +where + G: GraphBase, + G::NodeId: Eq + Ord, + for<'a> &'a G: IntoEdges + MakeEdgeRef, + K: Measure + Copy, +{ + type Error = ThetastarError; + + fn step( + &mut self, + strategy: &mut S, + ) -> Result< + ControlFlow<(K, Vec, R), ThetastarState>, + 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)) + } + } + } +}