use super::{Mesh3, MeshData3};
use crate::common::IndexMask;
use crate::geom3::point_cloud::PointCloud3;
use crate::{Point3, Result, SurfacePoint3, UnitVec3};
use faer::prelude::default;
use parry3d_f64::bounding_volume::Aabb;
use parry3d_f64::query::PointQuery;
use parry3d_f64::query::details::intersection_test_aabb_triangle;
use parry3d_f64::shape::Triangle;
use parry3d_f64::utils::hashmap::{Entry, HashMap};
use rayon::prelude::*;
const FACE_CHUNK: usize = 4096;
impl MeshData3 {
pub fn sample_voxel_surface(
&self,
voxel_size: f64,
face_mask: Option<&IndexMask>,
) -> Result<PointCloud3> {
let sps = compute_voxel_surface_points(self.points(), self.faces(), voxel_size, face_mask)?;
Ok(PointCloud3::from_surface_points(&sps))
}
}
impl Mesh3 {
pub fn sample_voxel_surface(
&self,
voxel_size: f64,
face_mask: Option<&IndexMask>,
) -> Result<PointCloud3> {
let sps = compute_voxel_surface_points(self.points(), self.faces(), voxel_size, face_mask)?;
Ok(PointCloud3::from_surface_points(&sps))
}
}
pub fn compute_voxel_surface_points(
points: &[Point3],
faces: &[[u32; 3]],
voxel_size: f64,
face_mask: Option<&IndexMask>,
) -> Result<Vec<SurfacePoint3>> {
validate(points, faces, voxel_size, face_mask)?;
let best = faces
.par_chunks(FACE_CHUNK)
.enumerate()
.map(|(chunk_i, chunk)| {
let base = chunk_i * FACE_CHUNK;
let mut local: HashMap<[i32; 3], CellBest> = HashMap::with_hasher(default());
for (offset, face) in chunk.iter().enumerate() {
let face_index = (base + offset) as u32;
if skip_face(face_mask, base + offset) {
continue;
}
let tri = triangle(points, face);
let Some(normal) = tri.normal() else {
continue;
};
rasterize_face(&tri, voxel_size, |key| {
let center = cell_center(key, voxel_size);
let point = tri.project_local_point(¢er, false).point;
let candidate = CellBest {
d2: (point - center).norm_squared(),
face: face_index,
point,
normal,
};
match local.entry(key) {
Entry::Occupied(mut e) => {
if candidate.beats(e.get()) {
e.insert(candidate);
}
}
Entry::Vacant(e) => {
e.insert(candidate);
}
}
});
}
local
})
.reduce(
|| HashMap::with_hasher(default()),
|mut a, b| {
for (key, candidate) in b {
match a.entry(key) {
Entry::Occupied(mut e) => {
if candidate.beats(e.get()) {
e.insert(candidate);
}
}
Entry::Vacant(e) => {
e.insert(candidate);
}
}
}
a
},
);
let mut cells: Vec<([i32; 3], CellBest)> = best.into_iter().collect();
cells.sort_unstable_by_key(|(key, _)| *key);
Ok(cells
.into_iter()
.map(|(_, c)| SurfacePoint3::new(c.point, c.normal))
.collect())
}
#[derive(Clone, Copy)]
struct CellBest {
d2: f64,
face: u32,
point: Point3,
normal: UnitVec3,
}
impl CellBest {
fn beats(&self, other: &CellBest) -> bool {
self.d2 < other.d2 || (self.d2 == other.d2 && self.face < other.face)
}
}
#[inline]
fn skip_face(face_mask: Option<&IndexMask>, face_index: usize) -> bool {
face_mask.is_some_and(|m| !m.get(face_index))
}
#[inline]
fn triangle(points: &[Point3], face: &[u32; 3]) -> Triangle {
Triangle::new(
points[face[0] as usize],
points[face[1] as usize],
points[face[2] as usize],
)
}
#[inline]
fn axis_key(coord: f64, voxel_size: f64) -> i32 {
(coord / voxel_size).floor() as i32
}
#[inline]
fn cell_center(key: [i32; 3], voxel_size: f64) -> Point3 {
Point3::new(
(key[0] as f64 + 0.5) * voxel_size,
(key[1] as f64 + 0.5) * voxel_size,
(key[2] as f64 + 0.5) * voxel_size,
)
}
#[inline]
fn cell_aabb(key: [i32; 3], voxel_size: f64) -> Aabb {
let mins = Point3::new(
key[0] as f64 * voxel_size,
key[1] as f64 * voxel_size,
key[2] as f64 * voxel_size,
);
let maxs = Point3::new(
mins.x + voxel_size,
mins.y + voxel_size,
mins.z + voxel_size,
);
Aabb::new(mins, maxs)
}
fn rasterize_face(tri: &Triangle, voxel_size: f64, mut emit: impl FnMut([i32; 3])) {
let mut lo = [0i32; 3];
let mut hi = [0i32; 3];
for d in 0..3 {
let a = tri.a[d];
let b = tri.b[d];
let c = tri.c[d];
lo[d] = axis_key(a.min(b).min(c), voxel_size);
hi[d] = axis_key(a.max(b).max(c), voxel_size);
}
if lo == hi {
emit(lo);
return;
}
for i in lo[0]..=hi[0] {
for j in lo[1]..=hi[1] {
for k in lo[2]..=hi[2] {
let key = [i, j, k];
if intersection_test_aabb_triangle(&cell_aabb(key, voxel_size), tri) {
emit(key);
}
}
}
}
}
fn validate(
points: &[Point3],
faces: &[[u32; 3]],
voxel_size: f64,
face_mask: Option<&IndexMask>,
) -> Result<()> {
if !voxel_size.is_finite() || voxel_size <= 0.0 {
return Err(format!("Voxel size must be finite and positive, got {voxel_size}").into());
}
if let Some(mask) = face_mask
&& mask.len() != faces.len()
{
return Err(format!(
"Face mask has {} entries but the mesh has {} faces",
mask.len(),
faces.len()
)
.into());
}
let limit = i32::MAX as f64 - 2.0;
for p in points {
for d in 0..3 {
let scaled = p[d] / voxel_size;
if !scaled.is_finite() || scaled.abs() > limit {
return Err(format!(
"A vertex at {} is {:.3e} cells from the origin on axis {d}, which a voxel \
grid cannot index. Use a larger voxel size, or move the mesh nearer the \
origin.",
p, scaled
)
.into());
}
}
}
Ok(())
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Iso3;
fn engine_blade() -> Mesh3 {
let path =
std::path::Path::new(env!("CARGO_MANIFEST_DIR")).join("tests/data/engine-blade.tcmesh");
let data = crate::io::read_tc_mesh_file(&path).expect("failed to load engine blade");
Mesh3::from_data(data, false).expect("failed to accelerate engine blade")
}
fn thin_wall(z_lower: f64, z_upper: f64) -> MeshData3 {
let mut points = Vec::new();
let mut faces = Vec::new();
for z in [z_lower, z_upper] {
let base = points.len() as u32;
points.push(Point3::new(0.0, 0.0, z));
points.push(Point3::new(4.0, 0.0, z));
points.push(Point3::new(4.0, 4.0, z));
points.push(Point3::new(0.0, 4.0, z));
faces.push([base, base + 1, base + 2]);
faces.push([base, base + 2, base + 3]);
}
MeshData3::new(points, faces).expect("the sheets are well formed")
}
fn occupied_cells(points: &[Point3], faces: &[[u32; 3]], voxel_size: f64) -> Vec<[i32; 3]> {
let mut keys = Vec::new();
for face in faces {
let tri = triangle(points, face);
if tri.normal().is_none() {
continue;
}
rasterize_face(&tri, voxel_size, |key| keys.push(key));
}
keys.sort_unstable();
keys.dedup();
keys
}
#[test]
fn surface_sampling_lands_on_the_mesh() {
for (name, mesh, voxel_size) in [
("sphere", Mesh3::create_sphere(10.0, 0.027).unwrap(), 1.0),
("bunny", Mesh3::stanford_bunny_res2(), 0.01),
("blade", engine_blade(), 2.0),
] {
let cloud = mesh
.sample_voxel_surface(voxel_size, None)
.expect("sampling failed");
assert!(!cloud.is_empty(), "{name} produced nothing");
let worst = cloud
.points()
.iter()
.map(|p| mesh.distance_closest_to(p))
.fold(0.0, f64::max);
assert!(
worst < 1e-9,
"{name}: a sampled point was {worst:.3e} from the surface"
);
}
}
#[test]
fn one_point_comes_out_per_occupied_cell() {
let mesh = Mesh3::create_sphere(10.0, 0.027).unwrap();
let voxel_size = 1.0;
let cloud = mesh.sample_voxel_surface(voxel_size, None).unwrap();
let cells = occupied_cells(mesh.points(), mesh.faces(), voxel_size);
assert!(!cells.is_empty());
assert_eq!(cloud.point_count(), cells.len());
}
#[test]
fn rasterization_matches_a_brute_force_sweep() {
let points = vec![
Point3::new(0.3, 0.2, 0.1),
Point3::new(7.4, 0.6, 2.2),
Point3::new(1.1, 6.9, 5.3),
];
let faces = vec![[0u32, 1, 2]];
let voxel_size = 0.5;
let got = occupied_cells(&points, &faces, voxel_size);
let tri = Triangle::new(points[0], points[1], points[2]);
let mut expected = Vec::new();
for i in -3..=20 {
for j in -3..=20 {
for k in -3..=20 {
let key = [i, j, k];
if intersection_test_aabb_triangle(&cell_aabb(key, voxel_size), &tri) {
expected.push(key);
}
}
}
}
expected.sort_unstable();
assert!(expected.len() > 100, "the sweep was too small to be a test");
assert_eq!(got, expected);
}
#[test]
fn every_face_centroid_lands_in_an_occupied_cell() {
let mesh = Mesh3::create_box(3.0, 5.0, 7.0, true);
let voxel_size = 0.25;
let occupied = occupied_cells(mesh.points(), mesh.faces(), voxel_size);
for centroid in mesh.compute_face_centers().unwrap() {
let key = [
axis_key(centroid.x, voxel_size),
axis_key(centroid.y, voxel_size),
axis_key(centroid.z, voxel_size),
];
assert!(
occupied.binary_search(&key).is_ok(),
"cell {key:?} was not reported"
);
}
}
#[test]
fn repeated_runs_are_bitwise_identical() {
let mesh = Mesh3::stanford_bunny_res2();
let voxel_size = 0.01;
let surface =
|| compute_voxel_surface_points(mesh.points(), mesh.faces(), voxel_size, None);
let a = surface().unwrap();
let b = surface().unwrap();
assert!(!a.is_empty());
assert_eq!(a.len(), b.len());
for (x, y) in a.iter().zip(b.iter()) {
assert_eq!(x.point, y.point);
assert_eq!(x.normal, y.normal);
}
}
#[test]
fn the_occupied_cells_do_not_depend_on_face_order() {
let mesh = Mesh3::stanford_bunny_res2();
let voxel_size = 0.01;
let mut reversed = mesh.faces().to_vec();
reversed.reverse();
let forward = occupied_cells(mesh.points(), mesh.faces(), voxel_size);
let backward = occupied_cells(mesh.points(), &reversed, voxel_size);
assert!(!forward.is_empty());
assert_eq!(forward, backward);
let forward =
compute_voxel_surface_points(mesh.points(), mesh.faces(), voxel_size, None).unwrap();
let backward =
compute_voxel_surface_points(mesh.points(), &reversed, voxel_size, None).unwrap();
assert_eq!(forward.len(), backward.len());
for (a, b) in forward.iter().zip(backward.iter()) {
assert!(
(a.point - b.point).norm() < 1e-12,
"a reordering moved a point from {} to {}",
a.point,
b.point
);
}
}
#[test]
fn normals_come_from_one_facet_and_are_never_blended() {
let mesh = Mesh3::create_box(10.0, 10.0, 10.0, true);
let cloud = mesh.sample_voxel_surface(1.0, None).unwrap();
let normals = cloud.point_normals().expect("normals were not written");
assert!(!normals.is_empty());
for n in normals {
let longest = n.x.abs().max(n.y.abs()).max(n.z.abs());
assert!(
(longest - 1.0).abs() < 1e-12,
"normal {n:?} is a blend of facets"
);
}
}
#[test]
fn a_thin_wall_puts_points_on_a_sheet_not_between_them() {
let voxel_size = 1.0;
let data = thin_wall(0.40, 0.55);
let cloud = data.sample_voxel_surface(voxel_size, None).unwrap();
assert!(!cloud.is_empty());
for p in cloud.points() {
assert!(
(p.z - 0.55).abs() < 1e-12,
"point at z = {} is not on the nearer sheet (an average would be 0.475)",
p.z
);
}
for n in cloud.point_normals().unwrap() {
assert!((n.z.abs() - 1.0).abs() < 1e-12, "normal {n:?} was blended");
}
}
#[test]
fn an_exact_tie_breaks_toward_the_lower_face_index() {
let voxel_size = 1.0;
let data = thin_wall(0.45, 0.55);
let cloud = data.sample_voxel_surface(voxel_size, None).unwrap();
assert!(!cloud.is_empty());
for p in cloud.points() {
assert!(
(p.z - 0.45).abs() < 1e-12,
"point at z = {} did not come from the lower-indexed sheet",
p.z
);
}
}
#[test]
fn a_degenerate_face_claims_nothing() {
let points = vec![
Point3::new(0.1, 0.1, 0.1),
Point3::new(0.2, 0.1, 0.1),
Point3::new(0.3, 0.1, 0.1),
];
let faces = vec![[0u32, 1, 2]];
assert!(
compute_voxel_surface_points(&points, &faces, 1.0, None)
.unwrap()
.is_empty()
);
assert!(occupied_cells(&points, &faces, 1.0).is_empty());
}
#[test]
fn a_face_mask_restricts_the_sampling() {
let mesh = Mesh3::create_box(10.0, 10.0, 10.0, true);
let voxel_size = 1.0;
let mask = IndexMask::try_from_indices(&[0, 1], mesh.faces().len()).unwrap();
let full = mesh.sample_voxel_surface(voxel_size, None).unwrap();
let part = mesh.sample_voxel_surface(voxel_size, Some(&mask)).unwrap();
assert!(!part.is_empty());
assert!(part.point_count() < full.point_count() / 4);
for p in part.points() {
assert!(mesh.distance_closest_to(p) < 1e-9);
}
}
#[test]
fn an_empty_mesh_samples_to_an_empty_cloud() {
let data = MeshData3::empty();
assert!(data.sample_voxel_surface(1.0, None).unwrap().is_empty());
}
#[test]
fn a_nonsense_voxel_size_is_rejected() {
let mesh = Mesh3::create_box(1.0, 1.0, 1.0, true);
for bad in [0.0, -1.0, f64::NAN, f64::INFINITY] {
assert!(
mesh.sample_voxel_surface(bad, None).is_err(),
"{bad} was accepted"
);
}
}
#[test]
fn a_mask_of_the_wrong_length_is_rejected() {
let mesh = Mesh3::create_box(1.0, 1.0, 1.0, true);
let wrong = IndexMask::new(mesh.faces().len() + 1, true);
assert!(mesh.sample_voxel_surface(0.25, Some(&wrong)).is_err());
}
#[test]
fn a_mesh_too_far_from_the_origin_to_index_is_rejected() {
let mesh = Mesh3::create_box(1.0, 1.0, 1.0, true)
.transform_copy(&Iso3::translation(1e6, 0.0, 0.0));
assert!(mesh.sample_voxel_surface(1e-6, None).is_err());
assert!(mesh.sample_voxel_surface(1.0, None).is_ok());
}
fn time_best<T>(mut f: impl FnMut() -> T) -> (T, f64) {
let mut best = f64::INFINITY;
let mut out = None;
for _ in 0..3 {
let t = std::time::Instant::now();
let r = f();
best = best.min(t.elapsed().as_secs_f64() * 1000.0);
out = Some(r);
}
(out.expect("ran at least once"), best)
}
fn deviation(mesh: &Mesh3, cloud: &PointCloud3) -> (f64, f64) {
let d: Vec<f64> = cloud
.points()
.iter()
.map(|p| mesh.distance_closest_to(p))
.collect();
let rms = (d.iter().map(|x| x * x).sum::<f64>() / d.len() as f64).sqrt();
(rms, d.iter().cloned().fold(0.0, f64::max))
}
#[test]
#[ignore]
fn report_against_the_existing_dense_sample_route() {
let mesh = engine_blade();
println!(
"\nengine blade: {} points, {} faces\n",
mesh.point_count(),
mesh.face_count()
);
println!(
"{:>6} {:<22} {:>10} {:>10} {:>12} {:>12}",
"v", "route", "points", "ms", "rms dev", "max dev"
);
for v in [4.0, 2.0, 1.0, 0.5] {
let (dense, dense_ms) = time_best(|| {
mesh.sample_dense(v / 2.0, None)
.unwrap()
.reduce_by_voxel(v)
.unwrap()
});
let (surface, surface_ms) = time_best(|| mesh.sample_voxel_surface(v, None).unwrap());
for (name, cloud, ms) in [
("dense sample + reduce", &dense, dense_ms),
("voxel surface", &surface, surface_ms),
] {
let (rms, max) = deviation(&mesh, cloud);
println!(
"{:>6} {:<22} {:>10} {:>10.1} {:>12.3e} {:>12.3e}",
v,
name,
cloud.point_count(),
ms,
rms,
max
);
}
println!();
}
}
}