Implement trimesh import (#21)

* Implement trimesh import

* Make clippy happy

* Write tests

* Write tests

* Finish tests

* Fix test message

* Fix coords in test

* Fix some clockwise checks

* Fix tests

* Remove duplicated code

* Remove duplicated code

* Add docs

* Remove unused dep

* Fix naming

* Start working under assumption of triangle vertex order

* Remove redundant code

* Remove redundant code

* Fix wrong indices being used

* Add comments

* Make clippy happy

* Improve docs

* Make delta configurable

* Fix missing field in initializer

* Simplify type

* Use modulo instead of branch

Co-authored-by: François <mockersf@gmail.com>

* Use modulo instead of branch

Co-authored-by: François <mockersf@gmail.com>

Co-authored-by: François <mockersf@gmail.com>

src/lib.rs

@@ -39,10 +39,12 @@ mod async_helpers;
 mod helpers;
 mod instance;
 mod primitives;
+mod trimesh;
 
 #[cfg(feature = "async")]
 pub use async_helpers::FuturePath;
 pub use primitives::{Polygon, Vertex};
+pub use trimesh::Triangle;
 
 use crate::instance::SearchInstance;
 
@@ -56,18 +58,33 @@ pub struct Path {
 }
 
 /// A navigation mesh
-#[derive(Debug, Default, Clone)]
+#[derive(Debug, Clone)]
 pub struct Mesh {
     /// List of `Vertex` in this mesh
     pub vertices: Vec<Vertex>,
     /// List of `Polygons` in this mesh
     pub polygons: Vec<Polygon>,
     baked_polygons: Option<BVH2d>,
     islands: Option<Vec<usize>>,
+    delta: f32,
     #[cfg(feature = "stats")]
     pub(crate) scenarios: Cell<u32>,
 }
 
+impl Default for Mesh {
+    fn default() -> Self {
+        Self {
+            delta: 0.1,
+            vertices: Default::default(),
+            polygons: Default::default(),
+            baked_polygons: Default::default(),
+            islands: Default::default(),
+            #[cfg(feature = "stats")]
+            scenarios: Cell::new(0),
+        }
+    }
+}
+
 struct Root(Vec2);
 
 impl PartialEq for Root {
@@ -187,10 +204,7 @@ impl Mesh {
         let mut mesh = Mesh {
             vertices,
             polygons,
-            baked_polygons: None,
-            islands: None,
-            #[cfg(feature = "stats")]
-            scenarios: Cell::new(0),
+            ..Default::default()
         };
         #[cfg(not(feature = "no-default-baking"))]
         mesh.bake();
@@ -346,6 +360,21 @@ impl Mesh {
         }
     }
 
+    /// The delta set by [`Mesh::set_delta`]
+    pub fn delta(&self) -> f32 {
+        self.delta
+    }
+
+    /// 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).
+    ///
+    /// Default is 0.1
+    pub fn set_delta(&mut self, delta: f32) -> &mut Self {
+        assert!(delta >= 0.0);
+        self.delta = delta;
+        self
+    }
+
     #[cfg_attr(feature = "tracing", instrument(skip_all))]
     #[cfg(test)]
     fn successors(&self, node: SearchNode, to: Vec2) -> Vec<SearchNode> {
@@ -422,7 +451,7 @@ impl Mesh {
 
     #[cfg_attr(feature = "tracing", instrument(skip_all))]
     fn get_point_location(&self, point: Vec2) -> u32 {
-        let delta = 0.1;
+        let delta = self.delta;
         [
             Vec2::new(0.0, 0.0),
             Vec2::new(delta, 0.0),
@@ -585,10 +614,7 @@ mod tests {
                 Polygon::new(vec![4, 5, 9, 8], true),
                 Polygon::new(vec![6, 7, 11, 10], true),
             ],
-            baked_polygons: None,
-            islands: None,
-            #[cfg(feature = "stats")]
-            scenarios: std::cell::Cell::new(0),
+            ..Default::default()
         }
     }
 
@@ -807,10 +833,7 @@ mod tests {
                 Polygon::new(vec![15, 18, 19, 16], true),
                 Polygon::new(vec![11, 17, 20, 21], true),
             ],
-            baked_polygons: None,
-            islands: None,
-            #[cfg(feature = "stats")]
-            scenarios: std::cell::Cell::new(0),
+            ..Default::default()
         }
     }
 

src/trimesh.rs

@@ -0,0 +1,248 @@
+use crate::{Mesh, Polygon, Vertex};
+use glam::Vec2;
+use std::cmp::Ordering;
+use std::iter;
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+/// A triangle described by the indices of three vertices passed to [`Mesh::from_trimesh`] in counterclockwise order
+pub struct Triangle(pub [usize; 3]);
+
+impl From<(usize, usize, usize)> for Triangle {
+    fn from((a, b, c): (usize, usize, usize)) -> Self {
+        Self([a, b, c])
+    }
+}
+impl From<[usize; 3]> for Triangle {
+    fn from(vertices: [usize; 3]) -> Self {
+        Self(vertices)
+    }
+}
+
+impl From<&Polygon> for Triangle {
+    fn from(value: &Polygon) -> Self {
+        let vertices = &value.vertices;
+        [vertices[0], vertices[1], vertices[2]]
+            .map(|index| index as usize)
+            .into()
+    }
+}
+
+impl Triangle {
+    fn contains(self, index: usize) -> bool {
+        self.0.contains(&index)
+    }
+
+    fn get_clockwise_neighbor(self, index: usize) -> usize {
+        let position = self.position(index);
+        self.0[(position + 1) % 3]
+    }
+
+    fn get_counterclockwise_neighbor(self, index: usize) -> usize {
+        let position = self.position(index);
+        self.0[(position + 2) % 3]
+    }
+    fn position(self, index: usize) -> usize {
+        self.0.into_iter().position(|i| i == index).unwrap()
+    }
+}
+
+impl Mesh {
+    /// Convert a mesh composed of [`Triangle`]s to a [`Mesh`]. Behaves like [`Mesh::new`], but does not require
+    /// any information about vertex or polygon neighbors.
+    pub fn from_trimesh(vertices: Vec<Vec2>, triangles: Vec<Triangle>) -> Self {
+        let mut vertices: Vec<_> = to_vertices(vertices, &triangles);
+        let polygons = to_polygons(triangles);
+        let unordered_vertices = vertices.clone();
+
+        // Order vertex polygon neighbors counterclockwise
+        for (vertex_index, vertex) in vertices.iter_mut().enumerate() {
+            vertex.polygons.sort_by(|index_a, index_b| {
+                let get_counterclockwise_edge = |index: isize| {
+                    // No -1 present yet, so the unwrap is safe
+                    let index = usize::try_from(index).unwrap();
+                    let neighbor_index = Triangle::from(&polygons[index])
+                        .get_counterclockwise_neighbor(vertex_index);
+                    let neighbor = &unordered_vertices[neighbor_index];
+                    neighbor.coords - vertex.coords
+                };
+                let edge_a = get_counterclockwise_edge(*index_a);
+                let edge_b = get_counterclockwise_edge(*index_b);
+                if edge_a.perp_dot(edge_b) > 0. {
+                    Ordering::Less
+                } else {
+                    Ordering::Greater
+                }
+            });
+
+            // Add obstacles (-1) as vertex neighbors
+            let mut polygons_including_obstacles = vec![vertex.polygons[0]];
+            for polygon_index in vertex
+                .polygons
+                .iter()
+                .cloned()
+                .skip(1)
+                .chain(iter::once(polygons_including_obstacles[0]))
+            {
+                let last_index = *polygons_including_obstacles.last().unwrap();
+                if last_index == -1 {
+                    polygons_including_obstacles.push(polygon_index);
+                    continue;
+                }
+                let triangle_at = |index: isize| {
+                    let polygon = &polygons[usize::try_from(index).unwrap()];
+                    Triangle::from(polygon)
+                };
+
+                let last_counterclockwise_neighbor =
+                    triangle_at(last_index).get_counterclockwise_neighbor(vertex_index);
+                let next_clockwise_neighbor =
+                    triangle_at(polygon_index).get_clockwise_neighbor(vertex_index);
+
+                if last_counterclockwise_neighbor != next_clockwise_neighbor {
+                    // The edges don't align; there's an obstacle here
+                    polygons_including_obstacles.push(-1);
+                }
+                polygons_including_obstacles.push(polygon_index);
+            }
+            // The first element is included in the end again
+            polygons_including_obstacles.remove(0);
+            vertex.polygons = polygons_including_obstacles;
+        }
+        // Recreate vertices because we now include obstacles, so vertices can now be properly identified as edges
+        let vertices: Vec<_> = vertices
+            .into_iter()
+            .map(|vertex| Vertex::new(vertex.coords, vertex.polygons))
+            .collect();
+        Self::new(vertices, polygons)
+    }
+}
+
+fn to_vertices(vertices: Vec<Vec2>, triangles: &[Triangle]) -> Vec<Vertex> {
+    vertices
+        .into_iter()
+        .enumerate()
+        .map(|(vertex_index, coords)| {
+            let neighbor_indices = triangles
+                .iter()
+                .enumerate()
+                .filter_map(|(polygon_index, vertex_indices_in_polygon)| {
+                    vertex_indices_in_polygon
+                        .contains(vertex_index)
+                        .then_some(polygon_index)
+                })
+                .map(|index| isize::try_from(index).unwrap())
+                .collect();
+            Vertex::new(coords, neighbor_indices)
+        })
+        .collect()
+}
+
+fn to_polygons(triangles: Vec<Triangle>) -> Vec<Polygon> {
+    let triangle_count = triangles.len();
+
+    triangles
+        .into_iter()
+        .map(|vertex_indices_in_polygon| {
+            // It's geometrically impossible for a triangle to have only one neighbor in a trimesh,
+            // except for the trivial case
+            let is_one_way = triangle_count == 1;
+            Polygon::new(
+                vertex_indices_in_polygon
+                    .0
+                    .map(|index| index as u32)
+                    .to_vec(),
+                is_one_way,
+            )
+        })
+        .collect()
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn generation_from_trimesh_is_same_as_regular() {
+        let regular_mesh = Mesh::new(
+            vec![
+                Vertex::new(Vec2::new(1., 1.), vec![0, 4, -1]), // 0
+                Vertex::new(Vec2::new(5., 1.), vec![-1, 1, 3, -1, 0]), // 1
+                Vertex::new(Vec2::new(5., 4.), vec![-1, 2, 1]), // 2
+                Vertex::new(Vec2::new(1., 4.), vec![-1, 4, -1, 3, 2]), // 3
+                Vertex::new(Vec2::new(2., 2.), vec![-1, 4, 0]), // 4
+                Vertex::new(Vec2::new(4., 3.), vec![1, 2, 3]),  // 5
+            ],
+            vec![
+                Polygon::new(vec![0, 1, 4], false), // 0
+                Polygon::new(vec![1, 2, 5], false), // 1
+                Polygon::new(vec![5, 2, 3], false), // 2
+                Polygon::new(vec![1, 5, 3], false), // 3
+                Polygon::new(vec![0, 4, 3], false), // 4
+            ],
+        );
+        let from_trimesh = Mesh::from_trimesh(
+            vec![
+                Vec2::new(1., 1.),
+                Vec2::new(5., 1.),
+                Vec2::new(5., 4.),
+                Vec2::new(1., 4.),
+                Vec2::new(2., 2.),
+                Vec2::new(4., 3.),
+            ],
+            vec![
+                (0, 1, 4).into(),
+                (1, 2, 5).into(),
+                (5, 2, 3).into(),
+                (1, 5, 3).into(),
+                (0, 4, 3).into(),
+            ],
+        );
+        assert_eq!(regular_mesh.polygons, from_trimesh.polygons);
+        for (index, (expected_vertex, actual_vertex)) in regular_mesh
+            .vertices
+            .iter()
+            .zip(from_trimesh.vertices.iter())
+            .enumerate()
+        {
+            assert_eq!(
+                expected_vertex.coords, actual_vertex.coords,
+                "\nvertex {index} does not have the expected coords.\nExpected vertices: {0:?}\nGot vertices: {1:?}",
+                regular_mesh.vertices, from_trimesh.vertices
+            );
+
+            assert_eq!(
+                expected_vertex.is_corner, actual_vertex.is_corner,
+                "\nvertex {index} does not have the expected value for `is_corner`.\nExpected vertices: {0:?}\nGot vertices: {1:?}",
+                regular_mesh.vertices, from_trimesh.vertices
+            );
+            let adjusted_actual = wrap_to_first(&actual_vertex.polygons, |index| *index != -1).unwrap_or_else(||
+                panic!("vertex {index}: Found only surrounded by obstacles.\nExpected vertices: {0:?}\nGot vertices: {1:?}",
+                       regular_mesh.vertices, from_trimesh.vertices));
+
+            let adjusted_expectation= wrap_to_first(&expected_vertex.polygons, |polygon| {
+                *polygon == adjusted_actual[0]
+            })
+                .unwrap_or_else(||
+                    panic!("vertex {index}: Failed to expected polygons.\nExpected vertices: {0:?}\nGot vertices: {1:?}",
+                           regular_mesh.vertices, from_trimesh.vertices));
+
+            assert_eq!(
+                adjusted_expectation, adjusted_actual,
+                "\nvertex {index} does not have the expected polygons.\nExpected vertices: {0:?}\nGot vertices: {1:?}",
+                regular_mesh.vertices, from_trimesh.vertices
+            );
+        }
+    }
+
+    fn wrap_to_first(polygons: &[isize], pred: impl Fn(&isize) -> bool) -> Option<Vec<isize>> {
+        let offset = polygons.iter().position(pred)?;
+        Some(
+            polygons
+                .iter()
+                .skip(offset)
+                .chain(polygons.iter().take(offset))
+                .cloned()
+                .collect(),
+        )
+    }
+}