GitCorner

Code

ossartcrafteffectcraft🌐
665bfac0b339
//! Small dense linear algebra for the camera solver, written from the textbook methods:
//!
//! - 3-vectors / 3 × 3 matrices and rotations (Rodrigues' formula and its inverse);
//! - the **one-sided Jacobi SVD** (Hestenes 1958; Golub & Van Loan, *Matrix Computations*,
//!   §8.6.4), accurate for the small, badly scaled systems of the 8-point algorithm;
//! - the **cyclic Jacobi eigenvalue method** for symmetric matrices (Golub & Van Loan §8.5);
//! - **Cholesky** factorisation for the bundle adjustment's normal equations;
//! - the closed-form **similarity alignment** of two point sets (Umeyama, "Least-squares
//!   estimation of transformation parameters between two point patterns", PAMI 1991).

pub type V3 = [f64; 3];
pub type M3 = [[f64; 3]; 3];

pub const I3: M3 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];

pub fn add(a: V3, b: V3) -> V3 {
    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}
pub fn sub(a: V3, b: V3) -> V3 {
    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
pub fn scale(a: V3, k: f64) -> V3 {
    [a[0] * k, a[1] * k, a[2] * k]
}
pub fn dot(a: V3, b: V3) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
pub fn cross(a: V3, b: V3) -> V3 {
    [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
}
pub fn norm(a: V3) -> f64 {
    dot(a, a).sqrt()
}
pub fn normalize(a: V3) -> V3 {
    let n = norm(a);
    if n > 0.0 { scale(a, 1.0 / n) } else { a }
}
pub fn mv(m: &M3, v: V3) -> V3 {
    [dot(m[0], v), dot(m[1], v), dot(m[2], v)]
}
/// `mᵀ v`.
pub fn mtv(m: &M3, v: V3) -> V3 {
    [m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2], m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2], m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2]]
}
pub fn mm(a: &M3, b: &M3) -> M3 {
    let mut o = [[0.0; 3]; 3];
    for (i, row) in o.iter_mut().enumerate() {
        for (j, v) in row.iter_mut().enumerate() {
            *v = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j];
        }
    }
    o
}
pub fn mt(a: &M3) -> M3 {
    [[a[0][0], a[1][0], a[2][0]], [a[0][1], a[1][1], a[2][1]], [a[0][2], a[1][2], a[2][2]]]
}
pub fn madd(a: &M3, b: &M3, k: f64) -> M3 {
    let mut o = *a;
    for i in 0..3 {
        for j in 0..3 {
            o[i][j] += k * b[i][j];
        }
    }
    o
}
pub fn mscale(a: &M3, k: f64) -> M3 {
    madd(&[[0.0; 3]; 3], a, k)
}
pub fn det3(m: &M3) -> f64 {
    m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])
}
pub fn skew(v: V3) -> M3 {
    [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]
}
pub fn outer(a: V3, b: V3) -> M3 {
    [[a[0] * b[0], a[0] * b[1], a[0] * b[2]], [a[1] * b[0], a[1] * b[1], a[1] * b[2]], [a[2] * b[0], a[2] * b[1], a[2] * b[2]]]
}
pub fn col(m: &M3, j: usize) -> V3 {
    [m[0][j], m[1][j], m[2][j]]
}
pub fn from_cols(a: V3, b: V3, c: V3) -> M3 {
    [[a[0], b[0], c[0]], [a[1], b[1], c[1]], [a[2], b[2], c[2]]]
}
pub fn inv3(m: &M3) -> Option<M3> {
    let d = det3(m);
    if d.abs() < 1e-300 {
        return None;
    }
    let c = |r0: usize, c0: usize, r1: usize, c1: usize| m[r0][c0] * m[r1][c1] - m[r0][c1] * m[r1][c0];
    let adj = [[c(1, 1, 2, 2), -c(0, 1, 2, 2), c(0, 1, 1, 2)], [-c(1, 0, 2, 2), c(0, 0, 2, 2), -c(0, 0, 1, 2)], [c(1, 0, 2, 1), -c(0, 0, 2, 1), c(0, 0, 1, 1)]];
    Some(mscale(&adj, 1.0 / d))
}

/// Rotation matrix of an axis-angle vector (Rodrigues' formula).
pub fn rodrigues(w: V3) -> M3 {
    let th = norm(w);
    let k = skew(w);
    if th < 1e-12 {
        return madd(&I3, &k, 1.0);
    }
    let a = th.sin() / th;
    let b = (1.0 - th.cos()) / (th * th);
    madd(&madd(&I3, &k, a), &mm(&k, &k), b)
}

/// Axis-angle vector of a rotation matrix (inverse of [`rodrigues`]).
pub fn log_rot(r: &M3) -> V3 {
    let c = ((r[0][0] + r[1][1] + r[2][2] - 1.0) * 0.5).clamp(-1.0, 1.0);
    let th = c.acos();
    let v = [r[2][1] - r[1][2], r[0][2] - r[2][0], r[1][0] - r[0][1]];
    if th < 1e-9 {
        return scale(v, 0.5);
    }
    if std::f64::consts::PI - th < 1e-6 {
        // Near π: the axis is the dominant column of R + I.
        let b = madd(r, &I3, 1.0);
        let j = (0..3).max_by(|&a, &bb| b[a][a].total_cmp(&b[bb][bb])).unwrap_or(0);
        let axis = normalize(col(&b, j));
        return scale(axis, th);
    }
    scale(v, th / (2.0 * th.sin()))
}

/// The rotation closest to `m` (Frobenius norm), via the SVD.
pub fn nearest_rotation(m: &M3) -> M3 {
    let (u, _, v) = svd3(m);
    let mut r = mm(&u, &mt(&v));
    if det3(&r) < 0.0 {
        let mut u2 = u;
        for row in u2.iter_mut() {
            row[2] = -row[2];
        }
        r = mm(&u2, &mt(&v));
    }
    r
}

/// Spherical interpolation between two rotations.
pub fn slerp(a: &M3, b: &M3, t: f64) -> M3 {
    let d = log_rot(&mm(b, &mt(a)));
    mm(&rodrigues(scale(d, t)), a)
}

/// Thin SVD `A = U diag(s) Vᵀ` of an `m × n` row-major matrix (one-sided Jacobi). `U` is
/// `max(m, n) × n` (rows beyond `m` belong to zero padding), `s` is sorted descending and `V`
/// is `n × n` (columns are the right singular vectors).
pub fn svd(a: &[f64], m: usize, n: usize) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
    let rows = m.max(n);
    let (mut u, mut s, v) = jacobi_factors(a, m, n);
    for (j, sj) in s.iter().enumerate() {
        if *sj > 1e-300 {
            for r in 0..rows {
                u[r * n + j] /= sj;
            }
        }
    }
    // Sort descending.
    let mut order: Vec<usize> = (0..n).collect();
    order.sort_by(|&a, &b| s[b].total_cmp(&s[a]));
    let (mut u2, mut v2) = (vec![0.0; rows * n], vec![0.0; n * n]);
    for (k, &j) in order.iter().enumerate() {
        for r in 0..rows {
            u2[r * n + k] = u[r * n + j];
        }
        for r in 0..n {
            v2[r * n + k] = v[r * n + j];
        }
    }
    s = order.iter().map(|&j| s[j]).collect();
    (u2, s, v2)
}

// Unnormalised and unsorted factors; full SVD and null-space queries share every sweep.
fn jacobi_factors(a: &[f64], m: usize, n: usize) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
    let rows = m.max(n);
    let mut u = vec![0.0; rows * n];
    u[..m * n].copy_from_slice(&a[..m * n]);
    let mut v = vec![0.0; n * n];
    for i in 0..n {
        v[i * n + i] = 1.0;
    }
    for _sweep in 0..60 {
        let mut off = 0.0f64;
        for p in 0..n {
            for q in p + 1..n {
                let (mut alpha, mut beta, mut gamma) = (0.0, 0.0, 0.0);
                for r in 0..rows {
                    let (x, y) = (u[r * n + p], u[r * n + q]);
                    alpha += x * x;
                    beta += y * y;
                    gamma += x * y;
                }
                if gamma == 0.0 {
                    continue;
                }
                let c0 = gamma.abs() / (alpha * beta).sqrt().max(1e-300);
                off = off.max(c0);
                if c0 < 1e-15 {
                    continue;
                }
                let zeta = (beta - alpha) / (2.0 * gamma);
                let t = zeta.signum() / (zeta.abs() + (1.0 + zeta * zeta).sqrt());
                let t = if zeta == 0.0 { 1.0 } else { t };
                let c = 1.0 / (1.0 + t * t).sqrt();
                let s = c * t;
                for r in 0..rows {
                    let (x, y) = (u[r * n + p], u[r * n + q]);
                    u[r * n + p] = c * x - s * y;
                    u[r * n + q] = s * x + c * y;
                }
                for r in 0..n {
                    let (x, y) = (v[r * n + p], v[r * n + q]);
                    v[r * n + p] = c * x - s * y;
                    v[r * n + q] = s * x + c * y;
                }
            }
        }
        if off < 1e-15 {
            break;
        }
    }
    let s: Vec<f64> = (0..n).map(|j| (0..rows).map(|r| u[r * n + j] * u[r * n + j]).sum::<f64>().sqrt()).collect();
    (u, s, v)
}

/// SVD of a 3 × 3 matrix: (U, singular values descending, V) with `m = U diag(s) Vᵀ`.
pub fn svd3(m: &M3) -> (M3, V3, M3) {
    let a: Vec<f64> = m.iter().flatten().copied().collect();
    let (u, s, v) = svd(&a, 3, 3);
    let g = |x: &[f64]| [[x[0], x[1], x[2]], [x[3], x[4], x[5]], [x[6], x[7], x[8]]];
    let mut um = g(&u);
    // Columns of U for zero singular values are undefined: complete them to an orthonormal basis.
    if s[2] < 1e-300 {
        let c = cross(col(&um, 0), col(&um, 1));
        for (r, row) in um.iter_mut().enumerate() {
            row[2] = c[r];
        }
    }
    (um, [s[0], s[1], s[2]], g(&v))
}

/// The unit vector `x` minimising `|A x|` (`m × n` row-major): the right singular vector of
/// the smallest singular value.
pub fn null_vector(a: &[f64], m: usize, n: usize) -> Vec<f64> {
    let (_, s, v) = jacobi_factors(a, m, n);
    // The full SVD uses a stable descending sort: the last of equal minima wins.
    let smallest = (0..n).min_by(|&i, &j| s[i].total_cmp(&s[j]).then_with(|| j.cmp(&i))).unwrap_or(0);
    (0..n).map(|r| v[r * n + smallest]).collect()
}

/// Eigen-decomposition of a symmetric `n × n` matrix (cyclic Jacobi): eigenvalues ascending
/// and the matching eigenvectors as the columns of the returned row-major matrix.
pub fn sym_eigen(a: &[f64], n: usize) -> (Vec<f64>, Vec<f64>) {
    let mut a = a.to_vec();
    let mut v = vec![0.0; n * n];
    for i in 0..n {
        v[i * n + i] = 1.0;
    }
    for _ in 0..100 {
        let mut off = 0.0;
        for p in 0..n {
            for q in p + 1..n {
                off += a[p * n + q] * a[p * n + q];
            }
        }
        if off < 1e-30 {
            break;
        }
        for p in 0..n {
            for q in p + 1..n {
                let apq = a[p * n + q];
                if apq.abs() < 1e-300 {
                    continue;
                }
                let theta = (a[q * n + q] - a[p * n + p]) / (2.0 * apq);
                let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt());
                let t = if theta == 0.0 { 1.0 } else { t };
                let c = 1.0 / (t * t + 1.0).sqrt();
                let s = t * c;
                for k in 0..n {
                    let (akp, akq) = (a[k * n + p], a[k * n + q]);
                    a[k * n + p] = c * akp - s * akq;
                    a[k * n + q] = s * akp + c * akq;
                }
                for k in 0..n {
                    let (apk, aqk) = (a[p * n + k], a[q * n + k]);
                    a[p * n + k] = c * apk - s * aqk;
                    a[q * n + k] = s * apk + c * aqk;
                }
                for k in 0..n {
                    let (vkp, vkq) = (v[k * n + p], v[k * n + q]);
                    v[k * n + p] = c * vkp - s * vkq;
                    v[k * n + q] = s * vkp + c * vkq;
                }
            }
        }
    }
    let mut order: Vec<usize> = (0..n).collect();
    order.sort_by(|&x, &y| a[x * n + x].total_cmp(&a[y * n + y]));
    let vals = order.iter().map(|&j| a[j * n + j]).collect();
    let mut vecs = vec![0.0; n * n];
    for (k, &j) in order.iter().enumerate() {
        for r in 0..n {
            vecs[r * n + k] = v[r * n + j];
        }
    }
    (vals, vecs)
}

/// Dot product, four lanes (lets the compiler vectorise).
fn dot_n(a: &[f64], b: &[f64]) -> f64 {
    let mut acc = [0.0f64; 4];
    let ((ca, ra), (cb, rb)) = (a.as_chunks::<4>(), b.as_chunks::<4>());
    for (x, y) in ca.iter().zip(cb) {
        for k in 0..4 {
            acc[k] += x[k] * y[k];
        }
    }
    let mut s = acc[0] + acc[1] + acc[2] + acc[3];
    for (x, y) in ra.iter().zip(rb) {
        s += x * y;
    }
    s
}

/// Solve `A x = b` for a symmetric positive definite `A` (`n × n`, row-major) in place by
/// Cholesky factorisation. Returns false when `A` is not positive definite.
pub fn cholesky_solve(a: &mut [f64], n: usize, b: &mut [f64]) -> bool {
    if n.checked_mul(n) != Some(a.len()) || b.len() != n {
        return false;
    }
    let mut band = n.saturating_sub(1);
    if b.iter().all(|v| v.is_finite()) {
        band = 0;
        'bandwidth: for i in (0..n).rev() {
            for j in 0..i.saturating_sub(band) {
                if a[i * n + j] != 0.0 {
                    band = i - j;
                    // Wide systems (including shared global parameters) use the dense path.
                    if band >= n / 2 {
                        band = n.saturating_sub(1);
                        break 'bandwidth;
                    }
                    break;
                }
            }
        }
    }
    for j in 0..n {
        let mut d = a[j * n + j];
        for k in j.saturating_sub(band)..j {
            d -= a[j * n + k] * a[j * n + k];
        }
        if d <= 0.0 || !d.is_finite() {
            return false;
        }
        let d = d.sqrt();
        a[j * n + j] = d;
        let (head, tail) = a.split_at_mut((j + 1) * n);
        let rowj = &head[j * n..j * n + j];
        // Rows below j (sequential: per-column parallel dispatch costs more than it saves).
        for (off, row) in tail.chunks_mut(n).take(band).enumerate() {
            // Removing full groups of four zero products preserves dot_n's lane order.
            let start = (j + 1 + off).saturating_sub(band) / 4 * 4;
            row[j] = (row[j] - dot_n(&row[start..j], &rowj[start..])) / d;
        }
    }
    // Forward: L y = b.
    for i in 0..n {
        let mut s = b[i];
        let start = i.saturating_sub(band);
        // Subtracting signed-zero products can change -0.0 to +0.0. Preserve those
        // operations when the accumulator is zero; nonzero values are unaffected.
        if s == 0.0 {
            for k in 0..start {
                s -= a[i * n + k] * b[k];
            }
        }
        for k in start..i {
            s -= a[i * n + k] * b[k];
        }
        b[i] = s / a[i * n + i];
    }
    // Back: Lᵀ x = y.
    for i in (0..n).rev() {
        let mut s = b[i];
        let end = i.saturating_add(band).saturating_add(1).min(n);
        for k in i + 1..end {
            s -= a[k * n + i] * b[k];
        }
        if s == 0.0 {
            for k in end..n {
                s -= a[k * n + i] * b[k];
            }
        }
        b[i] = s / a[i * n + i];
    }
    b.iter().all(|v| v.is_finite())
}

/// Preconditioned conjugate gradients with an independently recomputed residual check.
/// Failure to converge (or non-finite/non-positive curvature) requests the direct fallback.
pub(crate) fn conjugate_gradient(rhs: &[f64], apply: impl Fn(&[f64], &mut [f64]), precondition: impl Fn(&[f64], &mut [f64])) -> Option<Vec<f64>> {
    let n = rhs.len();
    let norm2 = dot_n(rhs, rhs);
    if !norm2.is_finite() {
        return None;
    }
    if rhs.iter().all(|v| *v == 0.0) {
        return Some(vec![0.0; n]);
    }
    let tolerance2 = norm2 * 1e-22;
    if tolerance2 == 0.0 {
        return None;
    }
    let mut x = vec![0.0; n];
    let mut r = rhs.to_vec();
    let mut z = vec![0.0; n];
    let mut ap = vec![0.0; n];
    precondition(&r, &mut z);
    let mut p = z.clone();
    let mut rz = dot_n(&r, &z);
    for _ in 0..n.saturating_mul(2).min(512) {
        if !rz.is_finite() || rz <= 0.0 {
            return None;
        }
        apply(&p, &mut ap);
        let curvature = dot_n(&p, &ap);
        if !curvature.is_finite() || curvature <= 0.0 {
            return None;
        }
        let alpha = rz / curvature;
        for i in 0..n {
            x[i] += alpha * p[i];
            r[i] -= alpha * ap[i];
        }
        if dot_n(&r, &r) <= tolerance2 {
            apply(&x, &mut ap);
            for i in 0..n {
                ap[i] = rhs[i] - ap[i];
            }
            return (x.iter().all(|v| v.is_finite()) && dot_n(&ap, &ap) <= 100.0 * tolerance2).then_some(x);
        }
        precondition(&r, &mut z);
        let next_rz = dot_n(&r, &z);
        let beta = next_rz / rz;
        for i in 0..n {
            p[i] = z[i] + beta * p[i];
        }
        rz = next_rz;
    }
    None
}

/// A similarity transform `x ↦ s R x + t`.
#[derive(Clone, Copy, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
pub struct Similarity {
    pub s: f64,
    pub r: M3,
    pub t: V3,
}

impl Default for Similarity {
    fn default() -> Self {
        Similarity { s: 1.0, r: I3, t: [0.0; 3] }
    }
}

impl Similarity {
    pub fn apply(&self, x: V3) -> V3 {
        add(scale(mv(&self.r, x), self.s), self.t)
    }
}

/// The similarity minimising `Σ |dst − (s R src + t)|²` (Umeyama 1991).
pub fn umeyama(src: &[V3], dst: &[V3]) -> Option<Similarity> {
    let n = src.len().min(dst.len());
    if n < 3 {
        return None;
    }
    let k = 1.0 / n as f64;
    let ms = scale(src[..n].iter().fold([0.0; 3], |a, b| add(a, *b)), k);
    let md = scale(dst[..n].iter().fold([0.0; 3], |a, b| add(a, *b)), k);
    let mut cov = [[0.0; 3]; 3];
    let mut var = 0.0;
    for i in 0..n {
        let (a, b) = (sub(src[i], ms), sub(dst[i], md));
        cov = madd(&cov, &outer(b, a), k);
        var += dot(a, a) * k;
    }
    if var < 1e-300 {
        return None;
    }
    let (u, d, v) = svd3(&cov);
    let mut sgn = [1.0, 1.0, 1.0];
    if det3(&u) * det3(&v) < 0.0 {
        sgn[2] = -1.0;
    }
    let ud = from_cols(scale(col(&u, 0), sgn[0]), scale(col(&u, 1), sgn[1]), scale(col(&u, 2), sgn[2]));
    let r = mm(&ud, &mt(&v));
    let s = (d[0] * sgn[0] + d[1] * sgn[1] + d[2] * sgn[2]) / var;
    let t = sub(md, scale(mv(&r, ms), s));
    Some(Similarity { s, r, t })
}

/// Euler angles (degrees) `[x, y, z]` with `m = Rx(x) Ry(y) Rz(z)`: After Effects' Orientation
/// order (Z is applied to the vector first, then Y, then X; see `effectcraft_geom::Mat4::orientation`).
pub fn euler_xyz(m: &M3) -> V3 {
    let sy = m[0][2].clamp(-1.0, 1.0);
    let y = sy.asin();
    let (x, z) = if sy.abs() < 0.999_999 { ((-m[1][2]).atan2(m[2][2]), (-m[0][1]).atan2(m[0][0])) } else { (m[2][1].atan2(m[1][1]), 0.0) };
    [x.to_degrees(), y.to_degrees(), z.to_degrees()]
}

/// `Rx(x) Ry(y) Rz(z)` from degrees.
pub fn from_euler_xyz(e: V3) -> M3 {
    let (sx, cx) = e[0].to_radians().sin_cos();
    let (sy, cy) = e[1].to_radians().sin_cos();
    let (sz, cz) = e[2].to_radians().sin_cos();
    let rx = [[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]];
    let ry = [[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]];
    let rz = [[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]];
    mm(&rx, &mm(&ry, &rz))
}

#[cfg(test)]
mod tests {
    use super::*;

    fn dense_cholesky_reference(a: &mut [f64], n: usize, b: &mut [f64]) -> bool {
        for j in 0..n {
            let mut d = a[j * n + j];
            for k in 0..j {
                d -= a[j * n + k] * a[j * n + k];
            }
            if d <= 0.0 || !d.is_finite() {
                return false;
            }
            let d = d.sqrt();
            a[j * n + j] = d;
            let (head, tail) = a.split_at_mut((j + 1) * n);
            let rowj = &head[j * n..j * n + j];
            // Rows below j (sequential: per-column parallel dispatch costs more than it saves).
            for row in tail.chunks_mut(n) {
                row[j] = (row[j] - dot_n(&row[..j], rowj)) / d;
            }
        }
        // Forward: L y = b.
        for i in 0..n {
            let mut s = b[i];
            for k in 0..i {
                s -= a[i * n + k] * b[k];
            }
            b[i] = s / a[i * n + i];
        }
        // Back: Lᵀ x = y.
        for i in (0..n).rev() {
            let mut s = b[i];
            for k in i + 1..n {
                s -= a[k * n + i] * b[k];
            }
            b[i] = s / a[i * n + i];
        }
        b.iter().all(|v| v.is_finite())
    }

    #[test]
    fn banded_cholesky_is_bit_identical_to_dense() {
        for n in [1usize, 2, 3, 4, 5, 6, 7, 8, 13, 31, 65, 127] {
            for width in [0usize, 1, 2, 5, 11, n - 1] {
                for seed in 0..3 {
                    for scale in [1.0, 1e-100, 1e100] {
                        let mut a = vec![0.0; n * n];
                        for i in 0..n {
                            for j in 0..i {
                                let value = if i - j <= width {
                                    (((i * 13 + j * 17 + seed) % 23) as f64 - 11.0) / 128.0
                                } else if (i + j) % 2 == 0 {
                                    -0.0
                                } else {
                                    0.0
                                };
                                a[i * n + j] = value;
                                a[j * n + i] = value;
                            }
                        }
                        for i in 0..n {
                            a[i * n + i] = a[i * n..(i + 1) * n].iter().map(|v| v.abs()).sum::<f64>() + 1.0;
                        }
                        for value in &mut a {
                            *value *= scale;
                        }
                        let mut rhs: Vec<f64> = (0..n).map(|i| if i % 7 == 0 { -0.0 } else { ((i * 5 + seed) % 17) as f64 - 8.0 }).collect();
                        let (mut reference_a, mut reference_rhs) = (a.clone(), rhs.clone());
                        assert!(dense_cholesky_reference(&mut reference_a, n, &mut reference_rhs));
                        assert!(cholesky_solve(&mut a, n, &mut rhs));
                        assert_eq!(
                            rhs.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
                            reference_rhs.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
                            "n={n} width={width} seed={seed} scale={scale}"
                        );
                        assert_eq!(
                            a.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
                            reference_a.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
                            "factor n={n} width={width}"
                        );
                    }
                }
            }
        }
    }

    #[test]
    fn banded_cholesky_preserves_underflow_and_signed_zero() {
        let n = 8;
        let mut a = vec![0.0; n * n];
        for i in 0..n {
            for j in 0..n {
                a[i * n + j] = if i == j {
                    1e300
                } else if (i + j) % 2 == 0 {
                    -0.0
                } else {
                    0.0
                };
            }
        }
        let mut rhs = vec![-1e-300, 1e-300, -0.0, 0.0, -1e-300, 1e-300, 0.0, -0.0];
        let (mut original, mut expected) = (a.clone(), rhs.clone());
        assert!(dense_cholesky_reference(&mut original, n, &mut expected));
        assert!(cholesky_solve(&mut a, n, &mut rhs));
        assert_eq!(rhs.iter().map(|v| v.to_bits()).collect::<Vec<_>>(), expected.iter().map(|v| v.to_bits()).collect::<Vec<_>>());
    }

    #[test]
    fn cholesky_rejects_invalid_dimensions_without_indexing() {
        assert!(!cholesky_solve(&mut [], usize::MAX, &mut []));
        assert!(!cholesky_solve(&mut [1.0], 2, &mut [1.0, 1.0]));
        assert!(!cholesky_solve(&mut [1.0], 1, &mut []));
        assert!(cholesky_solve(&mut [], 0, &mut []));
    }

    #[test]
    fn conjugate_gradient_checks_residual_and_bad_curvature() {
        let n = 64;
        let rhs: Vec<f64> = (0..n).map(|i| (i % 11) as f64 - 5.0).collect();
        let apply = |x: &[f64], y: &mut [f64]| {
            for i in 0..n {
                y[i] = 4.0 * x[i] + if i > 0 { x[i - 1] } else { 0.0 } + if i + 1 < n { x[i + 1] } else { 0.0 };
            }
        };
        let x = conjugate_gradient(&rhs, apply, |r, z| {
            for (r, z) in r.iter().zip(z) {
                *z = *r / 4.0;
            }
        })
        .unwrap();
        let mut product = vec![0.0; n];
        apply(&x, &mut product);
        let residual: f64 = rhs.iter().zip(product).map(|(b, ax)| (b - ax).powi(2)).sum();
        assert!(residual < 1e-18 * dot_n(&rhs, &rhs));
        assert!(conjugate_gradient(&[1.0], |x, y| y[0] = -x[0], |r, z| z.copy_from_slice(r)).is_none());
        assert!(conjugate_gradient(&[f64::NAN], |_, _| {}, |_, _| {}).is_none());
        assert_eq!(conjugate_gradient(&[0.0], |_, _| {}, |_, _| {}).unwrap(), vec![0.0]);
    }

    #[test]
    fn svd_reconstructs() {
        let a = [3.0, 1.0, 2.0, -1.0, 4.0, 0.5, 2.0, 2.0, 7.0, 0.1, -3.0, 1.0];
        let (u, s, v) = svd(&a, 4, 3);
        for r in 0..4 {
            for c in 0..3 {
                let x: f64 = (0..3).map(|k| u[r * 3 + k] * s[k] * v[c * 3 + k]).sum();
                assert!((x - a[r * 3 + c]).abs() < 1e-10);
            }
        }
        assert!(s[0] >= s[1] && s[1] >= s[2]);
    }

    #[test]
    fn null_vector_matches_full_svd_bits_including_ties() {
        for (m, n) in [(0, 0), (1, 1), (3, 3), (8, 9), (9, 9), (20, 3), (100, 9)] {
            for scale in [0.0, 1e-100, 1.0, 1e100] {
                let a: Vec<f64> = (0..m * n).map(|i| (((i * 37) % 31) as f64 - 15.0) * scale).collect();
                let (_, _, v) = svd(&a, m, n);
                let expected: Vec<_> = (0..n).map(|r| v[r * n + n - 1].to_bits()).collect();
                assert_eq!(null_vector(&a, m, n).iter().map(|v| v.to_bits()).collect::<Vec<_>>(), expected);
            }
        }
        let a = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0];
        assert_eq!(null_vector(&a, 3, 3), vec![0.0, 0.0, 1.0]);
    }

    #[test]
    fn rotations_round_trip() {
        let w = [0.3, -1.2, 0.7];
        let r = rodrigues(w);
        let w2 = log_rot(&r);
        assert!(norm(sub(w, w2)) < 1e-12);
        let e = euler_xyz(&r);
        let r2 = from_euler_xyz(e);
        for i in 0..3 {
            for j in 0..3 {
                assert!((r[i][j] - r2[i][j]).abs() < 1e-12);
            }
        }
        assert!((det3(&r) - 1.0).abs() < 1e-12);
    }

    #[test]
    fn eigen_and_cholesky() {
        let a = [4.0, 1.0, 0.5, 1.0, 3.0, 0.2, 0.5, 0.2, 2.0];
        let (vals, vecs) = sym_eigen(&a, 3);
        for k in 0..3 {
            let v = [vecs[k], vecs[3 + k], vecs[6 + k]];
            let av = [dot([a[0], a[1], a[2]], v), dot([a[3], a[4], a[5]], v), dot([a[6], a[7], a[8]], v)];
            assert!(norm(sub(av, scale(v, vals[k]))) < 1e-10);
        }
        let mut m = a;
        let mut b = [1.0, 2.0, 3.0];
        assert!(cholesky_solve(&mut m, 3, &mut b));
        let r = [dot([a[0], a[1], a[2]], b), dot([a[3], a[4], a[5]], b), dot([a[6], a[7], a[8]], b)];
        assert!(norm(sub(r, [1.0, 2.0, 3.0])) < 1e-12);
    }

    #[test]
    fn umeyama_recovers_a_similarity() {
        let t = Similarity { s: 2.5, r: rodrigues([0.1, 0.4, -0.3]), t: [1.0, -2.0, 5.0] };
        let src = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 2.0, 3.0]];
        let dst: Vec<V3> = src.iter().map(|p| t.apply(*p)).collect();
        let e = umeyama(&src, &dst).unwrap();
        assert!((e.s - 2.5).abs() < 1e-10);
        assert!(norm(sub(e.t, t.t)) < 1e-9);
    }
}