berkus
/
topola
mirror of https://codeberg.org/topola/topola.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Closes https://codeberg.org/topola/topola/issues/121
This commit is contained in:
Mikolaj Wielgus 2025-06-05 21:16:26 +02:00 committed by mikolaj
parent 1fea359a40
commit d0c304adbd
19 changed files with 432 additions and 411 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
committed.toml
View File

@ -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",

7
crates/topola-egui/src/viewport.rs
View File

@ -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) =

16
src/autorouter/autoroute.rs
View File

@ -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 {
fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
self.route.as_ref().map(|route| route.astar())
impl GetMaybeThetastarStepper for AutorouteExecutionStepper {
fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
self.route.as_ref().map(|route| route.thetastar())
}
}

4
src/autorouter/autorouter.rs
View File

@ -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")]

10
src/autorouter/compare_detours.rs
View File

@ -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 {
fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
self.autoroute.maybe_astar()
impl GetMaybeThetastarStepper for CompareDetoursExecutionStepper {
fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
self.autoroute.maybe_thetastar()
}
}

2
src/autorouter/execution.rs
View File

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

6
src/autorouter/invoker.rs
View File

@ -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<Navmesh, f64>> {
pub trait GetMaybeThetastarStepper {
fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
None
}
}

4
src/autorouter/measure_length.rs
View File

@ -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 {}

4
src/autorouter/place_via.rs
View File

@ -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 {}

4
src/autorouter/remove_bands.rs
View File

@ -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 {}

14
src/interactor/activity.rs
View File

@ -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<M: AccessMesadata> Abort<ActivityContext<'_, M>> for ActivityStepperWithSta
}
}
impl GetMaybeAstarStepper for ActivityStepperWithStatus {
fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
self.activity.maybe_astar()
impl GetMaybeThetastarStepper for ActivityStepperWithStatus {
fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
self.activity.maybe_thetastar()
}
}

8
src/interactor/interaction.rs
View File

@ -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 {
fn maybe_astar(&self) -> Option<&AstarStepper<Navmesh, f64>> {
impl GetMaybeThetastarStepper for InteractionStepper {
fn maybe_thetastar(&self) -> Option<&ThetastarStepper<Navmesh, f64>> {
todo!()
}
}

343
src/router/astar.rs
View File

@ -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))
}
}
}
}

2
src/router/mod.rs
View File

@ -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::*;

2
src/router/navcord.rs
View File

@ -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

2
src/router/navmesh.rs
View File

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

22
src/router/route.rs
View File

@ -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 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<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 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;

32
src/router/router.rs
View File

@ -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

359
src/router/thetastar.rs Normal file
View File

@ -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))
}
}
}
}