parry3d_f64/shape/
voxels.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Point, Real, Vector, DIM};
3use alloc::{vec, vec::Vec};
4
5/// The primitive shape all voxels from a [`Voxels`] is given.
6#[derive(Copy, Clone, Debug, PartialEq, Default)]
7#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
8pub enum VoxelPrimitiveGeometry {
9    /// Each voxel is modeled as a pseudo-ball, i.e., in flat areas it will act like a planar
10    /// surface but corners and edges will be rounded like a sphere.
11    ///
12    /// This is an approximation that is particularly relevant if the voxels are small and in large
13    /// number. This can significantly improve collision-detection performances (as well as solver
14    /// performances by generating less contacts points). However,this can introduce visual
15    /// artifacts around edges and corners where objects in contact with the voxel will appear to
16    /// slightly penetrate the corners/edges due to the spherical approximation.
17    PseudoBall,
18    /// Each voxel is modeled as a pseudo-cube, i.e., in flat areas it will act like a planar
19    /// surface but corner and edges will be sharp like a cube.
20    ///
21    /// This is what you would expect for the collision to match the rendered voxels. Use
22    /// [`VoxelPrimitiveGeometry::PseudoBall`] instead if some approximation around corners are acceptable
23    /// in exchange for better performances.
24    #[default]
25    PseudoCube,
26}
27
28/// Categorization of a voxel based on its neighbors.
29#[derive(Copy, Clone, Debug, PartialEq, Eq)]
30pub enum VoxelType {
31    /// The voxel is empty.
32    Empty,
33    /// The voxel is a vertex if all three coordinate axis directions have at
34    /// least one empty neighbor.
35    Vertex,
36    /// The voxel is on an edge if it has non-empty neighbors in both directions of
37    /// a single coordinate axis.
38    #[cfg(feature = "dim3")]
39    Edge,
40    /// The voxel is on an edge if it has non-empty neighbors in both directions of
41    /// two coordinate axes.
42    Face,
43    /// The voxel is on an edge if it has non-empty neighbors in both directions of
44    /// all coordinate axes.
45    Interior,
46}
47
48#[derive(Clone, Copy, Debug, Default, Eq, Hash, Ord, PartialEq, PartialOrd)]
49/// The status of the cell of an heightfield.
50pub struct AxisMask(u8);
51
52bitflags::bitflags! {
53    /// Flags for identifying signed directions along coordinate axes, or faces of a voxel.
54    impl AxisMask: u8 {
55        /// The direction or face along the `+x` coordinate axis.
56        const X_POS = 1 << 0;
57        /// The direction or face along the `-x` coordinate axis.
58        const X_NEG = 1 << 1;
59        /// The direction or face along the `+y` coordinate axis.
60        const Y_POS = 1 << 2;
61        /// The direction or face along the `-y` coordinate axis.
62        const Y_NEG = 1 << 3;
63        /// The direction or face along the `+z` coordinate axis.
64        #[cfg(feature= "dim3")]
65        const Z_POS = 1 << 4;
66        /// The direction or face along the `-z` coordinate axis.
67        #[cfg(feature= "dim3")]
68        const Z_NEG = 1 << 5;
69    }
70}
71
72/// Indicates the local shape of a voxel on each octant.
73///
74/// This provides geometric information of the shape’s exposed features on each octant.
75// This is an alternative to `FACES_TO_FEATURE_MASKS` that can be more convenient for some
76// collision-detection algorithms.
77#[derive(Copy, Clone, Debug, PartialEq, Eq)]
78pub struct OctantPattern;
79
80// NOTE: it is important that the max value of any OctantPattern variant
81//       is 7 because we don’t allocate more than 3 bits to store it in
82//      `FACES_TO_OCTANT_MASKS`.
83/// Indicates the local shape of a voxel on each octant.
84///
85/// This provides geometric information of the shape’s exposed features on each octant.
86// This is an alternative to `FACES_TO_FEATURE_MASKS` that can be more convenient for some
87// collision-detection algorithms.
88#[cfg(feature = "dim3")]
89impl OctantPattern {
90    /// The voxel doesn't have any exposed feature on the octant with this mask.
91    pub const INTERIOR: u32 = 0;
92    /// The voxel has an exposed vertex on the octant with this mask.
93    pub const VERTEX: u32 = 1;
94    /// The voxel has an exposed edges with direction X on the octant with this mask.
95    pub const EDGE_X: u32 = 2;
96    /// The voxel has an exposed edges with direction Y on the octant with this mask.
97    pub const EDGE_Y: u32 = 3;
98    /// The voxel has an exposed edges with direction Z on the octant with this mask.
99    pub const EDGE_Z: u32 = 4;
100    /// The voxel has an exposed face with normal X on the octant with this mask.
101    pub const FACE_X: u32 = 5;
102    /// The voxel has an exposed face with normal Y on the octant with this mask.
103    pub const FACE_Y: u32 = 6;
104    /// The voxel has an exposed face with normal Z on the octant with this mask.
105    pub const FACE_Z: u32 = 7;
106}
107
108// NOTE: it is important that the max value of any OctantPattern variant
109//       is 7 because we don’t allocate more than 3 bits to store it in
110//      `FACES_TO_OCTANT_MASKS`.
111/// Indicates the local shape of a voxel on each octant.
112///
113/// This provides geometric information of the shape’s exposed features on each octant.
114/// This is an alternative to `FACES_TO_FEATURE_MASKS` that can be more convenient for some
115/// collision-detection algorithms.
116#[cfg(feature = "dim2")]
117impl OctantPattern {
118    /// The voxel doesn't have any exposed feature on the octant with this mask.
119    pub const INTERIOR: u32 = 0;
120    /// The voxel has an exposed vertex on the octant with this mask.
121    pub const VERTEX: u32 = 1;
122    /// The voxel has an exposed face with normal X on the octant with this mask.
123    pub const FACE_X: u32 = 2;
124    /// The voxel has an exposed face with normal Y on the octant with this mask.
125    pub const FACE_Y: u32 = 3;
126}
127
128// The local neighborhood information is encoded in a 8-bits number in groups of two bits
129// per coordinate axis: `0bwwzzyyxx`. In each group of two bits, e.g. `xx`, the rightmost (resp.
130// leftmost) bit set to 1 means that the neighbor voxel with coordinate `+1` (resp `-1`) relative
131// to the current voxel along the `x` axis is filled. If the bit is 0, then the corresponding
132// neighbor is empty. See the `AxisMask` bitflags.
133// For example, in 2D, the mask `0b00_00_10_01` matches the following configuration (assuming +y goes
134// up, and +x goes right):
135//
136// ```txt
137//  0 0 0
138//  0 x 1
139//  0 1 0
140// ```
141//
142// The special value `0b01000000` indicates that the voxel is empty.
143// And the value `0b00111111` (`0b00001111` in 2D) indicates that the voxel is an interior voxel (its whole neighborhood
144// is filled).
145/// A description of the local neighborhood of a voxel.
146#[derive(Copy, Clone, Debug, PartialEq, Eq)]
147#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
148pub struct VoxelState(u8);
149
150impl VoxelState {
151    /// The value of empty voxels.
152    pub const EMPTY: VoxelState = VoxelState(EMPTY_FACE_MASK);
153    /// The value of a voxel with non-empty neighbors in all directions.
154    pub const INTERIOR: VoxelState = VoxelState(INTERIOR_FACE_MASK);
155
156    /// Is this voxel empty?
157    pub const fn is_empty(self) -> bool {
158        self.0 == EMPTY_FACE_MASK
159    }
160
161    /// A bit mask indicating which faces of the voxel don’t have any
162    /// adjacent non-empty voxel.
163    pub const fn free_faces(self) -> AxisMask {
164        if self.0 == INTERIOR_FACE_MASK || self.0 == EMPTY_FACE_MASK {
165            AxisMask::empty()
166        } else {
167            AxisMask::from_bits_truncate((!self.0) & INTERIOR_FACE_MASK)
168        }
169    }
170
171    /// The [`VoxelType`] of this voxel.
172    pub const fn voxel_type(self) -> VoxelType {
173        FACES_TO_VOXEL_TYPES[self.0 as usize]
174    }
175
176    // Bitmask indicating what vertices, edges, or faces of the voxel are "free".
177    pub(crate) const fn feature_mask(self) -> u16 {
178        FACES_TO_FEATURE_MASKS[self.0 as usize]
179    }
180
181    pub(crate) const fn octant_mask(self) -> u32 {
182        FACES_TO_OCTANT_MASKS[self.0 as usize]
183    }
184}
185
186/// Information associated to a voxel.
187#[derive(Copy, Clone, Debug, PartialEq)]
188pub struct VoxelData {
189    /// The temporary index in the internal voxels’ storage.
190    ///
191    /// This index can be invalidated after a call to [`Voxels::set_voxel`], or
192    /// [`Voxels::resize_domain`].
193    pub linear_id: u32,
194    /// The voxel’s integer grid coordinates.
195    pub grid_coords: Point<i32>,
196    /// The voxel’s center position in the local-space of the [`Voxels`] shape it is part of.
197    pub center: Point<Real>,
198    /// The voxel’s state, indicating if it’s empty or full.
199    pub state: VoxelState,
200}
201
202/// A shape made of axis-aligned, uniformly sized, cubes (aka. voxels).
203///
204/// This shape is specialized to handle voxel worlds and voxelized obojects efficiently why ensuring
205/// that collision-detection isn’t affected by the so-called "internal edges problem" that can create
206/// artifacts when another objects rolls or slides against a flat voxelized surface.
207///
208/// The internal storage is compact (but not sparse at the moment), storing only one byte per voxel
209/// in the allowed domain. This has a generally smaller memory footprint than a mesh representation
210/// of the voxels.
211#[derive(Clone, Debug)]
212#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
213pub struct Voxels {
214    domain_mins: Point<i32>,
215    domain_maxs: Point<i32>,
216    states: Vec<VoxelState>, // Somehow switch to a sparse representation?
217    primitive_geometry: VoxelPrimitiveGeometry,
218    voxel_size: Vector<Real>,
219}
220
221impl Voxels {
222    /// Initializes a voxel shapes from the voxels grid coordinates.
223    ///
224    /// Each voxel will have its bottom-left-back corner located at
225    /// `grid_coordinates * voxel_size`; and its center at `(grid_coordinates + 0.5) * voxel_size`.
226    pub fn new(
227        primitive_geometry: VoxelPrimitiveGeometry,
228        voxel_size: Vector<Real>,
229        grid_coordinates: &[Point<i32>],
230    ) -> Self {
231        // Ensure pseudo-balls always use uniform voxel sizes.
232        let voxel_size = match primitive_geometry {
233            VoxelPrimitiveGeometry::PseudoBall => Vector::repeat(voxel_size.x),
234            VoxelPrimitiveGeometry::PseudoCube => voxel_size,
235        };
236
237        let mut domain_mins = grid_coordinates[0];
238        let mut domain_maxs = grid_coordinates[0];
239
240        for vox in grid_coordinates {
241            domain_mins = domain_mins.inf(vox);
242            domain_maxs = domain_maxs.sup(vox);
243        }
244
245        domain_maxs += Vector::repeat(1);
246        let dimensions = domain_maxs - domain_mins;
247        let voxels_count = dimensions.product();
248        let mut result = Self {
249            domain_mins,
250            domain_maxs,
251            states: vec![VoxelState::EMPTY; voxels_count as usize],
252            primitive_geometry,
253            voxel_size,
254        };
255
256        for vox in grid_coordinates {
257            let index = result.linear_index(*vox);
258            result.states[index as usize] = VoxelState::INTERIOR;
259        }
260
261        result.recompute_voxels_data();
262        result
263    }
264
265    /// Computes a voxels shape from the set of `points`.
266    ///
267    /// The points are mapped to a regular grid centered at the provided point with smallest
268    /// coordinates, and with grid cell size equal to `scale`. It is OK if multiple points
269    /// fall into the same grid cell.
270    pub fn from_points(
271        primitive_geometry: VoxelPrimitiveGeometry,
272        voxel_size: Vector<Real>,
273        points: &[Point<Real>],
274    ) -> Self {
275        // Ensure pseudo-balls always use uniform voxel sizes.
276        let voxel_size = match primitive_geometry {
277            VoxelPrimitiveGeometry::PseudoBall => Vector::repeat(voxel_size.x),
278            VoxelPrimitiveGeometry::PseudoCube => voxel_size,
279        };
280
281        let voxels: Vec<_> = points
282            .iter()
283            .map(|pt| {
284                Point::from(
285                    pt.coords
286                        .component_div(&voxel_size)
287                        .map(|x| x.floor() as i32),
288                )
289            })
290            .collect();
291        Self::new(primitive_geometry, voxel_size, &voxels)
292    }
293
294    // TODO: support a crate like get_size2 (will require support on nalgebra too)?
295    /// An approximation of the memory usage (in bytes) for this struct plus
296    /// the memory it allocates dynamically.
297    pub fn total_memory_size(&self) -> usize {
298        size_of::<Self>() + self.heap_memory_size()
299    }
300
301    /// An approximation of the memory dynamically-allocated by this struct.
302    pub fn heap_memory_size(&self) -> usize {
303        // NOTE: if a new field is added to `Self`, adjust this function result.
304        let Self {
305            domain_mins: _,
306            domain_maxs: _,
307            states: data,
308            primitive_geometry: _,
309            voxel_size: _,
310        } = self;
311        data.capacity() * size_of::<VoxelState>()
312    }
313
314    /// The extents of the total axis-aligned volume covered by this [`Voxels`] shape.
315    ///
316    /// This accounts for all the voxels reserved in the internal buffer of `self`, including empty
317    /// ones.
318    pub fn extents(&self) -> Vector<Real> {
319        self.dimensions()
320            .cast::<Real>()
321            .component_mul(&self.voxel_size)
322    }
323
324    /// The center of this shape’s domain (accounting for both empty and filled voxels).
325    pub fn domain_center(&self) -> Point<Real> {
326        (self
327            .domain_mins
328            .coords
329            .cast::<Real>()
330            .component_mul(&self.voxel_size)
331            + self.extents() / 2.0)
332            .into()
333    }
334
335    /// Sets the size of each voxel along each local coordinate axis.
336    ///
337    /// If [`Self::primitive_geometry`] is [`VoxelPrimitiveGeometry::PseudoBall`], then all voxels
338    /// must be square, and only `size.x` is taken into account for setting the size.
339    pub fn set_voxel_size(&mut self, size: Vector<Real>) {
340        match self.primitive_geometry {
341            VoxelPrimitiveGeometry::PseudoBall => {
342                self.voxel_size = Vector::repeat(size.x);
343            }
344            VoxelPrimitiveGeometry::PseudoCube => {
345                self.voxel_size = size;
346            }
347        }
348    }
349
350    /// The valid semi-open range of voxel grid indices.
351    ///
352    /// With `let [mins, maxs] = voxels.domain();` the valid indices along the dimension `i` are
353    /// all the indices in the range `mins[i]..maxs[i]` (i.e. `maxs[i]` is excluded).
354    pub fn domain(&self) -> [&Point<i32>; 2] {
355        [&self.domain_mins, &self.domain_maxs]
356    }
357
358    /// The number of voxels along each coordinate axis.
359    pub fn dimensions(&self) -> Vector<u32> {
360        (self.domain_maxs - self.domain_mins).map(|e| e as u32)
361    }
362
363    /// The size of each voxel part this [`Voxels`] shape.
364    pub fn voxel_size(&self) -> Vector<Real> {
365        self.voxel_size
366    }
367
368    /// The shape each voxel is assumed to have.
369    pub fn primitive_geometry(&self) -> VoxelPrimitiveGeometry {
370        self.primitive_geometry
371    }
372
373    fn recompute_voxels_data(&mut self) {
374        for i in 0..self.states.len() {
375            let key = self.voxel_at_id(i as u32);
376            self.states[i] = self.compute_voxel_state(key);
377        }
378    }
379
380    /// Scale this shape.
381    pub fn scaled(mut self, scale: &Vector<Real>) -> Option<Self> {
382        self.voxel_size.component_mul_assign(scale);
383        Some(self)
384    }
385
386    /// Sets the voxel at the given grid coordinates, returning `None` if it lies outside [`Self::domain`].
387    ///
388    /// See [`Self::set_voxel`] for a method that automatically resizes the internal
389    /// storage of `self` if the key is out of the valid bounds.
390    pub fn try_set_voxel(&mut self, key: Point<i32>, is_filled: bool) -> Option<VoxelState> {
391        if key[0] < self.domain_mins[0]
392            || key[0] >= self.domain_maxs[0]
393            || key[1] < self.domain_mins[1]
394            || key[1] >= self.domain_maxs[1]
395        {
396            return None;
397        }
398
399        #[cfg(feature = "dim3")]
400        if key[2] < self.domain_mins[2] || key[2] >= self.domain_maxs[2] {
401            return None;
402        }
403
404        let id = self.linear_index(key);
405        let prev = self.states[id as usize];
406        let new = if is_filled {
407            VoxelState::INTERIOR
408        } else {
409            VoxelState::EMPTY
410        };
411
412        if prev.is_empty() ^ new.is_empty() {
413            self.states[id as usize] = new;
414            self.update_voxel_and_neighbors_state(key);
415        }
416
417        Some(prev)
418    }
419
420    /// Inserts a voxel at the given key, even if it is out of the bounds of this shape.
421    ///
422    /// If `is_filed` is `true` and the key lies out of the bounds on this shape, the internal
423    /// voxels storage will be resized automatically. If a resize happens, the cost of the insertion
424    /// is `O(n)` where `n` is the capacity of `self`. If no resize happens, then the cost of
425    /// insertion is `O(1)`.
426    ///
427    /// Use [`Self::try_set_voxel`] instead for a version that will be a no-op if the provided
428    /// coordinates are outside the [`Self::domain`], avoiding potential internal reallocations.
429    pub fn set_voxel(&mut self, key: Point<i32>, is_filled: bool) -> Option<VoxelState> {
430        if !self.is_voxel_in_bounds(key) && is_filled {
431            let dims = self.dimensions();
432
433            // Add 10% extra padding.
434            let extra = dims.map(|k| k * 10 / 100);
435            let mut new_domain_mins = self.domain_mins;
436            let mut new_domain_maxs = self.domain_maxs;
437
438            for k in 0..DIM {
439                if key[k] < self.domain_mins[k] {
440                    new_domain_mins[k] = key[k] - extra[k] as i32;
441                }
442
443                if key[k] >= self.domain_maxs[k] {
444                    new_domain_maxs[k] = key[k] + extra[k] as i32 + 1;
445                }
446            }
447
448            self.resize_domain(new_domain_mins, new_domain_maxs);
449
450            self.set_voxel(key, is_filled)
451        } else {
452            self.set_voxel(key, is_filled)
453        }
454    }
455
456    /// Set the model domain.
457    ///
458    /// The domain can be smaller or larger than the current one. Voxels in any overlap between
459    /// the current and new domain will be preserved.
460    ///
461    /// If for any index `i`, `domain_maxs[i] <= domain_mins[i]`, then the new domain is invalid
462    /// and this operation will result in a no-op.
463    pub fn resize_domain(&mut self, domain_mins: Point<i32>, domain_maxs: Point<i32>) {
464        if self.domain_mins == domain_mins && self.domain_maxs == domain_maxs {
465            // Nothing to change.
466            return;
467        }
468
469        if let Some(new_shape) = self.with_resized_domain(domain_mins, domain_maxs) {
470            *self = new_shape;
471        }
472    }
473
474    /// Clone this voxels shape with a new size.
475    ///
476    /// The domain can be smaller or larger than the current one. Voxels in any overlap between
477    /// the current and new domain will be preserved.
478    ///
479    /// If for any index `i`, `domain_maxs[i] <= domain_mins[i]`, then the new domain is invalid
480    /// and this operation returns `None`.
481    #[must_use]
482    pub fn with_resized_domain(
483        &self,
484        domain_mins: Point<i32>,
485        domain_maxs: Point<i32>,
486    ) -> Option<Self> {
487        if self.domain_mins == domain_mins && self.domain_maxs == domain_maxs {
488            // Nothing to change, just clone as-is.
489            return Some(self.clone());
490        }
491
492        let new_dim = domain_maxs - domain_mins;
493        if new_dim.iter().any(|d| *d <= 0) {
494            log::error!("Invalid domain provided for resizing a voxels shape. New domain: {:?} - {:?}; new domain size: {:?}", domain_mins, domain_maxs, new_dim);
495            return None;
496        }
497
498        let new_len = new_dim.iter().map(|x| *x as usize).product();
499
500        let mut new_shape = Self {
501            domain_mins,
502            domain_maxs,
503            states: vec![VoxelState::EMPTY; new_len],
504            primitive_geometry: self.primitive_geometry,
505            voxel_size: self.voxel_size,
506        };
507
508        for i in 0..self.states.len() {
509            let key = self.voxel_at_id(i as u32);
510            let new_i = new_shape.linear_index(key);
511            new_shape.states[new_i as usize] = self.states[i];
512        }
513
514        Some(new_shape)
515    }
516
517    /// Checks if the given key is within [`Self::domain`].
518    #[cfg(feature = "dim2")]
519    pub fn is_voxel_in_bounds(&self, key: Point<i32>) -> bool {
520        key[0] >= self.domain_mins[1]
521            && key[0] < self.domain_maxs[0]
522            && key[1] >= self.domain_mins[1]
523            && key[1] < self.domain_maxs[1]
524    }
525
526    /// Checks if the given key is within [`Self::domain`].
527    #[cfg(feature = "dim3")]
528    pub fn is_voxel_in_bounds(&self, key: Point<i32>) -> bool {
529        key[0] >= self.domain_mins[0]
530            && key[0] < self.domain_maxs[0]
531            && key[1] >= self.domain_mins[1]
532            && key[1] < self.domain_maxs[1]
533            && key[2] >= self.domain_mins[2]
534            && key[2] < self.domain_maxs[2]
535    }
536
537    fn update_voxel_and_neighbors_state(&mut self, key: Point<i32>) {
538        let key_id = self.linear_index(key) as usize;
539        let mut key_data = 0;
540        let center_is_empty = self.states[key_id].is_empty();
541
542        for k in 0..DIM {
543            if key[k] > self.domain_mins[k] {
544                let mut left = key;
545                left[k] -= 1;
546                let left_id = self.linear_index(left) as usize;
547
548                if !self.states[left_id].is_empty() {
549                    if center_is_empty {
550                        self.states[left_id].0 &= !(1 << (k * 2));
551                    } else {
552                        self.states[left_id].0 |= 1 << (k * 2);
553                        key_data |= 1 << (k * 2 + 1);
554                    }
555                }
556            }
557
558            if key[k] + 1 < self.domain_maxs[k] {
559                let mut right = key;
560                right[k] += 1;
561                let right_id = self.linear_index(right) as usize;
562
563                if !self.states[right_id].is_empty() {
564                    if center_is_empty {
565                        self.states[right_id].0 &= !(1 << (k * 2 + 1));
566                    } else {
567                        self.states[right_id].0 |= 1 << (k * 2 + 1);
568                        key_data |= 1 << (k * 2);
569                    }
570                }
571            }
572        }
573
574        if !center_is_empty {
575            self.states[key_id] = VoxelState(key_data);
576        }
577    }
578
579    /// The AABB of the voxel with the given quantized `key`.
580    pub fn voxel_aabb(&self, key: Point<i32>) -> Aabb {
581        let center = self.voxel_center(key);
582        let hext = self.voxel_size / 2.0;
583        Aabb::from_half_extents(center, hext)
584    }
585
586    /// Returns the state of a given voxel.
587    ///
588    /// Panics if the key is out of the bounds defined by [`Self::domain`].
589    pub fn voxel_state(&self, key: Point<i32>) -> VoxelState {
590        self.states[self.linear_index(key) as usize]
591    }
592
593    /// Calculates the grid coordinates of the voxel containing the given `point`, regardless
594    /// of [`Self::domain`].
595    pub fn voxel_at_point_unchecked(&self, point: Point<Real>) -> Point<i32> {
596        point
597            .coords
598            .component_div(&self.voxel_size)
599            .map(|x| x.floor() as i32)
600            .into()
601    }
602
603    /// Gets the key of the voxel containing the given `pt`.
604    ///
605    /// Note that the returned key might address a voxel that is empty.
606    /// `None` is returned if the point is out of the domain of `self`.
607    pub fn voxel_at_point(&self, pt: Point<Real>) -> Option<Point<i32>> {
608        let quant = self.voxel_at_point_unchecked(pt);
609        if quant[0] < self.domain_mins[0]
610            || quant[1] < self.domain_mins[1]
611            || quant[0] >= self.domain_maxs[0]
612            || quant[1] >= self.domain_maxs[1]
613        {
614            return None;
615        }
616
617        #[cfg(feature = "dim3")]
618        if quant[2] < self.domain_mins[2] || quant[2] >= self.domain_maxs[2] {
619            return None;
620        }
621
622        Some(quant)
623    }
624
625    /// Clamps an arbitrary voxel into the valid domain of `self`.
626    pub fn clamp_voxel(&self, key: Point<i32>) -> Point<i32> {
627        key.coords
628            .zip_zip_map(
629                &self.domain_mins.coords,
630                &self.domain_maxs.coords,
631                |k, min, max| k.clamp(min, max - 1),
632            )
633            .into()
634    }
635
636    /// The range of grid coordinates of voxels intersecting the given AABB.
637    ///
638    /// The returned range covers both empty and non-empty voxels, and is not limited to the
639    /// bounds defined by [`Self::domain`].
640    /// The range is semi, open, i.e., the range along each dimension `i` is understood as
641    /// the semi-open interval: `range[0][i]..range[1][i]`.
642    pub fn voxel_range_intersecting_local_aabb(&self, aabb: &Aabb) -> [Point<i32>; 2] {
643        let mins = aabb
644            .mins
645            .coords
646            .component_div(&self.voxel_size)
647            .map(|x| x.floor() as i32);
648        let maxs = aabb
649            .maxs
650            .coords
651            .component_div(&self.voxel_size)
652            .map(|x| x.ceil() as i32);
653        [mins.into(), maxs.into()]
654    }
655
656    /// The AABB of a given range of voxels.
657    ///
658    /// The AABB is computed independently of [`Self::domain`] and independently of whether
659    /// the voxels contained within are empty or not.
660    pub fn voxel_range_aabb(&self, mins: Point<i32>, maxs: Point<i32>) -> Aabb {
661        Aabb {
662            mins: mins
663                .cast::<Real>()
664                .coords
665                .component_mul(&self.voxel_size)
666                .into(),
667            maxs: maxs
668                .cast::<Real>()
669                .coords
670                .component_mul(&self.voxel_size)
671                .into(),
672        }
673    }
674
675    /// Aligns the given AABB with the voxelized grid.
676    ///
677    /// The aligned is calculated such that the returned AABB has corners lying at the grid
678    /// intersections (i.e. matches voxel corners) and fully contains the input `aabb`.
679    pub fn align_aabb_to_grid(&self, aabb: &Aabb) -> Aabb {
680        let mins = aabb
681            .mins
682            .coords
683            .zip_map(&self.voxel_size, |m, sz| (m / sz).floor() * m)
684            .into();
685        let maxs = aabb
686            .maxs
687            .coords
688            .zip_map(&self.voxel_size, |m, sz| (m / sz).ceil() * m)
689            .into();
690        Aabb { mins, maxs }
691    }
692
693    /// Iterates through every voxel intersecting the given aabb.
694    ///
695    /// Returns the voxel’s linearized id, center, and state.
696    pub fn voxels_intersecting_local_aabb(
697        &self,
698        aabb: &Aabb,
699    ) -> impl Iterator<Item = VoxelData> + '_ {
700        let [mins, maxs] = self.voxel_range_intersecting_local_aabb(aabb);
701        self.voxels_in_range(mins, maxs)
702    }
703
704    /// The center point of all the voxels in this shape (including empty ones).
705    ///
706    /// The voxel data associated to each center is provided to determine what kind of voxel
707    /// it is (and, in particular, if it is empty or full).
708    pub fn voxels(&self) -> impl Iterator<Item = VoxelData> + '_ {
709        self.voxels_in_range(self.domain_mins, self.domain_maxs)
710    }
711
712    /// Splits this voxels shape into two subshapes.
713    ///
714    /// The first subshape contains all the voxels which centers are inside the `aabb`.
715    /// The second subshape contains all the remaining voxels.
716    pub fn split_with_box(&self, aabb: &Aabb) -> (Option<Self>, Option<Self>) {
717        // TODO: optimize this?
718        let mut in_box = vec![];
719        let mut rest = vec![];
720        for vox in self.voxels() {
721            if !vox.state.is_empty() {
722                if aabb.contains_local_point(&vox.center) {
723                    in_box.push(vox.center);
724                } else {
725                    rest.push(vox.center);
726                }
727            }
728        }
729
730        let in_box = if !in_box.is_empty() {
731            Some(Voxels::from_points(
732                self.primitive_geometry,
733                self.voxel_size,
734                &in_box,
735            ))
736        } else {
737            None
738        };
739
740        let rest = if !rest.is_empty() {
741            Some(Voxels::from_points(
742                self.primitive_geometry,
743                self.voxel_size,
744                &rest,
745            ))
746        } else {
747            None
748        };
749
750        (in_box, rest)
751    }
752
753    /// Iterate through the data of all the voxels within the given (semi-open) voxel grid indices.
754    ///
755    /// Note that this yields both empty and non-empty voxels within the range. This does not
756    /// include any voxel that falls outside [`Self::domain`].
757    #[cfg(feature = "dim2")]
758    pub fn voxels_in_range(
759        &self,
760        mins: Point<i32>,
761        maxs: Point<i32>,
762    ) -> impl Iterator<Item = VoxelData> + '_ {
763        let mins = mins.coords.sup(&self.domain_mins.coords);
764        let maxs = maxs.coords.inf(&self.domain_maxs.coords);
765
766        (mins[0]..maxs[0]).flat_map(move |ix| {
767            (mins[1]..maxs[1]).map(move |iy| {
768                let grid_coords = Point::new(ix, iy);
769                let vid = self.linear_index(grid_coords);
770                let center =
771                    Vector::new(ix as Real + 0.5, iy as Real + 0.5).component_mul(&self.voxel_size);
772                VoxelData {
773                    linear_id: vid,
774                    grid_coords,
775                    center: center.into(),
776                    state: self.states[vid as usize],
777                }
778            })
779        })
780    }
781
782    /// Iterate through the data of all the voxels within the given (semi-open) voxel grid indices.
783    ///
784    /// Note that this yields both empty and non-empty voxels within the range. This does not
785    /// include any voxel that falls outside [`Self::domain`].
786    #[cfg(feature = "dim3")]
787    pub fn voxels_in_range(
788        &self,
789        mins: Point<i32>,
790        maxs: Point<i32>,
791    ) -> impl Iterator<Item = VoxelData> + '_ {
792        let mins = mins.coords.sup(&self.domain_mins.coords);
793        let maxs = maxs.coords.inf(&self.domain_maxs.coords);
794
795        (mins[0]..maxs[0]).flat_map(move |ix| {
796            (mins[1]..maxs[1]).flat_map(move |iy| {
797                (mins[2]..maxs[2]).map(move |iz| {
798                    let grid_coords = Point::new(ix, iy, iz);
799                    let vid = self.linear_index(grid_coords);
800                    let center = Vector::new(ix as Real + 0.5, iy as Real + 0.5, iz as Real + 0.5)
801                        .component_mul(&self.voxel_size)
802                        .into();
803                    VoxelData {
804                        linear_id: vid,
805                        grid_coords,
806                        center,
807                        state: self.states[vid as usize],
808                    }
809                })
810            })
811        })
812    }
813
814    /// The linearized index associated to the given voxel key.
815    #[cfg(feature = "dim2")]
816    pub fn linear_index(&self, voxel_key: Point<i32>) -> u32 {
817        let dims = self.dimensions();
818        let rel_key = voxel_key - self.domain_mins;
819        (rel_key.x + rel_key.y * dims[0] as i32) as u32
820    }
821
822    /// The linearized index associated to the given voxel key.
823    #[cfg(feature = "dim3")]
824    pub fn linear_index(&self, voxel_key: Point<i32>) -> u32 {
825        let dims = self.dimensions();
826        let rel_key = voxel_key - self.domain_mins;
827        rel_key.x as u32 + rel_key.y as u32 * dims[0] + rel_key.z as u32 * dims[0] * dims[1]
828    }
829
830    /// The key of the voxel at the given linearized index.
831    #[cfg(feature = "dim2")]
832    pub fn voxel_at_id(&self, linear_index: u32) -> Point<i32> {
833        let dim0 = self.domain_maxs[0] - self.domain_mins[0];
834        let y = linear_index as i32 / dim0;
835        let x = linear_index as i32 % dim0;
836        self.domain_mins + Vector::new(x, y)
837    }
838
839    /// The key of the voxel at the given linearized index.
840    #[cfg(feature = "dim3")]
841    pub fn voxel_at_id(&self, linear_index: u32) -> Point<i32> {
842        let dims = self.dimensions();
843
844        let d0d1 = dims[0] * dims[1];
845        let z = linear_index / d0d1;
846        let y = (linear_index - z * d0d1) / dims[0];
847        let x = linear_index % dims[0];
848        self.domain_mins + Vector::new(x as i32, y as i32, z as i32)
849    }
850
851    /// The center of the voxel with the given key.
852    pub fn voxel_center(&self, key: Point<i32>) -> Point<Real> {
853        (key.cast::<Real>() + Vector::repeat(0.5))
854            .coords
855            .component_mul(&self.voxel_size)
856            .into()
857    }
858
859    fn compute_voxel_state(&self, key: Point<i32>) -> VoxelState {
860        if self.states[self.linear_index(key) as usize].is_empty() {
861            return VoxelState::EMPTY;
862        }
863
864        let mut occupied_faces = 0;
865
866        for k in 0..DIM {
867            let (mut prev, mut next) = (key, key);
868            prev[k] -= 1;
869            next[k] += 1;
870
871            if key[k] + 1 < self.domain_maxs[k]
872                && !self.states[self.linear_index(next) as usize].is_empty()
873            {
874                occupied_faces |= 1 << (k * 2);
875            }
876            if key[k] > self.domain_mins[k]
877                && !self.states[self.linear_index(prev) as usize].is_empty()
878            {
879                occupied_faces |= 1 << (k * 2 + 1);
880            }
881        }
882
883        VoxelState(occupied_faces)
884    }
885}
886
887// NOTE: this code is used to generate the constant tables
888// FACES_TO_VOXEL_TYPES, FACES_TO_FEATURE_MASKS, FACES_TO_OCTANT_MASKS.
889#[allow(dead_code)]
890#[cfg(feature = "dim2")]
891#[cfg(test)]
892fn gen_const_tables() {
893    // The `j-th` bit of `faces_adj_to_vtx[i]` is set to 1, if the j-th face of the AABB (based on
894    // the face order depicted in `AABB::FACES_VERTEX_IDS`) is adjacent to the `i` vertex of the AABB
895    // (vertices are indexed as per the diagram depicted in the `FACES_VERTEX_IDS` doc.
896    // Each entry of this will always have exactly 3 bits set.
897    let mut faces_adj_to_vtx = [0usize; 4];
898
899    for fid in 0..4 {
900        let vids = Aabb::FACES_VERTEX_IDS[fid];
901        let key = 1 << fid;
902        faces_adj_to_vtx[vids.0] |= key;
903        faces_adj_to_vtx[vids.1] |= key;
904    }
905
906    /*
907     * FACES_TO_VOXEL_TYPES
908     */
909    std::println!("const FACES_TO_VOXEL_TYPES: [VoxelType; 17] = [");
910    'outer: for i in 0usize..16 {
911        // If any vertex of the voxel has three faces with no adjacent voxels,
912        // then the voxel type is Vertex.
913        for adjs in faces_adj_to_vtx.iter() {
914            if (*adjs & i) == 0 {
915                std::println!("VoxelType::Vertex,");
916                continue 'outer;
917            }
918        }
919
920        // If one face doesn’t have any adjacent voxel,
921        // then the voxel type is Face.
922        for fid in 0..4 {
923            if ((1 << fid) & i) == 0 {
924                std::println!("VoxelType::Face,");
925                continue 'outer;
926            }
927        }
928    }
929
930    // Add final entries for special values.
931    std::println!("VoxelType::Interior,");
932    std::println!("VoxelType::Empty,");
933    std::println!("];");
934
935    /*
936     * FACES_TO_FEATURE_MASKS
937     */
938    std::println!("const FACES_TO_FEATURE_MASKS: [u16; 17] = [");
939    for i in 0usize..16 {
940        // Each bit set indicates a convex vertex that can lead to collisions.
941        // The result will be nonzero only for `VoxelType::Vertex` voxels.
942        let mut vtx_key = 0;
943        for (vid, adjs) in faces_adj_to_vtx.iter().enumerate() {
944            if (*adjs & i) == 0 {
945                vtx_key |= 1 << vid;
946            }
947        }
948
949        if vtx_key != 0 {
950            std::println!("0b{:b},", vtx_key as u16);
951            continue;
952        }
953
954        // Each bit set indicates an exposed face that can lead to collisions.
955        // The result will be nonzero only for `VoxelType::Face` voxels.
956        let mut face_key = 0;
957        for fid in 0..4 {
958            if ((1 << fid) & i) == 0 {
959                face_key |= 1 << fid;
960            }
961        }
962
963        if face_key != 0 {
964            std::println!("0b{:b},", face_key as u16);
965            continue;
966        }
967    }
968
969    std::println!("0b{:b},", u16::MAX);
970    std::println!("0,");
971    std::println!("];");
972
973    /*
974     * Faces to octant masks.
975     */
976    std::println!("const FACES_TO_OCTANT_MASKS: [u32; 17] = [");
977    for i in 0usize..16 {
978        // First test if we have vertices.
979        let mut octant_mask = 0;
980        let mut set_mask = |mask, octant| {
981            // NOTE: we don’t overwrite any mask already set for the octant.
982            if (octant_mask >> (octant * 3)) & 0b0111 == 0 {
983                octant_mask |= mask << (octant * 3);
984            }
985        };
986
987        for (vid, adjs) in faces_adj_to_vtx.iter().enumerate() {
988            if (*adjs & i) == 0 {
989                set_mask(1, vid);
990            }
991        }
992
993        // This is the index of the normal of the faces given by
994        // Aabb::FACES_VERTEX_IDS.
995        const FX: u32 = OctantPattern::FACE_X;
996        const FY: u32 = OctantPattern::FACE_Y;
997        const FACE_NORMALS: [u32; 4] = [FX, FX, FY, FY];
998
999        #[allow(clippy::needless_range_loop)]
1000        for fid in 0..4 {
1001            if ((1 << fid) & i) == 0 {
1002                let vid = Aabb::FACES_VERTEX_IDS[fid];
1003                let mask = FACE_NORMALS[fid];
1004
1005                set_mask(mask, vid.0);
1006                set_mask(mask, vid.1);
1007            }
1008        }
1009        std::println!("0b{:b},", octant_mask);
1010    }
1011    std::println!("0,");
1012    std::println!("];");
1013}
1014
1015// NOTE: this code is used to generate the constant tables
1016// FACES_TO_VOXEL_TYPES, FACES_TO_FEATURE_MASKS, FACES_TO_OCTANT_MASKS.
1017#[allow(dead_code)]
1018#[cfg(feature = "dim3")]
1019#[cfg(test)]
1020fn gen_const_tables() {
1021    // The `j-th` bit of `faces_adj_to_vtx[i]` is set to 1, if the j-th face of the AABB (based on
1022    // the face order depicted in `AABB::FACES_VERTEX_IDS`) is adjacent to the `i` vertex of the AABB
1023    // (vertices are indexed as per the diagram depicted in the `FACES_VERTEX_IDS` doc.
1024    // Each entry of this will always have exactly 3 bits set.
1025    let mut faces_adj_to_vtx = [0usize; 8];
1026
1027    // The `j-th` bit of `faces_adj_to_vtx[i]` is set to 1, if the j-th edge of the AABB (based on
1028    // the edge order depicted in `AABB::EDGES_VERTEX_IDS`) is adjacent to the `i` vertex of the AABB
1029    // (vertices are indexed as per the diagram depicted in the `FACES_VERTEX_IDS` doc.
1030    // Each entry of this will always have exactly 2 bits set.
1031    let mut faces_adj_to_edge = [0usize; 12];
1032
1033    for fid in 0..6 {
1034        let vids = Aabb::FACES_VERTEX_IDS[fid];
1035        let key = 1 << fid;
1036        faces_adj_to_vtx[vids.0] |= key;
1037        faces_adj_to_vtx[vids.1] |= key;
1038        faces_adj_to_vtx[vids.2] |= key;
1039        faces_adj_to_vtx[vids.3] |= key;
1040    }
1041
1042    #[allow(clippy::needless_range_loop)]
1043    for eid in 0..12 {
1044        let evids = Aabb::EDGES_VERTEX_IDS[eid];
1045        for fid in 0..6 {
1046            let fvids = Aabb::FACES_VERTEX_IDS[fid];
1047            if (fvids.0 == evids.0
1048                || fvids.1 == evids.0
1049                || fvids.2 == evids.0
1050                || fvids.3 == evids.0)
1051                && (fvids.0 == evids.1
1052                    || fvids.1 == evids.1
1053                    || fvids.2 == evids.1
1054                    || fvids.3 == evids.1)
1055            {
1056                let key = 1 << fid;
1057                faces_adj_to_edge[eid] |= key;
1058            }
1059        }
1060    }
1061
1062    /*
1063     * FACES_TO_VOXEL_TYPES
1064     */
1065    std::println!("const FACES_TO_VOXEL_TYPES: [VoxelType; 65] = [");
1066    'outer: for i in 0usize..64 {
1067        // If any vertex of the voxel has three faces with no adjacent voxels,
1068        // then the voxel type is Vertex.
1069        for adjs in faces_adj_to_vtx.iter() {
1070            if (*adjs & i) == 0 {
1071                std::println!("VoxelType::Vertex,");
1072                continue 'outer;
1073            }
1074        }
1075
1076        // If any vertex of the voxel has three faces with no adjacent voxels,
1077        // then the voxel type is Edge.
1078        for adjs in faces_adj_to_edge.iter() {
1079            if (*adjs & i) == 0 {
1080                std::println!("VoxelType::Edge,");
1081                continue 'outer;
1082            }
1083        }
1084
1085        // If one face doesn’t have any adjacent voxel,
1086        // then the voxel type is Face.
1087        for fid in 0..6 {
1088            if ((1 << fid) & i) == 0 {
1089                std::println!("VoxelType::Face,");
1090                continue 'outer;
1091            }
1092        }
1093    }
1094
1095    // Add final entries for special values.
1096    std::println!("VoxelType::Interior,");
1097    std::println!("VoxelType::Empty,");
1098    std::println!("];");
1099
1100    /*
1101     * FACES_TO_FEATURE_MASKS
1102     */
1103    std::println!("const FACES_TO_FEATURE_MASKS: [u16; 65] = [");
1104    for i in 0usize..64 {
1105        // Each bit set indicates a convex vertex that can lead to collisions.
1106        // The result will be nonzero only for `VoxelType::Vertex` voxels.
1107        let mut vtx_key = 0;
1108        for (vid, adjs) in faces_adj_to_vtx.iter().enumerate() {
1109            if (*adjs & i) == 0 {
1110                vtx_key |= 1 << vid;
1111            }
1112        }
1113
1114        if vtx_key != 0 {
1115            std::println!("0b{:b},", vtx_key as u16);
1116            continue;
1117        }
1118
1119        // Each bit set indicates a convex edge that can lead to collisions.
1120        // The result will be nonzero only for `VoxelType::Edge` voxels.
1121        let mut edge_key = 0;
1122        for (eid, adjs) in faces_adj_to_edge.iter().enumerate() {
1123            if (*adjs & i) == 0 {
1124                edge_key |= 1 << eid;
1125            }
1126        }
1127
1128        if edge_key != 0 {
1129            std::println!("0b{:b},", edge_key as u16);
1130            continue;
1131        }
1132
1133        // Each bit set indicates an exposed face that can lead to collisions.
1134        // The result will be nonzero only for `VoxelType::Face` voxels.
1135        let mut face_key = 0;
1136        for fid in 0..6 {
1137            if ((1 << fid) & i) == 0 {
1138                face_key |= 1 << fid;
1139            }
1140        }
1141
1142        if face_key != 0 {
1143            std::println!("0b{:b},", face_key as u16);
1144            continue;
1145        }
1146    }
1147
1148    std::println!("0b{:b},", u16::MAX);
1149    std::println!("0,");
1150    std::println!("];");
1151
1152    /*
1153     * Faces to octant masks.
1154     */
1155    std::println!("const FACES_TO_OCTANT_MASKS: [u32; 65] = [");
1156    for i in 0usize..64 {
1157        // First test if we have vertices.
1158        let mut octant_mask = 0;
1159        let mut set_mask = |mask, octant| {
1160            // NOTE: we don’t overwrite any mask already set for the octant.
1161            if (octant_mask >> (octant * 3)) & 0b0111 == 0 {
1162                octant_mask |= mask << (octant * 3);
1163            }
1164        };
1165
1166        for (vid, adjs) in faces_adj_to_vtx.iter().enumerate() {
1167            if (*adjs & i) == 0 {
1168                set_mask(1, vid);
1169            }
1170        }
1171
1172        // This is the index of the axis porting the edges given by
1173        // Aabb::EDGES_VERTEX_IDS.
1174        const EX: u32 = OctantPattern::EDGE_X;
1175        const EY: u32 = OctantPattern::EDGE_Y;
1176        const EZ: u32 = OctantPattern::EDGE_Z;
1177        const EDGE_AXIS: [u32; 12] = [EX, EY, EX, EY, EX, EY, EX, EY, EZ, EZ, EZ, EZ];
1178        for (eid, adjs) in faces_adj_to_edge.iter().enumerate() {
1179            if (*adjs & i) == 0 {
1180                let vid = Aabb::EDGES_VERTEX_IDS[eid];
1181                let mask = EDGE_AXIS[eid];
1182
1183                set_mask(mask, vid.0);
1184                set_mask(mask, vid.1);
1185            }
1186        }
1187
1188        // This is the index of the normal of the faces given by
1189        // Aabb::FACES_VERTEX_IDS.
1190        const FX: u32 = OctantPattern::FACE_X;
1191        const FY: u32 = OctantPattern::FACE_Y;
1192        const FZ: u32 = OctantPattern::FACE_Z;
1193        const FACE_NORMALS: [u32; 6] = [FX, FX, FY, FY, FZ, FZ];
1194
1195        #[allow(clippy::needless_range_loop)]
1196        for fid in 0..6 {
1197            if ((1 << fid) & i) == 0 {
1198                let vid = Aabb::FACES_VERTEX_IDS[fid];
1199                let mask = FACE_NORMALS[fid];
1200
1201                set_mask(mask, vid.0);
1202                set_mask(mask, vid.1);
1203                set_mask(mask, vid.2);
1204                set_mask(mask, vid.3);
1205            }
1206        }
1207        std::println!("0b{:b},", octant_mask);
1208    }
1209    std::println!("0,");
1210    std::println!("];");
1211}
1212
1213// Index to the item of FACES_TO_VOXEL_TYPES which identifies interior voxels.
1214#[cfg(feature = "dim2")]
1215const INTERIOR_FACE_MASK: u8 = 0b0000_1111;
1216#[cfg(feature = "dim3")]
1217const INTERIOR_FACE_MASK: u8 = 0b0011_1111;
1218// Index to the item of FACES_TO_VOXEL_TYPES which identifies empty voxels.
1219
1220#[cfg(feature = "dim2")]
1221const EMPTY_FACE_MASK: u8 = 0b0001_0000;
1222#[cfg(feature = "dim3")]
1223const EMPTY_FACE_MASK: u8 = 0b0100_0000;
1224
1225/// The voxel type deduced from adjacency information.
1226///
1227/// See the documentation of [`VoxelType`] for additional information on what each enum variant
1228/// means.
1229///
1230/// In 3D there are 6 neighbor faces => 64 cases + 1 empty case.
1231#[cfg(feature = "dim3")]
1232const FACES_TO_VOXEL_TYPES: [VoxelType; 65] = [
1233    VoxelType::Vertex,
1234    VoxelType::Vertex,
1235    VoxelType::Vertex,
1236    VoxelType::Edge,
1237    VoxelType::Vertex,
1238    VoxelType::Vertex,
1239    VoxelType::Vertex,
1240    VoxelType::Edge,
1241    VoxelType::Vertex,
1242    VoxelType::Vertex,
1243    VoxelType::Vertex,
1244    VoxelType::Edge,
1245    VoxelType::Edge,
1246    VoxelType::Edge,
1247    VoxelType::Edge,
1248    VoxelType::Face,
1249    VoxelType::Vertex,
1250    VoxelType::Vertex,
1251    VoxelType::Vertex,
1252    VoxelType::Edge,
1253    VoxelType::Vertex,
1254    VoxelType::Vertex,
1255    VoxelType::Vertex,
1256    VoxelType::Edge,
1257    VoxelType::Vertex,
1258    VoxelType::Vertex,
1259    VoxelType::Vertex,
1260    VoxelType::Edge,
1261    VoxelType::Edge,
1262    VoxelType::Edge,
1263    VoxelType::Edge,
1264    VoxelType::Face,
1265    VoxelType::Vertex,
1266    VoxelType::Vertex,
1267    VoxelType::Vertex,
1268    VoxelType::Edge,
1269    VoxelType::Vertex,
1270    VoxelType::Vertex,
1271    VoxelType::Vertex,
1272    VoxelType::Edge,
1273    VoxelType::Vertex,
1274    VoxelType::Vertex,
1275    VoxelType::Vertex,
1276    VoxelType::Edge,
1277    VoxelType::Edge,
1278    VoxelType::Edge,
1279    VoxelType::Edge,
1280    VoxelType::Face,
1281    VoxelType::Edge,
1282    VoxelType::Edge,
1283    VoxelType::Edge,
1284    VoxelType::Face,
1285    VoxelType::Edge,
1286    VoxelType::Edge,
1287    VoxelType::Edge,
1288    VoxelType::Face,
1289    VoxelType::Edge,
1290    VoxelType::Edge,
1291    VoxelType::Edge,
1292    VoxelType::Face,
1293    VoxelType::Face,
1294    VoxelType::Face,
1295    VoxelType::Face,
1296    VoxelType::Interior,
1297    VoxelType::Empty,
1298];
1299
1300/// Indicates the convex features of each voxel that can lead to collisions.
1301///
1302/// The interpretation of each bit differs depending on the corresponding voxel type in
1303/// `FACES_TO_VOXEL_TYPES`:
1304/// - For `VoxelType::Vertex`: the i-th bit set to `1` indicates that the i-th AABB vertex is convex
1305///   and might lead to collisions.
1306/// - For `VoxelType::Edge`: the i-th bit set to `1` indicates that the i-th edge from `Aabb::EDGES_VERTEX_IDS`
1307///   is convex and might lead to collisions.
1308/// - For `VoxelType::Face`: the i-th bit set to `1` indicates that the i-th face from `Aabb::FACES_VERTEX_IDS`
1309///   is exposed and might lead to collisions.
1310#[cfg(feature = "dim3")]
1311const FACES_TO_FEATURE_MASKS: [u16; 65] = [
1312    0b11111111,
1313    0b10011001,
1314    0b1100110,
1315    0b1010101,
1316    0b110011,
1317    0b10001,
1318    0b100010,
1319    0b10001,
1320    0b11001100,
1321    0b10001000,
1322    0b1000100,
1323    0b1000100,
1324    0b10101010,
1325    0b10001000,
1326    0b100010,
1327    0b110000,
1328    0b1111,
1329    0b1001,
1330    0b110,
1331    0b101,
1332    0b11,
1333    0b1,
1334    0b10,
1335    0b1,
1336    0b1100,
1337    0b1000,
1338    0b100,
1339    0b100,
1340    0b1010,
1341    0b1000,
1342    0b10,
1343    0b100000,
1344    0b11110000,
1345    0b10010000,
1346    0b1100000,
1347    0b1010000,
1348    0b110000,
1349    0b10000,
1350    0b100000,
1351    0b10000,
1352    0b11000000,
1353    0b10000000,
1354    0b1000000,
1355    0b1000000,
1356    0b10100000,
1357    0b10000000,
1358    0b100000,
1359    0b10000,
1360    0b111100000000,
1361    0b100100000000,
1362    0b11000000000,
1363    0b1100,
1364    0b1100000000,
1365    0b100000000,
1366    0b1000000000,
1367    0b1000,
1368    0b110000000000,
1369    0b100000000000,
1370    0b10000000000,
1371    0b100,
1372    0b11,
1373    0b10,
1374    0b1,
1375    0b1111111111111111,
1376    0,
1377];
1378
1379/// Each octant is assigned three contiguous bits.
1380#[cfg(feature = "dim3")]
1381const FACES_TO_OCTANT_MASKS: [u32; 65] = [
1382    0b1001001001001001001001,
1383    0b1010010001001010010001,
1384    0b10001001010010001001010,
1385    0b10010010010010010010010,
1386    0b11011001001011011001001,
1387    0b11111010001011111010001,
1388    0b111011001010111011001010,
1389    0b111111010010111111010010,
1390    0b1001011011001001011011,
1391    0b1010111011001010111011,
1392    0b10001011111010001011111,
1393    0b10010111111010010111111,
1394    0b11011011011011011011011,
1395    0b11111111011011111111011,
1396    0b111011011111111011011111,
1397    0b111111111111111111111111,
1398    0b100100100100001001001001,
1399    0b100110110100001010010001,
1400    0b110100100110010001001010,
1401    0b110110110110010010010010,
1402    0b101101100100011011001001,
1403    0b101000110100011111010001,
1404    0b101100110111011001010,
1405    0b110110111111010010,
1406    0b100100101101001001011011,
1407    0b100110000101001010111011,
1408    0b110100101000010001011111,
1409    0b110110000000010010111111,
1410    0b101101101101011011011011,
1411    0b101000000101011111111011,
1412    0b101101000111011011111,
1413    0b111111111111,
1414    0b1001001001100100100100,
1415    0b1010010001100110110100,
1416    0b10001001010110100100110,
1417    0b10010010010110110110110,
1418    0b11011001001101101100100,
1419    0b11111010001101000110100,
1420    0b111011001010000101100110,
1421    0b111111010010000000110110,
1422    0b1001011011100100101101,
1423    0b1010111011100110000101,
1424    0b10001011111110100101000,
1425    0b10010111111110110000000,
1426    0b11011011011101101101101,
1427    0b11111111011101000000101,
1428    0b111011011111000101101000,
1429    0b111111111111000000000000,
1430    0b100100100100100100100100,
1431    0b100110110100100110110100,
1432    0b110100100110110100100110,
1433    0b110110110110110110110110,
1434    0b101101100100101101100100,
1435    0b101000110100101000110100,
1436    0b101100110000101100110,
1437    0b110110000000110110,
1438    0b100100101101100100101101,
1439    0b100110000101100110000101,
1440    0b110100101000110100101000,
1441    0b110110000000110110000000,
1442    0b101101101101101101101101,
1443    0b101000000101101000000101,
1444    0b101101000000101101000,
1445    0b0,
1446    0,
1447];
1448
1449#[cfg(feature = "dim2")]
1450const FACES_TO_VOXEL_TYPES: [VoxelType; 17] = [
1451    VoxelType::Vertex,
1452    VoxelType::Vertex,
1453    VoxelType::Vertex,
1454    VoxelType::Face,
1455    VoxelType::Vertex,
1456    VoxelType::Vertex,
1457    VoxelType::Vertex,
1458    VoxelType::Face,
1459    VoxelType::Vertex,
1460    VoxelType::Vertex,
1461    VoxelType::Vertex,
1462    VoxelType::Face,
1463    VoxelType::Face,
1464    VoxelType::Face,
1465    VoxelType::Face,
1466    VoxelType::Interior,
1467    VoxelType::Empty,
1468];
1469
1470#[cfg(feature = "dim2")]
1471const FACES_TO_FEATURE_MASKS: [u16; 17] = [
1472    0b1111,
1473    0b1001,
1474    0b110,
1475    0b1100,
1476    0b11,
1477    0b1,
1478    0b10,
1479    0b1000,
1480    0b1100,
1481    0b1000,
1482    0b100,
1483    0b100,
1484    0b11,
1485    0b10,
1486    0b1,
1487    0b1111111111111111,
1488    0,
1489];
1490
1491// NOTE: in 2D we are also using 3 bits per octant even though we technically only need two.
1492//       This keeps some collision-detection easier by avoiding some special-casing.
1493#[cfg(feature = "dim2")]
1494const FACES_TO_OCTANT_MASKS: [u32; 17] = [
1495    0b1001001001,
1496    0b1011011001,
1497    0b11001001011,
1498    0b11011011011,
1499    0b10010001001,
1500    0b10000011001,
1501    0b10001011,
1502    0b11011,
1503    0b1001010010,
1504    0b1011000010,
1505    0b11001010000,
1506    0b11011000000,
1507    0b10010010010,
1508    0b10000000010,
1509    0b10010000,
1510    0b0,
1511    0,
1512];
1513
1514#[cfg(test)]
1515mod test {
1516    #[test]
1517    fn gen_const_tables() {
1518        super::gen_const_tables();
1519    }
1520}