From 274ad166c19f394ac60c29d7ba53eafe821e5343 Mon Sep 17 00:00:00 2001 From: Mikolaj Wielgus Date: Thu, 3 Jul 2025 21:57:49 +0200 Subject: [PATCH] feat(router/navmesh): Add constraint edges for fixed segs --- src/router/navmesh.rs | 16 +++++++++- src/triangulation.rs | 72 +++++++++++++++++++++++-------------------- 2 files changed, 53 insertions(+), 35 deletions(-) diff --git a/src/router/navmesh.rs b/src/router/navmesh.rs index 963b6d8..283a079 100644 --- a/src/router/navmesh.rs +++ b/src/router/navmesh.rs @@ -24,7 +24,7 @@ use crate::{ dot::FixedDotIndex, gear::{GearIndex, GetNextGear}, graph::{GetMaybeNet, MakePrimitive, PrimitiveIndex}, - primitive::{MakePrimitiveShape, Primitive}, + primitive::{GetJoints, MakePrimitiveShape, Primitive}, rules::AccessRules, Drawing, }, @@ -192,6 +192,20 @@ impl Navmesh { pos: primitive.shape().center(), })?; } + PrimitiveIndex::FixedSeg(seg) => { + let (from_dot, to_dot) = layout.drawing().primitive(seg).joints(); + + triangulation.add_constraint_edge( + TrianvertexWeight { + node: from_dot.into(), + pos: from_dot.primitive(layout.drawing()).shape().center(), + }, + TrianvertexWeight { + node: to_dot.into(), + pos: to_dot.primitive(layout.drawing()).shape().center(), + }, + )?; + } PrimitiveIndex::FixedBend(bend) => { triangulation.add_vertex(TrianvertexWeight { node: bend.into(), diff --git a/src/triangulation.rs b/src/triangulation.rs index 423a9c7..c2c4cab 100644 --- a/src/triangulation.rs +++ b/src/triangulation.rs @@ -7,7 +7,9 @@ use std::{cmp::Ordering, marker::PhantomData}; use geo::algorithm::line_measures::{Distance, Euclidean}; use geo::{point, Point}; use petgraph::visit; -use spade::{handles::FixedVertexHandle, DelaunayTriangulation, HasPosition, InsertionError}; +use spade::{ + handles::FixedVertexHandle, ConstrainedDelaunayTriangulation, HasPosition, InsertionError, +}; use crate::graph::GetPetgraphIndex; @@ -15,9 +17,12 @@ pub trait GetTrianvertexNodeIndex { fn node_index(&self) -> I; } -#[derive(Debug, Clone)] +// No `Debug` derive because `spade::ConstrainedDelaunayTriangulation` does +// not implement `Debug`, though it could (and `spade::DelaunayTriangulation` +// actually does). +#[derive(Clone)] pub struct Triangulation + HasPosition, EW: Default> { - triangulation: DelaunayTriangulation, + cdt: ConstrainedDelaunayTriangulation, trianvertex_to_handle: Box<[Option]>, index_marker: PhantomData, } @@ -27,7 +32,7 @@ impl + HasPosition, EW: Defa { pub fn new(node_bound: usize) -> Self { Self { - triangulation: as spade::Triangulation>::new(), + cdt: as spade::Triangulation>::new(), trianvertex_to_handle: vec![None; node_bound].into_boxed_slice(), index_marker: PhantomData, } @@ -35,27 +40,29 @@ impl + HasPosition, EW: Defa pub fn add_vertex(&mut self, weight: VW) -> Result<(), InsertionError> { let index = weight.node_index().petgraph_index().index(); - self.trianvertex_to_handle[index] = Some(spade::Triangulation::insert( - &mut self.triangulation, - weight, - )?); + self.trianvertex_to_handle[index] = + Some(spade::Triangulation::insert(&mut self.cdt, weight)?); Ok(()) } + pub fn add_constraint_edge(&mut self, from: VW, to: VW) -> Result { + self.cdt.add_constraint_edge(from, to) + } + pub fn weight(&self, vertex: I) -> &VW { - spade::Triangulation::s(&self.triangulation) + spade::Triangulation::s(&self.cdt) .vertex_data(self.trianvertex_to_handle[vertex.petgraph_index().index()].unwrap()) } pub fn weight_mut(&mut self, vertex: I) -> &mut VW { spade::Triangulation::vertex_data_mut( - &mut self.triangulation, + &mut self.cdt, self.trianvertex_to_handle[vertex.petgraph_index().index()].unwrap(), ) } fn vertex(&self, handle: FixedVertexHandle) -> I { - spade::Triangulation::vertex(&self.triangulation, handle) + spade::Triangulation::vertex(&self.cdt, handle) .as_ref() .node_index() } @@ -68,8 +75,7 @@ impl + HasPosition, EW: Defa where ::Scalar: geo::CoordNum, { - let position = - spade::Triangulation::vertex(&self.triangulation, self.handle(vertex)).position(); + let position = spade::Triangulation::vertex(&self.cdt, self.handle(vertex)).position(); point! {x: position.x, y: position.y} } } @@ -152,7 +158,7 @@ impl< fn neighbors(self, vertex: Self::NodeId) -> Self::Neighbors { Box::new( - spade::Triangulation::vertex(&self.triangulation, self.handle(vertex)) + spade::Triangulation::vertex(&self.cdt, self.handle(vertex)) .out_edges() .map(|handle| self.vertex(handle.to().fix())), ) @@ -170,21 +176,19 @@ impl< type EdgeReferences = Box> + 'a>; fn edge_references(self) -> Self::EdgeReferences { - Box::new( - spade::Triangulation::directed_edges(&self.triangulation).map(|edge| { - let from = self.vertex(edge.from().fix()); - let to = self.vertex(edge.to().fix()); + Box::new(spade::Triangulation::directed_edges(&self.cdt).map(|edge| { + let from = self.vertex(edge.from().fix()); + let to = self.vertex(edge.to().fix()); - TriangulationEdgeReference { - from, - to, - weight: TriangulationEdgeWeightWrapper { - length: Euclidean::distance(&self.position(from), &self.position(to)), - weight: *edge.data(), - }, - } - }), - ) + TriangulationEdgeReference { + from, + to, + weight: TriangulationEdgeWeightWrapper { + length: Euclidean::distance(&self.position(from), &self.position(to)), + weight: *edge.data(), + }, + } + })) } } @@ -199,7 +203,7 @@ impl< fn edges(self, node: Self::NodeId) -> Self::Edges { Box::new( - spade::Triangulation::vertex(&self.triangulation, self.handle(node)) + spade::Triangulation::vertex(&self.cdt, self.handle(node)) .out_edges() .map(|edge| { let from = self.vertex(edge.from().fix()); @@ -229,8 +233,8 @@ impl< fn node_identifiers(self) -> Self::NodeIdentifiers { Box::new( - spade::Triangulation::fixed_vertices(&self.triangulation).map(|vertex| { - spade::Triangulation::s(&self.triangulation) + spade::Triangulation::fixed_vertices(&self.cdt).map(|vertex| { + spade::Triangulation::s(&self.cdt) .vertex_data(vertex) .node_index() }), @@ -280,8 +284,8 @@ impl< fn node_references(self) -> Self::NodeReferences { Box::new( - spade::Triangulation::fixed_vertices(&self.triangulation).map(|vertex| { - let weight = spade::Triangulation::s(&self.triangulation).vertex_data(vertex); + spade::Triangulation::fixed_vertices(&self.cdt).map(|vertex| { + let weight = spade::Triangulation::s(&self.cdt).vertex_data(vertex); TriangulationVertexReference { index: weight.node_index(), weight, @@ -307,7 +311,7 @@ impl< } fn from_index(&self, index: usize) -> I { - spade::Triangulation::s(&self.triangulation) + spade::Triangulation::s(&self.cdt) .vertex_data(self.trianvertex_to_handle[index].unwrap()) .node_index() }