mapping_algorithms/polygons/
graham_scan.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 core::fmt::Debug;
25use nalgebra::{ComplexField, Point2};
26use num_traits::{AsPrimitive, NumAssign};
27
28use crate::{
29    point_clouds::{downsample_point_cloud_voxel, lex_sort},
30    types::IsNan,
31    ToOwned, Vec, VecDeque,
32};
33
34use super::calculate_determinant;
35
36#[cfg_attr(
37    feature = "tracing",
38    tracing::instrument("Build Hull Segment", skip_all)
39)]
40fn build_hull_segment<
41    'a,
42    O: ComplexField + Copy + PartialOrd,
43    T: AsPrimitive<O> + Debug + IsNan + NumAssign + PartialOrd,
44>(
45    mut accumulator: VecDeque<&'a Point2<T>>,
46    current_point: &'a Point2<T>,
47) -> VecDeque<&'a Point2<T>> {
48    while accumulator.len() > 1
49        && calculate_determinant(
50            accumulator[accumulator.len() - 2],
51            accumulator[accumulator.len() - 1],
52            current_point,
53        ) <= O::zero()
54    {
55        accumulator.pop_back();
56    }
57
58    accumulator.push_back(current_point);
59    accumulator
60}
61
62/// Computes the convex hull of a set of points using the Graham Scan algorithm.
63/// Specifically the Monotone Chain variant
64/// This version sorts the points lexicographically before computing the convex hull.
65/// A downside of using this algorithm is that it does not handle collinear points well.
66///
67/// # Arguments
68/// * `points` - A slice of points to compute the convex hull of
69/// * `assume_sorted` - A boolean indicating whether the points are already sorted lexicographically, this is critical for correctness so make sure you don't set this to `true` unless you are sure the points are sorted
70/// * `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.
71///
72/// # Genericsoc -
73/// * `O` - The output type of the trigonometric functions, essentially the precision of the calculations
74/// * `T` - The type of the points, can be of any scalar type
75///
76/// # Returns
77/// 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
78#[cfg_attr(
79    feature = "tracing",
80    tracing::instrument("Construct Convex Hull Using Graham Scan", skip_all)
81)]
82pub fn graham_scan<O, T>(
83    points: &[Point2<T>],
84    assume_sorted: bool,
85    voxel_size: Option<O>,
86) -> Option<Vec<Point2<T>>>
87where
88    O: AsPrimitive<isize> + ComplexField + Copy + PartialOrd,
89    T: AsPrimitive<O> + Debug + IsNan + NumAssign + PartialOrd,
90    usize: AsPrimitive<T>,
91{
92    if points.len() < 3 {
93        return None;
94    }
95
96    let points_sorted;
97    let points_sorted_slice = match (assume_sorted, voxel_size) {
98        (true, None) => points,
99        (true, Some(voxel_size)) => {
100            points_sorted = downsample_point_cloud_voxel(points, voxel_size);
101            points_sorted.as_slice()
102        }
103        (false, None) => {
104            points_sorted = lex_sort(points)?;
105            points_sorted.as_slice()
106        }
107        (false, Some(voxel_size)) => {
108            let downsampled_points = downsample_point_cloud_voxel(points, voxel_size);
109            points_sorted = lex_sort(downsampled_points.as_slice())?;
110            points_sorted.as_slice()
111        }
112    };
113
114    let upper_hull = points_sorted_slice
115        .iter()
116        .fold(VecDeque::new(), |accumulator, point| {
117            build_hull_segment(accumulator, point)
118        });
119    let upper_hull_len = upper_hull.len();
120
121    let lower_hull = points_sorted_slice
122        .iter()
123        .rev()
124        .fold(VecDeque::new(), |accumulator, point| {
125            build_hull_segment(accumulator, point)
126        });
127    let lower_hull_len = lower_hull.len();
128
129    ((upper_hull_len + lower_hull_len - 2) > 2).then(|| {
130        upper_hull
131            .into_iter()
132            .take(upper_hull_len - 1)
133            .chain(lower_hull.into_iter().take(lower_hull_len - 1))
134            .map(ToOwned::to_owned)
135            .collect::<Vec<_>>()
136    })
137}
138
139#[cfg(feature = "pregenerated")]
140macro_rules! impl_graham_scan {
141    ($t:expr, $prec:expr, doc $doc:tt) => {
142        ::paste::paste! {
143
144            #[doc = "A premade variant of the Graham Scan algorithm function, made for " $doc " precision floating-point arithmetic, using " $t " as the point type."]
145            pub fn [<graham_scan_ $t>](input: &[Point2<$t>], assume_sorted: bool, voxel_size: Option<$prec>) -> Option<Vec<Point2<$t>>> {
146                super::graham_scan::<$prec, $t>(input, assume_sorted, voxel_size)
147            }
148        }
149    };
150    ($prec:expr, doc $doc:tt) => {
151        ::paste::paste! {
152            pub(super) mod [<$doc _precision>] {
153                use nalgebra::Point2;
154                use crate::Vec;
155
156                impl_graham_scan!(u8, $prec, doc $doc);
157                impl_graham_scan!(u16, $prec, doc $doc);
158                impl_graham_scan!(u32, $prec, doc $doc);
159                impl_graham_scan!(u64, $prec, doc $doc);
160                impl_graham_scan!(usize, $prec, doc $doc);
161
162                impl_graham_scan!(i8, $prec, doc $doc);
163                impl_graham_scan!(i16, $prec, doc $doc);
164                impl_graham_scan!(i32, $prec, doc $doc);
165                impl_graham_scan!(i64, $prec, doc $doc);
166                impl_graham_scan!(isize, $prec, doc $doc);
167
168                impl_graham_scan!(f32, $prec, doc $doc);
169                impl_graham_scan!(f64, $prec, doc $doc);
170            }
171        }
172    };
173}
174
175impl_graham_scan!(f32, doc single);
176impl_graham_scan!(f64, doc double);
177
178#[cfg(test)]
179mod tests {
180    use super::*;
181
182    #[test]
183    fn test_graham_scan() {
184        let point_cloud = Vec::from([
185            Point2::new(-9.792094, 6.501087),
186            Point2::new(-9.297297, 1.4398155),
187            Point2::new(-3.0243487, 1.593832),
188            Point2::new(-8.812742, 4.631695),
189            Point2::new(-1.0105534, 6.7442665),
190            Point2::new(5.054867, 5.4078026),
191            Point2::new(-6.2726927, -6.1140366),
192            Point2::new(3.9031277, 1.8494358),
193            Point2::new(9.026024, 4.158765),
194            Point2::new(3.3359728, -5.591101),
195            Point2::new(-1.2357492, 6.9803257),
196            Point2::new(0.83374596, 4.5202436),
197            Point2::new(-9.786989, -2.6681538),
198            Point2::new(8.778954, -0.79308414),
199            Point2::new(-8.8654585, -0.54637814),
200            Point2::new(-8.697586, -5.999858),
201            Point2::new(6.7509384, 3.09805),
202            Point2::new(3.8945904, -2.1427813),
203            Point2::new(-6.2516804, -0.11557007),
204            Point2::new(-0.11794281, -7.1581955),
205            Point2::new(-8.771274, 7.540737),
206            Point2::new(-7.273285, -2.2619143),
207            Point2::new(-8.424244, 2.289342),
208            Point2::new(3.288208, -7.215433),
209            Point2::new(-8.128621, -9.368102),
210            Point2::new(4.178275, 5.9939947),
211            Point2::new(-0.8436794, 5.534689),
212            Point2::new(6.414747, 2.2250452),
213            Point2::new(2.5352774, 5.041601),
214            Point2::new(-1.8946352, 3.5172358),
215            Point2::new(-0.6175842, -1.1188431),
216            Point2::new(-1.4161129, 9.456829),
217            Point2::new(-5.09273, -9.197719),
218            Point2::new(1.5842638, 4.6569405),
219            Point2::new(8.992567, -1.571001),
220            Point2::new(8.999609, -8.009958),
221            Point2::new(-6.1459584, -2.6052542),
222            Point2::new(-4.50453, 9.498695),
223            Point2::new(-1.3855286, 1.1417828),
224            Point2::new(-4.4411488, -3.7954373),
225            Point2::new(0.8548746, 8.935608),
226            Point2::new(2.503477, -6.5350723),
227            Point2::new(-3.7824507, -9.775844),
228            Point2::new(7.757971, 9.683109),
229            Point2::new(6.6446037, 3.5783281),
230            Point2::new(-3.5842485, 6.8722763),
231            Point2::new(3.4604778, 8.430944),
232            Point2::new(-5.876791, -6.3074894),
233            Point2::new(-2.1599998, 7.8314323),
234            Point2::new(6.1809216, -6.801429),
235        ]);
236        let hull = graham_scan::<f32, f32>(&point_cloud, false, None);
237
238        assert!(hull.is_some());
239        assert_eq!(
240            hull.unwrap(),
241            Vec::from([
242                Point2::new(-9.792094, 6.501087),
243                Point2::new(-8.771274, 7.540737),
244                Point2::new(-4.50453, 9.498695),
245                Point2::new(7.757971, 9.683109),
246                Point2::new(9.026024, 4.158765),
247                Point2::new(8.999609, -8.009958),
248                Point2::new(-3.7824507, -9.775844),
249                Point2::new(-8.128621, -9.368102),
250                Point2::new(-9.786989, -2.6681538),
251            ])
252        );
253    }
254
255    #[test]
256    fn test_not_enough_points() {
257        assert_eq!(graham_scan::<f32, f32>(&[], false, None), None);
258        assert_eq!(
259            graham_scan::<f32, f32>(&[Point2::new(1.0, 1.0)], false, None),
260            None
261        );
262        assert_eq!(
263            graham_scan::<f32, f32>(&[Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)], false, None),
264            None
265        );
266    }
267
268    #[test]
269    fn test_collinear_points() {
270        let points = Vec::from([
271            Point2::new(0.0, 0.0),
272            Point2::new(1.0, 1.0),
273            Point2::new(2.0, 2.0),
274        ]);
275        assert_eq!(graham_scan::<f32, f32>(&points, false, None), None);
276    }
277
278    #[test]
279    fn test_square_points() {
280        let points = Vec::from([
281            Point2::new(0.0, 0.0),
282            Point2::new(0.0, 1.0),
283            Point2::new(1.0, 1.0),
284            Point2::new(1.0, 0.0),
285            Point2::new(0.5, 0.5),
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!(
294            graham_scan::<f32, f32>(&points, false, None),
295            Some(expected)
296        );
297    }
298
299    #[test]
300    fn test_concave_shape() {
301        let points = Vec::from([
302            Point2::new(0.0, 0.0),
303            Point2::new(2.0, 0.0),
304            Point2::new(1.0, 1.0),
305            Point2::new(2.0, 2.0),
306            Point2::new(0.0, 2.0),
307        ]);
308        let expected = Vec::from([
309            Point2::new(0.0, 0.0),
310            Point2::new(0.0, 2.0),
311            Point2::new(2.0, 2.0),
312            Point2::new(2.0, 0.0),
313        ]);
314        assert_eq!(
315            graham_scan::<f32, f32>(&points, false, None),
316            Some(expected)
317        );
318    }
319
320    #[test]
321    fn test_duplicate_points() {
322        let points = Vec::from([
323            Point2::new(0.0, 0.0),
324            Point2::new(1.0, 1.0),
325            Point2::new(1.0, 1.0),
326            Point2::new(0.0, 1.0),
327            Point2::new(1.0, 0.0),
328        ]);
329        let expected = Vec::from([
330            Point2::new(0.0, 0.0),
331            Point2::new(0.0, 1.0),
332            Point2::new(1.0, 1.0),
333            Point2::new(1.0, 0.0),
334        ]);
335        assert_eq!(
336            graham_scan::<f32, f32>(&points, false, None),
337            Some(expected)
338        );
339    }
340
341    #[test]
342    fn test_negative_coordinates() {
343        let points = Vec::from([
344            Point2::new(-1.0, -1.0),
345            Point2::new(-1.0, 1.0),
346            Point2::new(1.0, 1.0),
347            Point2::new(1.0, -1.0),
348            Point2::new(0.0, 0.0),
349        ]);
350        let expected = Vec::from([
351            Point2::new(-1.0, -1.0),
352            Point2::new(-1.0, 1.0),
353            Point2::new(1.0, 1.0),
354            Point2::new(1.0, -1.0),
355        ]);
356        assert_eq!(
357            graham_scan::<f32, f32>(&points, false, None),
358            Some(expected)
359        );
360    }
361
362    #[test]
363    fn test_complex_shape() {
364        let points = Vec::from([
365            Point2::new(0.0, 0.0),
366            Point2::new(2.0, 0.0),
367            Point2::new(1.0, 1.0),
368            Point2::new(2.0, 2.0),
369            Point2::new(0.0, 2.0),
370            Point2::new(1.0, 3.0),
371            Point2::new(3.0, 1.0),
372        ]);
373
374        // point 2, 2 is not included in the hull, due to it being on the same line as 1, 3
375        // This is the downside of using a Graham Scan algorithm
376        let expected = Vec::from([
377            Point2::new(0.0, 0.0),
378            Point2::new(0.0, 2.0),
379            Point2::new(1.0, 3.0),
380            Point2::new(3.0, 1.0),
381            Point2::new(2.0, 0.0),
382        ]);
383        assert_eq!(
384            graham_scan::<f32, f32>(&points, false, None),
385            Some(expected)
386        );
387    }
388}
mapping_algorithms/polygons/graham_scan.rs

mapping_algorithms/polygons/
graham_scan.rs
