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

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

This commit is contained in:
Mikolaj Wielgus 2025-08-20 22:19:27 +02:00
parent 5a1cb564dc
commit b21f77c9ab
1 changed files with 95 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
src/router/thetastar.rs
View File

@ -138,12 +138,14 @@ pub trait MakeEdgeRef: IntoEdgeReferences {
pub enum ThetastarState<N: Copy, E: Copy> {
/// Visit a new navnode and copy all navedges going out of it.
Scanning,
/// Load a new navedge, try to probe it from current navnode's predecessor.
/// If this fails, transition to `VisitingProbeOnNavedge`. If there is no
/// more navedges, transition back to `Scanning`.
VisitingProbeOnLineOfSight(N),
/// Probe the currently loaded navedge from the current navnode.
VisitingProbeOnNavedge(N, E),
/// Load a new navedge.
VisitFrontierNavedge(N),
/// Backtrack by one navnode, then line-of-sight probe to target of the
/// currently visited navedge. Transition to this state again until a
/// condition is met, then continue the algorithm by transitioning to the
/// `ProbeOnNavedge(...)` state.
BacktrackAndProbeOnLineOfSight(N, E),
ProbeOnNavedge(N, E),
/// The probe is in place, retract it and continue to the next navedge.
Probing(N),
}
@ -303,36 +305,42 @@ where
self.edge_ids = self.graph.edges(navnode).map(|edge| edge.id()).collect();
self.state = ThetastarState::VisitingProbeOnLineOfSight(navnode);
self.state = ThetastarState::VisitFrontierNavedge(navnode);
Ok(ControlFlow::Continue(self.state))
}
ThetastarState::VisitingProbeOnLineOfSight(visited_navnode) => {
if let Some(curr_navedge) = self.edge_ids.pop() {
ThetastarState::VisitFrontierNavedge(visited_navnode) => {
if let Some(visited_navedge) = self.edge_ids.pop() {
self.state = ThetastarState::BacktrackAndProbeOnLineOfSight(
visited_navnode,
visited_navedge,
);
Ok(ControlFlow::Continue(self.state))
} else {
self.state = ThetastarState::Scanning;
Ok(ControlFlow::Continue(self.state))
}
}
ThetastarState::BacktrackAndProbeOnLineOfSight(visited_navnode, visited_navedge) => {
// This lookup can be unwrapped without fear of panic since the node was
// necessarily scored before adding it to `.visit_next`.
//let node_score = self.scores[&visited_navnode];
let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
let mut curr_navnode = visited_navnode;
let initial_from_navnode = (&self.graph).edge_ref(visited_navedge).source();
let to_navnode = (&self.graph).edge_ref(visited_navedge).target();
// Loop to repeatedly backtrack.
if let (Some(parent_navnode), Some(los_cost)) = loop {
let Some(parent_navnode) = self.path_tracker.predecessor(curr_navnode)
else {
break (None, None);
};
// Visit parent node.
if let Some(parent_navnode) = self.path_tracker.predecessor(visited_navnode) {
strategy.visit_navnode(&self.graph, parent_navnode, &self.path_tracker);
if let ControlFlow::Break(result) =
strategy.place_probe_to_navnode(&self.graph, to_navnode)
{
break (Some(parent_navnode), result);
}
curr_navnode = parent_navnode;
} {
let parent_score = self.scores[&parent_navnode];
match strategy.place_probe_to_navnode(&self.graph, to_navnode) {
ControlFlow::Continue(()) => {
// Transition to self to repeatedly backtrack.
self.state = ThetastarState::BacktrackAndProbeOnLineOfSight(
parent_navnode,
visited_navedge,
);
Ok(ControlFlow::Continue(self.state))
}
ControlFlow::Break(Some(los_cost)) => {
let next = to_navnode;
let next_score = parent_score + los_cost;
@ -347,9 +355,9 @@ where
// state to avoid complicating.
strategy.remove_probe(&self.graph);
self.state = ThetastarState::VisitingProbeOnNavedge(
self.state = ThetastarState::ProbeOnNavedge(
visited_navnode,
curr_navedge,
visited_navedge,
);
return Ok(ControlFlow::Continue(self.state));
}
@ -364,21 +372,32 @@ where
self.state = ThetastarState::Probing(visited_navnode);
Ok(ControlFlow::Continue(self.state))
}
ControlFlow::Break(None) => {
// Come back to initial navnode if drawing failed
// and the backtracking condition is not met.
strategy.visit_navnode(
&self.graph,
visited_navnode,
&self.path_tracker,
);
self.state = ThetastarState::ProbeOnNavedge(
initial_from_navnode,
visited_navedge,
);
Ok(ControlFlow::Continue(self.state))
}
}
} else {
// Come back from parent node if drawing from it failed.
strategy.visit_navnode(&self.graph, visited_navnode, &self.path_tracker);
self.state =
ThetastarState::VisitingProbeOnNavedge(visited_navnode, curr_navedge);
Ok(ControlFlow::Continue(self.state))
}
} else {
self.state = ThetastarState::Scanning;
self.state = ThetastarState::ProbeOnNavedge(visited_navnode, visited_navedge);
Ok(ControlFlow::Continue(self.state))
}
}
ThetastarState::VisitingProbeOnNavedge(visited_navnode, curr_navedge) => {
ThetastarState::ProbeOnNavedge(visited_navnode, visited_navedge) => {
let visited_score = self.scores[&visited_navnode];
let to_navnode = (&self.graph).edge_ref(curr_navedge).target();
let to_navnode = (&self.graph).edge_ref(visited_navedge).target();
if let ControlFlow::Break(Some(navedge_cost)) =
strategy.place_probe_to_navnode(&self.graph, to_navnode)
@ -406,14 +425,14 @@ where
self.state = ThetastarState::Probing(visited_navnode);
Ok(ControlFlow::Continue(self.state))
} else {
self.state = ThetastarState::VisitingProbeOnLineOfSight(visited_navnode);
self.state = ThetastarState::VisitFrontierNavedge(visited_navnode);
Ok(ControlFlow::Continue(self.state))
}
}
ThetastarState::Probing(visited_navnode) => {
strategy.remove_probe(&self.graph);
self.state = ThetastarState::VisitingProbeOnLineOfSight(visited_navnode);
self.state = ThetastarState::VisitFrontierNavedge(visited_navnode);
Ok(ControlFlow::Continue(self.state))
}
}