mapping_algorithms/point_clouds/
nearest_neighbour.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::{Bounded, NumOps};
26
27use crate::utils::distance_squared;
28
29/// Finds the closest matching target point to the passed source point.
30///
31/// # Arguments
32/// * `point`: A [`Point`], for which to find the closest point.
33/// * `all_points`: A slice of [`Point`], representing the target point cloud.
34///
35/// # Generics
36/// * `T`: Either an [`f32`] or [`f64`].
37/// * `N`: A const usize, representing the number of dimensions in the points.
38///
39/// # Returns
40/// An [`Option`] of [`Point`], representing said closest point, empty if no points were provided.
41#[inline]
42#[cfg_attr(
43    feature = "tracing",
44    tracing::instrument("Find Closest Points", skip_all)
45)]
46pub fn find_nearest_neighbour_naive<T, const N: usize>(
47    point: &Point<T, N>,
48    all_points: &[Point<T, N>],
49) -> Option<Point<T, N>>
50where
51    T: Bounded + Copy + Default + NumOps + PartialOrd + Scalar,
52{
53    if all_points.is_empty() {
54        return None;
55    }
56
57    let mut current_distance = T::max_value();
58    let mut current_point = all_points[0]; // Guaranteed to exist
59
60    for target_point in all_points.iter() {
61        let distance = distance_squared(point, target_point);
62        if distance < current_distance {
63            current_distance = distance;
64            current_point = *target_point;
65        }
66    }
67
68    Some(current_point)
69}
70
71#[cfg(test)]
72mod tests {
73    use nalgebra::{Point, Point2};
74
75    use crate::Vec;
76
77    use super::*;
78
79    #[test]
80    fn test_find_closest_point() {
81        // Given:
82        // A set of target points
83        let target_points = Vec::from([
84            Point2::new(1.0, 1.0),
85            Point2::new(2.0, 2.0),
86            Point2::new(5.0, 5.0),
87            Point2::new(8.0, 8.0),
88        ]);
89
90        // A transformed point
91        let transformed_point = Point2::new(4.0, 4.0);
92
93        // When:
94        // Finding the closest point
95        let closest_point = find_nearest_neighbour_naive(&transformed_point, &target_points);
96
97        // Expect the closest point to be (5.0, 5.0)
98        assert_eq!(closest_point, Some(Point2::new(5.0, 5.0)));
99    }
100
101    #[test]
102    fn test_find_closest_point_with_empty_target() {
103        // Given:
104        // An empty set of target points
105        let target_points: Vec<Point<f64, 2>> = Vec::new();
106
107        // A transformed point
108        let transformed_point = Point2::new(4.0, 4.0);
109
110        // This should panic as the target_points array is empty
111        assert_eq!(
112            find_nearest_neighbour_naive(&transformed_point, &target_points),
113            None
114        );
115    }
116}
mapping_algorithms/point_clouds/nearest_neighbour.rs

mapping_algorithms/point_clouds/
nearest_neighbour.rs
