mapping_algorithms/polygons/
jarvis_march.rs

1// SPDX-License-Identifier: MIT
2/*
3 * Copyright (c) [2023 - Present] Emily Matheys <emilymatt96@gmail.com>
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights
8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 * copies of the Software, and to permit persons to whom the Software is
10 * furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in all
13 * copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 * SOFTWARE.
22 */
23
24use nalgebra::{ComplexField, Point2, Scalar};
25use num_traits::{real::Real, AsPrimitive, NumAssign};
26
27use crate::{point_clouds::downsample_point_cloud_voxel, Ordering, Vec};
28
29use super::calculate_determinant;
30
31/// Computes the convex hull of a set of points using the Jarvis March algorithm.
32/// This algorithm uses a pivot point to find the next point in the convex hull by selecting for each point, the point which makes the largest outwards turn that is less than 180 degrees.
33///
34/// # Arguments
35/// * `points` - A slice of points to compute the convex hull of
36/// * `voxel_size` - An optional parameter specifying the voxel size by which to downsample the point cloud before computing the convex hull, useful in reducing errors due to close or identical vertices.
37///
38/// # Genericsoc -
39/// * `O` - The output type of the trigonometric functions, essentially the precision of the calculations
40/// * `T` - The type of the points, can be of any scalar type
41///
42/// # Returns
43/// An [`Option`] of [`Vec<Point2<T>>`] representing the convex hull, or [`None`] if there were not enough points to compute a convex hull, or if all points are collinear
44#[cfg_attr(
45    feature = "tracing",
46    tracing::instrument("Construct Convex Hull Using Jarvis March", skip_all)
47)]
48pub fn jarvis_march<O, T>(points: &[Point2<T>], voxel_size: Option<O>) -> Option<Vec<Point2<T>>>
49where
50    O: AsPrimitive<isize> + ComplexField + Copy + Real,
51    T: AsPrimitive<O> + NumAssign + PartialOrd + Scalar,
52    usize: AsPrimitive<T>,
53{
54    if points.len() < 3 {
55        return None;
56    }
57
58    let points_downsampled;
59    let points_downsampled_slice;
60    if let Some(voxel_size) = voxel_size {
61        points_downsampled = downsample_point_cloud_voxel(points, voxel_size);
62        points_downsampled_slice = points_downsampled.as_slice();
63    } else {
64        points_downsampled_slice = points;
65    }
66
67    // Get leftest, lowest point as a starting point
68    let mut current_vertex = points_downsampled_slice.iter().min_by(|a, b| {
69        a.coords
70            .iter()
71            .zip(b.coords.iter())
72            .fold(Ordering::Equal, |ord, (a, b)| {
73                ord.then_with(|| a.partial_cmp(b).unwrap())
74            })
75    })?;
76
77    let mut hull = Vec::with_capacity(points_downsampled_slice.len());
78    loop {
79        hull.push(*current_vertex);
80        let mut endpoint = &points_downsampled_slice[0];
81        for point in points_downsampled_slice {
82            if endpoint == current_vertex
83                || calculate_determinant(hull.last().unwrap(), endpoint, point) < O::zero()
84            {
85                endpoint = point;
86            }
87        }
88
89        current_vertex = endpoint;
90
91        if current_vertex == &hull[0] {
92            break;
93        }
94    }
95
96    if hull.len() < 3 {
97        return None;
98    }
99
100    Some(hull)
101}
102
103#[cfg(feature = "pregenerated")]
104macro_rules! impl_jarvis_march {
105    ($t:expr, $prec:expr, doc $doc:tt) => {
106        ::paste::paste! {
107
108            #[doc = "A premade variant of the Jarvis March algorithm function, made for " $doc " precision floating-point arithmetic, using " $t " as the point type."]
109            pub fn [<jarvis_march_ $t>](input: &[Point2<$t>], voxel_size: Option<$prec>) -> Option<Vec<Point2<$t>>> {
110                super::jarvis_march::<$prec, $t>(input, voxel_size)
111            }
112        }
113    };
114    ($prec:expr, doc $doc:tt) => {
115        ::paste::paste! {
116            pub(super) mod [<$doc _precision>] {
117                use nalgebra::Point2;
118                use crate::Vec;
119
120                impl_jarvis_march!(u8, $prec, doc $doc);
121                impl_jarvis_march!(u16, $prec, doc $doc);
122                impl_jarvis_march!(u32, $prec, doc $doc);
123                impl_jarvis_march!(u64, $prec, doc $doc);
124                impl_jarvis_march!(usize, $prec, doc $doc);
125
126                impl_jarvis_march!(i8, $prec, doc $doc);
127                impl_jarvis_march!(i16, $prec, doc $doc);
128                impl_jarvis_march!(i32, $prec, doc $doc);
129                impl_jarvis_march!(i64, $prec, doc $doc);
130                impl_jarvis_march!(isize, $prec, doc $doc);
131
132                impl_jarvis_march!(f32, $prec, doc $doc);
133                impl_jarvis_march!(f64, $prec, doc $doc);
134            }
135        }
136    };
137}
138
139impl_jarvis_march!(f32, doc single);
140impl_jarvis_march!(f64, doc double);
141
142#[cfg(test)]
143mod tests {
144    use super::*;
145
146    #[test]
147    fn test_jarvis_march() {
148        let point_cloud = Vec::from([
149            Point2::new(-9.792094, 6.501087),
150            Point2::new(-9.297297, 1.4398155),
151            Point2::new(-3.0243487, 1.593832),
152            Point2::new(-8.812742, 4.631695),
153            Point2::new(-1.0105534, 6.7442665),
154            Point2::new(5.054867, 5.4078026),
155            Point2::new(-6.2726927, -6.1140366),
156            Point2::new(3.9031277, 1.8494358),
157            Point2::new(9.026024, 4.158765),
158            Point2::new(3.3359728, -5.591101),
159            Point2::new(-1.2357492, 6.9803257),
160            Point2::new(0.83374596, 4.5202436),
161            Point2::new(-9.786989, -2.6681538),
162            Point2::new(8.778954, -0.79308414),
163            Point2::new(-8.8654585, -0.54637814),
164            Point2::new(-8.697586, -5.999858),
165            Point2::new(6.7509384, 3.09805),
166            Point2::new(3.8945904, -2.1427813),
167            Point2::new(-6.2516804, -0.11557007),
168            Point2::new(-0.11794281, -7.1581955),
169            Point2::new(-8.771274, 7.540737),
170            Point2::new(-7.273285, -2.2619143),
171            Point2::new(-8.424244, 2.289342),
172            Point2::new(3.288208, -7.215433),
173            Point2::new(-8.128621, -9.368102),
174            Point2::new(4.178275, 5.9939947),
175            Point2::new(-0.8436794, 5.534689),
176            Point2::new(6.414747, 2.2250452),
177            Point2::new(2.5352774, 5.041601),
178            Point2::new(-1.8946352, 3.5172358),
179            Point2::new(-0.6175842, -1.1188431),
180            Point2::new(-1.4161129, 9.456829),
181            Point2::new(-5.09273, -9.197719),
182            Point2::new(1.5842638, 4.6569405),
183            Point2::new(8.992567, -1.571001),
184            Point2::new(8.999609, -8.009958),
185            Point2::new(-6.1459584, -2.6052542),
186            Point2::new(-4.50453, 9.498695),
187            Point2::new(-1.3855286, 1.1417828),
188            Point2::new(-4.4411488, -3.7954373),
189            Point2::new(0.8548746, 8.935608),
190            Point2::new(2.503477, -6.5350723),
191            Point2::new(-3.7824507, -9.775844),
192            Point2::new(7.757971, 9.683109),
193            Point2::new(6.6446037, 3.5783281),
194            Point2::new(-3.5842485, 6.8722763),
195            Point2::new(3.4604778, 8.430944),
196            Point2::new(-5.876791, -6.3074894),
197            Point2::new(-2.1599998, 7.8314323),
198            Point2::new(6.1809216, -6.801429),
199        ]);
200        let hull = jarvis_march::<f32, f32>(&point_cloud, None);
201
202        assert!(hull.is_some());
203        assert_eq!(
204            hull.unwrap(),
205            Vec::from([
206                Point2::new(-9.792094, 6.501087),
207                Point2::new(-8.771274, 7.540737),
208                Point2::new(-4.50453, 9.498695),
209                Point2::new(7.757971, 9.683109),
210                Point2::new(9.026024, 4.158765),
211                Point2::new(8.999609, -8.009958),
212                Point2::new(-3.7824507, -9.775844),
213                Point2::new(-8.128621, -9.368102),
214                Point2::new(-9.786989, -2.6681538),
215            ])
216        );
217    }
218
219    #[test]
220    fn test_not_enough_points() {
221        assert_eq!(jarvis_march::<f32, f32>(&[], None), None);
222        assert_eq!(
223            jarvis_march::<f32, f32>(&[Point2::new(1.0, 1.0)], None),
224            None
225        );
226        assert_eq!(
227            jarvis_march::<f32, f32>(&[Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)], None),
228            None
229        );
230    }
231
232    #[test]
233    fn test_collinear_points() {
234        let points = Vec::from([
235            Point2::new(0.0, 0.0),
236            Point2::new(1.0, 1.0),
237            Point2::new(2.0, 2.0),
238        ]);
239        assert_eq!(jarvis_march::<f32, f32>(&points, None), None);
240    }
241
242    #[test]
243    fn test_square_points() {
244        let points = Vec::from([
245            Point2::new(0.0, 0.0),
246            Point2::new(0.0, 1.0),
247            Point2::new(1.0, 1.0),
248            Point2::new(1.0, 0.0),
249            Point2::new(0.5, 0.5),
250        ]);
251        let expected = Vec::from([
252            Point2::new(0.0, 0.0),
253            Point2::new(0.0, 1.0),
254            Point2::new(1.0, 1.0),
255            Point2::new(1.0, 0.0),
256        ]);
257        assert_eq!(jarvis_march::<f32, f32>(&points, None), Some(expected));
258    }
259
260    #[test]
261    fn test_concave_shape() {
262        let points = Vec::from([
263            Point2::new(0.0, 0.0),
264            Point2::new(2.0, 0.0),
265            Point2::new(1.0, 1.0),
266            Point2::new(2.0, 2.0),
267            Point2::new(0.0, 2.0),
268        ]);
269        let expected = Vec::from([
270            Point2::new(0.0, 0.0),
271            Point2::new(0.0, 2.0),
272            Point2::new(2.0, 2.0),
273            Point2::new(2.0, 0.0),
274        ]);
275        assert_eq!(jarvis_march::<f32, f32>(&points, None), Some(expected));
276    }
277
278    #[test]
279    fn test_duplicate_points() {
280        let points = Vec::from([
281            Point2::new(0.0, 0.0),
282            Point2::new(1.0, 1.0),
283            Point2::new(1.0, 1.0),
284            Point2::new(0.0, 1.0),
285            Point2::new(1.0, 0.0),
286        ]);
287        let expected = Vec::from([
288            Point2::new(0.0, 0.0),
289            Point2::new(0.0, 1.0),
290            Point2::new(1.0, 1.0),
291            Point2::new(1.0, 0.0),
292        ]);
293        assert_eq!(jarvis_march::<f32, f32>(&points, None), Some(expected));
294    }
295
296    #[test]
297    fn test_negative_coordinates() {
298        let points = Vec::from([
299            Point2::new(-1.0, -1.0),
300            Point2::new(-1.0, 1.0),
301            Point2::new(1.0, 1.0),
302            Point2::new(1.0, -1.0),
303            Point2::new(0.0, 0.0),
304        ]);
305        let expected = Vec::from([
306            Point2::new(-1.0, -1.0),
307            Point2::new(-1.0, 1.0),
308            Point2::new(1.0, 1.0),
309            Point2::new(1.0, -1.0),
310        ]);
311        assert_eq!(jarvis_march::<f32, f32>(&points, None), Some(expected));
312    }
313
314    #[test]
315    fn test_complex_shape() {
316        let points = Vec::from([
317            Point2::new(0.0, 0.0),
318            Point2::new(2.0, 0.0),
319            Point2::new(1.0, 1.0),
320            Point2::new(2.0, 2.0),
321            Point2::new(0.0, 2.0),
322            Point2::new(1.0, 3.0),
323            Point2::new(3.0, 1.0),
324        ]);
325        let expected = Vec::from([
326            Point2::new(0.0, 0.0),
327            Point2::new(0.0, 2.0),
328            Point2::new(1.0, 3.0),
329            Point2::new(2.0, 2.0),
330            Point2::new(3.0, 1.0),
331            Point2::new(2.0, 0.0),
332        ]);
333        assert_eq!(jarvis_march::<f32, f32>(&points, None), Some(expected));
334    }
335}
mapping_algorithms/polygons/jarvis_march.rs

mapping_algorithms/polygons/
jarvis_march.rs
