1use 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
184pub 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
251pub const REF_CORTICAL_MODULUS_GPA: f64 = 16.5;
253
254pub 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#[derive(Clone, Debug, PartialEq)]
271pub struct StiffnessPenalizedNricpConfig {
272 pub stiffness_schedule: Vec<f64>,
274 pub landmark_weight: f64,
276 pub translation_gamma: f64,
278 pub max_inner_iterations: usize,
280 pub convergence_tol: f64,
282 pub max_correspondence_dist_mm: Option<f64>,
284 pub min_jacobian_det: f64,
286 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#[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 pub per_vertex_affine_3x4: Vec<[[f64; 4]; 3]>,
313 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
358fn 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
410fn 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
499pub 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 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 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 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 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 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 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 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 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 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}