diff --git a/src/router/navcord.rs b/src/router/navcord.rs index d33ab46..148ea77 100644 --- a/src/router/navcord.rs +++ b/src/router/navcord.rs @@ -18,7 +18,7 @@ use crate::{ use super::{ draw::Draw, navcorder::{Navcorder, NavcorderException}, - navmesh::{NavigableNodeIndex, Navmesh, NavnodeIndex}, + navmesh::{NavigableNodeIndex, Navmesh, NavnodeIndex, NavnodeWeight}, }; /// The `Navcord` is a data structure that holds the movable non-borrowing data @@ -106,17 +106,20 @@ impl Navcord { navmesh: &Navmesh, to: NavnodeIndex, ) -> Result<(), NavcorderException> { + let to_node_weight = navmesh.node_weight(to).unwrap(); + if to == navmesh.destination_navnode() { - let to_node_weight = navmesh.node_weight(to).unwrap(); let NavigableNodeIndex::FixedDot(to_dot) = to_node_weight.node() else { unreachable!(); }; - self.final_termseg = Some(layout.finish(navmesh, self, to_dot).unwrap()); + self.final_termseg = Some(layout.finish(navmesh, self, to_dot)?); // NOTE: We don't update the head here because there is currently // no head variant that consists only of a seg, and I'm not sure if // there should be one. + } else if matches!(to_node_weight, NavnodeWeight::Leap(..)) { + // Nothing to do beyond pushing the navnode onto the path. } else { self.head = self.wrap(layout, navmesh, self.head, to)?.into(); } @@ -134,10 +137,15 @@ impl Navcord { pub fn step_back( &mut self, layout: &mut Layout, + navmesh: &Navmesh, ) -> Result<(), NavcorderException> { + let last_node_weight = navmesh.node_weight(*self.path.last().unwrap()); + if let Some(final_termseg) = self.final_termseg { layout.remove_termseg(&mut self.recorder, final_termseg); self.final_termseg = None; + } else if matches!(last_node_weight, Some(NavnodeWeight::Leap(..))) { + // Nothing to do beyond popping the navnode from the path. } else { if let Head::Cane(head) = self.head { self.head = layout.undo_cane(&mut self.recorder, head).unwrap(); diff --git a/src/router/navcorder.rs b/src/router/navcorder.rs index d37426f..1f6f589 100644 --- a/src/router/navcorder.rs +++ b/src/router/navcorder.rs @@ -54,7 +54,7 @@ pub trait Navcorder { path: &[NavnodeIndex], ) -> Result<(), NavcorderException>; - fn undo_path(&mut self, navcord: &mut Navcord, step_count: usize); + fn undo_path(&mut self, navmesh: &Navmesh, navcord: &mut Navcord, step_count: usize); } impl Navcorder for Layout { @@ -93,7 +93,7 @@ impl Navcorder for Layout { .count(); let length = navcord.path.len(); - self.undo_path(navcord, length - prefix_length); + self.undo_path(navmesh, navcord, length - prefix_length); self.path(navmesh, navcord, &path[prefix_length..]) } @@ -106,7 +106,7 @@ impl Navcorder for Layout { ) -> Result<(), NavcorderException> { for (i, vertex) in path.iter().enumerate() { if let Err(err) = navcord.step_to(self, navmesh, *vertex) { - self.undo_path(navcord, i); + self.undo_path(navmesh, navcord, i); return Err(err); } } @@ -115,9 +115,9 @@ impl Navcorder for Layout { } #[debug_ensures(navcord.path.len() == old(navcord.path.len() - step_count))] - fn undo_path(&mut self, navcord: &mut Navcord, step_count: usize) { + fn undo_path(&mut self, navmesh: &Navmesh, navcord: &mut Navcord, step_count: usize) { for _ in 0..step_count { - let _ = navcord.step_back(self); + let _ = navcord.step_back(self, navmesh); } } } diff --git a/src/router/route.rs b/src/router/route.rs index 6477013..5890c03 100644 --- a/src/router/route.rs +++ b/src/router/route.rs @@ -90,7 +90,7 @@ impl Step, BandTermsegIndex> for RouteStepper { // NOTE(fogti): The 1 instead 0 is because the first element in the path // is the source navnode. See also: `NavcordStepper::new`. for _ in 1..self.navcord.path.len() { - self.navcord.step_back(layout); + self.navcord.step_back(layout, &self.astar.graph); } Err(e) } diff --git a/src/router/router.rs b/src/router/router.rs index 5e8074a..0d1f862 100644 --- a/src/router/router.rs +++ b/src/router/router.rs @@ -136,8 +136,8 @@ impl AstarStrategy for RouterAst } } - fn remove_probe(&mut self, _navmesh: &Navmesh) { - self.navcord.step_back(self.layout); + fn remove_probe(&mut self, navmesh: &Navmesh) { + self.navcord.step_back(self.layout, navmesh); } fn estimate_cost(&mut self, navmesh: &Navmesh, vertex: NavnodeIndex) -> f64 {