1use nalgebra as na;
2use rayon::prelude::*;
3
4#[derive(Debug, Clone, Copy)]
5pub struct Point3d {
6 pub x: f32,
7 pub y: f32,
8 pub z: f32,
9 pub intensity: f32,
10}
11
12impl Point3d {
13 pub fn square(&self) -> f32 {
14 self.x * self.x + self.y * self.y + self.z * self.z
15 }
16 pub fn to_na_vec_f64(&self) -> na::Vector3<f64> {
17 na::Vector3::<f64>::new(self.x as f64, self.y as f64, self.z as f64)
18 }
19 pub fn to_na_point3_f64(&self) -> na::Point3<f64> {
20 na::Point3::<f64>::new(self.x as f64, self.y as f64, self.z as f64)
21 }
22}
23
24pub fn clip_point_cloud_by_distance(
25 point_cloud: &[Point3d],
26 min_distance: f32,
27 max_distance: f32,
28) -> Vec<Point3d> {
29 let min2 = min_distance * min_distance;
30 let max2 = max_distance * max_distance;
31 point_cloud
32 .par_iter()
33 .filter_map(|pt| {
34 let s = pt.square();
35 if s < min2 || s > max2 {
36 None
37 } else {
38 Some(*pt)
39 }
40 })
41 .collect()
42}