From 437e2f96d1955ea28769c8cb3ac2ad8449a4e525 Mon Sep 17 00:00:00 2001 From: Mikolaj Wielgus Date: Thu, 5 Jun 2025 22:57:00 +0200 Subject: [PATCH] fix(router/navcord): Correctly take into account length of final termseg --- src/router/navcord.rs | 56 ++++++++++++++++++++++++++++++++++------- src/router/router.rs | 24 +++--------------- src/router/thetastar.rs | 4 +-- 3 files changed, 52 insertions(+), 32 deletions(-) diff --git a/src/router/navcord.rs b/src/router/navcord.rs index f745d72..0b65119 100644 --- a/src/router/navcord.rs +++ b/src/router/navcord.rs @@ -9,9 +9,13 @@ use crate::{ drawing::{ band::BandTermsegIndex, dot::FixedDotIndex, + graph::MakePrimitive, head::{BareHead, CaneHead, Head}, + primitive::MakePrimitiveShape, rules::AccessRules, }, + geometry::shape::MeasureLength, + graph::MakeRef, layout::{Layout, LayoutEdit}, }; @@ -39,7 +43,7 @@ pub struct Navcord { /// The head of the currently routed band. pub head: Head, /// If the band is finished, stores the termseg that was used to finish it. - pub final_termseg: Option, + pub maybe_final_termseg: Option, /// The width of the currently routed band. pub width: f64, } @@ -56,7 +60,7 @@ impl Navcord { recorder, path: vec![source_navnode], head: BareHead { face: source }.into(), - final_termseg: None, + maybe_final_termseg: None, width, } } @@ -98,6 +102,8 @@ impl Navcord { /// Advance the navcord and the currently routed band by one step to the /// navnode `to`. + /// + /// Returns the length of the created track. #[debug_ensures(ret.is_ok() -> self.path.len() == old(self.path.len() + 1))] #[debug_ensures(ret.is_err() -> self.path.len() == old(self.path.len()))] pub fn step_to( @@ -105,28 +111,60 @@ impl Navcord { layout: &mut Layout, navmesh: &Navmesh, to: NavnodeIndex, - ) -> Result<(), NavcorderException> { - if to == navmesh.destination_navnode() { + ) -> 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 { unreachable!(); }; - self.final_termseg = Some(layout.finish(navmesh, self, to_dot)?); + let final_termseg = layout.finish(navmesh, self, to_dot)?; + self.maybe_final_termseg = Some(final_termseg); + + let final_termseg_length = final_termseg.primitive(layout.drawing()).shape().length(); + + let bend_length = match self.head { + Head::Cane(old_cane_head) => layout + .drawing() + .primitive(old_cane_head.cane.bend) + .shape() + .length(), + Head::Bare(..) => 0.0, + }; + + final_termseg_length + bend_length // 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 { + let old_head = self.head; + self.head = self.wrap(layout, navmesh, self.head, to)?.into(); - } + + let prev_bend_length = match old_head { + Head::Cane(old_cane_head) => layout + .drawing() + .primitive(old_cane_head.cane.bend) + .shape() + .length(), + Head::Bare(..) => 0.0, + }; + + prev_bend_length + // NOTE: the probe's bend length is always 0 here because such is + // the initial state of a cane (before getting extended, but this + // is never done for probes). So we could as well only measure the + // seg's length. + + self.head.ref_(layout.drawing()).length() + }; // Now that the new part of the trace has been created, push the // navnode `to` onto the currently attempted path to start from it // on the next `.step_to(...)` call or retreat from it later using // `.step_back(...)`. self.path.push(to); - Ok(()) + Ok(length) } /// Retreat the navcord and the currently routed band by one step. @@ -135,9 +173,9 @@ impl Navcord { &mut self, layout: &mut Layout, ) -> Result<(), NavcorderException> { - if let Some(final_termseg) = self.final_termseg { + if let Some(final_termseg) = self.maybe_final_termseg { layout.remove_termseg(&mut self.recorder, final_termseg); - self.final_termseg = None; + self.maybe_final_termseg = None; } else { if let Head::Cane(head) = self.head { self.head = layout.undo_cane(&mut self.recorder, head).unwrap(); diff --git a/src/router/router.rs b/src/router/router.rs index dc18504..98cb6d4 100644 --- a/src/router/router.rs +++ b/src/router/router.rs @@ -79,14 +79,14 @@ impl ThetastarStrategy // Set navcord members for consistency. The code would probably work // without this, since A* will terminate now anyway. - self.navcord.final_termseg = Some( + self.navcord.maybe_final_termseg = Some( self.layout .finish(navmesh, self.navcord, self.target) .map_err(|_| ())?, ); self.navcord.path.push(navnode); - Ok(self.navcord.final_termseg) + Ok(self.navcord.maybe_final_termseg) } else { self.layout .rework_path(navmesh, self.navcord, &new_path[..]) @@ -99,28 +99,10 @@ impl ThetastarStrategy navmesh: &Navmesh, probed_navnode: NavnodeIndex, ) -> Option { - let old_head = self.navcord.head; let result = self.navcord.step_to(self.layout, navmesh, probed_navnode); - let prev_bend_length = match old_head { - Head::Cane(old_cane_head) => self - .layout - .drawing() - .primitive(old_cane_head.cane.bend) - .shape() - .length(), - Head::Bare(..) => 0.0, - }; - - let probe_length = prev_bend_length - // NOTE: the probe's bend length is always 0 here because such is - // the initial state of a cane (before getting extended, but this - // is never done for probes). So we could as well only measure the - // seg's length. - + self.navcord.head.ref_(self.layout.drawing()).length(); - match result { - Ok(..) => Some(probe_length), + Ok(probe_length) => Some(probe_length), Err(err) => { if let NavcorderException::CannotDraw(draw_err) = err { let layout_err = match draw_err { diff --git a/src/router/thetastar.rs b/src/router/thetastar.rs index f7c9236..84cf10c 100644 --- a/src/router/thetastar.rs +++ b/src/router/thetastar.rs @@ -313,13 +313,13 @@ where } } ThetastarState::VisitingProbeOnNavedge(visited_navnode, curr_navedge) => { - let node_score = self.scores[&visited_navnode]; + let visited_score = self.scores[&visited_navnode]; let to_navnode = (&self.graph).edge_ref(curr_navedge).target(); if let Some(navedge_cost) = strategy.place_probe_to_navnode(&self.graph, to_navnode) { let next = to_navnode; - let next_score = node_score + navedge_cost; + let next_score = visited_score + navedge_cost; match self.scores.entry(next) { Entry::Occupied(mut entry) => {