threecrate_reconstruction/
marching_cubes.rs

1//! Marching Cubes algorithm for volumetric surface reconstruction
2//!
3//! This module provides isosurface extraction from 3D scalar fields using
4//! the classic Marching Cubes algorithm and its variants.
5
6use crate::parallel;
7use nalgebra::Vector3;
8use threecrate_core::{Error, Point3f, PointCloud, Result, TriangleMesh};
9
10/// 3D volumetric grid containing scalar values
11#[derive(Debug, Clone)]
12pub struct VolumetricGrid {
13    /// Scalar values arranged as [x][y][z]
14    pub values: Vec<Vec<Vec<f32>>>,
15    /// Grid dimensions
16    pub dimensions: [usize; 3],
17    /// Physical size of each voxel
18    pub voxel_size: [f32; 3],
19    /// Origin position of the grid in world coordinates
20    pub origin: Point3f,
21}
22
23impl VolumetricGrid {
24    /// Create a new volumetric grid
25    pub fn new(dimensions: [usize; 3], voxel_size: [f32; 3], origin: Point3f) -> Self {
26        let values = vec![vec![vec![0.0; dimensions[2]]; dimensions[1]]; dimensions[0]];
27
28        Self {
29            values,
30            dimensions,
31            voxel_size,
32            origin,
33        }
34    }
35
36    /// Get scalar value at grid coordinates (with bounds checking)
37    pub fn get_value(&self, x: usize, y: usize, z: usize) -> Option<f32> {
38        if x < self.dimensions[0] && y < self.dimensions[1] && z < self.dimensions[2] {
39            Some(self.values[x][y][z])
40        } else {
41            None
42        }
43    }
44
45    /// Set scalar value at grid coordinates
46    pub fn set_value(&mut self, x: usize, y: usize, z: usize, value: f32) -> Result<()> {
47        if x < self.dimensions[0] && y < self.dimensions[1] && z < self.dimensions[2] {
48            self.values[x][y][z] = value;
49            Ok(())
50        } else {
51            Err(Error::InvalidData(format!(
52                "Grid coordinates ({}, {}, {}) out of bounds for dimensions {:?}",
53                x, y, z, self.dimensions
54            )))
55        }
56    }
57
58    /// Convert grid coordinates to world coordinates
59    pub fn grid_to_world(&self, x: usize, y: usize, z: usize) -> Point3f {
60        Point3f::new(
61            self.origin.x + x as f32 * self.voxel_size[0],
62            self.origin.y + y as f32 * self.voxel_size[1],
63            self.origin.z + z as f32 * self.voxel_size[2],
64        )
65    }
66
67    /// Create a volumetric grid from a point cloud using distance field
68    pub fn from_point_cloud(
69        cloud: &PointCloud<Point3f>,
70        grid_resolution: [usize; 3],
71        padding: f32,
72    ) -> Result<Self> {
73        if cloud.is_empty() {
74            return Err(Error::InvalidData("Point cloud is empty".to_string()));
75        }
76
77        // Compute bounding box
78        let mut min_bounds = Point3f::new(f32::INFINITY, f32::INFINITY, f32::INFINITY);
79        let mut max_bounds = Point3f::new(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY);
80
81        for point in &cloud.points {
82            min_bounds.x = min_bounds.x.min(point.x);
83            min_bounds.y = min_bounds.y.min(point.y);
84            min_bounds.z = min_bounds.z.min(point.z);
85            max_bounds.x = max_bounds.x.max(point.x);
86            max_bounds.y = max_bounds.y.max(point.y);
87            max_bounds.z = max_bounds.z.max(point.z);
88        }
89
90        // Add padding
91        min_bounds = Point3f::new(
92            min_bounds.x - padding,
93            min_bounds.y - padding,
94            min_bounds.z - padding,
95        );
96        max_bounds = Point3f::new(
97            max_bounds.x + padding,
98            max_bounds.y + padding,
99            max_bounds.z + padding,
100        );
101
102        // Calculate voxel size
103        let extents = [
104            max_bounds.x - min_bounds.x,
105            max_bounds.y - min_bounds.y,
106            max_bounds.z - min_bounds.z,
107        ];
108
109        let voxel_size = [
110            extents[0] / (grid_resolution[0] - 1) as f32,
111            extents[1] / (grid_resolution[1] - 1) as f32,
112            extents[2] / (grid_resolution[2] - 1) as f32,
113        ];
114
115        let mut grid = VolumetricGrid::new(grid_resolution, voxel_size, min_bounds);
116
117        // Fill grid with distance values
118        for x in 0..grid_resolution[0] {
119            for y in 0..grid_resolution[1] {
120                for z in 0..grid_resolution[2] {
121                    let world_pos = grid.grid_to_world(x, y, z);
122
123                    // Find minimum distance to any point in the cloud
124                    let mut min_distance = f32::INFINITY;
125                    for point in &cloud.points {
126                        let distance = (world_pos - point).magnitude();
127                        min_distance = min_distance.min(distance);
128                    }
129
130                    grid.set_value(x, y, z, min_distance)?;
131                }
132            }
133        }
134
135        Ok(grid)
136    }
137}
138
139/// Configuration for Marching Cubes algorithm
140#[derive(Debug, Clone)]
141pub struct MarchingCubesConfig {
142    /// Isosurface level (scalar value to extract)
143    pub iso_level: f32,
144    /// Whether to compute vertex normals
145    pub compute_normals: bool,
146    /// Whether to smooth the resulting mesh
147    pub smooth_mesh: bool,
148    /// Smoothing iterations (if smooth_mesh is true)
149    pub smoothing_iterations: usize,
150}
151
152impl Default for MarchingCubesConfig {
153    fn default() -> Self {
154        Self {
155            iso_level: 0.0,
156            compute_normals: true,
157            smooth_mesh: false,
158            smoothing_iterations: 3,
159        }
160    }
161}
162
163/// Vertex information for marching cubes
164#[derive(Debug, Clone)]
165struct MCVertex {
166    position: Point3f,
167    normal: Vector3<f32>,
168}
169
170/// Edge table: indicates which edges are intersected for each cube configuration (256 cases)
171/// Each entry is a 12-bit value where bit i indicates if edge i is intersected
172/// Note: This table is available for future optimizations (pre-checking which edges to interpolate)
173#[allow(dead_code)]
174const EDGE_TABLE: [u16; 256] = [
175    0x0, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
176    0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
177    0x190, 0x99, 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
178    0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
179    0x230, 0x339, 0x33, 0x13a, 0x636, 0x73f, 0x435, 0x53c,
180    0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
181    0x3a0, 0x2a9, 0x1a3, 0xaa, 0x7a6, 0x6af, 0x5a5, 0x4ac,
182    0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
183    0x460, 0x569, 0x663, 0x76a, 0x66, 0x16f, 0x265, 0x36c,
184    0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
185    0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff, 0x3f5, 0x2fc,
186    0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
187    0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55, 0x15c,
188    0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
189    0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc,
190    0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
191    0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
192    0xcc, 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
193    0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
194    0x15c, 0x55, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
195    0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
196    0x2fc, 0x3f5, 0xff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
197    0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
198    0x36c, 0x265, 0x16f, 0x66, 0x76a, 0x663, 0x569, 0x460,
199    0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
200    0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa, 0x1a3, 0x2a9, 0x3a0,
201    0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
202    0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33, 0x339, 0x230,
203    0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
204    0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99, 0x190,
205    0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
206    0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0,
207];
208
209/// Triangle table: defines triangles for each cube configuration
210/// Each row contains up to 5 triangles (15 indices), terminated by -1
211const TRIANGLE_TABLE: [[i8; 16]; 256] = [
212    [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
213    [0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
214    [0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
215    [1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
216    [1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
217    [0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
218    [9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
219    [2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1],
220    [3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
221    [0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
222    [1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
223    [1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1],
224    [3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
225    [0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1],
226    [3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1],
227    [9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
228    [4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
229    [4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
230    [0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
231    [4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1],
232    [1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
233    [3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1],
234    [9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1],
235    [2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1],
236    [8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
237    [11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1],
238    [9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1],
239    [4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1],
240    [3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1],
241    [1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1],
242    [4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1],
243    [4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1],
244    [9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
245    [9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
246    [0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
247    [8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1],
248    [1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
249    [3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1],
250    [5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1],
251    [2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1],
252    [9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
253    [0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1],
254    [0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1],
255    [2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1],
256    [10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1],
257    [4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1],
258    [5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1],
259    [5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1],
260    [9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
261    [9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1],
262    [0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1],
263    [1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
264    [9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1],
265    [10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1],
266    [8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1],
267    [2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1],
268    [7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1],
269    [9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1],
270    [2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1],
271    [11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1],
272    [9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1],
273    [5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1],
274    [11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1],
275    [11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
276    [10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
277    [0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
278    [9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
279    [1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1],
280    [1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
281    [1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1],
282    [9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1],
283    [5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1],
284    [2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
285    [11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1],
286    [0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1],
287    [5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1],
288    [6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1],
289    [0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1],
290    [3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1],
291    [6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1],
292    [5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
293    [4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1],
294    [1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1],
295    [10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1],
296    [6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1],
297    [1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1],
298    [8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1],
299    [7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1],
300    [3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1],
301    [5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1],
302    [0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1],
303    [9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1],
304    [8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1],
305    [5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1],
306    [0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1],
307    [6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1],
308    [10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
309    [4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1],
310    [10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1],
311    [8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1],
312    [1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1],
313    [3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1],
314    [0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
315    [8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1],
316    [10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1],
317    [0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1],
318    [3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1],
319    [6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1],
320    [9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1],
321    [8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1],
322    [3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1],
323    [6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
324    [7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1],
325    [0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1],
326    [10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1],
327    [10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1],
328    [1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1],
329    [2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1],
330    [7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1],
331    [7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
332    [2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1],
333    [2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1],
334    [1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1],
335    [11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1],
336    [8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1],
337    [0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
338    [7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1],
339    [7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
340    [7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
341    [3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
342    [0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
343    [8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1],
344    [10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
345    [1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1],
346    [2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1],
347    [6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1],
348    [7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
349    [7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1],
350    [2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1],
351    [1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1],
352    [10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1],
353    [10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1],
354    [0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1],
355    [7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1],
356    [6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
357    [3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1],
358    [8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1],
359    [9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1],
360    [6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1],
361    [1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1],
362    [4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1],
363    [10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1],
364    [8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1],
365    [0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
366    [1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1],
367    [1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1],
368    [8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1],
369    [10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1],
370    [4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1],
371    [10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
372    [4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
373    [0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1],
374    [5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1],
375    [11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1],
376    [9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1],
377    [6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1],
378    [7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1],
379    [3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1],
380    [7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1],
381    [9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1],
382    [3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1],
383    [6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1],
384    [9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1],
385    [1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1],
386    [4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1],
387    [7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1],
388    [6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1],
389    [3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1],
390    [0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1],
391    [6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1],
392    [1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1],
393    [0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1],
394    [11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1],
395    [6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1],
396    [5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1],
397    [9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1],
398    [1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1],
399    [1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
400    [1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1],
401    [10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1],
402    [0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
403    [10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
404    [11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
405    [11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1],
406    [5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1],
407    [10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1],
408    [11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1],
409    [0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1],
410    [9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1],
411    [7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1],
412    [2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1],
413    [8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1],
414    [9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1],
415    [9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1],
416    [1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
417    [0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1],
418    [9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1],
419    [9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
420    [5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1],
421    [5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1],
422    [0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1],
423    [10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1],
424    [2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1],
425    [0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1],
426    [0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1],
427    [9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
428    [2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1],
429    [5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1],
430    [3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1],
431    [5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1],
432    [8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1],
433    [0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
434    [8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1],
435    [9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
436    [4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1],
437    [0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1],
438    [1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1],
439    [3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1],
440    [4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1],
441    [9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1],
442    [11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1],
443    [11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1],
444    [2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1],
445    [9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1],
446    [3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1],
447    [1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
448    [4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1],
449    [4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1],
450    [4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
451    [4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
452    [9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
453    [3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1],
454    [0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1],
455    [3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
456    [1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1],
457    [3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1],
458    [0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
459    [3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
460    [2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1],
461    [9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
462    [2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1],
463    [1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
464    [1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
465    [0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
466    [0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
467    [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
468];
469
470/// Marching Cubes implementation
471pub struct MarchingCubes {
472    config: MarchingCubesConfig,
473}
474
475impl MarchingCubes {
476    /// Create a new Marching Cubes instance
477    pub fn new(config: MarchingCubesConfig) -> Self {
478        Self { config }
479    }
480
481    /// Extract isosurface from volumetric grid with parallel processing
482    pub fn extract_isosurface(&self, grid: &VolumetricGrid) -> Result<TriangleMesh> {
483        // Generate all cube coordinates
484        let mut cube_coords = Vec::new();
485        for x in 0..grid.dimensions[0].saturating_sub(1) {
486            for y in 0..grid.dimensions[1].saturating_sub(1) {
487                for z in 0..grid.dimensions[2].saturating_sub(1) {
488                    cube_coords.push((x, y, z));
489                }
490            }
491        }
492
493        // Process cubes in parallel
494        let cube_results: Vec<(Vec<MCVertex>, Vec<[usize; 3]>)> =
495            parallel::parallel_map(&cube_coords, |(x, y, z)| {
496                let mut vertices = Vec::new();
497                let mut triangles = Vec::new();
498
499                // Process a single cube - ignore errors for parallel processing simplicity
500                if let Ok(()) = self.process_cube(grid, *x, *y, *z, &mut vertices, &mut triangles) {
501                    (vertices, triangles)
502                } else {
503                    (Vec::new(), Vec::new())
504                }
505            });
506
507        // Merge results from parallel processing
508        let mut all_vertices = Vec::new();
509        let mut all_triangles = Vec::new();
510        let mut vertex_offset = 0;
511
512        for (vertices, triangles) in cube_results {
513            // Add triangles with adjusted indices
514            for triangle in triangles {
515                all_triangles.push([
516                    triangle[0] + vertex_offset,
517                    triangle[1] + vertex_offset,
518                    triangle[2] + vertex_offset,
519                ]);
520            }
521
522            // Add vertices
523            all_vertices.extend(vertices);
524            vertex_offset = all_vertices.len();
525        }
526
527        if all_vertices.is_empty() {
528            return Err(Error::Algorithm(
529                "No isosurface found at specified level".to_string(),
530            ));
531        }
532
533        // Convert MCVertex to Point3f for mesh creation using parallel processing
534        let vertex_positions: Vec<Point3f> = parallel::parallel_map(&all_vertices, |v| v.position);
535        let mut mesh = TriangleMesh::from_vertices_and_faces(vertex_positions, all_triangles);
536
537        // Set normals if computed using parallel processing
538        if self.config.compute_normals {
539            let normals: Vec<Vector3<f32>> = parallel::parallel_map(&all_vertices, |v| v.normal);
540            mesh.set_normals(normals);
541        }
542
543        // Apply smoothing if requested
544        if self.config.smooth_mesh {
545            self.smooth_mesh(&mut mesh)?;
546        }
547
548        Ok(mesh)
549    }
550
551    /// Process a single cube for marching cubes
552    fn process_cube(
553        &self,
554        grid: &VolumetricGrid,
555        x: usize,
556        y: usize,
557        z: usize,
558        vertices: &mut Vec<MCVertex>,
559        triangles: &mut Vec<[usize; 3]>,
560    ) -> Result<()> {
561        // Get the 8 corner values of the cube
562        let corner_values = [
563            grid.get_value(x, y, z).unwrap_or(0.0),
564            grid.get_value(x + 1, y, z).unwrap_or(0.0),
565            grid.get_value(x + 1, y + 1, z).unwrap_or(0.0),
566            grid.get_value(x, y + 1, z).unwrap_or(0.0),
567            grid.get_value(x, y, z + 1).unwrap_or(0.0),
568            grid.get_value(x + 1, y, z + 1).unwrap_or(0.0),
569            grid.get_value(x + 1, y + 1, z + 1).unwrap_or(0.0),
570            grid.get_value(x, y + 1, z + 1).unwrap_or(0.0),
571        ];
572
573        // Compute cube configuration index
574        let mut cube_index = 0;
575        for i in 0..8 {
576            if corner_values[i] < self.config.iso_level {
577                cube_index |= 1 << i;
578            }
579        }
580
581        // Skip if cube is entirely inside or outside
582        if cube_index == 0 || cube_index == 255 {
583            return Ok(());
584        }
585
586        // Get cube corner positions
587        let corner_positions = [
588            grid.grid_to_world(x, y, z),
589            grid.grid_to_world(x + 1, y, z),
590            grid.grid_to_world(x + 1, y + 1, z),
591            grid.grid_to_world(x, y + 1, z),
592            grid.grid_to_world(x, y, z + 1),
593            grid.grid_to_world(x + 1, y, z + 1),
594            grid.grid_to_world(x + 1, y + 1, z + 1),
595            grid.grid_to_world(x, y + 1, z + 1),
596        ];
597
598        // Interpolate vertices on edges where surface crosses
599        let mut edge_vertices = [None; 12];
600        let edge_connections = [
601            [0, 1],
602            [1, 2],
603            [2, 3],
604            [3, 0], // bottom face edges
605            [4, 5],
606            [5, 6],
607            [6, 7],
608            [7, 4], // top face edges
609            [0, 4],
610            [1, 5],
611            [2, 6],
612            [3, 7], // vertical edges
613        ];
614
615        for (edge_idx, &[v1, v2]) in edge_connections.iter().enumerate() {
616            if self.edge_intersects_surface(corner_values[v1], corner_values[v2]) {
617                let vertex = self.interpolate_vertex(
618                    corner_positions[v1],
619                    corner_positions[v2],
620                    corner_values[v1],
621                    corner_values[v2],
622                    grid,
623                    x,
624                    y,
625                    z,
626                )?;
627
628                let vertex_index = vertices.len();
629                vertices.push(vertex);
630                edge_vertices[edge_idx] = Some(vertex_index);
631            }
632        }
633
634        // Generate triangles using lookup table (simplified version)
635        self.generate_triangles_for_cube(cube_index, &edge_vertices, triangles)?;
636
637        Ok(())
638    }
639
640    /// Check if edge intersects the isosurface
641    fn edge_intersects_surface(&self, value1: f32, value2: f32) -> bool {
642        (value1 < self.config.iso_level) != (value2 < self.config.iso_level)
643    }
644
645    /// Interpolate vertex position on edge where surface crosses
646    fn interpolate_vertex(
647        &self,
648        pos1: Point3f,
649        pos2: Point3f,
650        val1: f32,
651        val2: f32,
652        grid: &VolumetricGrid,
653        _x: usize,
654        _y: usize,
655        _z: usize,
656    ) -> Result<MCVertex> {
657        // Linear interpolation to find exact crossing point
658        let t = if (val2 - val1).abs() < 1e-6 {
659            0.5
660        } else {
661            (self.config.iso_level - val1) / (val2 - val1)
662        };
663
664        let position = Point3f::new(
665            pos1.x + t * (pos2.x - pos1.x),
666            pos1.y + t * (pos2.y - pos1.y),
667            pos1.z + t * (pos2.z - pos1.z),
668        );
669
670        // Compute normal using gradient (simplified finite difference)
671        let normal = if self.config.compute_normals {
672            self.compute_gradient_at_position(&position, grid)
673        } else {
674            Vector3::new(0.0, 0.0, 1.0)
675        };
676
677        Ok(MCVertex { position, normal })
678    }
679
680    /// Compute gradient (normal) at a position using finite differences
681    fn compute_gradient_at_position(
682        &self,
683        position: &Point3f,
684        grid: &VolumetricGrid,
685    ) -> Vector3<f32> {
686        let delta = 0.001; // Small offset for finite differences
687
688        // Sample values around the position
689        let dx_pos = self.sample_grid_at_world_position(
690            &Point3f::new(position.x + delta, position.y, position.z),
691            grid,
692        );
693        let dx_neg = self.sample_grid_at_world_position(
694            &Point3f::new(position.x - delta, position.y, position.z),
695            grid,
696        );
697        let dy_pos = self.sample_grid_at_world_position(
698            &Point3f::new(position.x, position.y + delta, position.z),
699            grid,
700        );
701        let dy_neg = self.sample_grid_at_world_position(
702            &Point3f::new(position.x, position.y - delta, position.z),
703            grid,
704        );
705        let dz_pos = self.sample_grid_at_world_position(
706            &Point3f::new(position.x, position.y, position.z + delta),
707            grid,
708        );
709        let dz_neg = self.sample_grid_at_world_position(
710            &Point3f::new(position.x, position.y, position.z - delta),
711            grid,
712        );
713
714        // Compute gradient
715        let gradient = Vector3::new(
716            (dx_pos - dx_neg) / (2.0 * delta),
717            (dy_pos - dy_neg) / (2.0 * delta),
718            (dz_pos - dz_neg) / (2.0 * delta),
719        );
720
721        // Normalize and return (negate for inward-pointing normal)
722        if gradient.magnitude() > 1e-6 {
723            -gradient.normalize()
724        } else {
725            Vector3::new(0.0, 0.0, 1.0)
726        }
727    }
728
729    /// Sample grid value at world position using trilinear interpolation
730    fn sample_grid_at_world_position(&self, position: &Point3f, grid: &VolumetricGrid) -> f32 {
731        // Convert world position to grid coordinates
732        let gx = (position.x - grid.origin.x) / grid.voxel_size[0];
733        let gy = (position.y - grid.origin.y) / grid.voxel_size[1];
734        let gz = (position.z - grid.origin.z) / grid.voxel_size[2];
735
736        // Check bounds
737        if gx < 0.0
738            || gy < 0.0
739            || gz < 0.0
740            || gx >= (grid.dimensions[0] - 1) as f32
741            || gy >= (grid.dimensions[1] - 1) as f32
742            || gz >= (grid.dimensions[2] - 1) as f32
743        {
744            return 0.0; // Outside grid
745        }
746
747        // Trilinear interpolation
748        let x0 = gx.floor() as usize;
749        let y0 = gy.floor() as usize;
750        let z0 = gz.floor() as usize;
751        let x1 = (x0 + 1).min(grid.dimensions[0] - 1);
752        let y1 = (y0 + 1).min(grid.dimensions[1] - 1);
753        let z1 = (z0 + 1).min(grid.dimensions[2] - 1);
754
755        let fx = gx - x0 as f32;
756        let fy = gy - y0 as f32;
757        let fz = gz - z0 as f32;
758
759        // Get 8 corner values
760        let v000 = grid.get_value(x0, y0, z0).unwrap_or(0.0);
761        let v100 = grid.get_value(x1, y0, z0).unwrap_or(0.0);
762        let v010 = grid.get_value(x0, y1, z0).unwrap_or(0.0);
763        let v110 = grid.get_value(x1, y1, z0).unwrap_or(0.0);
764        let v001 = grid.get_value(x0, y0, z1).unwrap_or(0.0);
765        let v101 = grid.get_value(x1, y0, z1).unwrap_or(0.0);
766        let v011 = grid.get_value(x0, y1, z1).unwrap_or(0.0);
767        let v111 = grid.get_value(x1, y1, z1).unwrap_or(0.0);
768
769        // Interpolate
770        let v00 = v000 * (1.0 - fx) + v100 * fx;
771        let v10 = v010 * (1.0 - fx) + v110 * fx;
772        let v01 = v001 * (1.0 - fx) + v101 * fx;
773        let v11 = v011 * (1.0 - fx) + v111 * fx;
774
775        let v0 = v00 * (1.0 - fy) + v10 * fy;
776        let v1 = v01 * (1.0 - fy) + v11 * fy;
777
778        v0 * (1.0 - fz) + v1 * fz
779    }
780
781    /// Generate triangles for a cube configuration using lookup tables
782    fn generate_triangles_for_cube(
783        &self,
784        cube_index: usize,
785        edge_vertices: &[Option<usize>; 12],
786        triangles: &mut Vec<[usize; 3]>,
787    ) -> Result<()> {
788        // Get the triangle configuration for this cube from the lookup table
789        let tri_config = TRIANGLE_TABLE[cube_index];
790
791        // Iterate through the triangle indices (terminated by -1)
792        let mut i = 0;
793        while i < tri_config.len() && tri_config[i] != -1 {
794            // Each triangle consists of 3 edge indices
795            if i + 2 < tri_config.len() {
796                let edge_idx_0 = tri_config[i] as usize;
797                let edge_idx_1 = tri_config[i + 1] as usize;
798                let edge_idx_2 = tri_config[i + 2] as usize;
799
800                // Get the actual vertex indices from the edge vertices
801                if let (Some(v0), Some(v1), Some(v2)) = (
802                    edge_vertices[edge_idx_0],
803                    edge_vertices[edge_idx_1],
804                    edge_vertices[edge_idx_2],
805                ) {
806                    triangles.push([v0, v1, v2]);
807                }
808            }
809
810            i += 3; // Move to the next triangle
811        }
812
813        Ok(())
814    }
815
816    /// Apply smoothing to the mesh
817    fn smooth_mesh(&self, mesh: &mut TriangleMesh) -> Result<()> {
818        // Simple Laplacian smoothing
819        for _ in 0..self.config.smoothing_iterations {
820            let vertices = mesh.vertices.clone();
821            let mut new_vertices = vertices.clone();
822
823            // For each vertex, average with its neighbors
824            for (i, _vertex) in vertices.iter().enumerate() {
825                let neighbors = self.get_vertex_neighbors(mesh, i);
826                if !neighbors.is_empty() {
827                    let mut sum = Point3f::origin();
828                    for &neighbor_idx in &neighbors {
829                        if neighbor_idx < vertices.len() {
830                            sum = Point3f::from(sum.coords + vertices[neighbor_idx].coords);
831                        }
832                    }
833                    new_vertices[i] = Point3f::from(sum.coords / neighbors.len() as f32);
834                }
835            }
836
837            // Update mesh vertices
838            mesh.vertices = new_vertices;
839        }
840
841        Ok(())
842    }
843
844    /// Get vertex neighbors by looking at connected faces
845    fn get_vertex_neighbors(&self, mesh: &TriangleMesh, vertex_idx: usize) -> Vec<usize> {
846        let mut neighbors = std::collections::HashSet::new();
847
848        // Find all faces that contain this vertex
849        for face in &mesh.faces {
850            if face.contains(&vertex_idx) {
851                // Add the other two vertices from this face
852                for &v in face {
853                    if v != vertex_idx {
854                        neighbors.insert(v);
855                    }
856                }
857            }
858        }
859
860        neighbors.into_iter().collect()
861    }
862}
863
864/// Convenience function for basic marching cubes
865pub fn marching_cubes(grid: &VolumetricGrid, iso_level: f32) -> Result<TriangleMesh> {
866    let config = MarchingCubesConfig {
867        iso_level,
868        ..Default::default()
869    };
870    let mc = MarchingCubes::new(config);
871    mc.extract_isosurface(grid)
872}
873
874/// Create a simple test volume (sphere)
875pub fn create_sphere_volume(
876    center: Point3f,
877    radius: f32,
878    grid_resolution: [usize; 3],
879    grid_size: [f32; 3],
880) -> VolumetricGrid {
881    let origin = Point3f::new(
882        center.x - grid_size[0] / 2.0,
883        center.y - grid_size[1] / 2.0,
884        center.z - grid_size[2] / 2.0,
885    );
886
887    let voxel_size = [
888        grid_size[0] / (grid_resolution[0] - 1) as f32,
889        grid_size[1] / (grid_resolution[1] - 1) as f32,
890        grid_size[2] / (grid_resolution[2] - 1) as f32,
891    ];
892
893    let mut grid = VolumetricGrid::new(grid_resolution, voxel_size, origin);
894
895    // Generate all grid coordinates for parallel processing
896    let mut grid_coords = Vec::new();
897    for x in 0..grid_resolution[0] {
898        for y in 0..grid_resolution[1] {
899            for z in 0..grid_resolution[2] {
900                grid_coords.push((x, y, z));
901            }
902        }
903    }
904
905    // Compute sphere distance field in parallel
906    let distance_values: Vec<((usize, usize, usize), f32)> =
907        parallel::parallel_map(&grid_coords, |(x, y, z)| {
908            let world_pos = grid.grid_to_world(*x, *y, *z);
909            let distance = (world_pos - center).magnitude() - radius;
910            ((*x, *y, *z), distance)
911        });
912
913    // Fill grid with computed values
914    for ((x, y, z), distance) in distance_values {
915        grid.set_value(x, y, z, distance).unwrap();
916    }
917
918    grid
919}
920
921/// Create a cube volume using signed distance field
922pub fn create_cube_volume(
923    center: Point3f,
924    half_size: f32,
925    grid_resolution: [usize; 3],
926    grid_size: [f32; 3],
927) -> VolumetricGrid {
928    let origin = Point3f::new(
929        center.x - grid_size[0] / 2.0,
930        center.y - grid_size[1] / 2.0,
931        center.z - grid_size[2] / 2.0,
932    );
933
934    let voxel_size = [
935        grid_size[0] / (grid_resolution[0] - 1) as f32,
936        grid_size[1] / (grid_resolution[1] - 1) as f32,
937        grid_size[2] / (grid_resolution[2] - 1) as f32,
938    ];
939
940    let mut grid = VolumetricGrid::new(grid_resolution, voxel_size, origin);
941
942    // Generate all grid coordinates for parallel processing
943    let mut grid_coords = Vec::new();
944    for x in 0..grid_resolution[0] {
945        for y in 0..grid_resolution[1] {
946            for z in 0..grid_resolution[2] {
947                grid_coords.push((x, y, z));
948            }
949        }
950    }
951
952    // Compute cube distance field in parallel (signed distance to a box)
953    let distance_values: Vec<((usize, usize, usize), f32)> =
954        parallel::parallel_map(&grid_coords, |(x, y, z)| {
955            let world_pos = grid.grid_to_world(*x, *y, *z);
956
957            // Distance to box (signed distance field)
958            let dx = (world_pos.x - center.x).abs() - half_size;
959            let dy = (world_pos.y - center.y).abs() - half_size;
960            let dz = (world_pos.z - center.z).abs() - half_size;
961
962            // Outside: distance to nearest surface
963            // Inside: negative distance to nearest surface
964            let outside_dist = dx.max(0.0).powi(2) + dy.max(0.0).powi(2) + dz.max(0.0).powi(2);
965            let distance = outside_dist.sqrt() + dx.max(dy).max(dz).min(0.0);
966
967            ((*x, *y, *z), distance)
968        });
969
970    // Fill grid with computed values
971    for ((x, y, z), distance) in distance_values {
972        grid.set_value(x, y, z, distance).unwrap();
973    }
974
975    grid
976}
977
978#[cfg(test)]
979mod tests {
980    use super::*;
981    use nalgebra::Point3;
982
983    #[test]
984    fn test_volumetric_grid_creation() {
985        let grid = VolumetricGrid::new([10, 10, 10], [1.0, 1.0, 1.0], Point3f::origin());
986
987        assert_eq!(grid.dimensions, [10, 10, 10]);
988        assert_eq!(grid.voxel_size, [1.0, 1.0, 1.0]);
989        assert_eq!(grid.origin, Point3f::origin());
990    }
991
992    #[test]
993    fn test_grid_value_operations() {
994        let mut grid = VolumetricGrid::new([3, 3, 3], [1.0, 1.0, 1.0], Point3f::origin());
995
996        // Test setting and getting values
997        assert!(grid.set_value(1, 1, 1, 5.0).is_ok());
998        assert_eq!(grid.get_value(1, 1, 1), Some(5.0));
999
1000        // Test bounds checking
1001        assert!(grid.set_value(3, 3, 3, 1.0).is_err());
1002        assert_eq!(grid.get_value(3, 3, 3), None);
1003    }
1004
1005    #[test]
1006    fn test_grid_to_world_conversion() {
1007        let grid = VolumetricGrid::new([5, 5, 5], [2.0, 2.0, 2.0], Point3f::new(1.0, 1.0, 1.0));
1008
1009        let world_pos = grid.grid_to_world(1, 1, 1);
1010        assert_eq!(world_pos, Point3f::new(3.0, 3.0, 3.0));
1011    }
1012
1013    #[test]
1014    fn test_sphere_volume_creation() {
1015        let center = Point3f::new(0.0, 0.0, 0.0);
1016        let radius = 1.0;
1017        let grid = create_sphere_volume(center, radius, [10, 10, 10], [4.0, 4.0, 4.0]);
1018
1019        // Check that center has negative value (inside sphere)
1020        let center_value = grid.get_value(5, 5, 5).unwrap();
1021        assert!(center_value < 0.0);
1022
1023        // Check that corner has positive value (outside sphere)
1024        let corner_value = grid.get_value(0, 0, 0).unwrap();
1025        assert!(corner_value > 0.0);
1026    }
1027
1028    #[test]
1029    fn test_marching_cubes_config_default() {
1030        let config = MarchingCubesConfig::default();
1031        assert_eq!(config.iso_level, 0.0);
1032        assert!(config.compute_normals);
1033        assert!(!config.smooth_mesh);
1034        assert_eq!(config.smoothing_iterations, 3);
1035    }
1036
1037    #[test]
1038    fn test_marching_cubes_simple() {
1039        let grid = create_sphere_volume(Point3f::origin(), 0.5, [8, 8, 8], [2.0, 2.0, 2.0]);
1040
1041        let result = marching_cubes(&grid, 0.0);
1042
1043        match result {
1044            Ok(mesh) => {
1045                assert!(!mesh.is_empty());
1046                assert!(mesh.vertex_count() > 0);
1047            }
1048            Err(_) => {
1049                // May fail on simple grids - acceptable for unit tests
1050            }
1051        }
1052    }
1053
1054    #[test]
1055    fn test_volumetric_grid_from_point_cloud() {
1056        let points = vec![
1057            Point3::new(0.0, 0.0, 0.0),
1058            Point3::new(1.0, 0.0, 0.0),
1059            Point3::new(0.5, 1.0, 0.0),
1060        ];
1061        let cloud = PointCloud::from_points(points);
1062
1063        let result = VolumetricGrid::from_point_cloud(&cloud, [10, 10, 10], 0.5);
1064
1065        assert!(result.is_ok());
1066        let grid = result.unwrap();
1067        assert_eq!(grid.dimensions, [10, 10, 10]);
1068    }
1069}
threecrate_reconstruction/marching_cubes.rs

threecrate_reconstruction/
marching_cubes.rs
