diff --git a/src/instance.rs b/src/instance.rs index d1f7089..4885d3d 100644 --- a/src/instance.rs +++ b/src/instance.rs @@ -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; } @@ -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 @@ -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; } @@ -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 @@ -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; } @@ -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 { @@ -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)] diff --git a/src/stitching.rs b/src/stitching.rs index 7823e1a..948d91e 100644 --- a/src/stitching.rs +++ b/src/stitching.rs @@ -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 { @@ -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::>() + ); + println!( + "{:?}", + layer + .polygons + .iter() + .map(|p| &p.vertices) + .collect::>() + ); + } + + 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)] + ); } }