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

2
src/router/draw.rs
View File

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

6
src/router/navcord.rs
View File

@ -78,7 +78,7 @@ impl Navcord {
.maybe_sense .maybe_sense
.ok_or(NavcorderException::CannotWrap)?; .ok_or(NavcorderException::CannotWrap)?;
match around_node_weight.node { match around_node_weight.binavnode {
BinavnodeNodeIndex::FixedDot(dot) => { BinavnodeNodeIndex::FixedDot(dot) => {
layout.cane_around_dot(&mut self.recorder, head, dot, sense, self.width) layout.cane_around_dot(&mut self.recorder, head, dot, sense, self.width)
} }
@ -113,8 +113,8 @@ impl Navcord {
to: NavnodeIndex, to: NavnodeIndex,
) -> Result<f64, NavcorderException> { ) -> Result<f64, NavcorderException> {
let length = if to == navmesh.destination_navnode() { let length = if to == navmesh.destination_navnode() {
let to_node_weight = navmesh.node_weight(to).unwrap(); let to_binavnode = navmesh.node_weight(to).unwrap().binavnode;
let BinavnodeNodeIndex::FixedDot(to_dot) = to_node_weight.node else { let BinavnodeNodeIndex::FixedDot(to_dot) = to_binavnode else {
unreachable!(); 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> /// during autorouting: <https://topola.dev/blog/2024/07/20/junejuly-2024-development-update/#advanced-debug-visualization>
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct NavnodeWeight { pub struct NavnodeWeight {
pub node: BinavnodeNodeIndex, pub binavnode: BinavnodeNodeIndex,
/// There are two navnodes for each navigable node: /// There are two navnodes for each navigable node:
/// one is clockwise (`Some(true)`), the other counterclockwise (`Some(false)`). /// one is clockwise (`Some(true)`), the other counterclockwise (`Some(false)`).
@ -167,7 +167,7 @@ impl Navmesh {
for trianvertex in prenavmesh.triangulation().node_identifiers() { for trianvertex in prenavmesh.triangulation().node_identifiers() {
if trianvertex == origin.into() { if trianvertex == origin.into() {
let navnode = graph.add_node(NavnodeWeight { let navnode = graph.add_node(NavnodeWeight {
node: trianvertex.into(), binavnode: trianvertex.into(),
maybe_sense: None, maybe_sense: None,
}); });
@ -175,7 +175,7 @@ impl Navmesh {
map.insert(trianvertex, vec![(navnode, navnode)]); map.insert(trianvertex, vec![(navnode, navnode)]);
} else if trianvertex == destination.into() { } else if trianvertex == destination.into() {
let navnode = graph.add_node(NavnodeWeight { let navnode = graph.add_node(NavnodeWeight {
node: trianvertex.into(), binavnode: trianvertex.into(),
maybe_sense: None, maybe_sense: None,
}); });
@ -276,12 +276,12 @@ impl Navmesh {
node: BinavnodeNodeIndex, node: BinavnodeNodeIndex,
) { ) {
let navnode1 = graph.add_node(NavnodeWeight { let navnode1 = graph.add_node(NavnodeWeight {
node, binavnode: node,
maybe_sense: Some(RotationSense::Counterclockwise), maybe_sense: Some(RotationSense::Counterclockwise),
}); });
let navnode2 = graph.add_node(NavnodeWeight { let navnode2 = graph.add_node(NavnodeWeight {
node, binavnode: node,
maybe_sense: Some(RotationSense::Clockwise), maybe_sense: Some(RotationSense::Clockwise),
}); });

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

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

61
src/router/router.rs
View File

@ -19,6 +19,7 @@ use crate::{
}, },
geometry::{primitive::PrimitiveShape, shape::AccessShape}, geometry::{primitive::PrimitiveShape, shape::AccessShape},
layout::{Layout, LayoutEdit}, layout::{Layout, LayoutEdit},
router::navmesh::BinavnodeNodeIndex,
}; };
use super::{ use super::{
@ -94,6 +95,7 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
fn place_probe_to_navnode( fn place_probe_to_navnode(
&mut self, &mut self,
navmesh: &Navmesh, navmesh: &Navmesh,
maybe_initial_parent_navnode: Option<NavnodeIndex>,
probed_navnode: NavnodeIndex, probed_navnode: NavnodeIndex,
) -> ControlFlow<Option<f64>> { ) -> ControlFlow<Option<f64>> {
let result = self.navcord.step_to(self.layout, navmesh, probed_navnode); let result = self.navcord.step_to(self.layout, navmesh, probed_navnode);
@ -103,7 +105,7 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
Err(err) => { Err(err) => {
if let NavcorderException::CannotDraw(draw_err) = err { if let NavcorderException::CannotDraw(draw_err) = err {
let layout_err = match draw_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::CannotFinishIn(.., layout_err) => layout_err,
DrawException::CannotWrapAround(.., 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 { let Some((ghost, obstacle)) = layout_err.maybe_ghost_and_obstacle() else {
return ControlFlow::Break(None); return ControlFlow::Break(None);
}; };
self.probe_ghosts = vec![*ghost]; self.probe_ghosts = vec![*ghost];
self.probe_obstacles = vec![obstacle]; 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 None
} }
@ -124,7 +181,7 @@ impl<R: AccessRules> ThetastarStrategy<Navmesh, f64, BandTermsegIndex>
} }
fn estimate_cost_to_goal(&mut self, navmesh: &Navmesh, vertex: NavnodeIndex) -> f64 { 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()) .primitive(self.layout.drawing())
.shape() .shape()
.center(); .center();

47
src/router/thetastar.rs
View File

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