diff --git a/src/board/board.rs b/src/board/board.rs index b906efe..14bf3cb 100644 --- a/src/board/board.rs +++ b/src/board/board.rs @@ -15,7 +15,7 @@ use crate::{ Layout, NodeIndex, }, math::Circle, - router::{navmesh::Navmesh, Route, RouterError}, + router::{navmesh::Navmesh, route::Route, Router, RouterError}, }; #[derive(Debug)] @@ -129,8 +129,8 @@ impl Board { .unwrap() .to_string(); - let mut router = Route::new_from_navmesh(self.layout_mut(), navmesh, 100.0); - let result = router.route_band(self.layout_mut(), 100.0); + let mut router = Router::new(self.layout_mut()); + let result = router.route(navmesh.source(), navmesh.target(), width); if let Ok(band) = result { self.pinname_pair_to_band diff --git a/src/router/mod.rs b/src/router/mod.rs index 063932a..a6391b5 100644 --- a/src/router/mod.rs +++ b/src/router/mod.rs @@ -2,7 +2,8 @@ pub mod astar; pub mod draw; pub mod navmesh; pub mod route; +pub mod router; pub mod trace; pub mod tracer; -//pub use router::*; +pub use router::*; diff --git a/src/router/route.rs b/src/router/route.rs index b067faa..6f3fe5b 100644 --- a/src/router/route.rs +++ b/src/router/route.rs @@ -27,141 +27,28 @@ use crate::{ }, trace::Trace, tracer::Tracer, + Router, RouterAstarStrategy, RouterError, RouterStatus, }, }; -#[derive(Error, Debug, Clone)] -#[error("routing failed")] -pub enum RouterError { - Navmesh(#[from] NavmeshError), - Astar(#[from] AstarError), -} - pub struct Route { astar: Astar, trace: Trace, } -struct RouterAstarStrategy<'a, R: RulesTrait> { - pub tracer: Tracer<'a, R>, - pub trace: &'a mut Trace, - pub target: FixedDotIndex, -} - -impl<'a, R: RulesTrait> RouterAstarStrategy<'a, R> { - pub fn new(tracer: Tracer<'a, R>, trace: &'a mut Trace, target: FixedDotIndex) -> Self { - Self { - tracer, - trace, - target, - } - } - - fn bihead_length(&self) -> f64 { - self.head_length(&self.trace.head) - + match self.trace.head.face() { - DotIndex::Fixed(..) => 0.0, - DotIndex::Loose(face) => { - self.head_length(&self.tracer.layout.drawing().guide().rear_head(face)) - } - } - } - - fn head_length(&self, head: &Head) -> f64 { - match head { - Head::Bare(..) => 0.0, - Head::Cane(cane_head) => { - self.tracer - .layout - .drawing() - .primitive(cane_head.cane.seg) - .shape() - .length() - + self - .tracer - .layout - .drawing() - .primitive(cane_head.cane.bend) - .shape() - .length() - } - } - } -} - -impl<'a, R: RulesTrait> AstarStrategy - for RouterAstarStrategy<'a, R> -{ - fn is_goal( - &mut self, - navmesh: &Navmesh, - vertex: NavvertexIndex, - tracker: &PathTracker, - ) -> Option { - let new_path = tracker.reconstruct_path_to(vertex); - let width = self.trace.width; - - self.tracer - .rework_path(navmesh, &mut self.trace, &new_path[..], width) - .unwrap(); - - self.tracer - .finish(navmesh, &mut self.trace, self.target, width) - .ok() - } - - fn edge_cost(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option { - if edge.target().petgraph_index() == self.target.petgraph_index() { - return None; - } - - let prev_bihead_length = self.bihead_length(); - - let width = self.trace.width; - let result = self - .trace - .step(&mut self.tracer, navmesh, edge.target(), width); - - let probe_length = self.bihead_length() - prev_bihead_length; - - if result.is_ok() { - self.trace.undo_step(&mut self.tracer); - Some(probe_length) - } else { - None - } - } - - 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()) - .shape() - .center(); - let end_point = self - .tracer - .layout - .drawing() - .primitive(self.target) - .shape() - .center(); - - end_point.euclidean_distance(&start_point) - } -} - impl Route { pub fn new( - layout: &mut Layout, + router: &mut Router, from: FixedDotIndex, to: FixedDotIndex, width: f64, ) -> Result { - let navmesh = Navmesh::new(layout, from, to)?; - Ok(Self::new_from_navmesh(layout, navmesh, width)) + let navmesh = Navmesh::new(router.layout(), from, to)?; + Ok(Self::new_from_navmesh(router, navmesh, width)) } pub fn new_from_navmesh( - layout: &mut Layout, + router: &mut Router, navmesh: Navmesh, width: f64, ) -> Self { @@ -169,7 +56,7 @@ impl Route { let source_navvertex = navmesh.source_navvertex(); let target = navmesh.target(); - let mut tracer = Tracer::new(layout); + let mut tracer = Tracer::new(router.layout_mut()); let mut trace = tracer.start(source, source_navvertex, width); let mut strategy = RouterAstarStrategy::new(tracer, &mut trace, target); @@ -178,24 +65,17 @@ impl Route { Self { astar, trace } } - pub fn route_band( + pub fn step( &mut self, - layout: &mut Layout, - _width: f64, - ) -> Result { - let tracer = Tracer::new(layout); + router: &mut Router, + ) -> Result { + let tracer = Tracer::new(router.layout_mut()); let target = self.astar.graph.target(); let mut strategy = RouterAstarStrategy::new(tracer, &mut self.trace, target); - loop { - let status = match self.astar.step(&mut strategy) { - Ok(status) => status, - Err(err) => return Err(err.into()), - }; - - if let AstarStatus::Finished(_cost, _path, band) = status { - return Ok(band); - } + match self.astar.step(&mut strategy)? { + AstarStatus::Running => Ok(RouterStatus::Running), + AstarStatus::Finished(_cost, _path, band) => Ok(RouterStatus::Finished(band)), } } } diff --git a/src/router/router.rs b/src/router/router.rs new file mode 100644 index 0000000..18c6a63 --- /dev/null +++ b/src/router/router.rs @@ -0,0 +1,190 @@ +use geo::EuclideanDistance; +use petgraph::{data::DataMap, visit::EdgeRef}; +use thiserror::Error; + +use crate::{ + drawing::{ + band::BandFirstSegIndex, + dot::{DotIndex, FixedDotIndex}, + graph::{MakePrimitive, PrimitiveIndex}, + guide::{Head, HeadTrait}, + primitive::MakePrimitiveShape, + rules::RulesTrait, + }, + geometry::{primitive::PrimitiveShapeTrait, shape::ShapeTrait}, + graph::GetPetgraphIndex, + layout::Layout, + router::{ + astar::{AstarError, AstarStatus, AstarStrategy, PathTracker}, + navmesh::{Navmesh, NavmeshEdgeReference, NavmeshError, NavvertexIndex}, + route::Route, + trace::Trace, + tracer::Tracer, + }, +}; + +#[derive(Error, Debug, Clone)] +#[error("routing failed")] +pub enum RouterError { + Navmesh(#[from] NavmeshError), + Astar(#[from] AstarError), +} + +pub enum RouterStatus { + Running, + Finished(BandFirstSegIndex), +} + +pub struct RouterAstarStrategy<'a, R: RulesTrait> { + pub tracer: Tracer<'a, R>, + pub trace: &'a mut Trace, + pub target: FixedDotIndex, +} + +impl<'a, R: RulesTrait> RouterAstarStrategy<'a, R> { + pub fn new(tracer: Tracer<'a, R>, trace: &'a mut Trace, target: FixedDotIndex) -> Self { + Self { + tracer, + trace, + target, + } + } + + fn bihead_length(&self) -> f64 { + self.head_length(&self.trace.head) + + match self.trace.head.face() { + DotIndex::Fixed(..) => 0.0, + DotIndex::Loose(face) => { + self.head_length(&self.tracer.layout.drawing().guide().rear_head(face)) + } + } + } + + fn head_length(&self, head: &Head) -> f64 { + match head { + Head::Bare(..) => 0.0, + Head::Cane(cane_head) => { + self.tracer + .layout + .drawing() + .primitive(cane_head.cane.seg) + .shape() + .length() + + self + .tracer + .layout + .drawing() + .primitive(cane_head.cane.bend) + .shape() + .length() + } + } + } +} + +impl<'a, R: RulesTrait> AstarStrategy + for RouterAstarStrategy<'a, R> +{ + fn is_goal( + &mut self, + navmesh: &Navmesh, + vertex: NavvertexIndex, + tracker: &PathTracker, + ) -> Option { + let new_path = tracker.reconstruct_path_to(vertex); + let width = self.trace.width; + + self.tracer + .rework_path(navmesh, &mut self.trace, &new_path[..], width) + .unwrap(); + + self.tracer + .finish(navmesh, &mut self.trace, self.target, width) + .ok() + } + + fn edge_cost(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option { + if edge.target().petgraph_index() == self.target.petgraph_index() { + return None; + } + + let prev_bihead_length = self.bihead_length(); + + let width = self.trace.width; + let result = self + .trace + .step(&mut self.tracer, navmesh, edge.target(), width); + + let probe_length = self.bihead_length() - prev_bihead_length; + + if result.is_ok() { + self.trace.undo_step(&mut self.tracer); + Some(probe_length) + } else { + None + } + } + + 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()) + .shape() + .center(); + let end_point = self + .tracer + .layout + .drawing() + .primitive(self.target) + .shape() + .center(); + + end_point.euclidean_distance(&start_point) + } +} + +pub struct Router<'a, R: RulesTrait> { + layout: &'a mut Layout, +} + +impl<'a, R: RulesTrait> Router<'a, R> { + pub fn new(layout: &'a mut Layout) -> Self { + Self { layout } + } + + pub fn route( + &mut self, + from: FixedDotIndex, + to: FixedDotIndex, + width: f64, + ) -> Result { + let mut route = self.route_walk(from, to, width)?; + + loop { + let status = match route.step(self) { + Ok(status) => status, + Err(err) => return Err(err), + }; + + if let RouterStatus::Finished(band) = status { + return Ok(band); + } + } + } + + pub fn route_walk( + &mut self, + from: FixedDotIndex, + to: FixedDotIndex, + width: f64, + ) -> Result { + Route::new(self, from, to, width) + } + + pub fn layout_mut(&mut self) -> &mut Layout { + &mut self.layout + } + + pub fn layout(&self) -> &Layout { + &self.layout + } +}