diff --git a/src/layers.rs b/src/layers.rs index 1fd2df3..19c977d 100644 --- a/src/layers.rs +++ b/src/layers.rs @@ -259,15 +259,14 @@ impl Layer { /// Find the closest point in the layer /// - /// This will continue until it finds a point in the layer - pub fn get_closest_point(&self, point: Vec2, delta: f32) -> Vec2 { - let mut step = 0; - loop { + /// This will stop after searching in circle of radius up to `delta` * `steps` distance + pub fn get_closest_point(&self, point: Vec2, delta: f32, steps: u32) -> Option { + for step in 0..=steps { if let Some((new_point, _)) = self.get_closest_point_inner(point, delta, step) { - return new_point; + return Some(new_point); } - step += 1; } + None } #[inline(always)] @@ -282,7 +281,6 @@ impl Layer { let angle = i as f32 * std::f32::consts::TAU / (sample * (step + 1)) as f32; let (x, y) = angle.sin_cos(); let new_point = point + vec2(x, y) * delta * step as f32; - let poly = if self.baked_polygons.is_none() { self.get_point_location_unit(new_point) } else { @@ -297,15 +295,16 @@ impl Layer { /// Find the closest point in the layer in the given direction /// - /// This will stop after going `delta` * 100 distance in the `towards` direction + /// This will stop after going `delta` * `steps` distance in the `towards` direction pub fn get_closest_point_towards( &self, point: Vec2, delta: f32, + steps: u32, towards: Vec2, ) -> Option { let direction = -(point - towards).normalize(); - for step in 0..100 { + for step in 0..steps { if let Some((new_point, _)) = self.get_closest_point_towards_inner(point, delta, direction, step) { @@ -778,49 +777,52 @@ mod tests { #[test] fn get_closest_point() { - let mesh = mesh_u_grid(); + let mut mesh = mesh_u_grid(); + mesh.steps = 100; + mesh.delta = 0.01; assert_eq!( - mesh.layers[0].get_closest_point(vec2(1.5, 1.5), 0.1), - vec2(1.5, 1.0) + mesh.layers[0].get_closest_point(vec2(1.5, 1.5), 0.1, 10), + Some(vec2(1.5, 1.0)) ); assert_eq!( - mesh.get_closest_point(vec2(1.5, 1.5), 0.1), - Coords { + mesh.get_closest_point(vec2(1.5, 1.5)), + Some(Coords { pos: vec2(1.5, 1.0), layer: Some(0), polygon_index: 1, - } + }) ); assert_eq!( - mesh.layers[0].get_closest_point(vec2(1.25, 1.5), 0.01), - vec2(1.25, 1.0) + mesh.layers[0].get_closest_point(vec2(1.25, 1.5), 0.01, 100), + Some(vec2(1.25, 1.0)) ); assert_eq!( - mesh.layers[1].get_closest_point(vec2(1.25, 1.5), 0.01), - vec2(1.0, 1.5) + mesh.layers[1].get_closest_point(vec2(1.25, 1.5), 0.01, 100), + Some(vec2(1.0, 1.5)) ); assert_eq!( - mesh.get_closest_point(vec2(1.25, 1.5), 0.01), - Coords { + mesh.get_closest_point(vec2(1.25, 1.5)), + Some(Coords { pos: vec2(1.0, 1.5), layer: Some(1), - polygon_index: 0, - } + polygon_index: U32Layer::from_layer_and_polygon(1, 0), + }) ); } #[test] fn get_closest_point_towards() { - let mesh = mesh_u_grid(); + let mut mesh = mesh_u_grid(); + mesh.steps = 10; assert_eq!( - mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.1, vec2(1.5, 0.5)), + mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.1, 10, vec2(1.5, 0.5)), Some(vec2(1.5, 1.0)) ); assert_eq!( - mesh.get_closest_point_towards(vec2(1.5, 1.5), 0.1, vec2(1.5, 0.5)), + mesh.get_closest_point_towards(vec2(1.5, 1.5), vec2(1.5, 0.5)), Some(Coords { pos: vec2(1.5, 1.0), layer: Some(0), @@ -829,20 +831,20 @@ mod tests { ); assert_eq!( - mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.1, vec2(0.5, 1.5)), + mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.1, 10, vec2(0.5, 1.5)), None ); assert_eq!( - mesh.get_closest_point_towards(vec2(1.5, 1.5), 0.1, vec2(0.5, 1.5)), + mesh.get_closest_point_towards(vec2(1.5, 1.5), vec2(0.5, 1.5)), Some(Coords { pos: vec2(1.0, 1.5), layer: Some(1), - polygon_index: 0, + polygon_index: U32Layer::from_layer_and_polygon(1, 0), }) ); assert_eq!( - mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.2, vec2(1.5, 0.5)), + mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.2, 10, vec2(1.5, 0.5)), Some(vec2(1.5, 0.9)) ); } diff --git a/src/lib.rs b/src/lib.rs index b916656..a35227b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -73,8 +73,10 @@ pub struct Path { pub struct Mesh { /// Layers of the NavMesh pub layers: Vec, - /// Variation used when checking if a point is in a mesh + /// Precision used when checking if a point is in a mesh pub delta: f32, + /// Number of steps before stopping searching for a point in a mesh + pub steps: u32, #[cfg(feature = "stats")] pub(crate) scenarios: Cell, } @@ -145,6 +147,7 @@ impl Default for Mesh { Self { layers: vec![], delta: 0.1, + steps: 2, #[cfg(feature = "stats")] scenarios: Cell::new(0), } @@ -252,19 +255,13 @@ impl Mesh { let starting_polygon_index = if from.polygon_index != u32::MAX { from.polygon_index } else { - self.get_point_location(from) + self.get_closest_point(from)?.polygon_index }; - if starting_polygon_index == u32::MAX { - return None; - } let ending_polygon = if to.polygon_index != u32::MAX { to.polygon_index } else { - self.get_point_location(to) + self.get_closest_point(to)?.polygon_index }; - if ending_polygon == u32::MAX { - return None; - } // TODO: fix islands detection with multiple layers, even if start and end are on the same layer if self.layers.len() == 1 { if let Some(islands) = self.layers[starting_polygon_index.layer() as usize] @@ -350,7 +347,7 @@ impl Mesh { } /// Set the delta for search with [`Mesh::path`], [`Mesh::get_path`], and [`Mesh::point_in_mesh`]. - /// A given point (x, y) will be searched in a square around a delimited by (x ± delta, y ± delta). + /// A given point P(x, y) will be searched in concentric circles around P of radius `delta` * ([`Mesh::steps`] - 1). /// /// Default is 0.1 pub fn set_delta(&mut self, delta: f32) -> &mut Self { @@ -359,6 +356,21 @@ impl Mesh { self } + /// The steps set by [`Mesh::set_steps`] + pub fn steps(&self) -> u32 { + self.steps + } + + /// Set the steps for search with [`Mesh::path`], [`Mesh::get_path`], and [`Mesh::point_in_mesh`]. + /// A given point P(x, y) will be searched in concentric circles around P of radius [`Mesh::delta`] * (`steps` - 1). + /// + /// Default is 2 + pub fn set_steps(&mut self, steps: u32) -> &mut Self { + assert!(steps != 0); + self.steps = steps; + self + } + #[cfg_attr(feature = "tracing", instrument(skip_all))] #[cfg(test)] fn successors(&self, node: SearchNode, to: Vec2) -> Vec { @@ -513,74 +525,79 @@ impl Mesh { /// Find the closest point in the mesh /// - /// This will continue until it finds a point in the layer - pub fn get_closest_point(&self, point: impl Into, delta: f32) -> Coords { + /// This will search in circles up to `Mesh::delta` * `Mesh::steps` distance away from the point + pub fn get_closest_point(&self, point: impl Into) -> Option { let point = point.into(); - let mut step = 0; if let Some(layer_index) = point.layer { - loop { - if let Some((new_point, polygon)) = self.layers[layer_index as usize] - .get_closest_point_inner(point.pos, delta, step) + let layer = &self.layers[layer_index as usize]; + for step in 0..self.steps { + if let Some((new_point, polygon)) = + layer.get_closest_point_inner(point.pos - layer.offset, self.delta, step) { - return Coords { - pos: new_point, + return Some(Coords { + pos: new_point + layer.offset, layer: Some(layer_index), - polygon_index: polygon, - }; + polygon_index: U32Layer::from_layer_and_polygon(layer_index, polygon), + }); } - step += 1; } } else { - loop { + for step in 0..self.steps { for (index, layer) in self.layers.iter().enumerate() { if let Some((new_point, polygon)) = - layer.get_closest_point_inner(point.pos, delta, step) + layer.get_closest_point_inner(point.pos - layer.offset, self.delta, step) { - return Coords { - pos: new_point, + return Some(Coords { + pos: new_point + layer.offset, layer: Some(index as u8), - polygon_index: polygon, - }; + polygon_index: U32Layer::from_layer_and_polygon(index as u8, polygon), + }); } } - step += 1; } } + None } /// Find the closest point in the mesh in the given direction /// - /// This will stop after going `delta` * 100 distance in the `towards` direction + /// This will search in a line up to `Mesh::delta` * `Mesh::steps` distance away from the point pub fn get_closest_point_towards( &self, point: impl Into, - delta: f32, towards: Vec2, ) -> Option { let point = point.into(); let direction = -(point.pos - towards).normalize(); if let Some(layer_index) = point.layer { - for step in 0..100 { - if let Some((new_point, polygon)) = self.layers[layer_index as usize] - .get_closest_point_towards_inner(point.pos, delta, direction, step) - { + let layer = &self.layers[layer_index as usize]; + for step in 0..self.steps { + if let Some((new_point, polygon)) = layer.get_closest_point_towards_inner( + point.pos - layer.offset, + self.delta, + direction, + step, + ) { return Some(Coords { - pos: new_point, + pos: new_point + layer.offset, layer: Some(layer_index), - polygon_index: polygon, + polygon_index: U32Layer::from_layer_and_polygon(layer_index, polygon), }); } } } else { - for step in 0..100 { + for step in 0..self.steps { for (index, layer) in self.layers.iter().enumerate() { - if let Some((new_point, polygon)) = - layer.get_closest_point_towards_inner(point.pos, delta, direction, step) - { + if let Some((new_point, polygon)) = layer.get_closest_point_towards_inner( + point.pos - layer.offset, + self.delta, + direction, + step, + ) { return Some(Coords { - pos: new_point, + pos: new_point + layer.offset, layer: Some(index as u8), - polygon_index: polygon, + polygon_index: U32Layer::from_layer_and_polygon(index as u8, polygon), }); } } @@ -1303,4 +1320,12 @@ mod tests { println!("{successor:?}"); } } + + #[test] + fn get_closest_point() { + let mesh = mesh_u_grid(); + let point_location = mesh.get_point_location(vec2(0.5, 0.5)); + let closest_point = mesh.get_closest_point(vec2(0.5, 0.5)).unwrap(); + assert_eq!(point_location, closest_point.polygon_index); + } }