fast_mesh_ops/

icp.rs

//! Rigid registration: point-to-plane ICP against a mesh and its BVH.
//!
//! [`icp()`] moves a cloud of points onto a fixed mesh. Each iteration finds
//! every point's closest point on the mesh with the BVH, keeps the closest
//! `overlap` fraction of them (trimmed ICP, so a partial overlap and outliers
//! do not pull the result), and solves one linearised least-squares step for
//! the rotation and translation that reduce the distances to those
//! triangles' planes.
//!
//! The iteration is local: it starts at the `initial` transform and finds the
//! nearest minimum, so the start has to be roughly right. "Converged" means
//! the iteration stopped moving with every degree of freedom constrained, not
//! that the alignment is the right one: read [`IcpReport::rmse`] for that.
//!
//! Directions with no constraint at all (a plane sliding in its own plane, a
//! tube along its axis) are left out of each step instead of being solved from
//! rounding noise.
//!
//! A settled iteration does not show that the data chose its pose: the facets
//! of a tessellated sphere, or noise on a plane, give every step something to
//! fit, so the normal equations look well conditioned and the iteration settles
//! on an arbitrary spin or slide. So [`icp()`] checks. Along each of the six
//! eigenmotions it displaces the pose and iterates again, on at most 10,000 of
//! the points. A pose is determined along a motion when the restart comes back,
//! or ends somewhere that fits clearly worse. When it stays displaced with a fit
//! as good, the data do not choose, and the motion is reported in
//! [`IcpReport::undetermined`] with the status [`IcpStatus::Underconstrained`].
//! This is a local test: a symmetry farther away than the displacements (a cube
//! turned by 90 degrees) is not seen.

use fast_mesh_bvh::Bvh;
use fast_mesh_query::{
    ClosestHit, ClosestOptions, QueryError, Rigid, closest_point_many, distance_to_triangle,
};
use fast_mesh_store::{Mesh, VertexId, par};

use crate::{OpsError, Result, filled, reserve};

/// Fewest correspondences that can determine a rigid transform.
const MIN_CORRESPONDENCES: usize = 6;

/// A motion is left out of the steps when its eigenvalue of the normalised
/// normal equations is below this fraction of the largest: it has no constraint
/// worth solving. The eigenvalue is a squared singular value, so this is a ratio
/// of `1e-3` in how strongly the distances respond to that motion. It sits above
/// the noise of `f32` positions, whose rounding jitters the face normals of a
/// finely tessellated, tilted plane. Passing it does not make a motion
/// determined: see [`undetermined_motions`].
pub(crate) const MIN_CONDITIONING: f64 = 1e-6;

/// The largest rotation of one step, in radians. Larger steps are shortened:
/// the linearisation means nothing that far out.
const MAX_STEP_ANGLE: f64 = 0.5;

/// The most points each restart of the determination check uses. The check's
/// cost is bounded by this, whatever the size of the input.
const PROBE_SAMPLES: usize = 10_000;

/// How far the determination check displaces the pose, as root mean square
/// point movement over the points' extent. Two distances, so that a repeating
/// surface whose period makes one of them land back on the same fit is seen by
/// the other.
const PROBE_DISTANCES: [f64; 2] = [0.1, 0.25];

/// The most steps of a restart of the determination check.
const PROBE_STEPS: usize = 12;

/// A restart stayed away if this fraction of the displacement along the motion
/// remains at its end. Measured restarts end near 0 or near 1 (`icp_survey` in
/// the tests, and `docs/benchmarks.md`).
const PROBE_MOVED: f64 = 0.25;

/// A different pose fits as well if its rmse is at most this multiple of the
/// pose under test's. Without a noise model, fits this close cannot be told
/// apart. Measured on `icp_survey`: at most 1.8 for flat motions, and at least
/// 3.5 for restarts that stayed away from a pose the data do determine.
const PROBE_AS_GOOD: f64 = 2.5;

/// ... plus this fraction of the extent, the level below which rmse is rounding
/// of the stored `f32` positions.
const PROBE_RMSE_FLOOR: f64 = 1e-6;

/// Distances per parallel chunk, and the fewest that go parallel.
const DISTANCE_CHUNK: usize = 1024;
const PAR_MIN_DISTANCES: usize = 4096;

/// Options for [`icp()`].
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct IcpOptions {
    /// The most steps to take. Zero evaluates the initial transform only.
    /// Default 50.
    pub max_iterations: usize,
    /// Stops when one step moves every point by at most this fraction of the
    /// points' extent (their largest distance from their centroid). Zero or
    /// greater. Default `1e-6`.
    pub tolerance: f64,
    /// The fraction of the points expected to lie over the fixed mesh, in
    /// `(0, 1]`: each iteration uses that many of them, closest first. Set it
    /// a little below the true overlap for partial scans. Default 1 (every
    /// point counts, so nothing is rejected).
    pub overlap: f64,
    /// Points farther than this from the fixed mesh are not used. Positive.
    /// Default `f32::INFINITY`.
    pub max_distance: f32,
}

impl Default for IcpOptions {
    fn default() -> Self {
        IcpOptions {
            max_iterations: 50,
            tolerance: 1e-6,
            overlap: 1.0,
            max_distance: f32::INFINITY,
        }
    }
}

/// Why [`icp()`] stopped.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum IcpStatus {
    /// A step fell below the tolerance, and the determination check (see the
    /// [module docs](self)) found every motion determined by the data.
    Converged,
    /// `max_iterations` steps ran out first, and no motion was found
    /// undetermined (the check does not run with zero iterations).
    MaxIterations,
    /// The data do not determine the pose: [`IcpReport::undetermined`] says
    /// along which motions, when the check could tell. The pose along those is
    /// arbitrary, whatever the rmse; distances are still minimised along the
    /// rest. Iteration may also have run out of steps, wandering along such a
    /// motion.
    Underconstrained,
    /// Fewer than six usable correspondences (see [`IcpOptions::max_distance`]).
    TooFewCorrespondences,
    /// A step took a point out of the range the query can represent.
    Diverged,
}

/// What [`icp()`] found. Every number is finite, except that `rmse` is
/// infinite when no point has a correspondence. `rmse` is measured in `f64` from
/// the points to the chosen triangles.
#[derive(Debug, Clone, PartialEq)]
pub struct IcpReport {
    /// The transform from the points to the fixed mesh's local coordinates.
    /// When the status is [`IcpStatus::Converged`] or
    /// [`IcpStatus::Underconstrained`], the pose iteration stopped at;
    /// otherwise the best pose seen (most points used, then lowest `rmse`).
    pub transform: Rigid,
    /// The root mean square distance from the used points to the fixed mesh
    /// at `transform`.
    pub rmse: f64,
    /// Steps taken.
    pub iterations: usize,
    /// Why iteration stopped.
    pub status: IcpStatus,
    /// Points given.
    pub samples: usize,
    /// Points used at `transform`.
    pub inliers: usize,
    /// The smallest eigenvalue over the largest of the normalised normal
    /// equations at the last step, in `[0, 1]`. Geometry only, with no reference
    /// to noise: a low value warns of a weak motion, but a high one does not
    /// show that the motion is determined, for which see `undetermined`. Zero
    /// when no step ran.
    pub conditioning: f64,
    /// The motions the data do not determine: moving the pose along one and
    /// iterating again ended at a different pose that fits as well. Empty when
    /// the status is [`IcpStatus::Converged`]. Weakest first.
    pub undetermined: Vec<IcpMotion>,
    /// The points' extent: their largest distance from their centroid.
    pub extent: f64,
}

/// A rigid motion the data do not determine, in the fixed mesh's local
/// coordinates: a point `p` moves along `translation + rotation x (p - about)`,
/// scaled so that the points move by 1 (root mean square).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct IcpMotion {
    /// The translation part.
    pub translation: [f64; 3],
    /// The rotation part, as an axis scaled by its rate.
    pub rotation: [f64; 3],
    /// The point the rotation turns about: the moved points' centroid.
    pub about: [f64; 3],
    /// The rmse of the other pose found along this motion (over at most
    /// 10,000 of the points).
    pub alternative_rmse: f64,
}

impl IcpReport {
    /// Whether the status is [`IcpStatus::Converged`].
    pub fn converged(&self) -> bool {
        self.status == IcpStatus::Converged
    }
}

/// Up to `max_samples` evenly spaced points on the surface of `mesh`, in its
/// stored local coordinates, without the origin: its triangles' centroids,
/// then the vertices its triangles use, the first of each stride of the two
/// lists together.
///
/// Vertices alone will not do on a coarse mesh: a vertex is equally close to
/// all the triangles around it, so its correspondences do not see all of a
/// corner's planes (a box's eight corners can all report the same two faces).
///
/// # Errors
/// [`OpsError::Store`] when out of memory.
pub fn sample_surface(mesh: &Mesh, max_samples: usize) -> Result<Vec<[f64; 3]>> {
    let mut used = filled(mesh.vertex_slots(), false)?;
    for t in 0..mesh.triangle_slots() {
        if let Some(c) = mesh.triangle(t as u32) {
            for v in c {
                used[v as usize] = true;
            }
        }
    }
    let total = mesh.triangle_count() + used.iter().filter(|&&u| u).count();
    let take = total.min(max_samples);
    let mut out = Vec::new();
    reserve(&mut out, take)?;
    if take == 0 {
        return Ok(out);
    }
    let mut seen = 0;
    let mut offer = |out: &mut Vec<[f64; 3]>, p: [f64; 3]| {
        // Taken when it starts a new stride of `total / take`. `u128`: the
        // product of two counts.
        let next = (out.len() as u128 * total as u128).div_ceil(take as u128);
        if out.len() < take && seen as u128 == next {
            out.push(p);
        }
        seen += 1;
    };
    for t in 0..mesh.triangle_slots() {
        let Some([a, b, c]) = mesh.triangle(t as u32) else {
            continue;
        };
        if let (Some(a), Some(b), Some(c)) = (mesh.position(a), mesh.position(b), mesh.position(c))
        {
            let centroid = std::array::from_fn(|i| {
                (f64::from(a[i]) + f64::from(b[i]) + f64::from(c[i])) / 3.0
            });
            offer(&mut out, centroid);
        }
    }
    for (v, _) in used.iter().enumerate().filter(|(_, u)| **u) {
        if let Some(p) = mesh.position(v as VertexId) {
            offer(&mut out, p.map(f64::from));
        }
    }
    Ok(out)
}

fn validate(points: &[[f64; 3]], fixed: &Mesh, options: &IcpOptions) -> Result<()> {
    let invalid = |m| Err(OpsError::InvalidInput(m));
    if points.is_empty() {
        return invalid("icp needs at least one point");
    }
    if u32::try_from(points.len()).is_err() {
        return invalid("icp takes fewer than 2^32 points");
    }
    if fixed.triangle_count() == 0 {
        return invalid("icp needs a fixed mesh with triangles");
    }
    if !points.iter().flatten().all(|c| c.is_finite()) {
        return invalid("icp points must be finite");
    }
    if !(options.tolerance >= 0.0 && options.tolerance.is_finite()) {
        return invalid("tolerance must be finite and zero or greater");
    }
    if !(options.overlap > 0.0 && options.overlap <= 1.0) {
        return invalid("overlap must be in (0, 1]");
    }
    if options.max_distance.is_nan() || options.max_distance <= 0.0 {
        return invalid("max_distance must be positive");
    }
    Ok(())
}

/// A correspondence kept this iteration.
#[derive(Clone, Copy)]
struct Kept {
    /// The point's index.
    point: u32,
    /// The closest distance.
    distance: f64,
}

/// The inputs and working buffers of one call.
struct Evaluator<'a> {
    points: &'a [[f64; 3]],
    fixed: &'a Mesh,
    bvh: &'a Bvh,
    query: ClosestOptions,
    keep: usize,
    moved: Vec<[f64; 3]>,
    rounded: Vec<[f32; 3]>,
    hits: Vec<Option<ClosestHit>>,
    distances: Vec<f64>,
    kept: Vec<Kept>,
}

impl<'a> Evaluator<'a> {
    fn new(
        points: &'a [[f64; 3]],
        fixed: &'a Mesh,
        bvh: &'a Bvh,
        options: &IcpOptions,
    ) -> Result<Self> {
        let n = points.len();
        let mut kept = Vec::new();
        reserve(&mut kept, n)?;
        Ok(Evaluator {
            points,
            fixed,
            bvh,
            query: ClosestOptions {
                max_distance: options.max_distance,
            },
            keep: (options.overlap * n as f64).ceil().clamp(1.0, n as f64) as usize,
            moved: filled(n, [0.0; 3])?,
            rounded: filled(n, [0.0; 3])?,
            hits: filled(n, None)?,
            distances: filled(n, 0.0)?,
            kept,
        })
    }

    /// Finds the correspondences at `transform` and trims them. Returns the
    /// rmse over the kept ones (infinite with none), or `None` when a moved
    /// point leaves `f32` range.
    fn evaluate(&mut self, transform: &Rigid) -> Result<Option<f64>> {
        for ((p, m), r) in self
            .points
            .iter()
            .zip(&mut self.moved)
            .zip(&mut self.rounded)
        {
            *m = apply(transform, *p);
            *r = m.map(|c| c as f32);
            if !r.iter().all(|c| c.is_finite()) {
                return Ok(None);
            }
        }
        closest_point_many(
            self.fixed,
            self.bvh,
            &self.rounded,
            &self.query,
            &mut self.hits,
        )?;
        // The query's `f32` distance picks the triangle. The distance itself is
        // measured again in `f64`, since on long thin triangles the `f32` one
        // is mostly rounding.
        {
            let (fixed, moved, hits) = (self.fixed, &self.moved, &self.hits);
            par::for_each_chunk_mut(
                &mut self.distances,
                DISTANCE_CHUNK,
                PAR_MIN_DISTANCES,
                |i, slots| {
                    let start = i * DISTANCE_CHUNK;
                    for (k, slot) in slots.iter_mut().enumerate() {
                        let Some(hit) = hits[start + k] else {
                            continue;
                        };
                        *slot = corners_of(fixed, hit).map_or(f64::from(hit.distance), |c| {
                            distance_to_triangle(moved[start + k], c)
                        });
                    }
                },
            );
        }
        self.kept.clear();
        for (i, hit) in self.hits.iter().enumerate() {
            if hit.is_some() {
                self.kept.push(Kept {
                    point: i as u32,
                    distance: self.distances[i],
                });
            }
        }
        if self.kept.len() > self.keep {
            // A total order, so the kept set is unique: it does not depend on
            // the selection's internals.
            let order = |a: &Kept, b: &Kept| {
                a.distance
                    .total_cmp(&b.distance)
                    .then(a.point.cmp(&b.point))
            };
            self.kept.select_nth_unstable_by(self.keep - 1, order);
            self.kept.truncate(self.keep);
        }
        if self.kept.is_empty() {
            return Ok(Some(f64::INFINITY));
        }
        let sum: f64 = self.kept.iter().map(|k| k.distance * k.distance).sum();
        Ok(Some((sum / self.kept.len() as f64).sqrt()))
    }

    /// The normal equations of the kept correspondences, about `center` with
    /// rotations scaled by `extent`: `(hessian, gradient, rows)`, in the
    /// unknowns `(translation, rotation * extent)`. Corners are read in `f64`,
    /// so the residuals carry no `f32` query rounding. A correspondence on a
    /// zero-area triangle has no plane and is skipped.
    #[allow(clippy::needless_range_loop)] // Matrix indices read better here.
    fn assemble(&self, center: [f64; 3], extent: f64) -> ([[f64; 6]; 6], [f64; 6], usize) {
        let mut h = [[0.0; 6]; 6];
        let mut g = [0.0; 6];
        let mut rows = 0;
        for k in &self.kept {
            let Some((a, n)) = self.hits[k.point as usize].and_then(|hit| self.plane(hit)) else {
                continue;
            };
            let p = self.moved[k.point as usize];
            let residual = dot(n, sub(p, a));
            let lever = cross(sub(p, center), n);
            let row = [
                n[0],
                n[1],
                n[2],
                lever[0] / extent,
                lever[1] / extent,
                lever[2] / extent,
            ];
            for (i, ri) in row.iter().enumerate() {
                g[i] -= ri * residual;
                for (j, rj) in row.iter().enumerate().skip(i) {
                    h[i][j] += ri * rj;
                }
            }
            rows += 1;
        }
        for i in 0..6 {
            for j in 0..i {
                h[i][j] = h[j][i];
            }
        }
        (h, g, rows)
    }

    /// A corner of the hit's triangle and its unit normal, or `None` for a
    /// triangle with no area.
    fn plane(&self, hit: ClosestHit) -> Option<([f64; 3], [f64; 3])> {
        let [a, b, c] = corners_of(self.fixed, hit)?;
        Some((a, unit(cross(sub(b, a), sub(c, a)))?))
    }
}

/// Aligns `points` to the mesh `fixed` by rigid, point-to-plane ICP, starting
/// from `initial`. See the [module docs](self).
///
/// The report's transform maps the points to `fixed`'s local coordinates (the
/// stored `f32` positions, without the origin). The correspondence search
/// rounds moved points to `f32`; the distances the solver minimises and reports
/// are computed in `f64`. Results do not depend on the `parallel` feature or the
/// thread count. The determination check adds a bounded cost: up to 12 restarts
/// of up to 12 steps on at most 10,000 points.
///
/// # Errors
/// [`OpsError::InvalidInput`] for no points, non-finite points, a fixed mesh
/// without triangles, or an invalid option; [`OpsError::Query`] for an invalid
/// `initial`, one that takes a point out of `f32` range, or a `bvh` that does
/// not match `fixed`; [`OpsError::Store`] when out of memory.
pub fn icp(
    points: &[[f64; 3]],
    fixed: &Mesh,
    bvh: &Bvh,
    initial: &Rigid,
    options: &IcpOptions,
) -> Result<IcpReport> {
    validate(points, fixed, options)?;
    initial.validate()?;
    let (center0, extent) = spread(points);
    let mut eval = Evaluator::new(points, fixed, bvh, options)?;
    let run = iterate(
        &mut eval,
        initial,
        center0,
        extent,
        options.max_iterations,
        options.tolerance,
    )?;
    if run.iterations == 0 && run.status == IcpStatus::Diverged {
        return Err(QueryError::InvalidInput("query point must be finite").into());
    }
    let mut status = run.status;
    let mut undetermined = Vec::new();
    // Only a pose iteration reached has anything to check; zero iterations
    // were asked to measure, not to determine.
    if run.iterations > 0
        && matches!(
            status,
            IcpStatus::Converged | IcpStatus::MaxIterations | IcpStatus::Underconstrained
        )
    {
        undetermined = undetermined_motions(points, fixed, bvh, &run, center0, extent, options)?;
        if !undetermined.is_empty() {
            status = IcpStatus::Underconstrained;
        }
    }
    Ok(IcpReport {
        transform: run.transform,
        rmse: run.rmse,
        iterations: run.iterations,
        status,
        samples: points.len(),
        inliers: run.inliers,
        conditioning: run.conditioning,
        undetermined,
        extent,
    })
}

/// Where one run of the iteration ended.
struct Run {
    transform: Rigid,
    rmse: f64,
    inliers: usize,
    iterations: usize,
    status: IcpStatus,
    conditioning: f64,
}

/// Iterates from `start`. When the first evaluation already leaves `f32` range
/// the status is [`IcpStatus::Diverged`] with no iterations.
fn iterate(
    eval: &mut Evaluator,
    start: &Rigid,
    center0: [f64; 3],
    extent: f64,
    max_iterations: usize,
    tolerance: f64,
) -> Result<Run> {
    let mut transform = *start;
    let mut best = (*start, f64::INFINITY, 0);
    let mut last = best;
    let mut evaluated = false;
    let mut iterations = 0;
    let mut conditioning = 0.0;
    // Set once a step fell below the tolerance: the next pass only evaluates.
    let mut settled = None;
    let status = loop {
        let Some(rmse) = eval.evaluate(&transform)? else {
            break IcpStatus::Diverged;
        };
        let inliers = eval.kept.len();
        last = (transform, rmse, inliers);
        if !evaluated || inliers > best.2 || (inliers == best.2 && rmse < best.1) {
            best = last;
        }
        evaluated = true;
        if let Some(status) = settled {
            break status;
        }
        if inliers < MIN_CORRESPONDENCES {
            break IcpStatus::TooFewCorrespondences;
        }
        if iterations == max_iterations {
            break IcpStatus::MaxIterations;
        }
        let center = apply(&transform, center0);
        let (h, g, rows) = eval.assemble(center, extent);
        if rows < MIN_CORRESPONDENCES {
            break IcpStatus::TooFewCorrespondences;
        }
        let (step, c) = solve(&h, &g);
        conditioning = c;
        iterations += 1;
        let rotation = [step[3] / extent, step[4] / extent, step[5] / extent];
        let angle = norm(rotation);
        let shrink = if angle > MAX_STEP_ANGLE {
            MAX_STEP_ANGLE / angle
        } else {
            1.0
        };
        let translation = [step[0], step[1], step[2]].map(|v| v * shrink);
        let rotation = rotation.map(|v| v * shrink);
        transform = compose_step(&transform, center, translation, rotation);
        // An upper bound on how far any point moved, over the extent.
        if (norm(translation) + norm(rotation) * extent) / extent <= tolerance {
            settled = Some(if conditioning >= MIN_CONDITIONING {
                IcpStatus::Converged
            } else {
                IcpStatus::Underconstrained
            });
        }
    };
    let (transform, rmse, inliers) = if settled == Some(status) { last } else { best };
    Ok(Run {
        transform,
        rmse,
        inliers,
        iterations,
        status,
        conditioning,
    })
}

/// The determination check: a pose is determined if the data bring the
/// iteration back to it. Along each of the six eigenmotions of the normal
/// equations at `run`'s pose, restarts displaced by [`PROBE_DISTANCES`] and
/// reports the motions along which a restart stayed away with a fit as good, so
/// that the data do not choose between the two.
fn undetermined_motions(
    points: &[[f64; 3]],
    fixed: &Mesh,
    bvh: &Bvh,
    run: &Run,
    center0: [f64; 3],
    extent: f64,
    options: &IcpOptions,
) -> Result<Vec<IcpMotion>> {
    let take = points.len().min(PROBE_SAMPLES);
    let mut subset = Vec::new();
    reserve(&mut subset, take)?;
    subset.extend((0..take).map(|i| points[i * points.len() / take]));
    let mut eval = Evaluator::new(&subset, fixed, bvh, options)?;
    let mut out = Vec::new();
    let Some(reference) = eval.evaluate(&run.transform)? else {
        return Ok(out);
    };
    if eval.kept.len() < MIN_CORRESPONDENCES {
        return Ok(out);
    }
    let center = apply(&run.transform, center0);
    let (h, _, rows) = eval.assemble(center, extent);
    if rows < MIN_CORRESPONDENCES {
        return Ok(out);
    }
    let mut here = Vec::new();
    reserve(&mut here, eval.moved.len())?;
    here.extend_from_slice(&eval.moved);
    let (values, vectors) = jacobi_eigen(h);
    // Weakest first, and independent of the order Jacobi left them in.
    let mut order = [0, 1, 2, 3, 4, 5];
    order.sort_by(|&a, &b| values[a].total_cmp(&values[b]).then(a.cmp(&b)));
    for k in order {
        let translation = [vectors[0][k], vectors[1][k], vectors[2][k]];
        let rotation = [vectors[3][k], vectors[4][k], vectors[5][k]].map(|v| v / extent);
        // Scaled so that the motion moves the points by one unit, rms.
        let sum: f64 = here
            .iter()
            .map(|p| {
                let d = add(translation, cross(rotation, sub(*p, center)));
                dot(d, d)
            })
            .sum();
        let rms = (sum / here.len() as f64).sqrt();
        if rms <= 0.0 {
            continue;
        }
        let translation = translation.map(|v| v / rms);
        let rotation = rotation.map(|v| v / rms);
        for fraction in PROBE_DISTANCES {
            let distance = fraction * extent;
            let start = compose_step(
                &run.transform,
                center,
                translation.map(|v| v * distance),
                rotation.map(|v| v * distance),
            );
            let restart = iterate(
                &mut eval,
                &start,
                center0,
                extent,
                PROBE_STEPS,
                options.tolerance,
            )?;
            if matches!(
                restart.status,
                IcpStatus::TooFewCorrespondences | IcpStatus::Diverged
            ) {
                continue;
            }
            // How much of the displacement along this motion is still there.
            // Only along it: a restart may also wander along another motion,
            // which is that motion's own result.
            let sum: f64 = subset
                .iter()
                .zip(&here)
                .map(|(p, h)| {
                    let along = add(translation, cross(rotation, sub(*h, center)));
                    dot(sub(apply(&restart.transform, *p), *h), along)
                })
                .sum();
            let moved = (sum / here.len() as f64).abs();
            let as_good = restart.rmse <= PROBE_AS_GOOD * reference + PROBE_RMSE_FLOOR * extent;
            if moved >= PROBE_MOVED * distance && as_good {
                out.push(IcpMotion {
                    translation,
                    rotation,
                    about: center,
                    alternative_rmse: restart.rmse,
                });
                break;
            }
        }
    }
    Ok(out)
}

fn corners_of(mesh: &Mesh, hit: ClosestHit) -> Option<[[f64; 3]; 3]> {
    let [a, b, c] = mesh.triangle(hit.triangle)?;
    Some([
        mesh.position(a)?.map(f64::from),
        mesh.position(b)?.map(f64::from),
        mesh.position(c)?.map(f64::from),
    ])
}

/// The centroid of `points` and their largest distance from it (1 when they
/// all coincide, so that dividing by it is safe).
fn spread(points: &[[f64; 3]]) -> ([f64; 3], f64) {
    let mut sum = [0.0; 3];
    for p in points {
        sum = add(sum, *p);
    }
    let center = sum.map(|c| c / points.len() as f64);
    let far = points
        .iter()
        .map(|p| dot(sub(*p, center), sub(*p, center)))
        .fold(0.0, f64::max)
        .sqrt();
    (center, if far > 0.0 { far } else { 1.0 })
}

/// Solves `h x = g` over the constrained eigendirections only, and returns
/// `x` with the conditioning (smallest eigenvalue over the largest).
fn solve(h: &[[f64; 6]; 6], g: &[f64; 6]) -> ([f64; 6], f64) {
    let (values, vectors) = jacobi_eigen(*h);
    let largest = values.iter().copied().fold(0.0, f64::max);
    let smallest = values
        .iter()
        .copied()
        .fold(f64::INFINITY, f64::min)
        .max(0.0);
    let mut x = [0.0; 6];
    if largest <= 0.0 {
        return (x, 0.0);
    }
    for (k, &value) in values.iter().enumerate() {
        if value < MIN_CONDITIONING * largest {
            continue;
        }
        let along = (0..6).map(|i| vectors[i][k] * g[i]).sum::<f64>() / value;
        for (xi, row) in x.iter_mut().zip(&vectors) {
            *xi += row[k] * along;
        }
    }
    (x, smallest / largest)
}

/// The eigenvalues of a symmetric matrix and its eigenvectors (columns), by
/// cyclic Jacobi rotations.
#[allow(clippy::needless_range_loop)] // Matrix indices read better here.
fn jacobi_eigen(mut a: [[f64; 6]; 6]) -> ([f64; 6], [[f64; 6]; 6]) {
    let mut v = [[0.0; 6]; 6];
    for (i, row) in v.iter_mut().enumerate() {
        row[i] = 1.0;
    }
    // Jacobi converges quadratically: 6 x 6 settles within a dozen sweeps.
    for _ in 0..32 {
        let off: f64 = (0..6)
            .flat_map(|i| (i + 1..6).map(move |j| (i, j)))
            .map(|(i, j)| a[i][j] * a[i][j])
            .sum();
        let diag: f64 = (0..6).map(|i| a[i][i] * a[i][i]).sum();
        if off <= 1e-36 * diag {
            break;
        }
        for p in 0..6 {
            for q in p + 1..6 {
                if a[p][q] == 0.0 {
                    continue;
                }
                let theta = (a[q][q] - a[p][p]) / (2.0 * a[p][q]);
                let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt());
                let c = 1.0 / (t * t + 1.0).sqrt();
                let s = t * c;
                for row in &mut a {
                    let (ap, aq) = (row[p], row[q]);
                    row[p] = c * ap - s * aq;
                    row[q] = s * ap + c * aq;
                }
                for k in 0..6 {
                    let (ap, aq) = (a[p][k], a[q][k]);
                    a[p][k] = c * ap - s * aq;
                    a[q][k] = s * ap + c * aq;
                }
                for row in &mut v {
                    let (vp, vq) = (row[p], row[q]);
                    row[p] = c * vp - s * vq;
                    row[q] = s * vp + c * vq;
                }
            }
        }
    }
    (std::array::from_fn(|i| a[i][i]), v)
}

/// `transform` followed by the rotation vector `rotation` about `center` and
/// the step `translation`, re-orthonormalised.
fn compose_step(
    transform: &Rigid,
    center: [f64; 3],
    translation: [f64; 3],
    rotation: [f64; 3],
) -> Rigid {
    let d = rotation_of(rotation);
    let r = &transform.rotation;
    let product: [[f64; 3]; 3] = std::array::from_fn(|i| {
        std::array::from_fn(|j| d[i][0] * r[0][j] + d[i][1] * r[1][j] + d[i][2] * r[2][j])
    });
    let t = sub(transform.translation, center);
    let turned = [dot(d[0], t), dot(d[1], t), dot(d[2], t)];
    Rigid {
        rotation: orthonormal(product),
        translation: add(add(center, translation), turned),
    }
}

/// Rodrigues' formula for a rotation vector.
fn rotation_of(w: [f64; 3]) -> [[f64; 3]; 3] {
    let angle = norm(w);
    // sin(a)/a and (1 - cos(a))/a², with their limits near zero.
    let (s, c) = if angle < 1e-8 {
        (1.0, 0.5)
    } else {
        (angle.sin() / angle, (1.0 - angle.cos()) / (angle * angle))
    };
    let [x, y, z] = w;
    [
        [
            1.0 - c * (y * y + z * z),
            c * x * y - s * z,
            c * x * z + s * y,
        ],
        [
            c * x * y + s * z,
            1.0 - c * (x * x + z * z),
            c * y * z - s * x,
        ],
        [
            c * x * z - s * y,
            c * y * z + s * x,
            1.0 - c * (x * x + y * y),
        ],
    ]
}

/// Gram-Schmidt on the rows; the third row is the cross product, which keeps
/// the determinant at +1. The input is within rounding of a rotation, so the
/// fallbacks are never reached in practice.
fn orthonormal(m: [[f64; 3]; 3]) -> [[f64; 3]; 3] {
    let r0 = unit(m[0]).unwrap_or([1.0, 0.0, 0.0]);
    let d = dot(m[1], r0);
    let r1 = unit([
        m[1][0] - d * r0[0],
        m[1][1] - d * r0[1],
        m[1][2] - d * r0[2],
    ])
    .unwrap_or([0.0, 1.0, 0.0]);
    [r0, r1, cross(r0, r1)]
}

fn apply(x: &Rigid, p: [f64; 3]) -> [f64; 3] {
    let r = &x.rotation;
    let t = x.translation;
    std::array::from_fn(|i| r[i][0] * p[0] + r[i][1] * p[1] + r[i][2] * p[2] + t[i])
}

fn add(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}

fn sub(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}

fn dot(a: [f64; 3], b: [f64; 3]) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}

fn norm(a: [f64; 3]) -> f64 {
    dot(a, a).sqrt()
}

/// `a` over its length, or `None` for the zero vector.
fn unit(a: [f64; 3]) -> Option<[f64; 3]> {
    let n = norm(a);
    (n > 0.0).then(|| a.map(|c| c / n))
}

#[cfg(test)]
#[path = "icp_tests.rs"]
mod tests;