Skip to main content

qualia_core_db/medical/reconstruction/
anatomy_reg.rs

1//! Source-bound patient-space (DICOM LPS mm) to anatomical/target coordinate frame
2//! registration with `FrameOfReferenceUID` verification, `SO(3)` / affine invertibility,
3//! roundtrip consistency enforcement, and distinct BLAKE3 derivation stamps.
4use crate::medical::dicom_geometry::{GeometryError, PatientImagePlane, ORTHONORMAL_TOLERANCE};
5use crate::solvers::linear_algebra::cholesky::{cholesky_factor, cholesky_solve};
6
7#[derive(Debug, Clone, Copy, PartialEq, Eq)]
8pub enum AnatomyRegError {
9    MissingFrameOfReference,
10    FrameOfReferenceMismatch,
11    Geometry(GeometryError),
12    NonFiniteTransform,
13    NonOrthonormalRotation,
14    ReflectionRejected,
15    SingularTransform,
16    InvalidAffineRow,
17    RoundtripResidualExceeded,
18    EmptyInput,
19    LengthMismatch,
20    InvalidWeight,
21    NumericalSolverFailed,
22}
23
24impl From<GeometryError> for AnatomyRegError {
25    fn from(e: GeometryError) -> Self {
26        Self::Geometry(e)
27    }
28}
29impl std::fmt::Display for AnatomyRegError {
30    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
31        write!(f, "anatomy registration error: {self:?}")
32    }
33}
34impl std::error::Error for AnatomyRegError {}
35
36#[derive(Clone, Copy, Debug)]
37pub enum AnatomyTransform {
38    Rigid {
39        rotation_3x3: [[f64; 3]; 3],
40        translation_mm: [f64; 3],
41    },
42    Affine4x4 {
43        matrix: [[f64; 4]; 4],
44    },
45}
46
47#[derive(Clone, Copy, Debug)]
48pub struct CrossFrameRegistrationCertificate<'a> {
49    pub source_frame_of_reference_uid: &'a str,
50    pub target_frame_of_reference_uid: &'a str,
51    pub certificate_blake3: [u8; 32],
52}
53
54#[derive(Clone, Debug)]
55pub struct RegisteredAnatomyPoint<'a> {
56    pub source_blake3: [u8; 32],
57    pub derivation_blake3: [u8; 32],
58    pub source_frame_of_reference_uid: &'a str,
59    pub target_frame_of_reference_uid: &'a str,
60    pub patient_lps_mm: [f64; 3],
61    pub anatomy_coords_mm: [f64; 3],
62    pub roundtrip_lps_mm: [f64; 3],
63    pub roundtrip_residual_mm: f64,
64    pub inverse_matrix_4x4: [[f64; 4]; 4],
65}
66
67fn mat3_det(r: &[[f64; 3]; 3]) -> f64 {
68    r[0][0] * (r[1][1] * r[2][2] - r[1][2] * r[2][1])
69        - r[0][1] * (r[1][0] * r[2][2] - r[1][2] * r[2][0])
70        + r[0][2] * (r[1][0] * r[2][1] - r[1][1] * r[2][0])
71}
72
73fn mat3_inv(m: &[[f64; 3]; 3]) -> Option<[[f64; 3]; 3]> {
74    let c00 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
75    let c01 = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]);
76    let c02 = m[1][0] * m[2][1] - m[1][1] * m[2][0];
77    let det = m[0][0] * c00 + m[0][1] * c01 + m[0][2] * c02;
78    if det.abs() < 1e-9 || !det.is_finite() {
79        return None;
80    }
81    let inv = 1.0 / det;
82    let c10 = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]);
83    let c11 = m[0][0] * m[2][2] - m[0][2] * m[2][0];
84    let c12 = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]);
85    let c20 = m[0][1] * m[1][2] - m[0][2] * m[1][1];
86    let c21 = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]);
87    let c22 = m[0][0] * m[1][1] - m[0][1] * m[1][0];
88    Some([
89        [c00 * inv, c10 * inv, c20 * inv],
90        [c01 * inv, c11 * inv, c21 * inv],
91        [c02 * inv, c12 * inv, c22 * inv],
92    ])
93}
94
95fn apply_4x4(m: &[[f64; 4]; 4], p: [f64; 3]) -> [f64; 3] {
96    [
97        m[0][0] * p[0] + m[0][1] * p[1] + m[0][2] * p[2] + m[0][3],
98        m[1][0] * p[0] + m[1][1] * p[1] + m[1][2] * p[2] + m[1][3],
99        m[2][0] * p[0] + m[2][1] * p[1] + m[2][2] * p[2] + m[2][3],
100    ]
101}
102
103fn validate_and_invert_transform(
104    tf: &AnatomyTransform,
105) -> Result<([[f64; 4]; 4], [[f64; 4]; 4]), AnatomyRegError> {
106    match *tf {
107        AnatomyTransform::Rigid { rotation_3x3: r, translation_mm: t } => {
108            if r.iter().flatten().chain(t.iter()).any(|v| !v.is_finite()) {
109                return Err(AnatomyRegError::NonFiniteTransform);
110            }
111            for i in 0..3 {
112                for j in 0..3 {
113                    let dot = r[0][i] * r[0][j] + r[1][i] * r[1][j] + r[2][i] * r[2][j];
114                    let expect = if i == j { 1.0 } else { 0.0 };
115                    if (dot - expect).abs() > ORTHONORMAL_TOLERANCE {
116                        return Err(AnatomyRegError::NonOrthonormalRotation);
117                    }
118                }
119            }
120            let det = mat3_det(&r);
121            if det < 0.0 {
122                return Err(AnatomyRegError::ReflectionRejected);
123            }
124            if (det - 1.0).abs() > ORTHONORMAL_TOLERANCE {
125                return Err(AnatomyRegError::NonOrthonormalRotation);
126            }
127            let fwd = [
128                [r[0][0], r[0][1], r[0][2], t[0]],
129                [r[1][0], r[1][1], r[1][2], t[1]],
130                [r[2][0], r[2][1], r[2][2], t[2]],
131                [0.0, 0.0, 0.0, 1.0],
132            ];
133            let rt = [
134                [r[0][0], r[1][0], r[2][0]],
135                [r[0][1], r[1][1], r[2][1]],
136                [r[0][2], r[1][2], r[2][2]],
137            ];
138            let inv_t = [
139                -(rt[0][0] * t[0] + rt[0][1] * t[1] + rt[0][2] * t[2]),
140                -(rt[1][0] * t[0] + rt[1][1] * t[1] + rt[1][2] * t[2]),
141                -(rt[2][0] * t[0] + rt[2][1] * t[1] + rt[2][2] * t[2]),
142            ];
143            let inv = [
144                [rt[0][0], rt[0][1], rt[0][2], inv_t[0]],
145                [rt[1][0], rt[1][1], rt[1][2], inv_t[1]],
146                [rt[2][0], rt[2][1], rt[2][2], inv_t[2]],
147                [0.0, 0.0, 0.0, 1.0],
148            ];
149            Ok((fwd, inv))
150        }
151        AnatomyTransform::Affine4x4 { matrix: m } => {
152            if m.iter().flatten().any(|v| !v.is_finite()) {
153                return Err(AnatomyRegError::NonFiniteTransform);
154            }
155            if (m[3][0]).abs() > 1e-9
156                || (m[3][1]).abs() > 1e-9
157                || (m[3][2]).abs() > 1e-9
158                || (m[3][3] - 1.0).abs() > 1e-9
159            {
160                return Err(AnatomyRegError::InvalidAffineRow);
161            }
162            let lin = [
163                [m[0][0], m[0][1], m[0][2]],
164                [m[1][0], m[1][1], m[1][2]],
165                [m[2][0], m[2][1], m[2][2]],
166            ];
167            let lin_inv = mat3_inv(&lin).ok_or(AnatomyRegError::SingularTransform)?;
168            let inv_t = [
169                -(lin_inv[0][0] * m[0][3] + lin_inv[0][1] * m[1][3] + lin_inv[0][2] * m[2][3]),
170                -(lin_inv[1][0] * m[0][3] + lin_inv[1][1] * m[1][3] + lin_inv[1][2] * m[2][3]),
171                -(lin_inv[2][0] * m[0][3] + lin_inv[2][1] * m[1][3] + lin_inv[2][2] * m[2][3]),
172            ];
173            let inv = [
174                [lin_inv[0][0], lin_inv[0][1], lin_inv[0][2], inv_t[0]],
175                [lin_inv[1][0], lin_inv[1][1], lin_inv[1][2], inv_t[1]],
176                [lin_inv[2][0], lin_inv[2][1], lin_inv[2][2], inv_t[2]],
177                [0.0, 0.0, 0.0, 1.0],
178            ];
179            Ok((m, inv))
180        }
181    }
182}
183
184/// Register image pixel `(col, row)` through `PatientImagePlane` and a verified `SO(3)` / affine
185/// anatomical transform, refusing missing or mismatched `FrameOfReferenceUID` without a certificate.
186pub fn register_patient_to_anatomy<'a>(
187    plane: &PatientImagePlane<'a>,
188    source_blake3: [u8; 32],
189    col: u32,
190    row: u32,
191    target_frame_of_reference_uid: &'a str,
192    transform: &AnatomyTransform,
193    cross_frame_cert: Option<&CrossFrameRegistrationCertificate<'a>>,
194    roundtrip_tolerance_mm: f64,
195) -> Result<RegisteredAnatomyPoint<'a>, AnatomyRegError> {
196    let src_for = plane
197        .frame_of_reference_uid()
198        .ok_or(AnatomyRegError::MissingFrameOfReference)?;
199    if target_frame_of_reference_uid.trim().is_empty() {
200        return Err(AnatomyRegError::MissingFrameOfReference);
201    }
202    if src_for != target_frame_of_reference_uid {
203        let cert_ok = cross_frame_cert.is_some_and(|c| {
204            c.source_frame_of_reference_uid == src_for
205                && c.target_frame_of_reference_uid == target_frame_of_reference_uid
206        });
207        if !cert_ok {
208            return Err(AnatomyRegError::FrameOfReferenceMismatch);
209        }
210    }
211
212    let (fwd, inv) = validate_and_invert_transform(transform)?;
213    let patient_lps_mm = plane.pixel_center_lps_mm(col, row)?;
214    let anatomy_coords_mm = apply_4x4(&fwd, patient_lps_mm);
215    let roundtrip_lps_mm = apply_4x4(&inv, anatomy_coords_mm);
216    if anatomy_coords_mm.iter().chain(roundtrip_lps_mm.iter()).any(|v| !v.is_finite()) {
217        return Err(AnatomyRegError::NonFiniteTransform);
218    }
219    let dx = roundtrip_lps_mm[0] - patient_lps_mm[0];
220    let dy = roundtrip_lps_mm[1] - patient_lps_mm[1];
221    let dz = roundtrip_lps_mm[2] - patient_lps_mm[2];
222    let roundtrip_residual_mm = (dx * dx + dy * dy + dz * dz).sqrt();
223    if roundtrip_residual_mm > roundtrip_tolerance_mm.max(1e-9) {
224        return Err(AnatomyRegError::RoundtripResidualExceeded);
225    }
226
227    let mut hasher = blake3::Hasher::new();
228    hasher.update(&source_blake3);
229    hasher.update(src_for.as_bytes());
230    hasher.update(target_frame_of_reference_uid.as_bytes());
231    if let Some(c) = cross_frame_cert {
232        hasher.update(&c.certificate_blake3);
233    }
234    for v in fwd.iter().flatten().chain(anatomy_coords_mm.iter()) {
235        hasher.update(&v.to_le_bytes());
236    }
237
238    Ok(RegisteredAnatomyPoint {
239        source_blake3,
240        derivation_blake3: *hasher.finalize().as_bytes(),
241        source_frame_of_reference_uid: src_for,
242        target_frame_of_reference_uid,
243        patient_lps_mm,
244        anatomy_coords_mm,
245        roundtrip_lps_mm,
246        roundtrip_residual_mm,
247        inverse_matrix_4x4: inv,
248    })
249}
250
251/// Canonical cortical bone elastic modulus reference (GPa) at `rho_app = 1.85 g/cm^3`.
252pub const REF_CORTICAL_MODULUS_GPA: f64 = 16.5;
253
254/// Compute cortical/trabecular bone mineral stiffness `E(rho_app)` in GPa via
255/// Carter-Hayes / Morgan-Keaveny power-law scaling for NRICP regularization weights.
256pub fn cortical_bmd_stiffness_gpa(apparent_density_g_cm3: f64, mineralization_mu: f64) -> f64 {
257    if !apparent_density_g_cm3.is_finite()
258        || !mineralization_mu.is_finite()
259        || apparent_density_g_cm3 <= 0.0
260        || mineralization_mu <= 0.0
261    {
262        return 0.0;
263    }
264    let r = (apparent_density_g_cm3 / 1.85).clamp(0.10, 1.40);
265    REF_CORTICAL_MODULUS_GPA * mineralization_mu.clamp(0.05, 1.5) * r.powf(2.2)
266}
267
268/// Configuration for stiffness-penalized locally affine Non-Rigid ICP (Amberg et al. 2007)
269/// weighted by per-vertex cortical BMD stiffness `E(rho_app)` and observation confidence `w_k = c_obs(t)`.
270#[derive(Clone, Debug, PartialEq)]
271pub struct StiffnessPenalizedNricpConfig {
272    /// Decreasing stiffness schedule across outer continuation stages (`alpha > 0`).
273    pub stiffness_schedule: Vec<f64>,
274    /// Weight on explicit landmark correspondences (`beta >= 0`).
275    pub landmark_weight: f64,
276    /// Relative stiffness weight on the translational column vs linear 3x3 part (`gamma > 0`).
277    pub translation_gamma: f64,
278    /// Maximum ICP correspondence-update iterations per stiffness stage.
279    pub max_inner_iterations: usize,
280    /// Frobenius step convergence threshold.
281    pub convergence_tol: f64,
282    /// Optional correspondence distance cutoff in mm (outliers beyond this receive weight 0).
283    pub max_correspondence_dist_mm: Option<f64>,
284    /// Minimum allowed local Jacobian determinant `det(A_i) > 0` preventing surface foldover.
285    pub min_jacobian_det: f64,
286    /// Weak prior anchor keeping unobserved disconnected vertices well-posed (`>= 0`).
287    pub prior_anchor_weight: f64,
288}
289
290impl Default for StiffnessPenalizedNricpConfig {
291    fn default() -> Self {
292        Self {
293            stiffness_schedule: vec![40.0, 15.0, 5.0, 1.5],
294            landmark_weight: 10.0,
295            translation_gamma: 1.0,
296            max_inner_iterations: 12,
297            convergence_tol: 1e-4,
298            max_correspondence_dist_mm: None,
299            min_jacobian_det: 1e-3,
300            prior_anchor_weight: 1e-6,
301        }
302    }
303}
304
305/// Output of [`register_stiffness_penalized_surface`] with per-vertex affine fields and residuals.
306#[derive(Clone, Debug)]
307pub struct NricpRegistrationResult {
308    pub source_blake3: [u8; 32],
309    pub derivation_blake3: [u8; 32],
310    pub deformed_vertices: Vec<[f64; 3]>,
311    /// Per-vertex locally affine `3x4` matrix `[A_i | t_i]`.
312    pub per_vertex_affine_3x4: Vec<[[f64; 4]; 3]>,
313    /// Per-vertex Jacobian determinant `det(A_i) > 0`.
314    pub per_vertex_jacobian_det: Vec<f64>,
315    pub mean_surface_residual_mm: f64,
316    pub max_surface_residual_mm: f64,
317    pub iterations_run: usize,
318    pub converged: bool,
319}
320
321fn dist2(a: [f64; 3], b: [f64; 3]) -> f64 {
322    let dx = a[0] - b[0];
323    let dy = a[1] - b[1];
324    let dz = a[2] - b[2];
325    dx * dx + dy * dy + dz * dz
326}
327
328fn apply_3x4(x: &[[f64; 4]; 3], v: [f64; 3]) -> [f64; 3] {
329    [
330        x[0][0] * v[0] + x[0][1] * v[1] + x[0][2] * v[2] + x[0][3],
331        x[1][0] * v[0] + x[1][1] * v[1] + x[1][2] * v[2] + x[1][3],
332        x[2][0] * v[0] + x[2][1] * v[1] + x[2][2] * v[2] + x[2][3],
333    ]
334}
335
336fn build_fallback_knn_edges(vertices: &[[f64; 3]], k: usize) -> Vec<(usize, usize)> {
337    let n = vertices.len();
338    if n <= 1 || k == 0 {
339        return Vec::new();
340    }
341    let mut edges = Vec::new();
342    for i in 0..n {
343        let mut nbrs: Vec<(f64, usize)> = (0..n)
344            .filter(|&j| j != i)
345            .map(|j| (dist2(vertices[i], vertices[j]), j))
346            .collect();
347        nbrs.sort_by(|a, b| a.0.total_cmp(&b.0).then_with(|| a.1.cmp(&b.1)));
348        for &(_, j) in nbrs.iter().take(k) {
349            let pair = if i < j { (i, j) } else { (j, i) };
350            edges.push(pair);
351        }
352    }
353    edges.sort_unstable();
354    edges.dedup();
355    edges
356}
357
358/// Enforce `det(A) >= min_det > 0` by blending toward `I_3` if a step folds a local patch.
359fn enforce_positive_det_3x4(x: &mut [[f64; 4]; 3], min_det: f64) -> f64 {
360    let a = [
361        [x[0][0], x[0][1], x[0][2]],
362        [x[1][0], x[1][1], x[1][2]],
363        [x[2][0], x[2][1], x[2][2]],
364    ];
365    let det = mat3_det(&a);
366    if det.is_finite() && det >= min_det {
367        return det;
368    }
369    let mut lo = 0.0_f64;
370    let mut hi = 1.0_f64;
371    for _ in 0..24 {
372        let mid = 0.5 * (lo + hi);
373        let blended = [
374            [
375                (1.0 - mid) + mid * a[0][0],
376                mid * a[0][1],
377                mid * a[0][2],
378            ],
379            [
380                mid * a[1][0],
381                (1.0 - mid) + mid * a[1][1],
382                mid * a[1][2],
383            ],
384            [
385                mid * a[2][0],
386                mid * a[2][1],
387                (1.0 - mid) + mid * a[2][2],
388            ],
389        ];
390        if mat3_det(&blended) >= min_det {
391            lo = mid;
392        } else {
393            hi = mid;
394        }
395    }
396    for r in 0..3 {
397        for c in 0..3 {
398            let id = if r == c { 1.0 } else { 0.0 };
399            x[r][c] = (1.0 - lo) * id + lo * a[r][c];
400        }
401    }
402    let safe_a = [
403        [x[0][0], x[0][1], x[0][2]],
404        [x[1][0], x[1][1], x[1][2]],
405        [x[2][0], x[2][1], x[2][2]],
406    ];
407    mat3_det(&safe_a).max(min_det)
408}
409
410/// Solve the `4n x 4n` SPD system `H x = b` via 4x4 block-Jacobi Preconditioned Conjugate Gradient.
411fn solve_nricp_pcg(
412    n: usize,
413    diag_blocks: &[[f64; 16]],
414    adj: &[Vec<(usize, f64)>],
415    gamma2: f64,
416    rhs: &[f64],
417    x: &mut [f64],
418) -> Result<(), AnatomyRegError> {
419    let dim = 4 * n;
420    let mut diag_chol = vec![[0.0_f64; 16]; n];
421    for i in 0..n {
422        if cholesky_factor(4, &diag_blocks[i], &mut diag_chol[i]).is_err() {
423            return Err(AnatomyRegError::NumericalSolverFailed);
424        }
425    }
426
427    let matvec = |v: &[f64], out: &mut [f64]| {
428        for i in 0..n {
429            let b = &diag_blocks[i];
430            let vi = &v[4 * i..4 * i + 4];
431            let oi = &mut out[4 * i..4 * i + 4];
432            for r in 0..4 {
433                oi[r] = b[r * 4] * vi[0]
434                    + b[r * 4 + 1] * vi[1]
435                    + b[r * 4 + 2] * vi[2]
436                    + b[r * 4 + 3] * vi[3];
437            }
438            for &(j, k_ij) in &adj[i] {
439                let vj = &v[4 * j..4 * j + 4];
440                oi[0] -= k_ij * vj[0];
441                oi[1] -= k_ij * vj[1];
442                oi[2] -= k_ij * vj[2];
443                oi[3] -= k_ij * gamma2 * vj[3];
444            }
445        }
446    };
447
448    let apply_precond = |r: &[f64], z: &mut [f64]| -> Result<(), AnatomyRegError> {
449        for i in 0..n {
450            cholesky_solve(4, &diag_chol[i], &r[4 * i..4 * i + 4], &mut z[4 * i..4 * i + 4])
451                .map_err(|_| AnatomyRegError::NumericalSolverFailed)?;
452        }
453        Ok(())
454    };
455
456    let mut ax = vec![0.0_f64; dim];
457    matvec(x, &mut ax);
458    let mut r: Vec<f64> = rhs.iter().zip(ax.iter()).map(|(b, a)| b - a).collect();
459    let mut z = vec![0.0_f64; dim];
460    apply_precond(&r, &mut z)?;
461    let mut p = z.clone();
462    let mut rz_old: f64 = r.iter().zip(z.iter()).map(|(ri, zi)| ri * zi).sum();
463    let mut ap = vec![0.0_f64; dim];
464
465    let max_cg = (dim * 2).max(64);
466    for _ in 0..max_cg {
467        if rz_old.abs() < 1e-22 {
468            break;
469        }
470        matvec(&p, &mut ap);
471        let p_ap: f64 = p.iter().zip(ap.iter()).map(|(pi, api)| pi * api).sum();
472        if !p_ap.is_finite() || p_ap <= 1e-24 {
473            break;
474        }
475        let alpha = rz_old / p_ap;
476        let mut r_norm2 = 0.0_f64;
477        for idx in 0..dim {
478            x[idx] += alpha * p[idx];
479            r[idx] -= alpha * ap[idx];
480            r_norm2 += r[idx] * r[idx];
481        }
482        if r_norm2.sqrt() < 1e-10 {
483            break;
484        }
485        apply_precond(&r, &mut z)?;
486        let rz_new: f64 = r.iter().zip(z.iter()).map(|(ri, zi)| ri * zi).sum();
487        if !rz_new.is_finite() || rz_old.abs() < 1e-30 {
488            break;
489        }
490        let beta = rz_new / rz_old;
491        for idx in 0..dim {
492            p[idx] = z[idx] + beta * p[idx];
493        }
494        rz_old = rz_new;
495    }
496    Ok(())
497}
498
499/// Stiffness-penalized locally affine Non-Rigid ICP surface registration incorporating
500/// cortical BMD stiffness weights `E(rho_app)` and temporal observation confidence weights `c_obs(t)`.
501pub fn register_stiffness_penalized_surface(
502    source_vertices: &[[f64; 3]],
503    source_edges: &[(usize, usize)],
504    target_vertices: &[[f64; 3]],
505    cortical_bmd_stiffness_gpa: &[f64],
506    observation_confidence_weights: &[f64],
507    landmarks: &[(usize, [f64; 3], f64)],
508    source_blake3: [u8; 32],
509    config: &StiffnessPenalizedNricpConfig,
510) -> Result<NricpRegistrationResult, AnatomyRegError> {
511    let n = source_vertices.len();
512    if n == 0 || target_vertices.is_empty() {
513        return Err(AnatomyRegError::EmptyInput);
514    }
515    if cortical_bmd_stiffness_gpa.len() != n || observation_confidence_weights.len() != n {
516        return Err(AnatomyRegError::LengthMismatch);
517    }
518    if source_vertices
519        .iter()
520        .chain(target_vertices.iter())
521        .flatten()
522        .any(|v| !v.is_finite())
523    {
524        return Err(AnatomyRegError::NonFiniteTransform);
525    }
526    if cortical_bmd_stiffness_gpa
527        .iter()
528        .any(|&s| !s.is_finite() || s <= 0.0)
529        || observation_confidence_weights
530            .iter()
531            .any(|&w| !w.is_finite() || w < 0.0 || w > 1.0)
532    {
533        return Err(AnatomyRegError::InvalidWeight);
534    }
535    if config.stiffness_schedule.is_empty()
536        || config.stiffness_schedule.iter().any(|&a| !a.is_finite() || a <= 0.0)
537        || !config.landmark_weight.is_finite()
538        || config.landmark_weight < 0.0
539        || !config.translation_gamma.is_finite()
540        || config.translation_gamma <= 0.0
541        || !config.min_jacobian_det.is_finite()
542        || config.min_jacobian_det <= 0.0
543        || !config.prior_anchor_weight.is_finite()
544        || config.prior_anchor_weight < 0.0
545    {
546        return Err(AnatomyRegError::InvalidWeight);
547    }
548    for &(i, j) in source_edges {
549        if i >= n || j >= n {
550            return Err(AnatomyRegError::LengthMismatch);
551        }
552    }
553    for &(idx, pt, w) in landmarks {
554        if idx >= n {
555            return Err(AnatomyRegError::LengthMismatch);
556        }
557        if pt.iter().any(|v| !v.is_finite()) {
558            return Err(AnatomyRegError::NonFiniteTransform);
559        }
560        if !w.is_finite() || w < 0.0 {
561            return Err(AnatomyRegError::InvalidWeight);
562        }
563    }
564
565    let owned_edges;
566    let edges: &[(usize, usize)] = if source_edges.is_empty() && n > 1 {
567        owned_edges = build_fallback_knn_edges(source_vertices, 4.min(n - 1));
568        &owned_edges
569    } else {
570        source_edges
571    };
572
573    // Normalize edge stiffness relative to canonical cortical reference (16.5 GPa)
574    let mut base_adj: Vec<Vec<(usize, f64)>> = vec![Vec::new(); n];
575    for &(i, j) in edges {
576        if i == j {
577            continue;
578        }
579        let s_ij = 0.5 * (cortical_bmd_stiffness_gpa[i] + cortical_bmd_stiffness_gpa[j])
580            / REF_CORTICAL_MODULUS_GPA;
581        base_adj[i].push((j, s_ij));
582        base_adj[j].push((i, s_ij));
583    }
584
585    let gamma2 = config.translation_gamma * config.translation_gamma;
586    let prior_eps = config.prior_anchor_weight.max(1e-8);
587    let max_dist2 = config.max_correspondence_dist_mm.map(|d| d * d);
588
589    let mut x_per_vert: Vec<[[f64; 4]; 3]> = vec![
590        [
591            [1.0, 0.0, 0.0, 0.0],
592            [0.0, 1.0, 0.0, 0.0],
593            [0.0, 0.0, 1.0, 0.0],
594        ];
595        n
596    ];
597    let mut deformed: Vec<[f64; 3]> = source_vertices.to_vec();
598    let mut total_iters = 0usize;
599    let mut last_stage_converged = false;
600
601    for &alpha in &config.stiffness_schedule {
602        let mut adj_alpha: Vec<Vec<(usize, f64)>> = Vec::with_capacity(n);
603        for nbrs in &base_adj {
604            adj_alpha.push(nbrs.iter().map(|&(j, s)| (j, alpha * s)).collect());
605        }
606
607        last_stage_converged = false;
608        for _ in 0..config.max_inner_iterations.max(1) {
609            total_iters += 1;
610            let mut diag_blocks = vec![[0.0_f64; 16]; n];
611            let mut rhs_axes = [vec![0.0_f64; 4 * n], vec![0.0_f64; 4 * n], vec![0.0_f64; 4 * n]];
612
613            // 1. Stiffness Laplacian + prior anchor
614            for i in 0..n {
615                let mut deg = 0.0_f64;
616                for &(_, k_ij) in &adj_alpha[i] {
617                    deg += k_ij;
618                }
619                let blk = &mut diag_blocks[i];
620                let stiff_diag = deg + prior_eps;
621                blk[0] += stiff_diag;
622                blk[5] += stiff_diag;
623                blk[10] += stiff_diag;
624                blk[15] += deg * gamma2 + prior_eps;
625
626                // Prior anchor pulls lightly toward identity [I | 0]
627                rhs_axes[0][4 * i] += prior_eps;
628                rhs_axes[1][4 * i + 1] += prior_eps;
629                rhs_axes[2][4 * i + 2] += prior_eps;
630
631                // 2. Nearest-neighbor data term weighted by observation confidence w_k = c_obs(t)
632                let w_conf = observation_confidence_weights[i];
633                if w_conf > 0.0 {
634                    let cur = deformed[i];
635                    let mut best_d2 = f64::INFINITY;
636                    let mut best_tgt = target_vertices[0];
637                    for &t in target_vertices {
638                        let d2 = dist2(cur, t);
639                        if d2 < best_d2 {
640                            best_d2 = d2;
641                            best_tgt = t;
642                        }
643                    }
644                    let accept = max_dist2.is_none_or(|lim| best_d2 <= lim);
645                    if accept {
646                        let v = source_vertices[i];
647                        let vh = [v[0], v[1], v[2], 1.0];
648                        for r in 0..4 {
649                            for c in 0..4 {
650                                blk[r * 4 + c] += w_conf * vh[r] * vh[c];
651                            }
652                        }
653                        for axis in 0..3 {
654                            for r in 0..4 {
655                                rhs_axes[axis][4 * i + r] += w_conf * best_tgt[axis] * vh[r];
656                            }
657                        }
658                    }
659                }
660            }
661
662            // 3. Landmark term
663            if config.landmark_weight > 0.0 {
664                for &(idx, tgt, l_conf) in landmarks {
665                    let w = config.landmark_weight * l_conf;
666                    if w <= 0.0 {
667                        continue;
668                    }
669                    let v = source_vertices[idx];
670                    let vh = [v[0], v[1], v[2], 1.0];
671                    let blk = &mut diag_blocks[idx];
672                    for r in 0..4 {
673                        for c in 0..4 {
674                            blk[r * 4 + c] += w * vh[r] * vh[c];
675                        }
676                    }
677                    for axis in 0..3 {
678                        for r in 0..4 {
679                            rhs_axes[axis][4 * idx + r] += w * tgt[axis] * vh[r];
680                        }
681                    }
682                }
683            }
684
685            // 4. Solve decoupled 4n x 4n SPD system for x, y, z rows
686            let mut max_step = 0.0_f64;
687            let mut next_x = x_per_vert.clone();
688            for axis in 0..3 {
689                let mut x_vec = vec![0.0_f64; 4 * n];
690                for i in 0..n {
691                    x_vec[4 * i..4 * i + 4].copy_from_slice(&x_per_vert[i][axis]);
692                }
693                solve_nricp_pcg(
694                    n,
695                    &diag_blocks,
696                    &adj_alpha,
697                    gamma2,
698                    &rhs_axes[axis],
699                    &mut x_vec,
700                )?;
701                for i in 0..n {
702                    next_x[i][axis].copy_from_slice(&x_vec[4 * i..4 * i + 4]);
703                }
704            }
705
706            for i in 0..n {
707                enforce_positive_det_3x4(&mut next_x[i], config.min_jacobian_det);
708                let mut diff2 = 0.0_f64;
709                for r in 0..3 {
710                    for c in 0..4 {
711                        let d = next_x[i][r][c] - x_per_vert[i][r][c];
712                        diff2 += d * d;
713                    }
714                }
715                max_step = max_step.max(diff2.sqrt());
716                deformed[i] = apply_3x4(&next_x[i], source_vertices[i]);
717            }
718            x_per_vert = next_x;
719
720            if max_step < config.convergence_tol {
721                last_stage_converged = true;
722                break;
723            }
724        }
725    }
726
727    let mut per_vertex_jacobian_det = Vec::with_capacity(n);
728    let mut sum_res = 0.0_f64;
729    let mut max_res = 0.0_f64;
730    for i in 0..n {
731        let x = &x_per_vert[i];
732        let a = [
733            [x[0][0], x[0][1], x[0][2]],
734            [x[1][0], x[1][1], x[1][2]],
735            [x[2][0], x[2][1], x[2][2]],
736        ];
737        per_vertex_jacobian_det.push(mat3_det(&a));
738
739        let cur = deformed[i];
740        let mut best_d2 = f64::INFINITY;
741        for &t in target_vertices {
742            let d2 = dist2(cur, t);
743            if d2 < best_d2 {
744                best_d2 = d2;
745            }
746        }
747        let d = best_d2.sqrt();
748        sum_res += d;
749        max_res = max_res.max(d);
750    }
751
752    let mut hasher = blake3::Hasher::new();
753    hasher.update(&source_blake3);
754    for p in deformed.iter().flatten() {
755        hasher.update(&p.to_le_bytes());
756    }
757
758    Ok(NricpRegistrationResult {
759        source_blake3,
760        derivation_blake3: *hasher.finalize().as_bytes(),
761        deformed_vertices: deformed,
762        per_vertex_affine_3x4: x_per_vert,
763        per_vertex_jacobian_det,
764        mean_surface_residual_mm: sum_res / n as f64,
765        max_surface_residual_mm: max_res,
766        iterations_run: total_iters,
767        converged: last_stage_converged,
768    })
769}
770
771#[cfg(test)]
772mod tests {
773    use super::*;
774
775    #[test]
776    fn nricp_recovers_non_uniform_surface_warp_with_positive_jacobian() {
777        let src = vec![
778            [-10.0, -10.0, -10.0],
779            [10.0, -10.0, -10.0],
780            [10.0, 10.0, -10.0],
781            [-10.0, 10.0, -10.0],
782            [-10.0, -10.0, 10.0],
783            [10.0, -10.0, 10.0],
784            [10.0, 10.0, 10.0],
785            [-10.0, 10.0, 10.0],
786        ];
787        let edges = vec![
788            (0, 1), (1, 2), (2, 3), (3, 0),
789            (4, 5), (5, 6), (6, 7), (7, 4),
790            (0, 4), (1, 5), (2, 6), (3, 7),
791        ];
792        let tgt: Vec<[f64; 3]> = src
793            .iter()
794            .map(|p| [p[0] * 1.15 + 2.0, p[1] * 0.92 - 1.0, p[2] * 1.08 + 0.5])
795            .collect();
796        let bmd = vec![16.5; src.len()];
797        let conf = vec![1.0; src.len()];
798        let landmarks = vec![(0, tgt[0], 1.0), (6, tgt[6], 1.0)];
799
800        let res = register_stiffness_penalized_surface(
801            &src,
802            &edges,
803            &tgt,
804            &bmd,
805            &conf,
806            &landmarks,
807            [0xAB; 32],
808            &StiffnessPenalizedNricpConfig::default(),
809        )
810        .unwrap();
811
812        assert!(res.mean_surface_residual_mm < 0.25, "mean residual {}", res.mean_surface_residual_mm);
813        assert!(res.per_vertex_jacobian_det.iter().all(|&d| d > 0.0));
814        assert_ne!(res.derivation_blake3, [0xAB; 32]);
815    }
816
817    #[test]
818    fn nricp_high_cortical_bmd_stiffness_resists_local_perturbation_more_than_low_bmd() {
819        let src = vec![
820            [0.0, 0.0, 0.0],
821            [10.0, 0.0, 0.0],
822            [20.0, 0.0, 0.0],
823            [0.0, 10.0, 0.0],
824            [10.0, 10.0, 0.0],
825            [20.0, 10.0, 0.0],
826        ];
827        let edges = vec![(0, 1), (1, 2), (3, 4), (4, 5), (0, 3), (1, 4), (2, 5)];
828        // Pull vertex 2 & 5 outward while vertex 0 & 3 are anchored
829        let mut tgt = src.clone();
830        tgt[2][0] += 6.0;
831        tgt[5][0] += 6.0;
832        let conf = vec![0.35; src.len()];
833        let landmarks = vec![(0, src[0], 1.0), (3, src[3], 1.0)];
834        let cfg = StiffnessPenalizedNricpConfig {
835            stiffness_schedule: vec![12.0],
836            max_inner_iterations: 6,
837            ..Default::default()
838        };
839
840        let stiff_bmd = vec![cortical_bmd_stiffness_gpa(1.90, 1.0); src.len()];
841        let osteo_bmd = vec![cortical_bmd_stiffness_gpa(0.75, 1.0); src.len()];
842
843        let res_stiff = register_stiffness_penalized_surface(
844            &src, &edges, &tgt, &stiff_bmd, &conf, &landmarks, [0x11; 32], &cfg,
845        ).unwrap();
846        let res_osteo = register_stiffness_penalized_surface(
847            &src, &edges, &tgt, &osteo_bmd, &conf, &landmarks, [0x11; 32], &cfg,
848        ).unwrap();
849
850        let disp_stiff = res_stiff.deformed_vertices[2][0] - src[2][0];
851        let disp_osteo = res_osteo.deformed_vertices[2][0] - src[2][0];
852        assert!(
853            disp_stiff < disp_osteo,
854            "stiffer cortical BMD ({disp_stiff}) should resist distortion more than osteopenic ({disp_osteo})"
855        );
856        assert!(res_stiff.per_vertex_jacobian_det.iter().all(|&d| d > 0.0));
857        assert!(res_osteo.per_vertex_jacobian_det.iter().all(|&d| d > 0.0));
858    }
859
860    #[test]
861    fn nricp_rejects_invalid_inputs_and_preserves_positive_det_on_extreme_fold() {
862        let src = vec![[0.0, 0.0, 0.0], [5.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 5.0]];
863        let tgt_flip = vec![[0.0, 0.0, 0.0], [-5.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 5.0]];
864        let bmd = vec![5.0; 4];
865        let conf = vec![1.0; 4];
866
867        // Fail-closed validation checks
868        assert_eq!(
869            register_stiffness_penalized_surface(
870                &[], &[], &tgt_flip, &[], &[], &[], [0; 32], &StiffnessPenalizedNricpConfig::default()
871            ).unwrap_err(),
872            AnatomyRegError::EmptyInput
873        );
874        assert_eq!(
875            register_stiffness_penalized_surface(
876                &src, &[], &tgt_flip, &[1.0], &conf, &[], [0; 32], &StiffnessPenalizedNricpConfig::default()
877            ).unwrap_err(),
878            AnatomyRegError::LengthMismatch
879        );
880        assert_eq!(
881            register_stiffness_penalized_surface(
882                &src, &[], &tgt_flip, &[-1.0, 1.0, 1.0, 1.0], &conf, &[], [0; 32], &StiffnessPenalizedNricpConfig::default()
883            ).unwrap_err(),
884            AnatomyRegError::InvalidWeight
885        );
886
887        // Extreme inverted target still yields det(A_i) >= min_jacobian_det > 0
888        let res = register_stiffness_penalized_surface(
889            &src, &[], &tgt_flip, &bmd, &conf, &[], [0x22; 32], &StiffnessPenalizedNricpConfig::default()
890        ).unwrap();
891        assert!(res.per_vertex_jacobian_det.iter().all(|&d| d >= 1e-3 - 1e-12));
892    }
893}