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

feat(router/router): Backtrack if initial parent was on the same compound

This commit is contained in:
Mikolaj Wielgus 2025-08-21 16:12:47 +02:00
parent b21f77c9ab
commit fca8e44269
7 changed files with 102 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
crates/topola-egui/src/displayer.rs
View File

@ -180,12 +180,12 @@ impl<'a> Displayer<'a> {
for edge in navmesh.edge_references() {
let mut from =
PrimitiveIndex::from(navmesh.node_weight(edge.source()).unwrap().node)
PrimitiveIndex::from(navmesh.node_weight(edge.source()).unwrap().binavnode)
.primitive(board.layout().drawing())
.shape()
.center();
let mut to =
PrimitiveIndex::from(navmesh.node_weight(edge.target()).unwrap().node)
PrimitiveIndex::from(navmesh.node_weight(edge.target()).unwrap().binavnode)
.primitive(board.layout().drawing())
.shape()
.center();
@ -278,7 +278,7 @@ impl<'a> Displayer<'a> {
let navnode = NavnodeIndex(index);
let primitive =
PrimitiveIndex::from(navmesh.node_weight(navnode).unwrap().node);
PrimitiveIndex::from(navmesh.node_weight(navnode).unwrap().binavnode);
let mut pos = primitive
.primitive(board.layout().drawing())
.shape()
@ -321,7 +321,7 @@ impl<'a> Displayer<'a> {
) -> Option<Circle> {
let drawing = board.layout().drawing();
let navnode = NavnodeIndex(index);
let primitive = PrimitiveIndex::from(navmesh.node_weight(navnode).unwrap().node);
let primitive = PrimitiveIndex::from(navmesh.node_weight(navnode).unwrap().binavnode);
if let Ok(dot) = DotIndex::try_from(primitive) {
Some(drawing.dot_circle(

2
src/router/draw.rs
View File

@ -29,7 +29,7 @@ use crate::{
#[derive(Error, Debug, Clone, Copy)]
pub enum DrawException {
#[error(transparent)]
NoTangents(#[from] NoBitangents),
NoBitangents(#[from] NoBitangents),
// TODO add real error messages + these should eventually use Display
#[error("cannot finish in {0:?}")]
CannotFinishIn(FixedDotIndex, #[source] DrawingException),

6
src/router/navcord.rs
View File

@ -78,7 +78,7 @@ impl Navcord {
.maybe_sense
.ok_or(NavcorderException::CannotWrap)?;
match around_node_weight.node {
match around_node_weight.binavnode {
BinavnodeNodeIndex::FixedDot(dot) => {
layout.cane_around_dot(&mut self.recorder, head, dot, sense, self.width)
}
@ -113,8 +113,8 @@ impl Navcord {
to: NavnodeIndex,
) -> Result<f64, NavcorderException> {
let length = if to == navmesh.destination_navnode() {
let to_node_weight = navmesh.node_weight(to).unwrap();
let BinavnodeNodeIndex::FixedDot(to_dot) = to_node_weight.node else {
let to_binavnode = navmesh.node_weight(to).unwrap().binavnode;
let BinavnodeNodeIndex::FixedDot(to_dot) = to_binavnode else {
unreachable!();
};

10
src/router/navmesh.rs
View File

@ -101,7 +101,7 @@ impl From<BinavnodeNodeIndex> for GearIndex {
/// during autorouting: <https://topola.dev/blog/2024/07/20/junejuly-2024-development-update/#advanced-debug-visualization>
#[derive(Debug, Clone)]
pub struct NavnodeWeight {
pub node: BinavnodeNodeIndex,
pub binavnode: BinavnodeNodeIndex,
/// There are two navnodes for each navigable node:
/// one is clockwise (`Some(true)`), the other counterclockwise (`Some(false)`).
@ -167,7 +167,7 @@ impl Navmesh {
for trianvertex in prenavmesh.triangulation().node_identifiers() {
if trianvertex == origin.into() {
let navnode = graph.add_node(NavnodeWeight {
node: trianvertex.into(),
binavnode: trianvertex.into(),
maybe_sense: None,
});
@ -175,7 +175,7 @@ impl Navmesh {
map.insert(trianvertex, vec![(navnode, navnode)]);
} else if trianvertex == destination.into() {
let navnode = graph.add_node(NavnodeWeight {
node: trianvertex.into(),
binavnode: trianvertex.into(),
maybe_sense: None,
});
@ -276,12 +276,12 @@ impl Navmesh {
node: BinavnodeNodeIndex,
) {
let navnode1 = graph.add_node(NavnodeWeight {
node,
binavnode: node,
maybe_sense: Some(RotationSense::Counterclockwise),
});
let navnode2 = graph.add_node(NavnodeWeight {
node,
binavnode: node,
maybe_sense: Some(RotationSense::Clockwise),
});

2
src/router/ng/mod.rs
View File

@ -255,7 +255,7 @@ impl EvalException {
],
Vec::new(),
),
Self::Draw(DrawException::NoTangents(_)) => (Vec::new(), Vec::new(), Vec::new()),
Self::Draw(DrawException::NoBitangents(_)) => (Vec::new(), Vec::new(), Vec::new()),
Self::Draw(DrawException::CannotFinishIn(_, dwxc))
| Self::Draw(DrawException::CannotWrapAround(_, dwxc)) => {
match dwxc.maybe_ghost_and_obstacle() {

61
src/router/router.rs
View File

@ -19,6 +19,7 @@ use crate::{
},
geometry::{primitive::PrimitiveShape, shape::AccessShape},
layout::{Layout, LayoutEdit},
router::navmesh::BinavnodeNodeIndex,
};
use super::{
@ -94,6 +95,7 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
fn place_probe_to_navnode(
&mut self,
navmesh: &Navmesh,
maybe_initial_parent_navnode: Option<NavnodeIndex>,
probed_navnode: NavnodeIndex,
) -> ControlFlow<Option<f64>> {
let result = self.navcord.step_to(self.layout, navmesh, probed_navnode);
@ -103,7 +105,7 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
Err(err) => {
if let NavcorderException::CannotDraw(draw_err) = err {
let layout_err = match draw_err {
DrawException::NoTangents(..) => return ControlFlow::Break(None),
DrawException::NoBitangents(..) => return ControlFlow::Break(None),
DrawException::CannotFinishIn(.., layout_err) => layout_err,
DrawException::CannotWrapAround(.., layout_err) => layout_err,
};
@ -111,8 +113,63 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
let Some((ghost, obstacle)) = layout_err.maybe_ghost_and_obstacle() else {
return ControlFlow::Break(None);
};
self.probe_ghosts = vec![*ghost];
self.probe_obstacles = vec![obstacle];
let Some(initial_parent_navnode) = maybe_initial_parent_navnode else {
return ControlFlow::Break(None);
};
let initial_parent_binavnode = navmesh
.node_weight(initial_parent_navnode)
.unwrap()
.binavnode;
let initial_parent_compounds: Vec<_> = match initial_parent_binavnode {
BinavnodeNodeIndex::FixedDot(dot) => {
self.layout.drawing().compounds(dot).collect()
}
BinavnodeNodeIndex::FixedBend(_bend) => {
todo!()
}
BinavnodeNodeIndex::LooseBend(bend) => {
self.layout.drawing().compounds(bend).collect()
}
};
let from_binavnode = navmesh
.node_weight(*self.navcord.path.last().unwrap())
.unwrap()
.binavnode;
let from_compounds: Vec<_> = match from_binavnode {
BinavnodeNodeIndex::FixedDot(dot) => {
self.layout.drawing().compounds(dot).collect()
}
BinavnodeNodeIndex::FixedBend(_bend) => {
todo!()
}
BinavnodeNodeIndex::LooseBend(bend) => {
self.layout.drawing().compounds(bend).collect()
}
};
if initial_parent_compounds.iter().any(|compound1| {
from_compounds
.iter()
.any(|compound2| compound1 == compound2)
}) {
// If we have failed to do a line-of-sight draw, but
// the current probe source navnode and the initial
// parent navnode are on the same compound (TODO:
// narrow this down from compound to poly), we continue
// backtracking. When ControlFlow::Continue(()) is
// returned here, the modified Theta* algorithm instead
// of visiting a new navedge recedes the navcord and
// with that the current probe source navnode.
return ControlFlow::Continue(());
}
}
None
}
@ -124,7 +181,7 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
}
fn estimate_cost_to_goal(&mut self, navmesh: &Navmesh, vertex: NavnodeIndex) -> f64 {
let start_point = PrimitiveIndex::from(navmesh.node_weight(vertex).unwrap().node)
let start_point = PrimitiveIndex::from(navmesh.node_weight(vertex).unwrap().binavnode)
.primitive(self.layout.drawing())
.shape()
.center();

47
src/router/thetastar.rs
View File

@ -123,6 +123,7 @@ where
fn place_probe_to_navnode(
&mut self,
graph: &G,
initial_parent_navnode: Option<G::NodeId>,
probed_navnode: G::NodeId,
) -> ControlFlow<Option<K>>;
fn remove_probe(&mut self, graph: &G);
@ -320,18 +321,23 @@ where
Ok(ControlFlow::Continue(self.state))
}
}
ThetastarState::BacktrackAndProbeOnLineOfSight(visited_navnode, visited_navedge) => {
ThetastarState::BacktrackAndProbeOnLineOfSight(curr_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 initial_from_navnode = (&self.graph).edge_ref(visited_navedge).source();
let to_navnode = (&self.graph).edge_ref(visited_navedge).target();
let initial_source_navnode = (&self.graph).edge_ref(visited_navedge).source();
let initial_parent_navnode = self.path_tracker.predecessor(initial_source_navnode);
let probed_navnode = (&self.graph).edge_ref(visited_navedge).target();
if let Some(parent_navnode) = self.path_tracker.predecessor(visited_navnode) {
if let Some(parent_navnode) = self.path_tracker.predecessor(curr_navnode) {
strategy.visit_navnode(&self.graph, parent_navnode, &self.path_tracker);
let parent_score = self.scores[&parent_navnode];
match strategy.place_probe_to_navnode(&self.graph, to_navnode) {
match strategy.place_probe_to_navnode(
&self.graph,
initial_parent_navnode,
probed_navnode,
) {
ControlFlow::Continue(()) => {
// Transition to self to repeatedly backtrack.
self.state = ThetastarState::BacktrackAndProbeOnLineOfSight(
@ -341,7 +347,7 @@ where
Ok(ControlFlow::Continue(self.state))
}
ControlFlow::Break(Some(los_cost)) => {
let next = to_navnode;
let next = probed_navnode;
let next_score = parent_score + los_cost;
match self.scores.entry(next) {
@ -356,7 +362,7 @@ where
strategy.remove_probe(&self.graph);
self.state = ThetastarState::ProbeOnNavedge(
visited_navnode,
initial_source_navnode,
visited_navedge,
);
return Ok(ControlFlow::Continue(self.state));
@ -370,7 +376,7 @@ where
self.push_to_frontier(next, next_score, parent_navnode, strategy);
self.state = ThetastarState::Probing(visited_navnode);
self.state = ThetastarState::Probing(curr_navnode);
Ok(ControlFlow::Continue(self.state))
}
ControlFlow::Break(None) => {
@ -378,31 +384,36 @@ where
// and the backtracking condition is not met.
strategy.visit_navnode(
&self.graph,
visited_navnode,
initial_source_navnode,
&self.path_tracker,
);
self.state = ThetastarState::ProbeOnNavedge(
initial_from_navnode,
initial_source_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::ProbeOnNavedge(visited_navnode, visited_navedge);
// Come back from current navnode if drawing from it failed.
strategy.visit_navnode(&self.graph, initial_source_navnode, &self.path_tracker);
self.state =
ThetastarState::ProbeOnNavedge(initial_source_navnode, visited_navedge);
Ok(ControlFlow::Continue(self.state))
}
}
ThetastarState::ProbeOnNavedge(visited_navnode, visited_navedge) => {
let visited_score = self.scores[&visited_navnode];
let to_navnode = (&self.graph).edge_ref(visited_navedge).target();
let initial_source_navnode = (&self.graph).edge_ref(visited_navedge).source();
let initial_parent_navnode = self.path_tracker.predecessor(initial_source_navnode);
let probed_navnode = (&self.graph).edge_ref(visited_navedge).target();
if let ControlFlow::Break(Some(navedge_cost)) =
strategy.place_probe_to_navnode(&self.graph, to_navnode)
{
let next = to_navnode;
if let ControlFlow::Break(Some(navedge_cost)) = strategy.place_probe_to_navnode(
&self.graph,
initial_parent_navnode,
probed_navnode,
) {
let next = probed_navnode;
let next_score = visited_score + navedge_cost;
match self.scores.entry(next) {