Add Hertel-Mehlhorn algorithm

src/shape/compound.rs

@@ -5,7 +5,11 @@
 use crate::bounding_volume::{BoundingSphere, BoundingVolume, AABB};
 use crate::math::{Isometry, Real};
 use crate::partitioning::QBVH;
+#[cfg(feature = "dim2")]
+use crate::shape::{ConvexPolygon, TriMesh, Triangle};
 use crate::shape::{Shape, SharedShape, SimdCompositeShape, TypedSimdCompositeShape};
+#[cfg(feature = "dim2")]
+use crate::transformation::hertel_mehlhorn;
 
 /// A compound shape with an aabb bounding volume.
 ///
@@ -59,6 +63,29 @@ impl Compound {
             aabb,
         }
     }
+
+    #[cfg(feature = "dim2")]
+    /// Create a compound shape from the `TriMesh`. This involves merging adjacent triangles into convex
+    /// polygons using the Hertel-Mehlhorn algorithm.
+    ///
+    /// Can fail and return `None` if any of the created shapes has close to zero or zero surface area.
+    pub fn decompose_trimesh(&self, trimesh: &TriMesh) -> Option<Self> {
+        let polygons = hertel_mehlhorn(trimesh.vertices(), trimesh.indices());
+        let shapes: Option<Vec<_>> = polygons
+            .into_iter()
+            .map(|points| {
+                match points.len() {
+                    3 => {
+                        let triangle = Triangle::new(points[0], points[1], points[2]);
+                        Some(SharedShape::new(triangle))
+                    }
+                    _ => ConvexPolygon::from_convex_polyline(points).map(SharedShape::new),
+                }
+                .map(|shape| (Isometry::identity(), shape))
+            })
+            .collect();
+        Some(Self::new(shapes?))
+    }
 }
 
 impl Compound {

src/shape/trimesh.rs

@@ -75,7 +75,7 @@ impl TriMesh {
         }
     }
 
-    /// Create a trimesh from a set of points assumed to describe a counter-clockwise non-convex polygon.
+    /// Create a `TriMesh` from a set of points assumed to describe a counter-clockwise non-convex polygon.
     ///
     /// This operation may fail if the input polygon is invalid, e.g. it is non-simple or has zero surface area.
     #[cfg(feature = "dim2")]

src/transformation/hertel_mehlhorn.rs

@@ -0,0 +1,172 @@
+use crate::math::{Point, Real};
+use crate::utils::point_in_triangle::{corner_direction, Orientation};
+
+/// Checks if the counter-clockwise polygon `poly` has an edge going counter-clockwise from `p1` to `p2`.
+/// Returns the edge point's indices in the second polygon. Returns `None` if none were found.
+fn find_edge_index_in_polygon(p1: u32, p2: u32, indices: &[u32]) -> Option<(usize, usize)> {
+    for i1 in 0..indices.len() {
+        let i2 = (i1 + 1) % indices.len();
+        if p1 == indices[i1] && p2 == indices[i2] {
+            return Some((i1, i2));
+        }
+    }
+    None
+}
+
+/// The Hertel-Mehlhorn algorithm.
+///
+/// Takes a set of triangles and returns a vector of convex polygons.
+///
+/// Time/Space complexity: O(n^2)/O(n) Where `n` is the number of points in the input polygon.
+///
+/// Quality of solution: This algorithm is a heuristic. At most four times the minimum number of convex
+/// polygons is created. However, in practice it works much better than that and often returns the optimal
+/// partitioning.
+///
+/// This algorithm is described in https://people.mpi-inf.mpg.de/~mehlhorn/ftp/FastTriangulation.pdf
+pub fn hertel_mehlhorn(vertices: &[Point<Real>], indices: &[[u32; 3]]) -> Vec<Vec<Point<Real>>> {
+    hertel_mehlhorn_idx(vertices, indices)
+        .into_iter()
+        .map(|poly_indices| {
+            poly_indices
+                .into_iter()
+                .map(|idx| vertices[idx as usize])
+                .collect()
+        })
+        .collect()
+}
+
+/// Internal implementation of the Hertel-Mehlhorn algorithm that returns the polygon indices.
+pub fn hertel_mehlhorn_idx(vertices: &[Point<Real>], indices: &[[u32; 3]]) -> Vec<Vec<u32>> {
+    let mut indices: Vec<Vec<u32>> = indices.iter().map(|indices| indices.to_vec()).collect();
+    // Iterate over all polygons.
+    let mut i_poly1 = 0;
+    while i_poly1 < indices.len() {
+        // Iterate over their points.
+        let mut i11 = 0;
+        while i11 < indices[i_poly1].len() {
+            let polygon1 = &indices[i_poly1];
+
+            // Get the next point index.
+            let i12 = (i11 + 1) % polygon1.len();
+
+            // Get the current edge.
+            let edge_start = polygon1[i11];
+            let edge_end = polygon1[i12];
+
+            // Iterate over the remaining polygons and find an edge to the current polygon.
+            let (i_poly2, edge_indices) = match indices
+                .iter()
+                .enumerate()
+                .skip(i_poly1 + 1)
+                .find_map(|(i, poly_indices)| {
+                    // Check if the edge is in the second polygon. Start and end are switched because
+                    // the edge direction is flipped in the second polygon.
+                    find_edge_index_in_polygon(edge_end, edge_start, poly_indices)
+                        .map(|edge_indices| (i, edge_indices))
+                }) {
+                Some(found) => found,
+                None => {
+                    // Go to the next point if there was no common edge.
+                    i11 += 1;
+                    continue;
+                }
+            };
+
+            // Check if the connections between the polygons are convex:
+            let (i21, i22) = edge_indices;
+            let polygon2 = &indices[i_poly2];
+
+            // First connection:
+            let i13 = (polygon1.len() + i11 - 1) % polygon1.len();
+            let i23 = (i22 + 1) % polygon2.len();
+            let p1 = &vertices[polygon2[i23] as usize];
+            let p2 = &vertices[polygon1[i13] as usize];
+            let p3 = &vertices[polygon1[i11] as usize];
+            // Go to the next point if this section isn't convex.
+            if corner_direction(p1, p2, p3) == Orientation::Cw {
+                i11 += 1;
+                continue;
+            }
+            // Second connection:
+            let i13 = (i12 + 1) % polygon1.len();
+            let i23 = (polygon2.len() + i21 - 1) % polygon2.len();
+            let p1 = &vertices[polygon1[i13] as usize];
+            let p2 = &vertices[polygon2[i23] as usize];
+            let p3 = &vertices[polygon1[i12] as usize];
+            // Go to the next point if this section isn't convex.
+            if corner_direction(p1, p2, p3) == Orientation::Cw {
+                i11 += 1;
+                continue;
+            }
+
+            // Connection is convex, merge the polygons.
+            let mut new_polygon = Vec::with_capacity(polygon1.len() + polygon2.len() - 2);
+            new_polygon.extend(polygon1.iter().cycle().skip(i12).take(polygon1.len() - 1));
+            new_polygon.extend(polygon2.iter().cycle().skip(i22).take(polygon2.len() - 1));
+
+            // Remove the polygon from the list.
+            let _ = indices.remove(i_poly2);
+            // Overwrite the first polygon with the new one.
+            indices[i_poly1] = new_polygon;
+            // Start from the first point.
+            i11 = 0;
+        }
+
+        // Go to the next polygon.
+        i_poly1 += 1;
+    }
+
+    indices
+}
+
+// --- Unit tests ----------------------------------------------------------------------------
+#[cfg(test)]
+mod tests {
+    use super::hertel_mehlhorn_idx;
+    use crate::math::Point;
+
+    #[test]
+    fn origin_outside_shape() {
+        // Expected result of convex decomposition:
+        // 4-----------------------3
+        // |  .       (2)       .  |
+        // |    .             .    |
+        // |       7-------0       |
+        // |  (1)  |       |  (3)  |
+        // |       |   °   |       |
+        // 5-------6       1-------2
+        let vertices = vec![
+            Point::new(2.0, 2.0),   // 0
+            Point::new(2.0, -2.0),  // 1
+            Point::new(4.0, -2.0),  // 2
+            Point::new(4.0, 4.0),   // 3
+            Point::new(-4.0, 4.0),  // 4
+            Point::new(-4.0, -2.0), // 5
+            Point::new(-2.0, -2.0), // 6
+            Point::new(-2.0, 2.0),  // 7
+        ];
+
+        let triangles = [
+            [5, 6, 7],
+            [4, 5, 7],
+            [3, 4, 7],
+            [3, 7, 0],
+            [2, 3, 0],
+            [2, 0, 1],
+        ];
+
+        let indices = hertel_mehlhorn_idx(&vertices, &triangles);
+
+        let expected_indices = vec![
+            // (1)
+            vec![5, 6, 7, 4],
+            // (2)
+            vec![3, 4, 7, 0],
+            // (3)
+            vec![2, 3, 0, 1],
+        ];
+
+        assert_eq!(indices, expected_indices);
+    }
+}

src/transformation/mod.rs

@@ -20,6 +20,10 @@ pub mod voxelization;
 #[cfg(feature = "dim2")]
 pub(crate) mod ear_clipping;
 #[cfg(feature = "dim2")]
+pub(crate) mod hertel_mehlhorn;
+#[cfg(feature = "dim2")]
+pub use hertel_mehlhorn::{hertel_mehlhorn, hertel_mehlhorn_idx};
+#[cfg(feature = "dim2")]
 mod to_polyline;
 #[cfg(feature = "dim3")]
 mod to_trimesh;