From fd23be7a8a86993701f74a2865128bb4f62e5f0c Mon Sep 17 00:00:00 2001 From: Mikolaj Wielgus Date: Thu, 4 Jul 2024 13:52:26 +0200 Subject: [PATCH] router: step on successful probes --- src/router/astar.rs | 17 +++++++++++++++-- src/router/route.rs | 8 +++++--- src/router/router.rs | 23 ++++++++++++----------- 3 files changed, 32 insertions(+), 16 deletions(-) diff --git a/src/router/astar.rs b/src/router/astar.rs index a2a954a..7bff639 100644 --- a/src/router/astar.rs +++ b/src/router/astar.rs @@ -104,11 +104,12 @@ where K: Measure + Copy, { fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker) -> Option; - fn probe<'a>( + fn place_probe<'a>( &mut self, graph: &'a G, edge: <&'a G as IntoEdgeReferences>::EdgeRef, ) -> Option; + fn remove_probe<'a>(&mut self, graph: &'a G); fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K; } @@ -131,6 +132,8 @@ where pub maybe_curr_node: Option, // FIXME: To work around edge references borrowing from the graph we collect then reiterate over tem. pub edge_ids: VecDeque, + // TODO: Rewrite this to be a well-designed state machine. + pub is_probing: bool, } #[derive(Error, Debug, Clone)] @@ -147,6 +150,7 @@ where for<'a> &'a G: IntoEdges + MakeEdgeRef, K: Measure + Copy, { + Probing, Probed, Visited, Finished(K, Vec, R), @@ -168,6 +172,7 @@ where path_tracker: PathTracker::::new(), maybe_curr_node: None, edge_ids: VecDeque::new(), + is_probing: false, }; let zero_score = K::default(); @@ -184,13 +189,18 @@ where strategy: &mut impl AstarStrategy, ) -> Result, AstarError> { if let Some(curr_node) = self.maybe_curr_node { + if self.is_probing { + strategy.remove_probe(&self.graph); + self.is_probing = false; + } + if let Some(edge_id) = 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_node]; let edge = (&self.graph).edge_ref(edge_id); - if let Some(edge_cost) = strategy.probe(&self.graph, edge) { + if let Some(edge_cost) = strategy.place_probe(&self.graph, edge) { let next = edge.target(); let next_score = node_score + edge_cost; @@ -212,6 +222,9 @@ where let next_estimate_score = next_score + strategy.estimate_cost(&self.graph, next); self.visit_next.push(MinScored(next_estimate_score, next)); + + self.is_probing = true; + return Ok(AstarStatus::Probing); } return Ok(AstarStatus::Probed); diff --git a/src/router/route.rs b/src/router/route.rs index 6e5da73..7fb1de1 100644 --- a/src/router/route.rs +++ b/src/router/route.rs @@ -85,12 +85,14 @@ impl Route { let mut strategy = RouterAstarStrategy::new(tracer, &mut self.trace, target); let result = match self.astar.step(&mut strategy)? { - AstarStatus::Probed | AstarStatus::Visited => Ok(RouterStatus::Running), + AstarStatus::Probing | AstarStatus::Probed | AstarStatus::Visited => { + Ok(RouterStatus::Running) + } AstarStatus::Finished(_cost, _path, band) => Ok(RouterStatus::Finished(band)), }; - self.ghosts = strategy.last_ghosts; - self.obstacles = strategy.last_obstacles; + self.ghosts = strategy.probe_ghosts; + self.obstacles = strategy.probe_obstacles; result } diff --git a/src/router/router.rs b/src/router/router.rs index df34897..0c737b4 100644 --- a/src/router/router.rs +++ b/src/router/router.rs @@ -47,8 +47,8 @@ pub struct RouterAstarStrategy<'a, R: AccessRules> { pub tracer: Tracer<'a, R>, pub trace: &'a mut Trace, pub target: FixedDotIndex, - pub last_ghosts: Vec, - pub last_obstacles: Vec, + pub probe_ghosts: Vec, + pub probe_obstacles: Vec, } impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> { @@ -57,8 +57,8 @@ impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> { tracer, trace, target, - last_ghosts: vec![], - last_obstacles: vec![], + probe_ghosts: vec![], + probe_obstacles: vec![], } } @@ -99,7 +99,7 @@ impl<'a, R: AccessRules> AstarStrategy .ok() } - fn probe(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option { + fn place_probe(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option { if edge.target().petgraph_index() == self.target.petgraph_index() { return None; } @@ -114,10 +114,7 @@ impl<'a, R: AccessRules> AstarStrategy let probe_length = self.bihead_length() - prev_bihead_length; match result { - Ok(..) => { - self.trace.undo_step(&mut self.tracer); - Some(probe_length) - } + Ok(..) => Some(probe_length), Err(err) => { if let TracerException::CannotDraw(draw_err) = err { let layout_err = match draw_err { @@ -135,14 +132,18 @@ impl<'a, R: AccessRules> AstarStrategy LayoutException::AlreadyConnected(..) => return None, }; - self.last_ghosts = vec![ghost]; - self.last_obstacles = vec![obstacle]; + self.probe_ghosts = vec![ghost]; + self.probe_obstacles = vec![obstacle]; } None } } } + fn remove_probe(&mut self, _navmesh: &Navmesh) { + self.trace.undo_step(&mut self.tracer); + } + fn estimate_cost(&mut self, navmesh: &Navmesh, vertex: NavvertexIndex) -> f64 { let start_point = PrimitiveIndex::from(navmesh.node_weight(vertex).unwrap().node) .primitive(self.tracer.layout.drawing())