Skip to content

Commit

Permalink
path through a navmesh with layers with their own offsets
Browse files Browse the repository at this point in the history
  • Loading branch information
@mockersf
mockersf committed Aug 8, 2024
1 parent 585fbc5 commit 4f4ac81
Show file tree
Hide file tree
Showing 2 changed files with 114 additions and 14 deletions.
42 changes: 30 additions & 12 deletions src/instance.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -358,9 +358,10 @@ impl<'m> SearchInstance<'m> {
[node.edge.1 as usize]
.coords
+ self.mesh.layers[node.previous_polygon_layer as usize].offset;
while target_layer.vertices[polygon.vertices[temp] as usize].coords
+ target_layer.offset
!= edge
while (target_layer.vertices[polygon.vertices[temp] as usize].coords
+ target_layer.offset)
.distance_squared(edge)
> 0.001
{
temp += 1;
}
Expand Down Expand Up @@ -649,7 +650,14 @@ impl<'m> SearchInstance<'m> {

#[cfg(debug_assertions)]
if self.debug {
println!("| going to {other_side:?}");
match other_side {
&u32::MAX => println!("| going to u32::MAX"),
_ => println!(
"| going to {:?} / {:?}",
other_side.layer(),
other_side.polygon()
),
}
}

// prune edges that don't have a polygon on the other side: cul de sac pruning
Expand Down Expand Up @@ -696,7 +704,7 @@ impl<'m> SearchInstance<'m> {
{
#[cfg(debug_assertions)]
if self.debug {
println!("x non observable on an intersection");
println!("x non observable on an intersection (right)");
}
continue;
}
Expand All @@ -709,9 +717,9 @@ impl<'m> SearchInstance<'m> {
&& vertex.polygons.iter().any(|p| {
*p == u32::MAX || self.blocked_layers.contains(&p.layer())
})))
&& vertex
.coords
.distance_squared(node.interval.0 + target_layer.offset)
&& (vertex.coords
+ self.mesh.layers[node.previous_polygon_layer as usize].offset)
.distance_squared(node.interval.0)
< EPSILON
{
node.interval.0
Expand All @@ -725,10 +733,12 @@ impl<'m> SearchInstance<'m> {
}
SuccessorType::Observable => node.root,
SuccessorType::LeftNonObservable => {
if successor.interval.1.distance_squared(end.coords) > EPSILON {
if (successor.interval.1).distance_squared(end.coords + target_layer.offset)
> EPSILON
{
#[cfg(debug_assertions)]
if self.debug {
println!("x non observable on an intersection");
println!("x non observable on an intersection (left)");
}
continue;
}
Expand All @@ -741,7 +751,10 @@ impl<'m> SearchInstance<'m> {
&& vertex.polygons.iter().any(|p| {
*p == u32::MAX || self.blocked_layers.contains(&p.layer())
})))
&& vertex.coords.distance_squared(node.interval.1) < EPSILON
&& (vertex.coords
+ self.mesh.layers[node.previous_polygon_layer as usize].offset)
.distance_squared(node.interval.1)
< EPSILON
{
node.interval.1
} else {
Expand Down Expand Up @@ -771,7 +784,12 @@ impl<'m> SearchInstance<'m> {
if self.node_buffer.len() == 1 && self.node_buffer[0].polygon_to != self.polygon_to {
#[cfg(feature = "verbose")]
for new_node in &self.node_buffer {
println!(" intermediate: {}", new_node);
println!(
" intermediate: {} -> to polygon {}/{}",
new_node,
new_node.polygon_to.layer(),
new_node.polygon_to.polygon()
);
}
node = self.node_buffer.drain(..).next().unwrap();
#[cfg(debug_assertions)]
Expand Down
86 changes: 84 additions & 2 deletions src/stitching.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,7 @@ impl Mesh {

#[cfg(test)]
mod tests {
use crate::{Layer, Mesh, Path, Polygon, Vertex};
use crate::{Layer, Mesh, Path, Polygon, Triangulation, Vertex};
use glam::{vec2, Vec2};

fn basic_mesh_with_layers() -> Mesh {
Expand Down Expand Up @@ -692,6 +692,88 @@ mod tests {
#[cfg(feature = "detailed-layers")]
path_with_layers: vec![(vec2(1.0, 0.5), 1), (vec2(1.5, 0.5), 1)],
}
)
);
let path = mesh.path(vec2(1.5, 0.5), vec2(0.5, 0.5)).unwrap();
assert_eq!(
path,
Path {
length: 1.0,
path: vec![vec2(0.5, 0.5)],
#[cfg(feature = "detailed-layers")]
path_with_layers: vec![(vec2(1.0, 0.5), 0), (vec2(0.5, 0.5), 0)],
}
);
}

#[test]
fn path_with_obstacle_stitch_layers_different_coordinates() {
let base_mesh = layers_different_coordinates();

let mut triangulation_a = Triangulation::from_mesh(&base_mesh, 0);
let mut triangulation_b = Triangulation::from_mesh(&base_mesh, 1);
for obstacle in [
// on the boundary
vec![
vec2(0.75, 0.25),
vec2(0.75, 0.75),
vec2(1.25, 0.75),
vec2(1.25, 0.25),
],
// on layer 0
vec![vec2(0.0, 0.0), vec2(0.0, 0.25), vec2(0.25, 0.0)],
vec![vec2(0.0, 1.0), vec2(0.0, 0.75), vec2(0.25, 1.0)],
// on layer 1
vec![vec2(2.0, 0.0), vec2(2.0, 0.25), vec2(1.75, 0.0)],
vec![vec2(2.0, 2.0), vec2(2.0, 1.75), vec2(1.75, 2.0)],
] {
triangulation_a.add_obstacle(obstacle.clone());
triangulation_b
.add_obstacle(obstacle.into_iter().map(|v| v - vec2(1.0, 0.0)).collect());
}
let mut mesh = Mesh::default();
let mut layer_a = triangulation_a.as_layer();
layer_a.remove_useless_vertices();
mesh.layers.push(layer_a);
let mut layer_b = triangulation_b.as_layer();
layer_b.remove_useless_vertices();
layer_b.offset = vec2(1.0, 0.0);
mesh.layers.push(layer_b);

for layer in &mesh.layers {
println!("====================");
println!(
"{:?}",
layer.vertices.iter().map(|v| v.coords).collect::<Vec<_>>()
);
println!(
"{:?}",
layer
.polygons
.iter()
.map(|p| &p.vertices)
.collect::<Vec<_>>()
);
}

let indices_from = mesh.layers[0].get_vertices_on_segment(vec2(1.0, 0.0), vec2(1.0, 1.0));
let indices_to = mesh.layers[1].get_vertices_on_segment(vec2(0.0, 0.0), vec2(0.0, 1.0));

let stitch_indices = indices_from
.into_iter()
.zip(indices_to.into_iter())
.collect();

mesh.stitch_at_vertices(vec![((0, 1), stitch_indices)], false);

let path = mesh.path(vec2(0.5, 0.5), vec2(1.5, 0.5)).unwrap();
assert_eq!(
path.path,
vec![vec2(0.75, 0.75), vec2(1.25, 0.75), vec2(1.5, 0.5)]
);
let path = mesh.path(vec2(1.5, 0.5), vec2(0.5, 0.5)).unwrap();
assert_eq!(
path.path,
vec![vec2(1.25, 0.75), vec2(0.75, 0.75), vec2(0.5, 0.5)]
);
}
}

0 comments on commit 4f4ac81

Please sign in to comment.