parry3d/transformation/voxelization/voxelized_volume.rs
1// Rust port, with modifications, of https://github.com/kmammou/v-hacd/blob/master/src/VHACD_Lib/src/vhacdVolume.cpp
2// By Khaled Mamou
3//
4// # License of the original C++ code:
5// > Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
6// > All rights reserved.
7// >
8// >
9// > Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
10// >
11// > 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
12// >
13// > 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
14// >
15// > 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission.
16// >
17// > THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
18
19use crate::bounding_volume::Aabb;
20use crate::math::{Point, Real, Vector, DIM};
21use crate::query;
22use crate::transformation::voxelization::{Voxel, VoxelSet};
23use alloc::sync::Arc;
24use alloc::vec::Vec;
25#[cfg(not(feature = "std"))]
26use na::ComplexField;
27
28/// Controls how voxelization determines which voxels are filled vs empty.
29///
30/// The fill mode is a critical parameter that determines the structure of the resulting voxel set.
31/// Choose the appropriate mode based on whether you need just the surface boundary or a solid volume.
32///
33/// # Variants
34///
35/// ## `SurfaceOnly`
36///
37/// Only voxels that intersect the input surface boundary (triangle mesh in 3D, polyline in 2D)
38/// are marked as filled. This creates a hollow shell representation.
39///
40/// **Use this when:**
41/// - You only need the surface boundary
42/// - Memory is limited (fewer voxels to store)
43/// - You're approximating a thin shell or surface
44///
45/// **Example:**
46/// ```
47/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
48/// use parry3d::transformation::voxelization::{FillMode, VoxelSet};
49/// use parry3d::shape::Ball;
50/// ///
51/// let ball = Ball::new(1.0);
52/// let (vertices, indices) = ball.to_trimesh(20, 20);
53///
54/// let surface_voxels = VoxelSet::voxelize(
55///     &vertices,
56///     &indices,
57///     10,
58///     FillMode::SurfaceOnly,  // Only the shell
59///     false,
60/// );
61///
62/// // All voxels are on the surface
63/// assert!(surface_voxels.voxels().iter().all(|v| v.is_on_surface));
64/// # }
65/// ```
66///
67/// ## `FloodFill`
68///
69/// Marks surface voxels AND all interior voxels as filled, creating a solid volume. Uses a
70/// flood-fill algorithm starting from outside the shape to determine inside vs outside.
71///
72/// **Fields:**
73/// - `detect_cavities`: If `true`, properly detects and preserves internal cavities/holes.
74///   If `false`, all enclosed spaces are filled (faster but may fill unintended regions).
75///
76/// - `detect_self_intersections` (2D only): If `true`, attempts to handle self-intersecting
77///   polylines correctly. More expensive but more robust.
78///
79/// **Use this when:**
80/// - You need volume information (mass properties, volume computation)
81/// - You want a solid representation for collision detection
82/// - You need to distinguish between interior and surface voxels
83///
84/// **Example:**
85/// ```
86/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
87/// use parry3d::transformation::voxelization::{FillMode, VoxelSet};
88/// use parry3d::shape::Ball;
89/// ///
90/// let ball = Ball::new(1.0);
91/// let (vertices, indices) = ball.to_trimesh(20, 20);
92///
93/// let solid_voxels = VoxelSet::voxelize(
94///     &vertices,
95///     &indices,
96///     15,
97///     FillMode::FloodFill {
98///         detect_cavities: false,  // No cavities in a sphere
99///     },
100///     false,
101/// );
102///
103/// // Mix of surface and interior voxels
104/// let surface_count = solid_voxels.voxels().iter().filter(|v| v.is_on_surface).count();
105/// let interior_count = solid_voxels.voxels().iter().filter(|v| !v.is_on_surface).count();
106/// assert!(surface_count > 0 && interior_count > 0);
107/// # }
108/// ```
109///
110/// # Performance Notes
111///
112/// - `SurfaceOnly` is faster and uses less memory than `FloodFill`
113/// - `detect_cavities = true` adds computational overhead but gives more accurate results
114/// - For simple convex shapes, `detect_cavities = false` is usually sufficient
115#[derive(Copy, Clone, Debug, PartialEq, Eq)]
116pub enum FillMode {
117    /// Only consider full the voxels intersecting the surface of the
118    /// shape being voxelized.
119    SurfaceOnly,
120    /// Use a flood-fill technique to consider fill the voxels intersecting
121    /// the surface of the shape being voxelized, as well as all the voxels
122    /// bounded of them.
123    FloodFill {
124        /// Detects holes inside of a solid contour.
125        detect_cavities: bool,
126        /// Attempts to properly handle self-intersections.
127        #[cfg(feature = "dim2")]
128        detect_self_intersections: bool,
129    },
130    // RaycastFill
131}
132
133impl Default for FillMode {
134    fn default() -> Self {
135        Self::FloodFill {
136            detect_cavities: false,
137            #[cfg(feature = "dim2")]
138            detect_self_intersections: false,
139        }
140    }
141}
142
143impl FillMode {
144    #[cfg(feature = "dim2")]
145    pub(crate) fn detect_cavities(self) -> bool {
146        match self {
147            FillMode::FloodFill {
148                detect_cavities, ..
149            } => detect_cavities,
150            _ => false,
151        }
152    }
153
154    #[cfg(feature = "dim2")]
155    pub(crate) fn detect_self_intersections(self) -> bool {
156        match self {
157            FillMode::FloodFill {
158                detect_self_intersections,
159                ..
160            } => detect_self_intersections,
161            _ => false,
162        }
163    }
164
165    #[cfg(feature = "dim3")]
166    pub(crate) fn detect_self_intersections(self) -> bool {
167        false
168    }
169}
170
171/// The state of a voxel during and after voxelization.
172///
173/// This enum represents the classification of each voxel in the grid. After voxelization completes,
174/// only three values are meaningful for end-user code: `PrimitiveOutsideSurface`,
175/// `PrimitiveInsideSurface`, and `PrimitiveOnSurface`. The other variants are intermediate
176/// states used during the flood-fill algorithm.
177///
178/// # Usage
179///
180/// Most users will work with [`VoxelSet`] which filters out outside voxels and only stores
181/// filled ones. However, if you're working directly with [`VoxelizedVolume`], you'll encounter
182/// all these values.
183///
184/// # Final States (After Voxelization)
185///
186/// - **`PrimitiveOutsideSurface`**: Voxel is completely outside the shape
187/// - **`PrimitiveInsideSurface`**: Voxel is fully inside the shape (only with [`FillMode::FloodFill`])
188/// - **`PrimitiveOnSurface`**: Voxel intersects the boundary surface
189///
190/// # Intermediate States (During Voxelization)
191///
192/// The following are temporary states used during the flood-fill algorithm and should be
193/// ignored by user code:
194/// - `PrimitiveUndefined`
195/// - `PrimitiveOutsideSurfaceToWalk`
196/// - `PrimitiveInsideSurfaceToWalk`
197/// - `PrimitiveOnSurfaceNoWalk`
198/// - `PrimitiveOnSurfaceToWalk1`
199/// - `PrimitiveOnSurfaceToWalk2`
200///
201/// # Example
202///
203/// ```
204/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
205/// use parry3d::transformation::voxelization::{FillMode, VoxelValue, VoxelizedVolume};
206/// use parry3d::shape::Cuboid;
207/// use nalgebra::Vector3;
208///
209/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0));
210/// let (vertices, indices) = cuboid.to_trimesh();
211///
212/// // Create a dense voxelized volume
213/// let volume = VoxelizedVolume::voxelize(
214///     &vertices,
215///     &indices,
216///     8,
217///     FillMode::FloodFill { detect_cavities: false },
218///     false,
219/// );
220///
221/// // Query individual voxel values
222/// let resolution = volume.resolution();
223/// for i in 0..resolution[0] {
224///     for j in 0..resolution[1] {
225///         for k in 0..resolution[2] {
226///             match volume.voxel(i, j, k) {
227///                 VoxelValue::PrimitiveOnSurface => {
228///                     // This voxel is on the boundary
229///                 }
230///                 VoxelValue::PrimitiveInsideSurface => {
231///                     // This voxel is inside the shape
232///                 }
233///                 VoxelValue::PrimitiveOutsideSurface => {
234///                     // This voxel is outside the shape
235///                 }
236///                 _ => {
237///                     // Should not happen after voxelization completes
238///                 }
239///             }
240///         }
241///     }
242/// }
243/// # }
244/// ```
245#[derive(Copy, Clone, Debug, PartialEq, Eq)]
246pub enum VoxelValue {
247    /// Intermediate value, should be ignored by end-user code.
248    PrimitiveUndefined,
249    /// Intermediate value, should be ignored by end-user code.
250    PrimitiveOutsideSurfaceToWalk,
251    /// Intermediate value, should be ignored by end-user code.
252    PrimitiveInsideSurfaceToWalk,
253    /// Intermediate value, should be ignored by end-user code.
254    PrimitiveOnSurfaceNoWalk,
255    /// Intermediate value, should be ignored by end-user code.
256    PrimitiveOnSurfaceToWalk1,
257    /// Intermediate value, should be ignored by end-user code.
258    PrimitiveOnSurfaceToWalk2,
259    /// A voxel that is outside of the voxelized shape.
260    PrimitiveOutsideSurface,
261    /// A voxel that is on the interior of the voxelized shape.
262    PrimitiveInsideSurface,
263    /// Voxel that intersects the surface of the voxelized shape.
264    PrimitiveOnSurface,
265}
266
267#[derive(Copy, Clone, Debug, PartialEq, Eq)]
268struct VoxelData {
269    #[cfg(feature = "dim2")]
270    multiplicity: u32,
271    num_primitive_intersections: u32,
272}
273
274/// A dense voxel grid storing the state of every voxel in a cubic volume.
275///
276/// `VoxelizedVolume` is the intermediate representation used during the voxelization process.
277/// Unlike [`VoxelSet`] which only stores filled voxels, `VoxelizedVolume` stores a complete
278/// dense 3D array where every grid cell has an associated [`VoxelValue`].
279///
280/// # When to Use
281///
282/// Most users should use [`VoxelSet`] instead, which is converted from `VoxelizedVolume`
283/// automatically and is much more memory-efficient. You might want to use `VoxelizedVolume`
284/// directly if you need to:
285///
286/// - Query the state of ALL voxels, including empty ones
287/// - Implement custom voxel processing algorithms
288/// - Generate visualizations showing both filled and empty voxels
289///
290/// # Memory Usage
291///
292/// `VoxelizedVolume` stores **all** voxels in a dense 3D array:
293/// - Memory usage: `O(resolution^3)` in 3D or `O(resolution^2)` in 2D
294/// - A 100×100×100 grid requires 1 million voxel entries
295///
296/// For this reason, [`VoxelSet`] is usually preferred for storage.
297///
298/// # Conversion
299///
300/// `VoxelizedVolume` automatically converts to `VoxelSet`:
301///
302/// ```
303/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
304/// use parry3d::transformation::voxelization::{FillMode, VoxelSet, VoxelizedVolume};
305/// use parry3d::shape::Ball;
306/// ///
307/// let ball = Ball::new(1.0);
308/// let (vertices, indices) = ball.to_trimesh(10, 10);
309///
310/// // Create dense volume
311/// let volume = VoxelizedVolume::voxelize(
312///     &vertices,
313///     &indices,
314///     10,
315///     FillMode::SurfaceOnly,
316///     false,
317/// );
318///
319/// // Convert to sparse set (automatic via Into trait)
320/// let voxel_set: VoxelSet = volume.into();
321/// # }
322/// ```
323///
324/// # Example: Querying All Voxels
325///
326/// ```
327/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
328/// use parry3d::transformation::voxelization::{FillMode, VoxelValue, VoxelizedVolume};
329/// use parry3d::shape::Cuboid;
330/// use nalgebra::Vector3;
331///
332/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0));
333/// let (vertices, indices) = cuboid.to_trimesh();
334///
335/// let volume = VoxelizedVolume::voxelize(
336///     &vertices,
337///     &indices,
338///     8,
339///     FillMode::FloodFill { detect_cavities: false },
340///     false,
341/// );
342///
343/// // Count voxels by state
344/// let resolution = volume.resolution();
345/// let mut inside = 0;
346/// let mut surface = 0;
347/// let mut outside = 0;
348///
349/// for i in 0..resolution[0] {
350///     for j in 0..resolution[1] {
351///         for k in 0..resolution[2] {
352///             match volume.voxel(i, j, k) {
353///                 VoxelValue::PrimitiveInsideSurface => inside += 1,
354///                 VoxelValue::PrimitiveOnSurface => surface += 1,
355///                 VoxelValue::PrimitiveOutsideSurface => outside += 1,
356///                 _ => {}
357///             }
358///         }
359///     }
360/// }
361///
362/// println!("Inside: {}, Surface: {}, Outside: {}", inside, surface, outside);
363/// # }
364/// ```
365pub struct VoxelizedVolume {
366    origin: Point<Real>,
367    scale: Real,
368    resolution: [u32; DIM],
369    values: Vec<VoxelValue>,
370    data: Vec<VoxelData>,
371    primitive_intersections: Vec<(u32, u32)>,
372}
373
374impl VoxelizedVolume {
375    /// Voxelizes the given shape described by its boundary:
376    /// a triangle mesh (in 3D) or polyline (in 2D).
377    ///
378    /// # Parameters
379    /// * `points` - The vertex buffer of the boundary of the shape to voxelize.
380    /// * `indices` - The index buffer of the boundary of the shape to voxelize.
381    /// * `resolution` - Controls the number of subdivision done along each axis. This number
382    ///   is the number of subdivisions along the axis where the input shape has the largest extent.
383    ///   The other dimensions will have a different automatically-determined resolution (in order to
384    ///   keep the voxels cubic).
385    /// * `fill_mode` - Controls what is being voxelized.
386    /// * `keep_voxel_to_primitives_map` - If set to `true` a map between the voxels
387    ///   and the primitives (3D triangles or 2D segments) it intersects will be computed.
388    pub fn with_voxel_size(
389        points: &[Point<Real>],
390        indices: &[[u32; DIM]],
391        voxel_size: Real,
392        fill_mode: FillMode,
393        keep_voxel_to_primitives_map: bool,
394    ) -> Self {
395        let mut result = VoxelizedVolume {
396            resolution: [0; DIM],
397            origin: Point::origin(),
398            scale: voxel_size,
399            values: Vec::new(),
400            data: Vec::new(),
401            primitive_intersections: Vec::new(),
402        };
403
404        if points.is_empty() {
405            return result;
406        }
407
408        let aabb = crate::bounding_volume::details::local_point_cloud_aabb_ref(points);
409        result.origin = aabb.mins;
410        result.resolution = (aabb.extents() / voxel_size)
411            .map(|x| (x.ceil() as u32).max(2) + 1)
412            .into();
413
414        result.do_voxelize(points, indices, fill_mode, keep_voxel_to_primitives_map);
415        result
416    }
417
418    /// Voxelizes the given shape described by its boundary:
419    /// a triangle mesh (in 3D) or polyline (in 2D).
420    ///
421    /// # Parameters
422    /// * `points` - The vertex buffer of the boundary of the shape to voxelize.
423    /// * `indices` - The index buffer of the boundary of the shape to voxelize.
424    /// * `resolution` - Controls the number of subdivision done along each axis. This number
425    ///   is the number of subdivisions along the axis where the input shape has the largest extent.
426    ///   The other dimensions will have a different automatically-determined resolution (in order to
427    ///   keep the voxels cubic).
428    /// * `fill_mode` - Controls what is being voxelized.
429    /// * `keep_voxel_to_primitives_map` - If set to `true` a map between the voxels
430    ///   and the primitives (3D triangles or 2D segments) it intersects will be computed.
431    pub fn voxelize(
432        points: &[Point<Real>],
433        indices: &[[u32; DIM]],
434        resolution: u32,
435        fill_mode: FillMode,
436        keep_voxel_to_primitives_map: bool,
437    ) -> Self {
438        let mut result = VoxelizedVolume {
439            resolution: [0; DIM],
440            origin: Point::origin(),
441            scale: 1.0,
442            values: Vec::new(),
443            data: Vec::new(),
444            primitive_intersections: Vec::new(),
445        };
446
447        if points.is_empty() {
448            return result;
449        }
450
451        let aabb = crate::bounding_volume::details::local_point_cloud_aabb_ref(points);
452        result.origin = aabb.mins;
453
454        let d = aabb.maxs - aabb.mins;
455        let r;
456
457        #[cfg(feature = "dim2")]
458        if d[0] > d[1] {
459            r = d[0];
460            result.resolution[0] = resolution;
461            result.resolution[1] = 2 + (resolution as Real * d[1] / d[0]) as u32;
462        } else {
463            r = d[1];
464            result.resolution[1] = resolution;
465            result.resolution[0] = 2 + (resolution as Real * d[0] / d[1]) as u32;
466        }
467
468        #[cfg(feature = "dim3")]
469        if d[0] >= d[1] && d[0] >= d[2] {
470            r = d[0];
471            result.resolution[0] = resolution;
472            result.resolution[1] = 2 + (resolution as Real * d[1] / d[0]) as u32;
473            result.resolution[2] = 2 + (resolution as Real * d[2] / d[0]) as u32;
474        } else if d[1] >= d[0] && d[1] >= d[2] {
475            r = d[1];
476            result.resolution[1] = resolution;
477            result.resolution[0] = 2 + (resolution as Real * d[0] / d[1]) as u32;
478            result.resolution[2] = 2 + (resolution as Real * d[2] / d[1]) as u32;
479        } else {
480            r = d[2];
481            result.resolution[2] = resolution;
482            result.resolution[0] = 2 + (resolution as Real * d[0] / d[2]) as u32;
483            result.resolution[1] = 2 + (resolution as Real * d[1] / d[2]) as u32;
484        }
485
486        result.scale = r / (resolution as Real - 1.0);
487        result.do_voxelize(points, indices, fill_mode, keep_voxel_to_primitives_map);
488        result
489    }
490
491    fn do_voxelize(
492        &mut self,
493        points: &[Point<Real>],
494        indices: &[[u32; DIM]],
495        fill_mode: FillMode,
496        keep_voxel_to_primitives_map: bool,
497    ) {
498        let inv_scale = 1.0 / self.scale;
499        self.allocate();
500
501        let mut tri_pts = [Point::origin(); DIM];
502        let box_half_size = Vector::repeat(0.5);
503        let mut ijk0 = Vector::repeat(0u32);
504        let mut ijk1 = Vector::repeat(0u32);
505
506        let detect_self_intersections = fill_mode.detect_self_intersections();
507        #[cfg(feature = "dim2")]
508        let lock_high_multiplicities =
509            fill_mode.detect_cavities() && fill_mode.detect_self_intersections();
510
511        for (tri_id, tri) in indices.iter().enumerate() {
512            // Find the range of voxels potentially intersecting the triangle.
513            for c in 0..DIM {
514                let pt = points[tri[c] as usize];
515                tri_pts[c] = (pt - self.origin.coords) * inv_scale;
516
517                let i = (tri_pts[c].x + 0.5) as u32;
518                let j = (tri_pts[c].y + 0.5) as u32;
519                #[cfg(feature = "dim3")]
520                let k = (tri_pts[c].z + 0.5) as u32;
521
522                assert!(i < self.resolution[0] && j < self.resolution[1]);
523                #[cfg(feature = "dim3")]
524                assert!(k < self.resolution[2]);
525
526                #[cfg(feature = "dim2")]
527                let ijk = Vector::new(i, j);
528                #[cfg(feature = "dim3")]
529                let ijk = Vector::new(i, j, k);
530
531                if c == 0 {
532                    ijk0 = ijk;
533                    ijk1 = ijk;
534                } else {
535                    ijk0 = ijk0.inf(&ijk);
536                    ijk1 = ijk1.sup(&ijk);
537                }
538            }
539
540            ijk0.apply(|e| *e = e.saturating_sub(1));
541            ijk1 = ijk1
542                .map(|e| e + 1)
543                .inf(&Point::from(self.resolution).coords);
544
545            #[cfg(feature = "dim2")]
546            let range_k = 0..1;
547            #[cfg(feature = "dim3")]
548            let range_k = ijk0.z..ijk1.z;
549
550            // Determine exactly what voxel intersect the triangle.
551            for i in ijk0.x..ijk1.x {
552                for j in ijk0.y..ijk1.y {
553                    for k in range_k.clone() {
554                        #[cfg(feature = "dim2")]
555                        let pt = Point::new(i as Real, j as Real);
556                        #[cfg(feature = "dim3")]
557                        let pt = Point::new(i as Real, j as Real, k as Real);
558
559                        let id = self.voxel_index(i, j, k);
560                        let value = &mut self.values[id as usize];
561                        let data = &mut self.data[id as usize];
562
563                        if detect_self_intersections
564                            || keep_voxel_to_primitives_map
565                            || *value == VoxelValue::PrimitiveUndefined
566                        {
567                            let aabb = Aabb::from_half_extents(pt, box_half_size);
568
569                            #[cfg(feature = "dim2")]
570                            if !detect_self_intersections {
571                                let segment = crate::shape::Segment::from(tri_pts);
572                                let intersect =
573                                    query::details::intersection_test_aabb_segment(&aabb, &segment);
574
575                                if intersect {
576                                    if keep_voxel_to_primitives_map {
577                                        data.num_primitive_intersections += 1;
578                                        self.primitive_intersections.push((id, tri_id as u32));
579                                    }
580
581                                    *value = VoxelValue::PrimitiveOnSurface;
582                                }
583                            } else if let Some(params) =
584                                aabb.clip_line_parameters(&tri_pts[0], &(tri_pts[1] - tri_pts[0]))
585                            {
586                                let eps = 0.0; // -1.0e-6;
587
588                                assert!(params.0 <= params.1);
589                                if params.0 > 1.0 + eps || params.1 < 0.0 - eps {
590                                    continue;
591                                }
592
593                                if keep_voxel_to_primitives_map {
594                                    data.num_primitive_intersections += 1;
595                                    self.primitive_intersections.push((id, tri_id as u32));
596                                }
597
598                                if data.multiplicity > 4 && lock_high_multiplicities {
599                                    *value = VoxelValue::PrimitiveOnSurfaceNoWalk;
600                                } else {
601                                    *value = VoxelValue::PrimitiveOnSurface;
602                                }
603                            };
604
605                            #[cfg(feature = "dim3")]
606                            {
607                                let triangle = crate::shape::Triangle::from(tri_pts);
608                                let intersect = query::details::intersection_test_aabb_triangle(
609                                    &aabb, &triangle,
610                                );
611
612                                if intersect {
613                                    *value = VoxelValue::PrimitiveOnSurface;
614
615                                    if keep_voxel_to_primitives_map {
616                                        data.num_primitive_intersections += 1;
617                                        self.primitive_intersections.push((id, tri_id as u32));
618                                    }
619                                }
620                            };
621                        }
622                    }
623                }
624            }
625        }
626
627        match fill_mode {
628            FillMode::SurfaceOnly => {
629                for value in &mut self.values {
630                    if *value != VoxelValue::PrimitiveOnSurface {
631                        *value = VoxelValue::PrimitiveOutsideSurface
632                    }
633                }
634            }
635            FillMode::FloodFill {
636                detect_cavities, ..
637            } => {
638                #[cfg(feature = "dim2")]
639                {
640                    self.mark_outside_surface(0, 0, self.resolution[0], 1);
641                    self.mark_outside_surface(
642                        0,
643                        self.resolution[1] - 1,
644                        self.resolution[0],
645                        self.resolution[1],
646                    );
647                    self.mark_outside_surface(0, 0, 1, self.resolution[1]);
648                    self.mark_outside_surface(
649                        self.resolution[0] - 1,
650                        0,
651                        self.resolution[0],
652                        self.resolution[1],
653                    );
654                }
655
656                #[cfg(feature = "dim3")]
657                {
658                    self.mark_outside_surface(0, 0, 0, self.resolution[0], self.resolution[1], 1);
659                    self.mark_outside_surface(
660                        0,
661                        0,
662                        self.resolution[2] - 1,
663                        self.resolution[0],
664                        self.resolution[1],
665                        self.resolution[2],
666                    );
667                    self.mark_outside_surface(0, 0, 0, self.resolution[0], 1, self.resolution[2]);
668                    self.mark_outside_surface(
669                        0,
670                        self.resolution[1] - 1,
671                        0,
672                        self.resolution[0],
673                        self.resolution[1],
674                        self.resolution[2],
675                    );
676                    self.mark_outside_surface(0, 0, 0, 1, self.resolution[1], self.resolution[2]);
677                    self.mark_outside_surface(
678                        self.resolution[0] - 1,
679                        0,
680                        0,
681                        self.resolution[0],
682                        self.resolution[1],
683                        self.resolution[2],
684                    );
685                }
686
687                if detect_cavities {
688                    let _ = self.propagate_values(
689                        VoxelValue::PrimitiveOutsideSurfaceToWalk,
690                        VoxelValue::PrimitiveOutsideSurface,
691                        None,
692                        VoxelValue::PrimitiveOnSurfaceToWalk1,
693                    );
694
695                    loop {
696                        if !self.propagate_values(
697                            VoxelValue::PrimitiveInsideSurfaceToWalk,
698                            VoxelValue::PrimitiveInsideSurface,
699                            Some(VoxelValue::PrimitiveOnSurfaceToWalk1),
700                            VoxelValue::PrimitiveOnSurfaceToWalk2,
701                        ) {
702                            break;
703                        }
704
705                        if !self.propagate_values(
706                            VoxelValue::PrimitiveOutsideSurfaceToWalk,
707                            VoxelValue::PrimitiveOutsideSurface,
708                            Some(VoxelValue::PrimitiveOnSurfaceToWalk2),
709                            VoxelValue::PrimitiveOnSurfaceToWalk1,
710                        ) {
711                            break;
712                        }
713                    }
714
715                    for voxel in &mut self.values {
716                        if *voxel == VoxelValue::PrimitiveOnSurfaceToWalk1
717                            || *voxel == VoxelValue::PrimitiveOnSurfaceToWalk2
718                            || *voxel == VoxelValue::PrimitiveOnSurfaceNoWalk
719                        {
720                            *voxel = VoxelValue::PrimitiveOnSurface;
721                        }
722                    }
723                } else {
724                    let _ = self.propagate_values(
725                        VoxelValue::PrimitiveOutsideSurfaceToWalk,
726                        VoxelValue::PrimitiveOutsideSurface,
727                        None,
728                        VoxelValue::PrimitiveOnSurface,
729                    );
730
731                    self.replace_value(
732                        VoxelValue::PrimitiveUndefined,
733                        VoxelValue::PrimitiveInsideSurface,
734                    );
735                }
736            }
737        }
738    }
739
740    /// The number of voxel subdivisions along each coordinate axis.
741    pub fn resolution(&self) -> [u32; DIM] {
742        self.resolution
743    }
744
745    /// The scale factor that needs to be applied to the voxels of `self`
746    /// in order to give them the size matching the original model's size.
747    pub fn scale(&self) -> Real {
748        self.scale
749    }
750
751    fn allocate(&mut self) {
752        #[cfg(feature = "dim2")]
753        let len = self.resolution[0] * self.resolution[1];
754        #[cfg(feature = "dim3")]
755        let len = self.resolution[0] * self.resolution[1] * self.resolution[2];
756        self.values
757            .resize(len as usize, VoxelValue::PrimitiveUndefined);
758        self.data.resize(
759            len as usize,
760            VoxelData {
761                #[cfg(feature = "dim2")]
762                multiplicity: 0,
763                num_primitive_intersections: 0,
764            },
765        );
766    }
767
768    fn voxel_index(&self, i: u32, j: u32, _k: u32) -> u32 {
769        #[cfg(feature = "dim2")]
770        return i + j * self.resolution[0];
771        #[cfg(feature = "dim3")]
772        return i + j * self.resolution[0] + _k * self.resolution[0] * self.resolution[1];
773    }
774
775    fn voxel_mut(&mut self, i: u32, j: u32, k: u32) -> &mut VoxelValue {
776        let idx = self.voxel_index(i, j, k);
777        &mut self.values[idx as usize]
778    }
779
780    /// The value of the given voxel.
781    ///
782    /// In 2D, the `k` argument is ignored.
783    pub fn voxel(&self, i: u32, j: u32, k: u32) -> VoxelValue {
784        let idx = self.voxel_index(i, j, k);
785        self.values[idx as usize]
786    }
787
788    /// Mark all the PrimitiveUndefined voxels within the given bounds as PrimitiveOutsideSurfaceToWalk.
789    #[cfg(feature = "dim2")]
790    fn mark_outside_surface(&mut self, i0: u32, j0: u32, i1: u32, j1: u32) {
791        for i in i0..i1 {
792            for j in j0..j1 {
793                let v = self.voxel_mut(i, j, 0);
794
795                if *v == VoxelValue::PrimitiveUndefined {
796                    *v = VoxelValue::PrimitiveOutsideSurfaceToWalk;
797                }
798            }
799        }
800    }
801
802    /// Mark all the PrimitiveUndefined voxels within the given bounds as PrimitiveOutsideSurfaceToWalk.
803    #[cfg(feature = "dim3")]
804    fn mark_outside_surface(&mut self, i0: u32, j0: u32, k0: u32, i1: u32, j1: u32, k1: u32) {
805        for i in i0..i1 {
806            for j in j0..j1 {
807                for k in k0..k1 {
808                    let v = self.voxel_mut(i, j, k);
809
810                    if *v == VoxelValue::PrimitiveUndefined {
811                        *v = VoxelValue::PrimitiveOutsideSurfaceToWalk;
812                    }
813                }
814            }
815        }
816    }
817
818    fn walk_forward(
819        primitive_undefined_value_to_set: VoxelValue,
820        on_surface_value_to_set: VoxelValue,
821        start: isize,
822        end: isize,
823        mut ptr: isize,
824        out: &mut [VoxelValue],
825        stride: isize,
826        max_distance: isize,
827    ) {
828        let mut i = start;
829        let mut count = 0;
830
831        while count < max_distance && i < end {
832            if out[ptr as usize] == VoxelValue::PrimitiveUndefined {
833                out[ptr as usize] = primitive_undefined_value_to_set;
834            } else if out[ptr as usize] == VoxelValue::PrimitiveOnSurface {
835                out[ptr as usize] = on_surface_value_to_set;
836                break;
837            } else {
838                break;
839            }
840
841            i += 1;
842            ptr += stride;
843            count += 1;
844        }
845    }
846
847    fn walk_backward(
848        primitive_undefined_value_to_set: VoxelValue,
849        on_surface_value_to_set: VoxelValue,
850        start: isize,
851        end: isize,
852        mut ptr: isize,
853        out: &mut [VoxelValue],
854        stride: isize,
855        max_distance: isize,
856    ) {
857        let mut i = start;
858        let mut count = 0;
859
860        while count < max_distance && i >= end {
861            if out[ptr as usize] == VoxelValue::PrimitiveUndefined {
862                out[ptr as usize] = primitive_undefined_value_to_set;
863            } else if out[ptr as usize] == VoxelValue::PrimitiveOnSurface {
864                out[ptr as usize] = on_surface_value_to_set;
865                break;
866            } else {
867                break;
868            }
869
870            i -= 1;
871            ptr -= stride;
872            count += 1;
873        }
874    }
875
876    fn propagate_values(
877        &mut self,
878        inside_surface_value_to_walk: VoxelValue,
879        inside_surface_value_to_set: VoxelValue,
880        on_surface_value_to_walk: Option<VoxelValue>,
881        on_surface_value_to_set: VoxelValue,
882    ) -> bool {
883        let mut voxels_walked;
884        let mut walked_at_least_once = false;
885        let i0 = self.resolution[0];
886        let j0 = self.resolution[1];
887        #[cfg(feature = "dim2")]
888        let k0 = 1;
889        #[cfg(feature = "dim3")]
890        let k0 = self.resolution[2];
891
892        // Avoid striding too far in each direction to stay in L1 cache as much as possible.
893        // The cache size required for the walk is roughly (4 * walk_distance * 64) since
894        // the k direction doesn't count as it's walking byte per byte directly in a cache lines.
895        // ~16k is required for a walk distance of 64 in each directions.
896        let walk_distance = 64;
897
898        // using the stride directly instead of calling get_voxel for each iterations saves
899        // a lot of multiplications and pipeline stalls due to values dependencies on imul.
900        let istride = self.voxel_index(1, 0, 0) as isize - self.voxel_index(0, 0, 0) as isize;
901        let jstride = self.voxel_index(0, 1, 0) as isize - self.voxel_index(0, 0, 0) as isize;
902        #[cfg(feature = "dim3")]
903        let kstride = self.voxel_index(0, 0, 1) as isize - self.voxel_index(0, 0, 0) as isize;
904
905        // It might seem counter intuitive to go over the whole voxel range multiple times
906        // but since we do the run in memory order, it leaves us with far fewer cache misses
907        // than a BFS algorithm and it has the additional benefit of not requiring us to
908        // store and manipulate a fifo for recursion that might become huge when the number
909        // of voxels is large.
910        // This will outperform the BFS algorithm by several orders of magnitude in practice.
911        loop {
912            voxels_walked = 0;
913
914            for i in 0..i0 {
915                for j in 0..j0 {
916                    for k in 0..k0 {
917                        let idx = self.voxel_index(i, j, k) as isize;
918                        let voxel = self.voxel_mut(i, j, k);
919
920                        if *voxel == inside_surface_value_to_walk {
921                            voxels_walked += 1;
922                            walked_at_least_once = true;
923                            *voxel = inside_surface_value_to_set;
924                        } else if Some(*voxel) != on_surface_value_to_walk {
925                            continue;
926                        }
927
928                        // walk in each direction to mark other voxel that should be walked.
929                        // this will generate a 3d pattern that will help the overall
930                        // algorithm converge faster while remaining cache friendly.
931                        #[cfg(feature = "dim3")]
932                        Self::walk_forward(
933                            inside_surface_value_to_walk,
934                            on_surface_value_to_set,
935                            k as isize + 1,
936                            k0 as isize,
937                            idx + kstride,
938                            &mut self.values,
939                            kstride,
940                            walk_distance,
941                        );
942                        #[cfg(feature = "dim3")]
943                        Self::walk_backward(
944                            inside_surface_value_to_walk,
945                            on_surface_value_to_set,
946                            k as isize - 1,
947                            0,
948                            idx - kstride,
949                            &mut self.values,
950                            kstride,
951                            walk_distance,
952                        );
953
954                        Self::walk_forward(
955                            inside_surface_value_to_walk,
956                            on_surface_value_to_set,
957                            j as isize + 1,
958                            j0 as isize,
959                            idx + jstride,
960                            &mut self.values,
961                            jstride,
962                            walk_distance,
963                        );
964                        Self::walk_backward(
965                            inside_surface_value_to_walk,
966                            on_surface_value_to_set,
967                            j as isize - 1,
968                            0,
969                            idx - jstride,
970                            &mut self.values,
971                            jstride,
972                            walk_distance,
973                        );
974
975                        Self::walk_forward(
976                            inside_surface_value_to_walk,
977                            on_surface_value_to_set,
978                            (i + 1) as isize,
979                            i0 as isize,
980                            idx + istride,
981                            &mut self.values,
982                            istride,
983                            walk_distance,
984                        );
985                        Self::walk_backward(
986                            inside_surface_value_to_walk,
987                            on_surface_value_to_set,
988                            i as isize - 1,
989                            0,
990                            idx - istride,
991                            &mut self.values,
992                            istride,
993                            walk_distance,
994                        );
995                    }
996                }
997            }
998
999            if voxels_walked == 0 {
1000                break;
1001            }
1002        }
1003
1004        walked_at_least_once
1005    }
1006
1007    fn replace_value(&mut self, current_value: VoxelValue, new_value: VoxelValue) {
1008        for voxel in &mut self.values {
1009            if *voxel == current_value {
1010                *voxel = new_value;
1011            }
1012        }
1013    }
1014
1015    /// Naive conversion of all the voxels with the given `value` to a triangle-mesh.
1016    ///
1017    /// This conversion is extremely naive: it will simply collect all the 12 triangles forming
1018    /// the faces of each voxel. No actual boundary extraction is done.
1019    #[cfg(feature = "dim3")]
1020    pub fn to_trimesh(&self, value: VoxelValue) -> (Vec<Point<Real>>, Vec<[u32; DIM]>) {
1021        let mut vertices = Vec::new();
1022        let mut indices = Vec::new();
1023
1024        for i in 0..self.resolution[0] {
1025            for j in 0..self.resolution[1] {
1026                for k in 0..self.resolution[2] {
1027                    let voxel = self.voxel(i, j, k);
1028
1029                    if voxel == value {
1030                        let ijk = Vector::new(i as Real, j as Real, k as Real);
1031
1032                        let shifts = [
1033                            Vector::new(-0.5, -0.5, -0.5),
1034                            Vector::new(0.5, -0.5, -0.5),
1035                            Vector::new(0.5, 0.5, -0.5),
1036                            Vector::new(-0.5, 0.5, -0.5),
1037                            Vector::new(-0.5, -0.5, 0.5),
1038                            Vector::new(0.5, -0.5, 0.5),
1039                            Vector::new(0.5, 0.5, 0.5),
1040                            Vector::new(-0.5, 0.5, 0.5),
1041                        ];
1042
1043                        for shift in &shifts {
1044                            vertices.push(self.origin + (ijk + shift) * self.scale);
1045                        }
1046
1047                        let s = vertices.len() as u32;
1048                        indices.push([s, s + 2, s + 1]);
1049                        indices.push([s, s + 3, s + 2]);
1050                        indices.push([s + 4, s + 5, s + 6]);
1051                        indices.push([s + 4, s + 6, s + 7]);
1052                        indices.push([s + 7, s + 6, s + 2]);
1053                        indices.push([s + 7, s + 2, s + 3]);
1054                        indices.push([s + 4, s + 1, s + 5]);
1055                        indices.push([s + 4, s, s + 1]);
1056                        indices.push([s + 6, s + 5, s + 1]);
1057                        indices.push([s + 6, s + 1, s + 2]);
1058                        indices.push([s + 7, s, s + 4]);
1059                        indices.push([s + 7, s + 3, s]);
1060                    }
1061                }
1062            }
1063        }
1064
1065        (vertices, indices)
1066    }
1067}
1068
1069impl From<VoxelizedVolume> for VoxelSet {
1070    fn from(mut shape: VoxelizedVolume) -> Self {
1071        let mut curr_intersection_index = 0;
1072        let mut vset = VoxelSet::new();
1073        let mut vset_intersections = Vec::new();
1074        vset.origin = shape.origin;
1075        vset.scale = shape.scale;
1076
1077        #[cfg(feature = "dim2")]
1078        let k1 = 1;
1079        #[cfg(feature = "dim3")]
1080        let k1 = shape.resolution[2];
1081
1082        for i in 0..shape.resolution[0] {
1083            for j in 0..shape.resolution[1] {
1084                for k in 0..k1 {
1085                    let id = shape.voxel_index(i, j, k) as usize;
1086                    let value = shape.values[id];
1087                    #[cfg(feature = "dim2")]
1088                    let coords = Point::new(i, j);
1089                    #[cfg(feature = "dim3")]
1090                    let coords = Point::new(i, j, k);
1091
1092                    if value == VoxelValue::PrimitiveInsideSurface {
1093                        let voxel = Voxel {
1094                            coords,
1095                            is_on_surface: false,
1096                            intersections_range: (curr_intersection_index, curr_intersection_index),
1097                        };
1098                        vset.voxels.push(voxel);
1099                    } else if value == VoxelValue::PrimitiveOnSurface {
1100                        let mut voxel = Voxel {
1101                            coords,
1102                            is_on_surface: true,
1103                            intersections_range: (curr_intersection_index, curr_intersection_index),
1104                        };
1105
1106                        if !shape.primitive_intersections.is_empty() {
1107                            let num_intersections =
1108                                shape.data[id].num_primitive_intersections as usize;
1109                            // We store the index where we should write the intersection on the
1110                            // vset into num_primitive_intersections. That way we can reuse it
1111                            // afterwards when copying the set of intersection into a single
1112                            // flat Vec.
1113                            shape.data[id].num_primitive_intersections =
1114                                curr_intersection_index as u32;
1115                            curr_intersection_index += num_intersections;
1116                            voxel.intersections_range.1 = curr_intersection_index;
1117                        }
1118
1119                        vset.voxels.push(voxel);
1120                    }
1121                }
1122            }
1123        }
1124
1125        if !shape.primitive_intersections.is_empty() {
1126            vset_intersections.resize(shape.primitive_intersections.len(), 0);
1127            for (voxel_id, prim_id) in shape.primitive_intersections {
1128                let num_inter = &mut shape.data[voxel_id as usize].num_primitive_intersections;
1129                vset_intersections[*num_inter as usize] = prim_id;
1130                *num_inter += 1;
1131            }
1132        }
1133
1134        vset.intersections = Arc::new(vset_intersections);
1135
1136        vset
1137    }
1138}
1139
1140/*
1141fn traceRay(
1142    mesh: &RaycastMesh,
1143    start: Real,
1144    dir: &Vector<Real>,
1145    inside_count: &mut u32,
1146    outside_count: &mut u32,
1147) {
1148    let out_t;
1149    let u;
1150    let v;
1151    let w;
1152    let face_sign;
1153    let face_index;
1154    let hit = raycast_mesh.raycast(start, dir, out_t, u, v, w, face_sign, face_index);
1155
1156    if hit {
1157        if face_sign >= 0 {
1158            *inside_count += 1;
1159        } else {
1160            *outside_count += 1;
1161        }
1162    }
1163}
1164
1165
1166fn raycast_fill(volume: &Volume, raycast_mesh: &RaycastMesh) {
1167if !raycast_mesh {
1168    return;
1169}
1170
1171let scale = volume.scale;
1172let bmin = volume.min_bb;
1173
1174let i0 = volume.resolution[0];
1175let j0 = volume.resolution[1];
1176let k0 = volume.resolution[2];
1177
1178for i in 0..i0 {
1179    for j in 0..j0 {
1180        for k in 0..k0 {
1181            let voxel = volume.get_voxel(i, j, k);
1182
1183            if voxel != VoxelValue::PrimitiveOnSurface {
1184                let start = Vector::new(
1185                    i as Real * scale + bmin[0],
1186                    j as Real * scale + bmin[1],
1187                    k as Real * scale + bmin[2],
1188                );
1189
1190                let mut inside_count = 0;
1191                let mut outside_count = 0;
1192
1193                let directions = [
1194                    Vector::x(),
1195                    -Vector::x(),
1196                    Vector::y(),
1197                    -Vector::y(),
1198                    Vector::z(),
1199                    -Vector::z(),
1200                ];
1201
1202                for r in 0..6 {
1203                    traceRay(
1204                        raycast_mesh,
1205                        start,
1206                        &directions[r * 3],
1207                        &mut inside_count,
1208                        &mut outside_count,
1209                    );
1210
1211                    // Early out if we hit the outside of the mesh
1212                    if outside_count != 0 {
1213                        break;
1214                    }
1215
1216                    // Early out if we accumulated 3 inside hits
1217                    if inside_count >= 3 {
1218                        break;
1219                    }
1220                }
1221
1222                if outside_count == 0 && inside_count >= 3 {
1223                    volume.set_voxel(i, j, k, VoxelValue::PrimitiveInsideSurface);
1224                } else {
1225                    volume.set_voxel(i, j, k, VoxelValue::PrimitiveOutsideSurface);
1226                }
1227            }
1228        }
1229    }
1230}
1231}
1232 */