spatialrust_features/
normal.rs

1use spatialrust_core::{
2    DType, DeviceKind, ExecutionOutput, ExecutionPolicy, ExecutionReceipt, FieldSemantic,
3    HasPositions3, PointBuffer, PointBufferSet, PointCloud, PointField, PointSchema, SpatialError,
4    SpatialResult, TransferDirection,
5};
6use spatialrust_math::{symmetric_eigen3, Mat3, Vec3};
7use spatialrust_search::{parallel_worker_count, KdTree, Neighbor, RadiusSearchIndex};
8
9use crate::estimator::FeatureEstimator;
10
11/// Minimum point count before GPU normal estimation is selected under `Auto`.
12///
13/// The k-NN GPU path (default MVP config) modestly wins at large counts; the
14/// radius/grid GPU path is much faster but requires `search_radius`.
15pub const DEFAULT_GPU_MIN_POINTS_NORMAL: usize = 10_000;
16
17/// Configuration for covariance-based normal estimation.
18#[derive(Clone, Copy, Debug, PartialEq)]
19pub struct NormalEstimationConfig {
20    /// Number of nearest neighbors to use when `search_radius` is `None`.
21    pub k_neighbors: usize,
22    /// Optional radius search instead of fixed `k`.
23    pub search_radius: Option<f32>,
24    /// Minimum number of neighbors required to estimate a valid normal.
25    pub min_neighbors: usize,
26    /// Optional viewpoint used to orient normals consistently.
27    pub viewpoint: Option<Vec3<f32>>,
28    /// Minimum input point count before GPU execution is considered under `Auto`.
29    ///
30    /// `None` always uses GPU when requested.
31    pub gpu_min_points: Option<usize>,
32}
33
34impl Default for NormalEstimationConfig {
35    fn default() -> Self {
36        Self {
37            k_neighbors: 20,
38            search_radius: None,
39            min_neighbors: 3,
40            viewpoint: None,
41            gpu_min_points: Some(DEFAULT_GPU_MIN_POINTS_NORMAL),
42        }
43    }
44}
45
46impl NormalEstimationConfig {
47    /// Creates a k-NN normal estimation config.
48    #[must_use]
49    pub const fn k_neighbors(k_neighbors: usize) -> Self {
50        Self {
51            k_neighbors,
52            search_radius: None,
53            min_neighbors: 3,
54            viewpoint: None,
55            gpu_min_points: Some(DEFAULT_GPU_MIN_POINTS_NORMAL),
56        }
57    }
58
59    /// Disables the GPU point-count heuristic so GPU is always used when requested.
60    #[must_use]
61    pub const fn without_gpu_min_points(mut self) -> Self {
62        self.gpu_min_points = None;
63        self
64    }
65
66    /// Returns the point-count threshold used by [`ExecutionPolicy::Auto`].
67    #[must_use]
68    pub const fn effective_gpu_min_points(&self) -> Option<usize> {
69        self.gpu_min_points
70    }
71}
72
73/// Result metadata for normal estimation.
74#[derive(Clone, Debug, Default, PartialEq, Eq)]
75pub struct NormalEstimationResult {
76    /// Number of points with valid normals.
77    pub valid_count: usize,
78    /// Number of points with invalid normals.
79    pub invalid_count: usize,
80}
81
82/// Covariance-based normal estimator.
83#[derive(Clone, Debug, PartialEq)]
84pub struct NormalEstimator {
85    config: NormalEstimationConfig,
86}
87
88impl NormalEstimator {
89    /// Creates a normal estimator from config.
90    #[must_use]
91    pub const fn new(config: NormalEstimationConfig) -> Self {
92        Self { config }
93    }
94
95    /// Returns the estimator config.
96    #[must_use]
97    pub const fn config(&self) -> NormalEstimationConfig {
98        self.config
99    }
100
101    /// Estimates normals and curvature, returning output cloud and diagnostics.
102    pub fn estimate_with_diagnostics(
103        &self,
104        input: &PointCloud,
105    ) -> SpatialResult<(PointCloud, NormalEstimationResult)> {
106        if input.is_empty() {
107            return Ok((input.clone(), NormalEstimationResult::default()));
108        }
109        if self.config.search_radius.is_some_and(|radius| radius < 0.0) {
110            return Err(SpatialError::InvalidArgument("search_radius must be non-negative".into()));
111        }
112
113        let (x, y, z) = input.positions3()?;
114        let tree = KdTree::from_slices(x, y, z);
115
116        let mut nx = vec![f32::NAN; input.len()];
117        let mut ny = vec![f32::NAN; input.len()];
118        let mut nz = vec![f32::NAN; input.len()];
119        let mut curvature = vec![0.0_f32; input.len()];
120        let mut valid_count = 0usize;
121        let mut invalid_count = 0usize;
122
123        let worker_count = parallel_worker_count(input.len());
124        if worker_count == 1 {
125            let chunk = estimate_normal_range(self.config, &tree, x, y, z, 0, input.len());
126            nx = chunk.nx;
127            ny = chunk.ny;
128            nz = chunk.nz;
129            curvature = chunk.curvature;
130            valid_count = chunk.valid_count;
131            invalid_count = chunk.invalid_count;
132        } else {
133            let chunks = std::thread::scope(|scope| {
134                use spatialrust_search::parallel_index_ranges;
135
136                let mut handles = Vec::new();
137                let config = self.config;
138                let tree_ref = &tree;
139                for range in parallel_index_ranges(input.len(), worker_count) {
140                    handles.push(scope.spawn(move || {
141                        estimate_normal_range(config, tree_ref, x, y, z, range.start, range.end)
142                    }));
143                }
144
145                handles
146                    .into_iter()
147                    .map(|handle| handle.join().expect("normal estimation worker panicked"))
148                    .collect::<Vec<_>>()
149            });
150
151            for chunk in chunks {
152                let end = chunk.start + chunk.nx.len();
153                nx[chunk.start..end].copy_from_slice(&chunk.nx);
154                ny[chunk.start..end].copy_from_slice(&chunk.ny);
155                nz[chunk.start..end].copy_from_slice(&chunk.nz);
156                curvature[chunk.start..end].copy_from_slice(&chunk.curvature);
157                valid_count += chunk.valid_count;
158                invalid_count += chunk.invalid_count;
159            }
160        }
161
162        let output = build_output_cloud(input, nx, ny, nz, curvature)?;
163        Ok((output, NormalEstimationResult { valid_count, invalid_count }))
164    }
165
166    /// Estimates normals using the given execution policy.
167    ///
168    /// With the `feature-normal-gpu` feature, [`ExecutionPolicy::Auto`]
169    /// runs covariance analysis on wgpu when the input meets
170    /// [`NormalEstimationConfig::effective_gpu_min_points`]. An explicit GPU
171    /// policy is strict and does not use the threshold; use `Auto` for fallback.
172    pub fn estimate_with_policy(
173        &self,
174        input: &PointCloud,
175        policy: ExecutionPolicy,
176    ) -> SpatialResult<PointCloud> {
177        self.estimate_with_policy_and_receipt(input, policy).map(ExecutionOutput::into_output)
178    }
179
180    /// Estimates normals and returns execution/transfer accounting.
181    pub fn estimate_with_policy_and_receipt(
182        &self,
183        input: &PointCloud,
184        policy: ExecutionPolicy,
185    ) -> SpatialResult<ExecutionOutput<PointCloud>> {
186        policy.validate()?;
187        let resolved_policy = self.receipt_policy(input, policy)?;
188        let output = self.estimate_policy_output(input, policy)?;
189        let mut receipt = ExecutionReceipt::new(policy, resolved_policy);
190        receipt.record_stage("normal-estimation");
191        if matches!(resolved_policy, ExecutionPolicy::Gpu(DeviceKind::Wgpu)) {
192            let input_bytes = (input.len() * 3 * std::mem::size_of::<f32>()) as u64;
193            let neighbor_bytes = if self.config.search_radius.is_none() {
194                (input.len().saturating_mul(self.config.k_neighbors.max(1))
195                    * std::mem::size_of::<u32>()) as u64
196            } else {
197                0
198            };
199            let output_bytes = (output.len() * 4 * std::mem::size_of::<f32>()) as u64;
200            receipt.record_transfer(TransferDirection::HostToDevice, input_bytes);
201            receipt.record_transfer(TransferDirection::HostToDevice, neighbor_bytes);
202            receipt.record_transfer(TransferDirection::DeviceToHost, output_bytes);
203            receipt.record_stage("gpu-readback");
204        }
205        Ok(ExecutionOutput::new(output, receipt))
206    }
207
208    fn estimate_policy_output(
209        &self,
210        input: &PointCloud,
211        policy: ExecutionPolicy,
212    ) -> SpatialResult<PointCloud> {
213        #[cfg(feature = "feature-normal-gpu")]
214        {
215            let resolved = self.resolve_policy(input, policy)?;
216            if matches!(resolved, ExecutionPolicy::Gpu(DeviceKind::Wgpu)) {
217                return crate::normal_gpu::GpuNormalEstimator::new(self.config).estimate(input);
218            }
219        }
220
221        #[cfg(not(feature = "feature-normal-gpu"))]
222        if policy.requests_gpu() {
223            return Err(SpatialError::InvalidArgument(
224                "GPU normal estimation requires the feature-normal-gpu feature".to_owned(),
225            ));
226        }
227
228        let _ = policy;
229        self.estimate(input)
230    }
231
232    fn receipt_policy(
233        &self,
234        input: &PointCloud,
235        policy: ExecutionPolicy,
236    ) -> SpatialResult<ExecutionPolicy> {
237        #[cfg(feature = "feature-normal-gpu")]
238        {
239            self.resolve_policy(input, policy)
240        }
241        #[cfg(not(feature = "feature-normal-gpu"))]
242        {
243            let _ = input;
244            Ok(match policy {
245                ExecutionPolicy::Auto => ExecutionPolicy::CpuSingle,
246                other => other,
247            })
248        }
249    }
250
251    /// Returns whether the given policy selects the GPU backend for this input.
252    #[cfg(feature = "feature-normal-gpu")]
253    pub fn selects_gpu_backend(&self, input: &PointCloud, policy: ExecutionPolicy) -> bool {
254        matches!(
255            self.resolve_policy(input, policy).ok(),
256            Some(ExecutionPolicy::Gpu(DeviceKind::Wgpu))
257        )
258    }
259
260    /// Returns whether a GPU uniform-grid normal pass fits the input bounds at `radius`.
261    #[cfg(feature = "feature-normal-gpu")]
262    pub fn gpu_grid_fits(&self, input: &PointCloud, radius: f32) -> bool {
263        let Ok((x, y, z)) = input.positions3() else {
264            return false;
265        };
266        spatialrust_gpu::uniform_grid_fits(x, y, z, radius)
267    }
268
269    #[cfg(feature = "feature-normal-gpu")]
270    fn should_use_gpu(&self, input: &PointCloud) -> bool {
271        self.config.effective_gpu_min_points().map_or(true, |min_points| input.len() >= min_points)
272            && spatialrust_gpu::WgpuRuntime::shared().is_ok()
273    }
274
275    #[cfg(feature = "feature-normal-gpu")]
276    fn resolve_policy(
277        &self,
278        input: &PointCloud,
279        policy: ExecutionPolicy,
280    ) -> SpatialResult<ExecutionPolicy> {
281        match policy {
282            ExecutionPolicy::Auto => {
283                if self.should_use_gpu(input) {
284                    Ok(ExecutionPolicy::Gpu(DeviceKind::Wgpu))
285                } else {
286                    Ok(ExecutionPolicy::CpuSingle)
287                }
288            }
289            ExecutionPolicy::Gpu(DeviceKind::Cpu) => Err(SpatialError::InvalidArgument(
290                "GPU execution policy cannot target the CPU device".to_owned(),
291            )),
292            ExecutionPolicy::Gpu(DeviceKind::Cuda) => Err(SpatialError::InvalidArgument(
293                "CUDA normal estimation is not available".to_owned(),
294            )),
295            other => Ok(other),
296        }
297    }
298}
299
300impl FeatureEstimator for NormalEstimator {
301    fn name(&self) -> &'static str {
302        "NormalEstimator"
303    }
304
305    fn estimate(&self, input: &PointCloud) -> SpatialResult<PointCloud> {
306        self.estimate_with_diagnostics(input).map(|(cloud, _)| cloud)
307    }
308}
309
310#[derive(Debug)]
311struct NormalChunk {
312    start: usize,
313    nx: Vec<f32>,
314    ny: Vec<f32>,
315    nz: Vec<f32>,
316    curvature: Vec<f32>,
317    valid_count: usize,
318    invalid_count: usize,
319}
320
321fn estimate_normal_range(
322    config: NormalEstimationConfig,
323    tree: &KdTree,
324    x: &[f32],
325    y: &[f32],
326    z: &[f32],
327    start: usize,
328    end: usize,
329) -> NormalChunk {
330    let len = end - start;
331    let mut nx = vec![f32::NAN; len];
332    let mut ny = vec![f32::NAN; len];
333    let mut nz = vec![f32::NAN; len];
334    let mut curvature = vec![0.0_f32; len];
335    let mut valid_count = 0usize;
336    let mut invalid_count = 0usize;
337    let mut neighbor_buffer = Vec::with_capacity(config.k_neighbors.saturating_add(1));
338    let mut index_buffer = Vec::with_capacity(config.k_neighbors);
339
340    for index in start..end {
341        query_neighbors_into(config, tree, x, y, z, index, &mut neighbor_buffer, &mut index_buffer);
342        let local = index - start;
343        if index_buffer.len() < config.min_neighbors {
344            invalid_count += 1;
345            continue;
346        }
347
348        let Some((normal, curv)) = estimate_normal_from_neighbors(x, y, z, index, &index_buffer)
349        else {
350            invalid_count += 1;
351            continue;
352        };
353
354        let oriented = if let Some(viewpoint) = config.viewpoint {
355            orient_normal_towards_viewpoint(normal, point_xyz(x, y, z, index), viewpoint)
356        } else {
357            normal
358        };
359
360        nx[local] = oriented.x;
361        ny[local] = oriented.y;
362        nz[local] = oriented.z;
363        curvature[local] = curv;
364        valid_count += 1;
365    }
366
367    NormalChunk { start, nx, ny, nz, curvature, valid_count, invalid_count }
368}
369
370fn query_neighbors_into(
371    config: NormalEstimationConfig,
372    tree: &KdTree,
373    x: &[f32],
374    y: &[f32],
375    z: &[f32],
376    index: usize,
377    neighbor_buffer: &mut Vec<Neighbor>,
378    index_buffer: &mut Vec<usize>,
379) {
380    index_buffer.clear();
381    if let Some(radius) = config.search_radius {
382        for neighbor in tree.radius_search(x[index], y[index], z[index], radius) {
383            if neighbor.index != index {
384                index_buffer.push(neighbor.index);
385            }
386        }
387    } else {
388        tree.nearest_k_unsorted_into(
389            x[index],
390            y[index],
391            z[index],
392            config.k_neighbors.saturating_add(1),
393            neighbor_buffer,
394        );
395        for neighbor in neighbor_buffer.iter() {
396            if neighbor.index != index {
397                index_buffer.push(neighbor.index);
398                if index_buffer.len() == config.k_neighbors {
399                    break;
400                }
401            }
402        }
403    }
404}
405
406/// Orients a normal to point towards the viewpoint when possible.
407#[must_use]
408pub fn orient_normal_towards_viewpoint(
409    mut normal: Vec3<f32>,
410    point: Vec3<f32>,
411    viewpoint: Vec3<f32>,
412) -> Vec3<f32> {
413    let view_direction =
414        Vec3::new(viewpoint.x - point.x, viewpoint.y - point.y, viewpoint.z - point.z);
415    if normal.dot(view_direction) < 0.0 {
416        normal.x = -normal.x;
417        normal.y = -normal.y;
418        normal.z = -normal.z;
419    }
420    normal.normalize()
421}
422
423fn point_xyz(x: &[f32], y: &[f32], z: &[f32], index: usize) -> Vec3<f32> {
424    Vec3::new(x[index], y[index], z[index])
425}
426
427fn estimate_normal_from_neighbors(
428    x: &[f32],
429    y: &[f32],
430    z: &[f32],
431    _center_index: usize,
432    neighbors: &[usize],
433) -> Option<(Vec3<f32>, f32)> {
434    let mut mean_x = 0.0_f32;
435    let mut mean_y = 0.0_f32;
436    let mut mean_z = 0.0_f32;
437    for &index in neighbors {
438        mean_x += x[index];
439        mean_y += y[index];
440        mean_z += z[index];
441    }
442    let count = neighbors.len() as f32;
443    mean_x /= count;
444    mean_y /= count;
445    mean_z /= count;
446
447    let mut c00 = 0.0_f32;
448    let mut c11 = 0.0_f32;
449    let mut c22 = 0.0_f32;
450    let mut c01 = 0.0_f32;
451    let mut c02 = 0.0_f32;
452    let mut c12 = 0.0_f32;
453    for &index in neighbors {
454        let dx = x[index] - mean_x;
455        let dy = y[index] - mean_y;
456        let dz = z[index] - mean_z;
457        c00 += dx * dx;
458        c11 += dy * dy;
459        c22 += dz * dz;
460        c01 += dx * dy;
461        c02 += dx * dz;
462        c12 += dy * dz;
463    }
464    let inv = 1.0 / count;
465    smallest_eigenpair_for_covariance(
466        c00 * inv,
467        c11 * inv,
468        c22 * inv,
469        c01 * inv,
470        c02 * inv,
471        c12 * inv,
472    )
473}
474
475fn smallest_eigenpair_for_covariance(
476    c00: f32,
477    c11: f32,
478    c22: f32,
479    c01: f32,
480    c02: f32,
481    c12: f32,
482) -> Option<(Vec3<f32>, f32)> {
483    let eigenvalues = symmetric_eigenvalues3(c00, c11, c22, c01, c02, c12);
484    let lambda = eigenvalues[0];
485    let normal =
486        eigenvector_for_eigenvalue(c00, c11, c22, c01, c02, c12, lambda).unwrap_or_else(|| {
487            let covariance = Mat3::<f64>::from_rows(
488                [c00 as f64, c01 as f64, c02 as f64],
489                [c01 as f64, c11 as f64, c12 as f64],
490                [c02 as f64, c12 as f64, c22 as f64],
491            );
492            let eigen = symmetric_eigen3(covariance);
493            Vec3::new(
494                eigen.eigenvectors.m[0][0] as f32,
495                eigen.eigenvectors.m[1][0] as f32,
496                eigen.eigenvectors.m[2][0] as f32,
497            )
498            .normalize()
499        });
500
501    let sum = eigenvalues[0] + eigenvalues[1] + eigenvalues[2];
502    let curvature = if sum > 0.0 { eigenvalues[0] / sum } else { 0.0 };
503    Some((normal.normalize(), curvature))
504}
505
506fn symmetric_eigenvalues3(c00: f32, c11: f32, c22: f32, c01: f32, c02: f32, c12: f32) -> [f32; 3] {
507    let p1 = c01 * c01 + c02 * c02 + c12 * c12;
508    if p1 <= f32::EPSILON {
509        let mut values = [c00, c11, c22];
510        values.sort_by(|a, b| a.partial_cmp(b).unwrap());
511        return values;
512    }
513
514    let q = (c00 + c11 + c22) / 3.0;
515    let b00 = c00 - q;
516    let b11 = c11 - q;
517    let b22 = c22 - q;
518    let p2 = b00 * b00 + b11 * b11 + b22 * b22 + 2.0 * p1;
519    let p = (p2 / 6.0).sqrt();
520    if p <= f32::EPSILON {
521        return [q, q, q];
522    }
523
524    let inv_p = 1.0 / p;
525    let n00 = b00 * inv_p;
526    let n11 = b11 * inv_p;
527    let n22 = b22 * inv_p;
528    let n01 = c01 * inv_p;
529    let n02 = c02 * inv_p;
530    let n12 = c12 * inv_p;
531    let det = n00 * (n11 * n22 - n12 * n12) - n01 * (n01 * n22 - n12 * n02)
532        + n02 * (n01 * n12 - n11 * n02);
533    let r = (det * 0.5).clamp(-1.0, 1.0);
534    let phi = r.acos() / 3.0;
535
536    let largest = q + 2.0 * p * phi.cos();
537    let smallest = q + 2.0 * p * (phi + 2.0 * std::f32::consts::PI / 3.0).cos();
538    let middle = 3.0 * q - largest - smallest;
539    let mut values = [smallest, middle, largest];
540    values.sort_by(|a, b| a.partial_cmp(b).unwrap());
541    values
542}
543
544fn eigenvector_for_eigenvalue(
545    c00: f32,
546    c11: f32,
547    c22: f32,
548    c01: f32,
549    c02: f32,
550    c12: f32,
551    lambda: f32,
552) -> Option<Vec3<f32>> {
553    let row0 = Vec3::new(c00 - lambda, c01, c02);
554    let row1 = Vec3::new(c01, c11 - lambda, c12);
555    let row2 = Vec3::new(c02, c12, c22 - lambda);
556
557    let candidates = [row0.cross(row1), row0.cross(row2), row1.cross(row2)];
558    let mut best = candidates[0];
559    let mut best_norm = best.length_squared();
560    for candidate in candidates.into_iter().skip(1) {
561        let norm = candidate.length_squared();
562        if norm > best_norm {
563            best = candidate;
564            best_norm = norm;
565        }
566    }
567
568    if best_norm <= 1e-24 {
569        None
570    } else {
571        Some(best.normalize())
572    }
573}
574
575pub(crate) fn build_output_cloud(
576    input: &PointCloud,
577    nx: Vec<f32>,
578    ny: Vec<f32>,
579    nz: Vec<f32>,
580    curvature: Vec<f32>,
581) -> SpatialResult<PointCloud> {
582    let mut schema = input.schema().clone();
583    ensure_field(&mut schema, "normal_x", FieldSemantic::NormalX, DType::F32);
584    ensure_field(&mut schema, "normal_y", FieldSemantic::NormalY, DType::F32);
585    ensure_field(&mut schema, "normal_z", FieldSemantic::NormalZ, DType::F32);
586    ensure_field(&mut schema, "curvature", FieldSemantic::Curvature, DType::F32);
587
588    let mut buffers = PointBufferSet::new();
589    for field in input.schema().fields() {
590        let source = input.field(&field.name)?;
591        buffers.insert(field.name.clone(), clone_buffer(source)?);
592    }
593    buffers.insert("normal_x".to_owned(), PointBuffer::from_f32(nx));
594    buffers.insert("normal_y".to_owned(), PointBuffer::from_f32(ny));
595    buffers.insert("normal_z".to_owned(), PointBuffer::from_f32(nz));
596    buffers.insert("curvature".to_owned(), PointBuffer::from_f32(curvature));
597
598    PointCloud::try_from_parts(schema, buffers, input.metadata().clone())
599}
600
601fn ensure_field(schema: &mut PointSchema, name: &str, semantic: FieldSemantic, dtype: DType) {
602    if schema.find_semantic(semantic).is_none() {
603        *schema = schema.clone().with_field(PointField::scalar(name, semantic, dtype));
604    }
605}
606
607fn clone_buffer(buffer: &PointBuffer) -> SpatialResult<PointBuffer> {
608    Ok(match buffer {
609        PointBuffer::F32(values) => PointBuffer::from_f32(values.clone()),
610        PointBuffer::F64(values) => PointBuffer::F64(values.clone()),
611        PointBuffer::U8(values) => PointBuffer::U8(values.clone()),
612        PointBuffer::U16(values) => PointBuffer::U16(values.clone()),
613        PointBuffer::U32(values) => PointBuffer::U32(values.clone()),
614        PointBuffer::I32(values) => PointBuffer::I32(values.clone()),
615    })
616}
617
618#[cfg(test)]
619mod tests {
620    use super::{orient_normal_towards_viewpoint, NormalEstimationConfig, NormalEstimator};
621    use crate::FeatureEstimator;
622    use spatialrust_core::{
623        DeviceKind, ExecutionPolicy, HasNormals3, PointCloudBuilder, StandardSchemas,
624    };
625    use spatialrust_math::Vec3;
626
627    fn plane_cloud() -> spatialrust_core::PointCloud {
628        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
629        for x in 0..5 {
630            for y in 0..5 {
631                builder.push_point([x as f32, y as f32, 0.0]).unwrap();
632            }
633        }
634        builder.build().unwrap()
635    }
636
637    fn tilted_plane_cloud() -> spatialrust_core::PointCloud {
638        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
639        for x in 0..7 {
640            for y in 0..7 {
641                let fx = x as f32 * 0.2;
642                let fy = y as f32 * 0.2;
643                let z = 0.2 * fx - 0.3 * fy + 0.1;
644                builder.push_point([fx, fy, z]).unwrap();
645            }
646        }
647        builder.build().unwrap()
648    }
649
650    #[test]
651    fn estimates_plane_normals_upwards() {
652        let input = plane_cloud();
653        let estimator = NormalEstimator::new(NormalEstimationConfig {
654            k_neighbors: 8,
655            min_neighbors: 3,
656            viewpoint: Some(Vec3::new(0.0, 0.0, 10.0)),
657            ..NormalEstimationConfig::default()
658        });
659        let (output, stats) = estimator.estimate_with_diagnostics(&input).unwrap();
660        assert_eq!(stats.valid_count, input.len());
661        assert_eq!(stats.invalid_count, 0);
662
663        let (_, _, nz) = output.normals3().unwrap();
664        for value in nz {
665            assert!((*value - 1.0).abs() < 0.1, "expected upward normal, got {value}");
666        }
667    }
668
669    #[test]
670    fn estimates_tilted_plane_normals() {
671        let input = tilted_plane_cloud();
672        let estimator = NormalEstimator::new(NormalEstimationConfig {
673            k_neighbors: 12,
674            min_neighbors: 3,
675            viewpoint: Some(Vec3::new(0.0, 0.0, 10.0)),
676            ..NormalEstimationConfig::default()
677        });
678        let output = estimator.estimate(&input).unwrap();
679        let (nx, ny, nz) = output.normals3().unwrap();
680        let expected = Vec3::new(-0.2, 0.3, 1.0).normalize();
681
682        for index in 0..input.len() {
683            let actual = Vec3::new(nx[index], ny[index], nz[index]).normalize();
684            assert!(actual.dot(expected) > 0.98, "tilted plane normal was {actual:?}");
685        }
686    }
687
688    #[test]
689    fn orient_normal_towards_viewpoint_works() {
690        let normal = Vec3::new(0.0, 0.0, -1.0);
691        let point = Vec3::new(0.0, 0.0, 0.0);
692        let viewpoint = Vec3::new(0.0, 0.0, 1.0);
693        let oriented = orient_normal_towards_viewpoint(normal, point, viewpoint);
694        assert!(oriented.z > 0.0);
695    }
696
697    #[test]
698    fn adds_curvature_field() {
699        let input = plane_cloud();
700        let estimator = NormalEstimator::new(NormalEstimationConfig::k_neighbors(10));
701        let output = estimator.estimate(&input).unwrap();
702        assert!(output.field("curvature").is_ok());
703    }
704
705    #[test]
706    fn rejects_unsupported_explicit_gpu_policy() {
707        let estimator = NormalEstimator::new(NormalEstimationConfig::default());
708        let error = estimator
709            .estimate_with_policy(&plane_cloud(), ExecutionPolicy::Gpu(DeviceKind::Cuda))
710            .unwrap_err();
711        assert!(matches!(error, spatialrust_core::SpatialError::InvalidArgument(_)));
712    }
713}
spatialrust_features/normal.rs

spatialrust_features/
normal.rs
