use crate::Result;
use crate::common::IndexMask;
use crate::common::PCoords;
use crate::na::SVector;
use faer::prelude::default;
use parry3d_f64::utils::hashmap::HashMap;
pub fn compute_first_per_voxel_mask<const D: usize>(
points: &[impl PCoords<D>],
voxel_size: f64,
) -> IndexMask {
let mut voxel_map = HashMap::with_hasher(default());
let mut mask = IndexMask::new(points.len(), false);
for (i, xyz) in points.iter().enumerate() {
let mut key: SVector<i32, D> = SVector::zeros();
for (d, &coord) in xyz.coords().iter().enumerate() {
key[d] = (coord / voxel_size).floor() as i32;
}
if !voxel_map.contains_key(&key) {
voxel_map.insert(key, i);
mask.set(i, true);
}
}
mask
}
pub struct VoxelGroups {
indices: Vec<u32>,
offsets: Vec<u32>,
}
impl VoxelGroups {
pub fn len(&self) -> usize {
self.offsets.len() - 1
}
pub fn point_count(&self) -> usize {
self.indices.len()
}
pub fn is_empty(&self) -> bool {
self.len() == 0
}
pub fn group(&self, i: usize) -> &[u32] {
&self.indices[self.offsets[i] as usize..self.offsets[i + 1] as usize]
}
pub fn iter(&self) -> impl Iterator<Item = &[u32]> {
(0..self.len()).map(|i| self.group(i))
}
}
pub fn compute_voxel_groups<const D: usize>(
points: &[impl PCoords<D>],
voxel_size: f64,
) -> Result<VoxelGroups> {
if !voxel_size.is_finite() || voxel_size <= 0.0 {
return Err(format!("Voxel size must be finite and positive, got {voxel_size}").into());
}
if points.len() > u32::MAX as usize {
return Err(format!(
"Voxel grouping indexes points with u32 and cannot take {} of them",
points.len()
)
.into());
}
if points.is_empty() {
return Ok(VoxelGroups {
indices: Vec::new(),
offsets: vec![0],
});
}
let mut keyed: Vec<([i32; D], u32)> = points
.iter()
.enumerate()
.map(|(i, p)| {
let mut key = [0i32; D];
for (d, &coord) in p.coords().iter().enumerate() {
key[d] = (coord / voxel_size).floor() as i32;
}
(key, i as u32)
})
.collect();
keyed.sort_unstable();
let mut indices: Vec<u32> = Vec::with_capacity(keyed.len());
let mut offsets: Vec<u32> = vec![0];
let mut current = keyed[0].0;
for (key, i) in keyed {
if key != current {
offsets.push(indices.len() as u32);
current = key;
}
indices.push(i);
}
offsets.push(indices.len() as u32);
Ok(VoxelGroups { indices, offsets })
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geom2::Aabb2;
use crate::{Point2, Point3};
#[test]
fn downsample_grid() -> Result<()> {
let mut points = Vec::new();
let s0 = 0.01;
let s1 = s0 * 10.0;
for i in 0..1000 {
for j in 0..1000 {
points.push(Point2::new(
i as f64 * s0 + s0 / 2.0,
j as f64 * s0 + s0 / 2.0,
));
}
}
let mask = compute_first_per_voxel_mask(&points, 0.1);
let thinned = mask.clone_indices_of(&points)?;
for i in 0..100 {
for j in 0..100 {
let x = i as f64 * s1;
let y = j as f64 * s1;
let aabb = Aabb2::new(Point2::new(x, y), Point2::new(x + s1, y + s1));
let count = thinned
.iter()
.filter(|p| aabb.contains_local_point(p))
.count();
assert_eq!(count, 1);
}
}
Ok(())
}
#[test]
fn voxel_groups_partition_every_point_exactly_once() {
let points: Vec<Point3> = (0..500)
.map(|i| {
let t = i as f64 * 0.37;
Point3::new(t.sin() * 10.0, t.cos() * 10.0, t * 0.1)
})
.collect();
let groups = compute_voxel_groups(&points, 1.0).expect("grouping failed");
let mut seen = vec![0usize; points.len()];
for g in groups.iter() {
assert!(!g.is_empty(), "a reported group was empty");
for &i in g {
seen[i as usize] += 1;
}
}
assert!(
seen.iter().all(|&c| c == 1),
"every point must appear in exactly one group"
);
}
#[test]
fn voxel_groups_put_co_located_points_together() {
let points = vec![
Point3::new(0.1, 0.1, 0.1),
Point3::new(0.2, 0.2, 0.2),
Point3::new(5.1, 0.1, 0.1),
Point3::new(5.2, 0.2, 0.2),
Point3::new(5.3, 0.3, 0.3),
Point3::new(10.1, 0.1, 0.1),
];
let groups = compute_voxel_groups(&points, 1.0).expect("grouping failed");
let sizes: Vec<usize> = groups.iter().map(|g| g.len()).collect();
assert_eq!(sizes, vec![2, 3, 1]);
assert_eq!(groups.group(0), &[0, 1]);
assert_eq!(groups.group(1), &[2, 3, 4]);
assert_eq!(groups.group(2), &[5]);
}
#[test]
fn voxel_groups_are_ordered_deterministically() {
let points = vec![
Point3::new(5.2, 0.0, 0.0),
Point3::new(0.1, 0.0, 0.0),
Point3::new(5.1, 0.0, 0.0),
Point3::new(0.2, 0.0, 0.0),
];
let groups = compute_voxel_groups(&points, 1.0).expect("grouping failed");
assert_eq!(groups.len(), 2);
assert_eq!(groups.group(0), &[1, 3]);
assert_eq!(groups.group(1), &[0, 2]);
}
#[test]
fn voxel_groups_handle_negative_coordinates() {
let points = vec![
Point3::new(-0.5, -0.5, -0.5),
Point3::new(-1.5, -0.5, -0.5),
Point3::new(-0.9, -0.9, -0.9),
];
let groups = compute_voxel_groups(&points, 1.0).expect("grouping failed");
assert_eq!(groups.len(), 2);
assert_eq!(groups.group(0), &[1]);
assert_eq!(groups.group(1), &[0, 2]);
}
#[test]
fn voxel_groups_reject_a_nonsense_size() {
let points = vec![Point3::origin()];
assert!(compute_voxel_groups(&points, 0.0).is_err());
assert!(compute_voxel_groups(&points, -1.0).is_err());
assert!(compute_voxel_groups(&points, f64::NAN).is_err());
assert!(compute_voxel_groups(&points, f64::INFINITY).is_err());
}
#[test]
fn voxel_groups_of_nothing_is_nothing() {
let points: Vec<Point3> = Vec::new();
let groups = compute_voxel_groups(&points, 1.0).expect("grouping failed");
assert_eq!(groups.len(), 0);
assert!(groups.is_empty());
}
#[test]
fn voxel_groups_agree_with_the_thinning_mask_on_which_point_comes_first() {
let points: Vec<Point3> = (0..300)
.map(|i| {
let t = i as f64 * 0.21;
Point3::new(t.sin() * 4.0, t.cos() * 4.0, (t * 0.5).sin() * 4.0)
})
.collect();
let groups = compute_voxel_groups(&points, 0.75).expect("grouping failed");
let mask = compute_first_per_voxel_mask(&points, 0.75);
let mut from_groups: Vec<usize> = groups.iter().map(|g| g[0] as usize).collect();
from_groups.sort_unstable();
assert_eq!(from_groups, mask.to_indices());
}
}