Skip to content

Commit

Permalink
helper function to find a point in the navmesh
Browse files Browse the repository at this point in the history
  • Loading branch information
@mockersf
mockersf committed Sep 21, 2024
1 parent 6dcd1c2 commit eb7b60e
Show file tree
Hide file tree
Showing 2 changed files with 260 additions and 27 deletions.
180 changes: 155 additions & 25 deletions src/layers.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -256,11 +256,88 @@ impl Layer {
});
vertices
}

/// 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 {
if let Some((new_point, _)) = self.get_closest_point_inner(point, delta, step) {
return new_point;
}
step += 1;
}
}

#[inline(always)]
pub(crate) fn get_closest_point_inner(
&self,
point: Vec2,
delta: f32,
step: u32,
) -> Option<(Vec2, u32)> {
let sample = 10;
for i in 0..=(sample * step) {
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 {
self.get_point_location_unit_baked(new_point)
};
if poly != u32::MAX {
return Some((new_point, poly));
}
}
return None;
}

/// Find the closest point in the layer in the given direction
///
/// This will stop after going `delta` * 100 distance in the `towards` direction
pub fn get_closest_point_towards(
&self,
point: Vec2,
delta: f32,
towards: Vec2,
) -> Option<Vec2> {
let direction = -(point - towards).normalize();
for step in 0..100 {
if let Some((new_point, _)) =
self.get_closest_point_towards_inner(point, delta, direction, step)
{
return Some(new_point);
}
}
return None;
}

pub(crate) fn get_closest_point_towards_inner(
&self,
point: Vec2,
delta: f32,
direction: Vec2,
step: u32,
) -> Option<(Vec2, u32)> {
let point = point + direction * delta * step as f32;
let poly = if self.baked_polygons.is_none() {
self.get_point_location_unit(point)
} else {
self.get_point_location_unit_baked(point)
};
if poly != u32::MAX {
return Some((point, poly));
}
return None;
}
}

#[cfg(test)]
mod tests {
use std::collections::HashSet;
use std::{collections::HashSet, u32};

#[cfg(feature = "detailed-layers")]
use crate::helpers::line_intersect_segment;
Expand Down Expand Up @@ -633,14 +710,8 @@ mod tests {
let mesh = mesh_overlapping_layers();
let path = dbg!(mesh
.path(
Coords {
pos: vec2(2.5, 1.5),
layer: Some(0)
},
Coords {
pos: vec2(2.5, 1.5),
layer: Some(1)
},
Coords::on_layer(vec2(2.5, 1.5), 0),
Coords::on_layer(vec2(2.5, 1.5), 1),
)
.unwrap());
assert_eq!(
Expand All @@ -659,14 +730,8 @@ mod tests {

let path_back = dbg!(mesh
.path(
Coords {
pos: vec2(2.5, 1.5),
layer: Some(1)
},
Coords {
pos: vec2(2.5, 1.5),
layer: Some(0)
},
Coords::on_layer(vec2(2.5, 1.5), 1),
Coords::on_layer(vec2(2.5, 1.5), 0),
)
.unwrap());
assert_eq!(
Expand All @@ -688,17 +753,11 @@ mod tests {
fn find_point_on_layer() {
let mesh = mesh_overlapping_layers();
assert_eq!(
mesh.get_point_location(Coords {
pos: vec2(2.5, 1.5),
layer: Some(0)
}),
mesh.get_point_location(Coords::on_layer(vec2(2.5, 1.5), 0)),
1
);
assert_eq!(
mesh.get_point_location(Coords {
pos: vec2(2.5, 1.5),
layer: Some(1)
}),
mesh.get_point_location(Coords::on_layer(vec2(2.5, 1.5), 1)),
u32::from_layer_and_polygon(1, 0)
);
}
Expand All @@ -715,4 +774,75 @@ mod tests {
vec![0, 1, 2, 3]
);
}

#[test]
fn get_closest_point() {
let mesh = mesh_u_grid();

assert_eq!(
mesh.layers[0].get_closest_point(vec2(1.5, 1.5), 0.1),
vec2(1.5, 1.0)
);
assert_eq!(
mesh.get_closest_point(vec2(1.5, 1.5), 0.1),
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)
);
assert_eq!(
mesh.layers[1].get_closest_point(vec2(1.25, 1.5), 0.01),
vec2(1.0, 1.5)
);
assert_eq!(
mesh.get_closest_point(vec2(1.25, 1.5), 0.01),
Coords {
pos: vec2(1.0, 1.5),
layer: Some(1),
polygon_index: 0,
}
);
}

#[test]
fn get_closest_point_towards() {
let mesh = mesh_u_grid();

assert_eq!(
mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.1, 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)),
Some(Coords {
pos: vec2(1.5, 1.0),
layer: Some(0),
polygon_index: 1,
})
);

assert_eq!(
mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.1, 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)),
Some(Coords {
pos: vec2(1.0, 1.5),
layer: Some(1),
polygon_index: 0,
})
);

assert_eq!(
mesh.layers[0].get_closest_point_towards(vec2(1.5, 1.5), 0.2, vec2(1.5, 0.5)),
Some(vec2(1.5, 0.9))
);
}
}
107 changes: 105 additions & 2 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,13 +88,29 @@ pub struct Coords {
///
/// If specified, the point will be searched in that layer only.
pub layer: Option<u8>,
/// internal: this coords have been built by a search on the mesh that found the polygon index
/// if used for a path, this will be used directly instead of searching for it again in the mesh
/// default value is u32::MAX which means it hasn't been searched
polygon_index: u32,
}

impl From<Vec2> for Coords {
fn from(value: Vec2) -> Self {
Coords {
pos: value,
layer: None,
polygon_index: u32::MAX,
}
}
}

impl Coords {
/// A point on the navigation mesh on the specified layer
pub fn on_layer(pos: Vec2, layer: u8) -> Self {
Coords {
pos,
layer: Some(layer),
polygon_index: u32::MAX,
}
}
}
Expand Down Expand Up @@ -218,11 +234,19 @@ impl Mesh {
let from = from.into();
let to = to.into();

let starting_polygon_index = self.get_point_location(from);
let starting_polygon_index = if from.polygon_index != u32::MAX {
from.polygon_index
} else {
self.get_point_location(from)
};
if starting_polygon_index == u32::MAX {
return None;
}
let ending_polygon = self.get_point_location(to);
let ending_polygon = if to.polygon_index != u32::MAX {
to.polygon_index
} else {
self.get_point_location(to)
};
if ending_polygon == u32::MAX {
return None;
}
Expand Down Expand Up @@ -410,6 +434,7 @@ impl Mesh {
.map(|p| Coords {
pos: coords.pos,
layer: Some(p.layer()),
polygon_index: *p,
})
.collect()
}
Expand Down Expand Up @@ -470,6 +495,84 @@ impl Mesh {
.collect()
}
}

/// 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 {
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)
{
return Coords {
pos: new_point,
layer: Some(layer_index),
polygon_index: polygon,
};
}
step += 1;
}
} else {
loop {
for (index, layer) in self.layers.iter().enumerate() {
if let Some((new_point, polygon)) =
layer.get_closest_point_inner(point.pos, delta, step)
{
return Coords {
pos: new_point,
layer: Some(index as u8),
polygon_index: polygon,
};
}
}
step += 1;
}
}
}

/// Find the closest point in the mesh in the given direction
///
/// This will stop after going `delta` * 100 distance in the `towards` direction
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)
{
return Some(Coords {
pos: new_point,
layer: Some(layer_index),
polygon_index: polygon,
});
}
}
} else {
for step in 0..100 {
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)
{
return Some(Coords {
pos: new_point,
layer: Some(index as u8),
polygon_index: polygon,
});
}
}
}
}
return None;
}
}

#[derive(PartialEq, Debug)]
Expand Down

0 comments on commit eb7b60e

Please sign in to comment.