parry2d/transformation/hertel_mehlhorn.rs
1//! Hertel-Mehlhorn algorithm for convex partitioning.
2//! Based on <https://github.com/ivanfratric/polypartition>, contributed by embotech AG.
3
4use alloc::vec::Vec;
5
6use crate::math::Vector;
7use crate::utils::point_in_triangle::{corner_direction, Orientation};
8
9/// Checks if the counter-clockwise polygon `poly` has an edge going counter-clockwise from `p1` to `p2`.
10/// Returns the edge point's indices in the second polygon. Returns `None` if none were found.
11fn find_edge_index_in_polygon(p1: u32, p2: u32, indices: &[u32]) -> Option<(usize, usize)> {
12 for i1 in 0..indices.len() {
13 let i2 = (i1 + 1) % indices.len();
14 if p1 == indices[i1] && p2 == indices[i2] {
15 return Some((i1, i2));
16 }
17 }
18 None
19}
20
21/// The Hertel-Mehlhorn algorithm.
22///
23/// Takes a set of triangles and returns a vector of convex polygons.
24///
25/// Time/Space complexity: O(n^2)/O(n) Where `n` is the number of points in the input polygon.
26///
27/// Quality of solution: This algorithm is a heuristic. At most four times the minimum number of convex
28/// polygons is created. However, in practice it works much better than that and often returns the optimal
29/// partitioning.
30///
31/// This algorithm is described in <https://people.mpi-inf.mpg.de/~mehlhorn/ftp/FastTriangulation.pdf>.
32pub fn hertel_mehlhorn(vertices: &[Vector], indices: &[[u32; 3]]) -> Vec<Vec<Vector>> {
33 hertel_mehlhorn_idx(vertices, indices)
34 .into_iter()
35 .map(|poly_indices| {
36 poly_indices
37 .into_iter()
38 .map(|idx| vertices[idx as usize])
39 .collect()
40 })
41 .collect()
42}
43
44/// Internal implementation of the Hertel-Mehlhorn algorithm that returns the polygon indices.
45pub fn hertel_mehlhorn_idx(vertices: &[Vector], indices: &[[u32; 3]]) -> Vec<Vec<u32>> {
46 let mut indices: Vec<Vec<u32>> = indices.iter().map(|indices| indices.to_vec()).collect();
47 // Iterate over all polygons.
48 let mut i_poly1 = 0;
49 while i_poly1 < indices.len() {
50 // Iterate over their points.
51 let mut i11 = 0;
52 while i11 < indices[i_poly1].len() {
53 let polygon1 = &indices[i_poly1];
54
55 // Get the next point index.
56 let i12 = (i11 + 1) % polygon1.len();
57
58 // Get the current edge.
59 let edge_start = polygon1[i11];
60 let edge_end = polygon1[i12];
61
62 // Iterate over the remaining polygons and find an edge to the current polygon.
63 let (i_poly2, edge_indices) = match indices
64 .iter()
65 .enumerate()
66 .skip(i_poly1 + 1)
67 .find_map(|(i, poly_indices)| {
68 // Check if the edge is in the second polygon. Start and end are switched because
69 // the edge direction is flipped in the second polygon.
70 find_edge_index_in_polygon(edge_end, edge_start, poly_indices)
71 .map(|edge_indices| (i, edge_indices))
72 }) {
73 Some(found) => found,
74 None => {
75 // Go to the next point if there was no common edge.
76 i11 += 1;
77 continue;
78 }
79 };
80
81 // Check if the connections between the polygons are convex:
82 let (i21, i22) = edge_indices;
83 let polygon2 = &indices[i_poly2];
84
85 // First connection:
86 let i13 = (polygon1.len() + i11 - 1) % polygon1.len();
87 let i23 = (i22 + 1) % polygon2.len();
88 let p1 = vertices[polygon2[i23] as usize];
89 let p2 = vertices[polygon1[i13] as usize];
90 let p3 = vertices[polygon1[i11] as usize];
91 // Go to the next point if this section isn't convex.
92 if corner_direction(p1, p2, p3) == Orientation::Cw {
93 i11 += 1;
94 continue;
95 }
96 // Second connection:
97 let i13 = (i12 + 1) % polygon1.len();
98 let i23 = (polygon2.len() + i21 - 1) % polygon2.len();
99 let p1 = vertices[polygon1[i13] as usize];
100 let p2 = vertices[polygon2[i23] as usize];
101 let p3 = vertices[polygon1[i12] as usize];
102 // Go to the next point if this section isn't convex.
103 if corner_direction(p1, p2, p3) == Orientation::Cw {
104 i11 += 1;
105 continue;
106 }
107
108 // Connection is convex, merge the polygons.
109 let mut new_polygon = Vec::with_capacity(polygon1.len() + polygon2.len() - 2);
110 new_polygon.extend(polygon1.iter().cycle().skip(i12).take(polygon1.len() - 1));
111 new_polygon.extend(polygon2.iter().cycle().skip(i22).take(polygon2.len() - 1));
112
113 // `swap_remove` is O(1) and safe here: it moves the last polygon into slot
114 // `i_poly2 > i_poly1`, which the edge search above re-scans, so no merge
115 // candidate is skipped — only the output order changes.
116 let _ = indices.swap_remove(i_poly2);
117 // Overwrite the first polygon with the new one.
118 indices[i_poly1] = new_polygon;
119 // Start from the first point.
120 i11 = 0;
121 }
122
123 // Go to the next polygon.
124 i_poly1 += 1;
125 }
126
127 indices
128}
129
130// --- Unit tests ----------------------------------------------------------------------------
131#[cfg(test)]
132mod tests {
133 use super::hertel_mehlhorn_idx;
134 use crate::math::Vector;
135 use alloc::vec::Vec;
136
137 /// Normalizes a decomposition for an order-insensitive comparison, since the
138 /// algorithm's output order is unspecified.
139 fn canonicalize(mut polygons: Vec<Vec<u32>>) -> Vec<Vec<u32>> {
140 for poly in &mut polygons {
141 let min_pos = (0..poly.len()).min_by_key(|&i| poly[i]).unwrap();
142 poly.rotate_left(min_pos);
143 }
144 polygons.sort();
145 polygons
146 }
147
148 #[test]
149 fn origin_outside_shape() {
150 // Expected result of convex decomposition:
151 // 4-----------------------3
152 // | . (2) . |
153 // | . . |
154 // | 7-------0 |
155 // | (1) | | (3) |
156 // | | ° | |
157 // 5-------6 1-------2
158 let vertices = vec![
159 Vector::new(2.0, 2.0), // 0
160 Vector::new(2.0, -2.0), // 1
161 Vector::new(4.0, -2.0), // 2
162 Vector::new(4.0, 4.0), // 3
163 Vector::new(-4.0, 4.0), // 4
164 Vector::new(-4.0, -2.0), // 5
165 Vector::new(-2.0, -2.0), // 6
166 Vector::new(-2.0, 2.0), // 7
167 ];
168
169 let triangles = [
170 [5, 6, 7],
171 [4, 5, 7],
172 [3, 4, 7],
173 [3, 7, 0],
174 [2, 3, 0],
175 [2, 0, 1],
176 ];
177
178 let indices = hertel_mehlhorn_idx(&vertices, &triangles);
179
180 let expected_indices = vec![
181 // (1)
182 vec![5, 6, 7, 4],
183 // (2)
184 vec![3, 4, 7, 0],
185 // (3)
186 vec![2, 3, 0, 1],
187 ];
188
189 // Compare the decompositions as sets of (cyclic) polygons: the output order
190 // is unspecified.
191 assert_eq!(canonicalize(indices), canonicalize(expected_indices));
192 }
193}