diff --git a/crates/topola-egui/src/displayer.rs b/crates/topola-egui/src/displayer.rs index ca472bf..0b77c12 100644 --- a/crates/topola-egui/src/displayer.rs +++ b/crates/topola-egui/src/displayer.rs @@ -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 { 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( diff --git a/src/router/draw.rs b/src/router/draw.rs index f99ac61..5e74d39 100644 --- a/src/router/draw.rs +++ b/src/router/draw.rs @@ -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), diff --git a/src/router/navcord.rs b/src/router/navcord.rs index 0b65119..d9e7b4b 100644 --- a/src/router/navcord.rs +++ b/src/router/navcord.rs @@ -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 { 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!(); }; diff --git a/src/router/navmesh.rs b/src/router/navmesh.rs index f3ef85e..d068a6d 100644 --- a/src/router/navmesh.rs +++ b/src/router/navmesh.rs @@ -101,7 +101,7 @@ impl From for GearIndex { /// during autorouting: #[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), }); diff --git a/src/router/ng/mod.rs b/src/router/ng/mod.rs index fca0faf..cab54c7 100644 --- a/src/router/ng/mod.rs +++ b/src/router/ng/mod.rs @@ -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() { diff --git a/src/router/router.rs b/src/router/router.rs index 12e3ef5..20d6dfd 100644 --- a/src/router/router.rs +++ b/src/router/router.rs @@ -19,6 +19,7 @@ use crate::{ }, geometry::{primitive::PrimitiveShape, shape::AccessShape}, layout::{Layout, LayoutEdit}, + router::navmesh::BinavnodeNodeIndex, }; use super::{ @@ -94,6 +95,7 @@ impl ThetastarStrategy fn place_probe_to_navnode( &mut self, navmesh: &Navmesh, + maybe_initial_parent_navnode: Option, probed_navnode: NavnodeIndex, ) -> ControlFlow> { let result = self.navcord.step_to(self.layout, navmesh, probed_navnode); @@ -103,7 +105,7 @@ impl ThetastarStrategy 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 ThetastarStrategy 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 ThetastarStrategy } 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(); diff --git a/src/router/thetastar.rs b/src/router/thetastar.rs index 51d5e06..74b29ee 100644 --- a/src/router/thetastar.rs +++ b/src/router/thetastar.rs @@ -123,6 +123,7 @@ where fn place_probe_to_navnode( &mut self, graph: &G, + initial_parent_navnode: Option, probed_navnode: G::NodeId, ) -> ControlFlow>; 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) {