qsm_core/bet/
mesh.rs

1//! Mesh utilities for BET
2
3/// Build neighbor matrix for vectorized neighbor lookups
4///
5/// Returns (neighbor_matrix, neighbor_counts) where:
6/// - neighbor_matrix[i] contains the indices of vertex i's neighbors (padded with usize::MAX)
7/// - neighbor_counts[i] is the number of valid neighbors for vertex i
8pub fn build_neighbor_matrix(n_vertices: usize, faces: &[[usize; 3]], max_neighbors: usize) -> (Vec<Vec<usize>>, Vec<usize>) {
9    let mut neighbor_lists: Vec<Vec<usize>> = vec![Vec::new(); n_vertices];
10
11    for &[v0, v1, v2] in faces {
12        // Add edges v0-v1, v1-v2, v2-v0
13        for &(a, b) in &[(v0, v1), (v1, v2), (v2, v0)] {
14            if !neighbor_lists[a].contains(&b) {
15                neighbor_lists[a].push(b);
16            }
17            if !neighbor_lists[b].contains(&a) {
18                neighbor_lists[b].push(a);
19            }
20        }
21    }
22
23    // Find actual max neighbors
24    let actual_max = neighbor_lists.iter().map(|n| n.len()).max().unwrap_or(0);
25    let padded_max = actual_max.max(max_neighbors);
26
27    // Build padded matrix
28    let mut neighbor_matrix: Vec<Vec<usize>> = Vec::with_capacity(n_vertices);
29    let mut neighbor_counts: Vec<usize> = Vec::with_capacity(n_vertices);
30
31    for neighs in neighbor_lists {
32        neighbor_counts.push(neighs.len());
33        let mut row = neighs;
34        row.resize(padded_max, usize::MAX);
35        neighbor_matrix.push(row);
36    }
37
38    (neighbor_matrix, neighbor_counts)
39}
40
41#[cfg(test)]
42/// Compute outward-pointing normals at each vertex
43pub fn compute_vertex_normals(vertices: &[[f64; 3]], faces: &[[usize; 3]]) -> Vec<[f64; 3]> {
44    let n_vertices = vertices.len();
45    let mut normals: Vec<[f64; 3]> = vec![[0.0, 0.0, 0.0]; n_vertices];
46
47    for &[i0, i1, i2] in faces {
48        let v0 = vertices[i0];
49        let v1 = vertices[i1];
50        let v2 = vertices[i2];
51
52        // Edge vectors
53        let e1 = [v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]];
54        let e2 = [v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]];
55
56        // Face normal (cross product)
57        let face_normal = [
58            e1[1] * e2[2] - e1[2] * e2[1],
59            e1[2] * e2[0] - e1[0] * e2[2],
60            e1[0] * e2[1] - e1[1] * e2[0],
61        ];
62
63        // Normalize
64        let norm = (face_normal[0].powi(2) + face_normal[1].powi(2) + face_normal[2].powi(2)).sqrt();
65        let face_normal = if norm > 1e-10 {
66            [face_normal[0] / norm, face_normal[1] / norm, face_normal[2] / norm]
67        } else {
68            [0.0, 0.0, 0.0]
69        };
70
71        // Accumulate at vertices
72        for &idx in &[i0, i1, i2] {
73            normals[idx][0] += face_normal[0];
74            normals[idx][1] += face_normal[1];
75            normals[idx][2] += face_normal[2];
76        }
77    }
78
79    // Normalize all vertex normals
80    for n in normals.iter_mut() {
81        let norm = (n[0].powi(2) + n[1].powi(2) + n[2].powi(2)).sqrt();
82        if norm > 1e-10 {
83            n[0] /= norm;
84            n[1] /= norm;
85            n[2] /= norm;
86        }
87    }
88
89    normals
90}
91
92/// bet2's `Mpoint::local_normal`: the sum of its triangles' `Triangle::normal()`,
93/// `(v2 − v0) × (v1 − v0)` (not normalised, so area-weighted), normalised.
94pub fn bet2_vertex_normals(vertices: &[[f64; 3]], faces: &[[usize; 3]]) -> Vec<[f64; 3]> {
95    let mut normals: Vec<[f64; 3]> = vec![[0.0, 0.0, 0.0]; vertices.len()];
96    for &[i0, i1, i2] in faces {
97        let (v0, v1, v2) = (vertices[i0], vertices[i1], vertices[i2]);
98        let a = [v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]];
99        let b = [v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]];
100        let n = [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
101        for &idx in &[i0, i1, i2] {
102            for c in 0..3 {
103                normals[idx][c] += n[c];
104            }
105        }
106    }
107    for n in normals.iter_mut() {
108        let norm = (n[0] * n[0] + n[1] * n[1] + n[2] * n[2]).sqrt();
109        if norm > 0.0 {
110            for c in n.iter_mut() {
111                *c /= norm;
112            }
113        }
114    }
115    normals
116}
117
118/// Compute mean edge length for vertices already in mm coordinates
119pub fn compute_mean_edge_length_mm(vertices_mm: &[[f64; 3]], faces: &[[usize; 3]]) -> f64 {
120    let mut total_length = 0.0;
121    let mut count = 0;
122
123    for &[i0, i1, i2] in faces {
124        // Edge v0-v1
125        let dx = vertices_mm[i1][0] - vertices_mm[i0][0];
126        let dy = vertices_mm[i1][1] - vertices_mm[i0][1];
127        let dz = vertices_mm[i1][2] - vertices_mm[i0][2];
128        total_length += (dx*dx + dy*dy + dz*dz).sqrt();
129
130        // Edge v1-v2
131        let dx = vertices_mm[i2][0] - vertices_mm[i1][0];
132        let dy = vertices_mm[i2][1] - vertices_mm[i1][1];
133        let dz = vertices_mm[i2][2] - vertices_mm[i1][2];
134        total_length += (dx*dx + dy*dy + dz*dz).sqrt();
135
136        // Edge v2-v0
137        let dx = vertices_mm[i0][0] - vertices_mm[i2][0];
138        let dy = vertices_mm[i0][1] - vertices_mm[i2][1];
139        let dz = vertices_mm[i0][2] - vertices_mm[i2][2];
140        total_length += (dx*dx + dy*dy + dz*dz).sqrt();
141
142        count += 3;
143    }
144
145    if count > 0 {
146        total_length / count as f64
147    } else {
148        1.0
149    }
150}
151
152/// Compute distance between two vertices (already in mm)
153fn vertex_distance(v1: &[f64; 3], v2: &[f64; 3]) -> f64 {
154    let dx = v2[0] - v1[0];
155    let dy = v2[1] - v1[1];
156    let dz = v2[2] - v1[2];
157    (dx * dx + dy * dy + dz * dz).sqrt()
158}
159
160/// Compute self-intersection heuristic by comparing vertex distances
161/// between current and original mesh.
162///
163/// This is based on FSL-BET2's self_intersection method which:
164/// 1. Computes mean edge length for both meshes (ml, mlo)
165/// 2. For vertex pairs that are currently close (< ml apart), checks if they've
166///    gotten significantly closer than they were in the original mesh
167/// 3. Accumulates squared differences in normalized distances
168///
169/// Returns a scalar value where > 4000 indicates likely self-intersection.
170/// The threshold of 4000 matches FSL-BET2's self_intersection_threshold.
171///
172/// Note: Vertices are expected to be in mm coordinates.
173pub fn self_intersection_heuristic(
174    current_vertices: &[[f64; 3]],
175    original_vertices: &[[f64; 3]],
176    faces: &[[usize; 3]],
177    _voxel_size: &[f64; 3], // kept for API compatibility, not used (vertices are in mm)
178) -> f64 {
179    if current_vertices.len() != original_vertices.len() {
180        return f64::MAX;
181    }
182
183    let n = current_vertices.len();
184
185    // Compute mean edge length for normalization (like FSL's ml and mlo)
186    // Vertices are in mm, so use the mm version
187    let ml = compute_mean_edge_length_mm(current_vertices, faces);
188    let mlo = compute_mean_edge_length_mm(original_vertices, faces);
189
190    if ml < 1e-10 || mlo < 1e-10 {
191        return f64::MAX;
192    }
193
194    let ml_sq = ml * ml;
195    let mut intersection = 0.0;
196
197    // FSL compares all vertex pairs, but only counts pairs where current distance < ml
198    // This detects when non-adjacent vertices have gotten too close (mesh folding)
199    // For efficiency, we sample a subset of pairs for large meshes
200    let step = if n > 500 { (n / 500).max(1) } else { 1 };
201
202    for i in (0..n).step_by(step) {
203        for j in (i + 1..n).step_by(step) {
204            // Current distance squared (vertices already in mm)
205            let dx = current_vertices[j][0] - current_vertices[i][0];
206            let dy = current_vertices[j][1] - current_vertices[i][1];
207            let dz = current_vertices[j][2] - current_vertices[i][2];
208            let curr_dist_sq = dx * dx + dy * dy + dz * dz;
209
210            // Only consider pairs that are currently close (< ml apart)
211            // This is the key insight from FSL - we're looking for folding
212            if curr_dist_sq < ml_sq {
213                let curr_dist = curr_dist_sq.sqrt();
214                let orig_dist = vertex_distance(&original_vertices[i], &original_vertices[j]);
215
216                // Normalize distances
217                let dist = curr_dist / ml;
218                let disto = orig_dist / mlo;
219
220                // Accumulate squared difference
221                let diff = dist - disto;
222                intersection += diff * diff;
223            }
224        }
225    }
226
227    intersection
228}
229
230#[cfg(test)]
231mod tests {
232    use super::*;
233    use crate::bet::icosphere::create_icosphere;
234
235    #[test]
236    fn test_neighbor_matrix() {
237        let (vertices, faces) = create_icosphere(1);
238        let (neighbor_matrix, neighbor_counts) = build_neighbor_matrix(vertices.len(), &faces, 6);
239
240        assert_eq!(neighbor_matrix.len(), vertices.len());
241        assert_eq!(neighbor_counts.len(), vertices.len());
242
243        // Each vertex should have at least 1 neighbor
244        for &count in &neighbor_counts {
245            assert!(count >= 1);
246        }
247    }
248
249    #[test]
250    fn test_vertex_normals() {
251        let (vertices, faces) = create_icosphere(1);
252        let normals = compute_vertex_normals(&vertices, &faces);
253
254        assert_eq!(normals.len(), vertices.len());
255
256        // Normals should be unit length and point outward (same direction as vertex)
257        for (v, n) in vertices.iter().zip(normals.iter()) {
258            let norm = (n[0].powi(2) + n[1].powi(2) + n[2].powi(2)).sqrt();
259            assert!((norm - 1.0).abs() < 1e-6, "Normal not unit length");
260
261            // Dot product with vertex should be positive (outward pointing)
262            let dot = v[0] * n[0] + v[1] * n[1] + v[2] * n[2];
263            assert!(dot > 0.9, "Normal not pointing outward");
264        }
265    }
266
267    #[test]
268    fn test_neighbor_matrix_symmetry() {
269        // Neighbor relationships must be symmetric: if A neighbors B then B neighbors A
270        let (vertices, faces) = create_icosphere(2);
271        let n = vertices.len();
272        let (neighbor_matrix, neighbor_counts) = build_neighbor_matrix(n, &faces, 6);
273
274        for i in 0..n {
275            for j in 0..neighbor_counts[i] {
276                let ni = neighbor_matrix[i][j];
277                // ni should have i in its neighbor list
278                let found = (0..neighbor_counts[ni]).any(|k| neighbor_matrix[ni][k] == i);
279                assert!(found, "Asymmetric neighbors: {} -> {} but not reverse", i, ni);
280            }
281        }
282    }
283
284    #[test]
285    fn test_neighbor_matrix_single_triangle() {
286        // Minimal mesh: one triangle with 3 vertices
287        let faces = vec![[0, 1, 2]];
288        let (neighbor_matrix, neighbor_counts) = build_neighbor_matrix(3, &faces, 6);
289
290        assert_eq!(neighbor_counts.len(), 3);
291        // Each vertex in a single triangle should have exactly 2 neighbors
292        for &count in &neighbor_counts {
293            assert_eq!(count, 2);
294        }
295
296        // Padding should be usize::MAX
297        for row in &neighbor_matrix {
298            for &val in row.iter().skip(2) {
299                assert_eq!(val, usize::MAX);
300            }
301        }
302    }
303
304    #[test]
305    fn test_neighbor_matrix_max_neighbors_padding() {
306        // When max_neighbors > actual neighbors, rows should be padded
307        let faces = vec![[0, 1, 2]];
308        let (neighbor_matrix, _neighbor_counts) = build_neighbor_matrix(3, &faces, 10);
309
310        // Each row should have at least 10 entries (max_neighbors)
311        for row in &neighbor_matrix {
312            assert!(row.len() >= 10);
313        }
314    }
315
316
317    #[test]
318    fn test_compute_mean_edge_length_mm_unit_triangle() {
319        let vertices_mm: Vec<[f64; 3]> = vec![
320            [0.0, 0.0, 0.0],
321            [1.0, 0.0, 0.0],
322            [0.0, 1.0, 0.0],
323        ];
324        let faces = vec![[0, 1, 2]];
325
326        let mel = compute_mean_edge_length_mm(&vertices_mm, &faces);
327        // Edge lengths: 1.0, 1.0, sqrt(2) = 1.414
328        // Mean = (1 + 1 + 1.414) / 3 = 1.138
329        let expected = (1.0 + 1.0 + 2.0_f64.sqrt()) / 3.0;
330        assert!((mel - expected).abs() < 1e-10, "mel={}, expected={}", mel, expected);
331    }
332
333    #[test]
334    fn test_compute_mean_edge_length_mm_empty_faces() {
335        let vertices_mm: Vec<[f64; 3]> = vec![[0.0, 0.0, 0.0]];
336        let faces: Vec<[usize; 3]> = vec![];
337
338        let mel = compute_mean_edge_length_mm(&vertices_mm, &faces);
339        // With no faces, should return fallback of 1.0
340        assert!((mel - 1.0).abs() < 1e-10);
341    }
342
343
344    #[test]
345    fn test_compute_mean_edge_length_mm_icosphere() {
346        // Icosphere with subdivision 2, scaled by radius 5.0
347        let (unit_verts, faces) = create_icosphere(2);
348        let radius = 5.0;
349        let vertices_mm: Vec<[f64; 3]> = unit_verts
350            .iter()
351            .map(|v| [v[0] * radius, v[1] * radius, v[2] * radius])
352            .collect();
353
354        let mel = compute_mean_edge_length_mm(&vertices_mm, &faces);
355        assert!(mel > 0.0 && mel.is_finite(), "mel should be positive and finite, got {}", mel);
356        // For a sphere of radius 5, edges should be a fraction of the radius
357        assert!(mel < radius, "Edge length should be smaller than radius");
358    }
359
360    #[test]
361    fn test_vertex_normals_degenerate_face() {
362        // Include a degenerate triangle (zero area) alongside a valid one
363        let vertices: Vec<[f64; 3]> = vec![
364            [0.0, 0.0, 0.0],
365            [1.0, 0.0, 0.0],
366            [0.0, 1.0, 0.0],
367            [0.5, 0.0, 0.0], // collinear with v0 and v1
368        ];
369        let faces = vec![
370            [0, 1, 2],  // valid triangle
371            [0, 1, 3],  // degenerate (collinear points)
372        ];
373
374        let normals = compute_vertex_normals(&vertices, &faces);
375        assert_eq!(normals.len(), 4);
376        // The valid triangle's normal contribution should still produce finite normals
377        for n in &normals {
378            assert!(n[0].is_finite() && n[1].is_finite() && n[2].is_finite());
379        }
380    }
381
382    #[test]
383    fn test_vertex_normals_icosphere_subdivision_2() {
384        // Higher subdivision level
385        let (vertices, faces) = create_icosphere(2);
386        let normals = compute_vertex_normals(&vertices, &faces);
387
388        assert_eq!(normals.len(), vertices.len());
389        for (v, n) in vertices.iter().zip(normals.iter()) {
390            let norm = (n[0].powi(2) + n[1].powi(2) + n[2].powi(2)).sqrt();
391            assert!((norm - 1.0).abs() < 1e-6, "Normal not unit length: {}", norm);
392
393            let dot = v[0] * n[0] + v[1] * n[1] + v[2] * n[2];
394            assert!(dot > 0.9, "Normal not pointing outward, dot={}", dot);
395        }
396    }
397
398    #[test]
399    fn test_vertex_distance() {
400        let v1 = [0.0, 0.0, 0.0];
401        let v2 = [3.0, 4.0, 0.0];
402        let d = vertex_distance(&v1, &v2);
403        assert!((d - 5.0).abs() < 1e-10, "Expected 5.0, got {}", d);
404
405        // Same point
406        let d0 = vertex_distance(&v1, &v1);
407        assert!((d0 - 0.0).abs() < 1e-10);
408
409        // 3D case
410        let v3 = [1.0, 2.0, 3.0];
411        let v4 = [4.0, 6.0, 3.0];
412        let d34 = vertex_distance(&v3, &v4);
413        let expected = (9.0 + 16.0 + 0.0_f64).sqrt();
414        assert!((d34 - expected).abs() < 1e-10);
415    }
416
417    #[test]
418    fn test_self_intersection_heuristic_no_intersection() {
419        // An icosphere that hasn't changed should have very low score
420        let (vertices, faces) = create_icosphere(1);
421        let radius = 5.0;
422        let scaled: Vec<[f64; 3]> = vertices
423            .iter()
424            .map(|v| [v[0] * radius, v[1] * radius, v[2] * radius])
425            .collect();
426
427        let score = self_intersection_heuristic(&scaled, &scaled, &faces, &[1.0, 1.0, 1.0]);
428        // Identical meshes should yield 0 (no differences)
429        assert!((score - 0.0).abs() < 1e-6, "Expected ~0, got {}", score);
430    }
431
432    #[test]
433    fn test_self_intersection_heuristic_expanded_mesh() {
434        // Slightly expand the mesh uniformly -- should not indicate self-intersection
435        let (vertices, faces) = create_icosphere(1);
436        let radius = 5.0;
437        let original: Vec<[f64; 3]> = vertices
438            .iter()
439            .map(|v| [v[0] * radius, v[1] * radius, v[2] * radius])
440            .collect();
441
442        let expanded: Vec<[f64; 3]> = vertices
443            .iter()
444            .map(|v| [v[0] * radius * 1.1, v[1] * radius * 1.1, v[2] * radius * 1.1])
445            .collect();
446
447        let score = self_intersection_heuristic(&expanded, &original, &faces, &[1.0, 1.0, 1.0]);
448        assert!(score.is_finite(), "Score should be finite, got {}", score);
449        // Uniform expansion should have a very low score (no folding)
450        assert!(score < 4000.0, "Uniform expansion should not trigger self-intersection, score={}", score);
451    }
452
453    #[test]
454    fn test_self_intersection_heuristic_mismatched_vertices() {
455        let faces = vec![[0, 1, 2]];
456        let v1: Vec<[f64; 3]> = vec![[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]];
457        let v2: Vec<[f64; 3]> = vec![[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]; // wrong size
458
459        let score = self_intersection_heuristic(&v1, &v2, &faces, &[1.0, 1.0, 1.0]);
460        assert_eq!(score, f64::MAX);
461    }
462
463    #[test]
464    fn test_self_intersection_heuristic_collapsed_mesh() {
465        // Collapse all vertices to the center -- extreme folding
466        let (vertices, faces) = create_icosphere(1);
467        let radius = 5.0;
468        let original: Vec<[f64; 3]> = vertices
469            .iter()
470            .map(|v| [v[0] * radius, v[1] * radius, v[2] * radius])
471            .collect();
472
473        // Collapse to a single point
474        let collapsed: Vec<[f64; 3]> = vec![[0.0, 0.0, 0.0]; vertices.len()];
475
476        let score = self_intersection_heuristic(&collapsed, &original, &faces, &[1.0, 1.0, 1.0]);
477        // All vertices at same point means ml ~ 0, which should give MAX
478        assert_eq!(score, f64::MAX, "Collapsed mesh should have MAX score");
479    }
480
481}
qsm_core/bet/mesh.rs

qsm_core/bet/
mesh.rs
