mapping_algorithms/point_clouds/
lex_sort.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::{Point, Scalar};
25use num_traits::Zero;
26
27use crate::{types::IsNan, Ordering, Vec};
28
29fn validate_input<T: Scalar + PartialOrd + IsNan, const N: usize>(input: &[Point<T, N>]) -> bool {
30    !(N.is_zero() || input.iter().any(|a| a.coords.iter().any(|b| b.is_nan())))
31}
32
33fn lex_sort_func<T: Scalar + PartialOrd + IsNan, const N: usize>(
34    a: &Point<T, N>,
35    b: &Point<T, N>,
36) -> Ordering {
37    (0..N).fold(Ordering::Equal, |ord, i| {
38        ord.then_with(|| a.coords[i].partial_cmp(&b.coords[i]).unwrap())
39    })
40}
41
42/// Sorts a point cloud in lexicographical order.
43/// Modifies the inserted slice in place.
44/// # Arguments
45/// * `input`: a mutable slice of [`Point`], representing the point cloud.
46///
47/// # Returns
48/// a [`bool`], indicating if the operation was successful.
49#[cfg_attr(
50    feature = "tracing",
51    tracing::instrument("Lexicographical Sort In Place", skip_all)
52)]
53pub fn lex_sort_in_place<T: Scalar + PartialOrd + IsNan, const N: usize>(
54    input: &mut [Point<T, N>],
55) -> bool {
56    if !validate_input(input) {
57        return false;
58    }
59
60    input.sort_by(lex_sort_func);
61    true
62}
63
64/// Sorts a copy of the point cloud in lexicographical order.
65/// # Arguments
66/// * `input`: a slice of [`Point`], representing the point cloud.
67///
68/// # Returns
69/// [`Some`] containing a vector of [`Point`]s, if the operation was successful.
70/// Otherwise, returns [`None`].
71#[cfg_attr(
72    feature = "tracing",
73    tracing::instrument("Lexicographical Sort With Copy", skip_all)
74)]
75pub fn lex_sort<T: Scalar + PartialOrd + IsNan, const N: usize>(
76    input: &[Point<T, N>],
77) -> Option<Vec<Point<T, N>>> {
78    if !validate_input(input) {
79        return None;
80    }
81
82    let mut input = input.to_vec();
83    input.sort_by(lex_sort_func);
84    Some(input)
85}
86
87/// Sorts the point cloud in lexicographical order, returning a [`Vec`] of references to the original points, in order..
88/// # Arguments
89/// * `input`: a slice of [`Point`], representing the point cloud.
90///
91/// # Returns
92/// [`Some`] containing a vector of &[`Point`]s, if the operation was successful.
93/// Otherwise, returns [`None`].
94#[cfg_attr(
95    feature = "tracing",
96    tracing::instrument("Lexicographical Sort With References", skip_all)
97)]
98pub fn lex_sort_ref<T: Scalar + PartialOrd + IsNan, const N: usize>(
99    input: &[Point<T, N>],
100) -> Option<Vec<&Point<T, N>>> {
101    if !validate_input(input) {
102        return None;
103    }
104
105    let mut refs = input.iter().collect::<Vec<_>>();
106    refs.sort_by(|&a, &b| lex_sort_func(a, b));
107    Some(refs)
108}
109
110#[cfg(test)]
111mod tests {
112    use nalgebra::Point3;
113
114    use crate::point_clouds::generate_point_cloud;
115
116    use super::*;
117
118    #[test]
119    fn test_lex_sort_in_place() {
120        let mut input = Vec::from([
121            Point3::new(1.0, 2.0, 3.0),
122            Point3::new(1.0, 2.0, 4.0),
123            Point3::new(1.0, 2.0, 2.0),
124        ]);
125        assert!(lex_sort_in_place(&mut input));
126        assert_eq!(
127            input,
128            Vec::from([
129                Point3::new(1.0, 2.0, 2.0),
130                Point3::new(1.0, 2.0, 3.0),
131                Point3::new(1.0, 2.0, 4.0)
132            ])
133        );
134    }
135
136    #[test]
137    fn test_lex_sort_in_place_nan() {
138        let mut input = Vec::from([
139            Point3::new(1.0, 2.0, 3.0),
140            Point3::new(1.0, 2.0, 4.0),
141            Point3::new(1.0, 2.0, f32::NAN),
142        ]);
143        assert!(!lex_sort_in_place(&mut input));
144    }
145
146    #[test]
147    fn test_lex_sort() {
148        let input = Vec::from([
149            Point3::new(1.0, 2.0, 3.0),
150            Point3::new(1.0, 2.0, 4.0),
151            Point3::new(1.0, 2.0, 2.0),
152        ]);
153        assert_eq!(
154            lex_sort(&input),
155            Some(Vec::from([
156                Point3::new(1.0, 2.0, 2.0),
157                Point3::new(1.0, 2.0, 3.0),
158                Point3::new(1.0, 2.0, 4.0)
159            ]))
160        );
161    }
162
163    #[test]
164    fn ensure_starting_point() {
165        let input = generate_point_cloud(100, [-10.0..=10.0, -10.0..=10.0]);
166        let sorted = lex_sort(&input).unwrap();
167
168        assert_eq!(
169            sorted
170                .iter()
171                .fold(f64::INFINITY, |acc, it| { acc.min(it.x) }),
172            sorted[0].x
173        );
174    }
175
176    #[test]
177    fn test_lex_sort_nan() {
178        let input = Vec::from([
179            Point3::new(1.0, 2.0, 3.0),
180            Point3::new(1.0, 2.0, 4.0),
181            Point3::new(1.0, 2.0, f32::NAN),
182        ]);
183        assert_eq!(lex_sort(&input), None);
184    }
185
186    // Test coplanar points
187    #[test]
188    fn test_lex_sort_coplanar() {
189        let input = Vec::from([
190            Point3::new(1.0, 4.0, 3.0),
191            Point3::new(1.0, 2.0, 3.0),
192            Point3::new(1.0, 2.0, 1.0),
193        ]);
194        assert_eq!(
195            lex_sort(&input),
196            Some(Vec::from([
197                Point3::new(1.0, 2.0, 1.0),
198                Point3::new(1.0, 2.0, 3.0),
199                Point3::new(1.0, 4.0, 3.0)
200            ]))
201        );
202    }
203}
mapping_algorithms/point_clouds/lex_sort.rs

mapping_algorithms/point_clouds/
lex_sort.rs
