berkus
/
topola
mirror of https://codeberg.org/topola/topola.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

router: re-encapsulate graph in `Navmesh` and vertices in `VertexIndex` 

As in previous commit, this was done in the past, but I removed it when
improving `Navmesh` to hold each navvertex instead of calculating tem
during access.
This commit is contained in:
Mikolaj Wielgus 2024-06-26 22:34:59 +02:00
parent 5254f768e5
commit 0419904b3f
3 changed files with 73 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/router/navmesh.rs
View File

@ -3,6 +3,7 @@ use std::collections::HashMap;
use enum_dispatch::enum_dispatch;
use geo::Point;
use petgraph::{
data::DataMap,
graph::UnGraph,
stable_graph::NodeIndex,
visit::{
@ -106,9 +107,9 @@ pub enum NavmeshError {
pub struct Navmesh {
graph: UnGraph<NavvertexWeight, (), usize>,
source: FixedDotIndex,
source_navvertex: NodeIndex<usize>,
source_navvertex: NavvertexIndex,
target: FixedDotIndex,
target_navvertex: NodeIndex<usize>,
target_navvertex: NavvertexIndex,
}
impl Navmesh {
@ -204,9 +205,9 @@ impl Navmesh {
Ok(Self {
graph,
source,
source_navvertex: source_navvertex.unwrap(),
source_navvertex: NavvertexIndex(source_navvertex.unwrap()),
target,
target_navvertex: target_navvertex.unwrap(),
target_navvertex: NavvertexIndex(target_navvertex.unwrap()),
})
}
@ -218,7 +219,7 @@ impl Navmesh {
self.source
}
pub fn source_navvertex(&self) -> NodeIndex<usize> {
pub fn source_navvertex(&self) -> NavvertexIndex {
self.source_navvertex
}
@ -226,7 +227,7 @@ impl Navmesh {
self.target
}
pub fn target_navvertex(&self) -> NodeIndex<usize> {
pub fn target_navvertex(&self) -> NavvertexIndex {
self.target_navvertex
}
}
@ -241,6 +242,16 @@ impl Data for Navmesh {
type EdgeWeight = ();
}
impl DataMap for Navmesh {
fn node_weight(&self, vertex: Self::NodeId) -> Option<&Self::NodeWeight> {
self.graph.node_weight(vertex.petgraph_index())
}
fn edge_weight(&self, _edge: Self::EdgeId) -> Option<&Self::EdgeWeight> {
None
}
}
#[derive(Debug, Clone, Copy)]
pub struct NavmeshEdgeReference {
from: NavvertexIndex,

42
src/router/router.rs
View File

@ -1,5 +1,6 @@
use geo::EuclideanDistance;
use petgraph::{
data::DataMap,
graph::{EdgeReference, NodeIndex, UnGraph},
visit::EdgeRef,
};
@ -20,7 +21,10 @@ use crate::{
router::{
astar::{astar, AstarError, AstarStrategy, PathTracker},
draw::DrawException,
navmesh::{BinavvertexNodeIndex, Navmesh, NavmeshError, NavvertexWeight},
navmesh::{
BinavvertexNodeIndex, Navmesh, NavmeshEdgeReference, NavmeshError, NavvertexIndex,
NavvertexWeight,
},
tracer::{Trace, Tracer},
},
};
@ -83,33 +87,29 @@ impl<'a, R: RulesTrait> RouterAstarStrategy<'a, R> {
}
}
impl<'a, R: RulesTrait> AstarStrategy<&UnGraph<NavvertexWeight, (), usize>, f64, BandFirstSegIndex>
impl<'a, R: RulesTrait> AstarStrategy<&Navmesh, f64, BandFirstSegIndex>
for RouterAstarStrategy<'a, R>
{
fn is_goal(
&mut self,
graph: &&UnGraph<NavvertexWeight, (), usize>,
vertex: NodeIndex<usize>,
tracker: &PathTracker<&UnGraph<NavvertexWeight, (), usize>>,
navmesh: &&Navmesh,
vertex: NavvertexIndex,
tracker: &PathTracker<&Navmesh>,
) -> Option<BandFirstSegIndex> {
let new_path = tracker.reconstruct_path_to(vertex);
let width = self.trace.width;
self.tracer
.rework_path(*graph, &mut self.trace, &new_path[..], width)
.rework_path(navmesh, &mut self.trace, &new_path[..], width)
.unwrap();
self.tracer
.finish(*graph, &mut self.trace, self.target, width)
.finish(navmesh, &mut self.trace, self.target, width)
.ok()
}
fn edge_cost(
&mut self,
graph: &&UnGraph<NavvertexWeight, (), usize>,
edge: EdgeReference<(), usize>,
) -> Option<f64> {
if edge.target() == self.target.petgraph_index() {
fn edge_cost(&mut self, navmesh: &&Navmesh, edge: NavmeshEdgeReference) -> Option<f64> {
if edge.target().petgraph_index() == self.target.petgraph_index() {
return None;
}
@ -118,24 +118,20 @@ impl<'a, R: RulesTrait> AstarStrategy<&UnGraph<NavvertexWeight, (), usize>, f64,
let width = self.trace.width;
let result = self
.tracer
.step(*graph, &mut self.trace, edge.target(), width);
.step(navmesh, &mut self.trace, edge.target(), width);
let probe_length = self.bihead_length() - prev_bihead_length;
if result.is_ok() {
self.tracer.undo_step(*graph, &mut self.trace);
self.tracer.undo_step(navmesh, &mut self.trace);
Some(probe_length)
} else {
None
}
}
fn estimate_cost(
&mut self,
graph: &&UnGraph<NavvertexWeight, (), usize>,
vertex: NodeIndex<usize>,
) -> f64 {
let start_point = PrimitiveIndex::from(graph.node_weight(vertex).unwrap().node)
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();
@ -172,14 +168,14 @@ impl Router {
) -> Result<BandFirstSegIndex, RouterError> {
let mut tracer = Tracer::new(layout);
let trace = tracer.start(
self.navmesh.graph(),
&self.navmesh,
self.navmesh.source(),
self.navmesh.source_navvertex(),
width,
);
let (_cost, _path, band) = astar(
self.navmesh.graph(),
&self.navmesh,
self.navmesh.source_navvertex(),
&mut RouterAstarStrategy::new(tracer, trace, self.navmesh.target()),
)?;

88
src/router/tracer.rs
View File

@ -1,5 +1,8 @@
use contracts::{debug_ensures, debug_requires};
use petgraph::graph::{NodeIndex, UnGraph};
use petgraph::{
data::DataMap,
graph::{NodeIndex, UnGraph},
};
use thiserror::Error;
use crate::{
@ -14,7 +17,7 @@ use crate::{
layout::Layout,
router::{
draw::{Draw, DrawException},
navmesh::{BinavvertexNodeIndex, NavvertexWeight},
navmesh::{BinavvertexNodeIndex, Navmesh, NavvertexIndex, NavvertexWeight},
},
};
@ -28,7 +31,7 @@ pub enum TracerException {
#[derive(Debug)]
pub struct Trace {
pub path: Vec<NodeIndex<usize>>,
pub path: Vec<NavvertexIndex>,
pub head: Head,
pub width: f64,
}
@ -45,9 +48,9 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
pub fn start(
&mut self,
_graph: &UnGraph<NavvertexWeight, (), usize>,
_navmesh: &Navmesh,
source: FixedDotIndex,
source_navvertex: NodeIndex<usize>,
source_navvertex: NavvertexIndex,
width: f64,
) -> Trace {
Trace {
@ -59,7 +62,7 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
pub fn finish(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
_navmesh: &Navmesh,
trace: &mut Trace,
target: FixedDotIndex,
width: f64,
@ -71,9 +74,9 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
#[debug_ensures(ret.is_ok() -> trace.path.len() == path.len())]
pub fn rework_path(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navmesh: &Navmesh,
trace: &mut Trace,
path: &[NodeIndex<usize>],
path: &[NavvertexIndex],
width: f64,
) -> Result<(), TracerException> {
let prefix_length = trace
@ -84,21 +87,21 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
.count();
let length = trace.path.len();
self.undo_path(graph, trace, length - prefix_length);
Ok::<(), TracerException>(self.path(graph, trace, &path[prefix_length..], width)?)
self.undo_path(navmesh, trace, length - prefix_length);
Ok::<(), TracerException>(self.path(navmesh, trace, &path[prefix_length..], width)?)
}
#[debug_ensures(ret.is_ok() -> trace.path.len() == old(trace.path.len() + path.len()))]
pub fn path(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navmesh: &Navmesh,
trace: &mut Trace,
path: &[NodeIndex<usize>],
path: &[NavvertexIndex],
width: f64,
) -> Result<(), TracerException> {
for (i, vertex) in path.iter().enumerate() {
if let Err(err) = self.step(graph, trace, *vertex, width) {
self.undo_path(graph, trace, i);
if let Err(err) = self.step(navmesh, trace, *vertex, width) {
self.undo_path(navmesh, trace, i);
return Err(err.into());
}
}
@ -107,14 +110,9 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
}
#[debug_ensures(trace.path.len() == old(trace.path.len() - step_count))]
pub fn undo_path(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
trace: &mut Trace,
step_count: usize,
) {
pub fn undo_path(&mut self, navmesh: &Navmesh, trace: &mut Trace, step_count: usize) {
for _ in 0..step_count {
self.undo_step(graph, trace);
self.undo_step(navmesh, trace);
}
}
@ -123,12 +121,12 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
#[debug_ensures(ret.is_err() -> trace.path.len() == old(trace.path.len()))]
pub fn step(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navmesh: &Navmesh,
trace: &mut Trace,
to: NodeIndex<usize>,
to: NavvertexIndex,
width: f64,
) -> Result<(), TracerException> {
trace.head = self.wrap(graph, trace.head, to, width)?.into();
trace.head = self.wrap(navmesh, trace.head, to, width)?.into();
trace.path.push(to);
Ok::<(), TracerException>(())
@ -136,29 +134,29 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
fn wrap(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navmesh: &Navmesh,
head: Head,
around: NodeIndex<usize>,
around: NavvertexIndex,
width: f64,
) -> Result<CaneHead, TracerException> {
let cw = self
.maybe_cw(graph, around)
.maybe_cw(navmesh, around)
.ok_or(TracerException::CannotWrap)?;
match self.binavvertex(graph, around) {
match self.binavvertex(navmesh, around) {
BinavvertexNodeIndex::FixedDot(dot) => {
self.wrap_around_fixed_dot(graph, head, dot, cw, width)
self.wrap_around_fixed_dot(navmesh, head, dot, cw, width)
}
BinavvertexNodeIndex::FixedBend(_fixed_bend) => todo!(),
BinavvertexNodeIndex::LooseBend(loose_bend) => {
self.wrap_around_loose_bend(graph, head, loose_bend, cw, width)
self.wrap_around_loose_bend(navmesh, head, loose_bend, cw, width)
}
}
}
fn wrap_around_fixed_dot(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
_navmesh: &Navmesh,
head: Head,
around: FixedDotIndex,
cw: bool,
@ -169,7 +167,7 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
fn wrap_around_loose_bend(
&mut self,
graph: &UnGraph<NavvertexWeight, (), usize>,
_navmesh: &Navmesh,
head: Head,
around: LooseBendIndex,
cw: bool,
@ -179,7 +177,7 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
}
#[debug_ensures(trace.path.len() == old(trace.path.len() - 1))]
pub fn undo_step(&mut self, graph: &UnGraph<NavvertexWeight, (), usize>, trace: &mut Trace) {
pub fn undo_step(&mut self, _navmesh: &Navmesh, trace: &mut Trace) {
if let Head::Cane(head) = trace.head {
trace.head = Draw::new(self.layout).undo_cane(head).unwrap();
} else {
@ -189,27 +187,15 @@ impl<'a, R: RulesTrait> Tracer<'a, R> {
trace.path.pop();
}
fn maybe_cw(
&self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navvertex: NodeIndex<usize>,
) -> Option<bool> {
graph.node_weight(navvertex).unwrap().maybe_cw
fn maybe_cw(&self, navmesh: &Navmesh, navvertex: NavvertexIndex) -> Option<bool> {
navmesh.node_weight(navvertex).unwrap().maybe_cw
}
fn binavvertex(
&self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navvertex: NodeIndex<usize>,
) -> BinavvertexNodeIndex {
graph.node_weight(navvertex).unwrap().node
fn binavvertex(&self, navmesh: &Navmesh, navvertex: NavvertexIndex) -> BinavvertexNodeIndex {
navmesh.node_weight(navvertex).unwrap().node
}
fn primitive(
&self,
graph: &UnGraph<NavvertexWeight, (), usize>,
navvertex: NodeIndex<usize>,
) -> PrimitiveIndex {
self.binavvertex(graph, navvertex).into()
fn primitive(&self, navmesh: &Navmesh, navvertex: NavvertexIndex) -> PrimitiveIndex {
self.binavvertex(navmesh, navvertex).into()
}
}