Skip to content

Commit

Permalink
when getting path, search in circle instead of a square
Browse files Browse the repository at this point in the history
  • Loading branch information
@mockersf
mockersf committed Sep 21, 2024
1 parent 5bf2bb4 commit 032bd2f
Show file tree
Hide file tree
Showing 2 changed files with 99 additions and 72 deletions.
62 changes: 32 additions & 30 deletions src/layers.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Vec2> {
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)]
Expand All @@ -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 {
Expand All @@ -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<Vec2> {
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)
{
Expand Down Expand Up @@ -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),
Expand All @@ -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))
);
}
Expand Down
109 changes: 67 additions & 42 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,10 @@ pub struct Path {
pub struct Mesh {
/// Layers of the NavMesh
pub layers: Vec<Layer>,
/// 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<u32>,
}
Expand Down Expand Up @@ -145,6 +147,7 @@ impl Default for Mesh {
Self {
layers: vec![],
delta: 0.1,
steps: 2,
#[cfg(feature = "stats")]
scenarios: Cell::new(0),
}
Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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 {
Expand All @@ -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<SearchNode> {
Expand Down Expand Up @@ -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<Coords>, 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<Coords>) -> Option<Coords> {
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<Coords>,
delta: f32,
towards: Vec2,
) -> Option<Coords> {
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),
});
}
}
Expand Down Expand Up @@ -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);
}
}

0 comments on commit 032bd2f

Please sign in to comment.