ogeom_algo/register/
symmetry.rs

1//! The plane a part is its own mirror image in.
2
3use nalgebra::SymmetricEigen;
4use ogeom_core::{OgeomResult, ogeom_bail, parallel};
5use ogeom_math::{Direction, Plane, Point, Vector};
6use ogeom_mesh::Bvh;
7
8use super::icp::{Mesh, Pair, Pose, Samples, rms};
9use super::{Icp, SymmetryFit};
10
11/// The plane the points are the mirror image of the mesh `target` was
12/// built over in, from each of `seeds`, the best scored.
13///
14/// From a seed, each round reflects the samples in the plane, registers
15/// the reflection back onto the mesh from where it lies (as [`icp`] does
16/// from the identity, whatever [`Icp::starts`] says), and bisects each kept
17/// sample and its registered image: the new plane's normal is the
18/// direction the samples' displacements to their images share (the
19/// eigenvector of the largest eigenvalue of the sum of their outer
20/// products, in the sense of the old normal), and it passes through the
21/// mean of their midpoints. Where the reflection in a plane registers
22/// with no motion the plane bisects every sample and its image already,
23/// so the rounds stop once a round moves no sample's reflection further
24/// than the confusion distance, or after [`Icp::max_iter`] rounds.
25///
26/// The seeds are ranked by the capped mean distance to the mesh of every
27/// sample reflected in the plane each ends at, ties going to the earlier
28/// seed; [`SymmetryFit::rms`] is the RMS of the kept ones'. `normals`, one
29/// per point, are reflected with them and used only for a facing limit.
30///
31/// [`icp`]: fn@super::icp
32///
33/// # Errors
34///
35/// As [`Icp::check`];
36/// [`OgeomError::Domain`](ogeom_core::OgeomError::Domain) for no points, a
37/// point not finite, an empty mesh or no seeds;
38/// [`OgeomError::Dimension`](ogeom_core::OgeomError::Dimension) for normals
39/// not one per point.
40pub fn symmetry_plane(
41    points: &[Point],
42    normals: &[Vector],
43    target: &Bvh,
44    seeds: &[Plane],
45    opts: &Icp,
46) -> OgeomResult<SymmetryFit> {
47    if seeds.is_empty() {
48        ogeom_bail!(Domain, "no seed planes to start from");
49    }
50    let mesh = Mesh::of(target)?;
51    let samples = Samples::of(points, Some(normals), opts)?;
52    let fits = parallel::map_ordered(seeds, |_, seed| from_seed(&mesh, &samples, *seed, opts));
53    let mut best: Option<SymmetryFit> = None;
54    for found in fits {
55        let found = found?;
56        if best.as_ref().is_none_or(|b| found.score < b.score) {
57            best = Some(found);
58        }
59    }
60    best.map_or_else(|| ogeom_bail!(Domain, "no seed planes to start from"), Ok)
61}
62
63/// The rounds from one seed, and the fit of the plane they end at.
64fn from_seed(mesh: &Mesh, samples: &Samples, seed: Plane, opts: &Icp) -> OgeomResult<SymmetryFit> {
65    let reach = opts.tol.confusion();
66    let mut plane = Mirror::of(&seed);
67    let mut converged = false;
68    for _ in 0..opts.max_iter {
69        let mirrored = plane.reflected(samples);
70        let run = mesh.register(&mirrored, opts, Pose::IDENTITY)?;
71        let Some(next) = plane.bisecting(samples, &run.pose, &run.paired.kept, opts) else {
72            break;
73        };
74        let moves = samples
75            .points
76            .iter()
77            .map(|&p| (next.reflect(p) - plane.reflect(p)).magnitude())
78            .fold(0.0, f64::max);
79        plane = next;
80        if moves <= reach {
81            converged = true;
82            break;
83        }
84    }
85    let paired = mesh.pair(&plane.reflected(samples), opts, &Pose::IDENTITY)?;
86    let normal = Direction::new(plane.normal, opts.tol)?;
87    Ok(SymmetryFit {
88        plane: Plane::through(plane.origin, normal),
89        rms: rms(&paired.kept),
90        matched: paired.kept.len(),
91        sampled: samples.points.len(),
92        score: paired.score,
93        converged,
94    })
95}
96
97/// A plane as a point on it and its unit normal.
98#[derive(Debug, Clone, Copy)]
99struct Mirror {
100    origin: Point,
101    normal: Vector,
102}
103
104impl Mirror {
105    fn of(plane: &Plane) -> Self {
106        let frame = plane.frame();
107        Self {
108            origin: frame.origin(),
109            normal: frame.z().vector(),
110        }
111    }
112
113    fn reflect(&self, p: Point) -> Point {
114        p - self.normal * (2.0 * (p - self.origin).dot(self.normal))
115    }
116
117    fn reflect_vector(&self, v: Vector) -> Vector {
118        v - self.normal * (2.0 * v.dot(self.normal))
119    }
120
121    /// The samples and their normals reflected in the plane.
122    fn reflected(&self, samples: &Samples) -> Samples {
123        samples.mapped(|p| self.reflect(p), |v| self.reflect_vector(v))
124    }
125
126    /// The plane bisecting each kept sample and its reflection registered
127    /// by `pose`; `None` where the displacements share no direction.
128    fn bisecting(&self, samples: &Samples, pose: &Pose, kept: &[Pair], opts: &Icp) -> Option<Self> {
129        let first = samples.points[kept.first()?.sample];
130        let m = pose.q.to_matrix();
131        let mut spread = nalgebra::Matrix3::<f64>::zeros();
132        let mut middle = Vector::ZERO;
133        for pair in kept {
134            let p = samples.points[pair.sample];
135            let image = pose.apply(&m, self.reflect(p));
136            let d = p - image;
137            let d = nalgebra::Vector3::new(d.x, d.y, d.z);
138            spread += d * d.transpose();
139            middle += (p - first + (image - first)) * 0.5;
140        }
141        let eigen = SymmetricEigen::new(spread);
142        let top = eigen.eigenvalues.imax();
143        if eigen.eigenvalues[top].is_nan() || eigen.eigenvalues[top] <= 0.0 {
144            return None;
145        }
146        let v = eigen.eigenvectors.column(top);
147        let normal = Direction::new(Vector::new(v[0], v[1], v[2]), opts.tol)
148            .ok()?
149            .vector();
150        let normal = if normal.dot(self.normal) < 0.0 {
151            -normal
152        } else {
153            normal
154        };
155        #[allow(clippy::cast_precision_loss, reason = "pair counts are far below 2^52")]
156        let count = kept.len() as f64;
157        Some(Self {
158            origin: first + middle / count,
159            normal,
160        })
161    }
162}
ogeom_algo/register/symmetry.rs

ogeom_algo/register/
symmetry.rs
