diff --git a/crates/parry2d/tests/deformable_polyline.rs b/crates/parry2d/tests/deformable_polyline.rs new file mode 100644 index 00000000..ae84b658 --- /dev/null +++ b/crates/parry2d/tests/deformable_polyline.rs @@ -0,0 +1,201 @@ +//! A `Polyline` flagged `DEFORMABLE` and updated with `set_vertices` must give correct contact +//! manifolds at a fixed relative pose: the cached contact points of the rigid path may not be +//! reused after the vertices moved. + +use parry2d::bounding_volume::{Aabb, BoundingVolume}; +use parry2d::math::{Pose, Real, Vector}; +use parry2d::query::{ + ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, PersistentQueryDispatcher, +}; +use parry2d::shape::{Ball, Cuboid, Polyline, PolylineFlags, Shape}; + +/// A horizontal line strip at `y = 0` from `-half` to `half` with `n` segments. +fn strip(n: usize, half: Real) -> Vec { + let step = 2.0 * half / n as Real; + (0..=n) + .map(|i| Vector::new(-half + i as Real * step, 0.0)) + .collect() +} + +struct Pair { + manifolds: Vec>, + workspace: Option, +} + +impl Pair { + fn new() -> Self { + Self { + manifolds: Vec::new(), + workspace: None, + } + } + + fn update(&mut self, polyline: &Polyline, other: &dyn Shape, pos12: &Pose) { + DefaultQueryDispatcher + .contact_manifolds( + pos12, + polyline, + other, + 0.1, + &mut self.manifolds, + &mut self.workspace, + ) + .unwrap(); + } + + fn deepest(&self) -> Option { + self.manifolds + .iter() + .flat_map(|m| m.points.iter().map(|p| p.dist)) + .fold(None, |acc: Option, d| { + Some(acc.map_or(d, |a| a.min(d))) + }) + } +} + +fn translated(vertices: &[Vector], shift: Vector) -> Vec { + vertices.iter().map(|v| *v + shift).collect() +} + +/// A cuboid rests on the polyline; the polyline then rises into it at the same `pos12`: the +/// segment vs cuboid generator reuses its cached points when the pose did not move, so the rigid +/// path reports the stale penetration and the deformable one the new one. +#[test] +fn cuboid_penetration_follows_the_moved_vertices() { + let vertices = strip(4, 2.0); + let cuboid = Cuboid::new(Vector::splat(0.5)); + let pos12 = Pose::translation(0.0, 0.45); + + for deformable in [false, true] { + let flags = if deformable { + PolylineFlags::DEFORMABLE + } else { + PolylineFlags::empty() + }; + let mut polyline = Polyline::with_flags(vertices.clone(), None, flags); + let mut pair = Pair::new(); + + pair.update(&polyline, &cuboid, &pos12); + let dist0 = pair.deepest().unwrap(); + assert!((dist0 + 0.05).abs() < 1.0e-4, "dist0 = {dist0}"); + + polyline.set_vertices(&translated(&vertices, Vector::new(0.0, 0.2))); + pair.update(&polyline, &cuboid, &pos12); + let dist1 = pair.deepest().unwrap(); + + if deformable { + assert!((dist1 + 0.25).abs() < 1.0e-4, "dist1 = {dist1}"); + } else { + assert!( + (dist1 - dist0).abs() < 1.0e-6, + "the rigid path is expected to keep its stale points (dist1 = {dist1})" + ); + } + } +} + +/// The ball ends up on the other side of the polyline after the vertices moved past it: the +/// contact normal must flip with the geometry. +#[test] +fn ball_on_the_other_side_after_the_polyline_moved_past_it() { + let vertices = strip(4, 2.0); + let ball = Ball::new(0.5); + let pos12 = Pose::translation(0.0, 0.4); + let mut polyline = Polyline::with_flags(vertices.clone(), None, PolylineFlags::DEFORMABLE); + let mut pair = Pair::new(); + + pair.update(&polyline, &ball, &pos12); + let normal0 = pair + .manifolds + .iter() + .find(|m| !m.points.is_empty()) + .unwrap() + .local_n1; + assert!(normal0.y > 0.99, "normal0 = {normal0:?}"); + + polyline.set_vertices(&translated(&vertices, Vector::new(0.0, 0.8))); + pair.update(&polyline, &ball, &pos12); + let normal1 = pair + .manifolds + .iter() + .find(|m| !m.points.is_empty()) + .unwrap() + .local_n1; + assert!(normal1.y < -0.99, "normal1 = {normal1:?}"); + let dist = pair.deepest().unwrap(); + assert!((dist + 0.1).abs() < 1.0e-4, "dist = {dist}"); +} + +/// After `set_vertices`, the refitted BVH must answer AABB queries exactly like a rebuilt one. +#[test] +fn bvh_refit_after_set_vertices_matches_brute_force() { + let vertices = strip(40, 10.0); + let mut polyline = Polyline::new(vertices.clone(), None); + + let deformed: Vec = vertices + .iter() + .map(|v| Vector::new(v.x, (v.x * 0.7).sin() * 2.0)) + .collect(); + polyline.set_vertices(&deformed); + assert_eq!(polyline.vertices(), &deformed[..]); + + let rebuilt = Polyline::new(deformed.clone(), None); + assert!(polyline + .local_aabb() + .mins + .abs_diff_eq(rebuilt.local_aabb().mins, 1.0e-6)); + assert!(polyline + .local_aabb() + .maxs + .abs_diff_eq(rebuilt.local_aabb().maxs, 1.0e-6)); + + let queries = [ + Aabb::new(Vector::new(-1.0, -0.5), Vector::new(1.0, 0.5)), + Aabb::new(Vector::new(2.0, 1.0), Vector::new(6.0, 3.0)), + Aabb::new(Vector::new(-9.0, 0.5), Vector::new(-6.0, 1.5)), + ]; + for query in &queries { + let mut found: Vec = polyline.bvh().intersect_aabb(query).collect(); + found.sort_unstable(); + let mut expected: Vec = (0..polyline.num_segments() as u32) + .filter(|i| polyline.segment(*i).local_aabb().intersects(query)) + .collect(); + expected.sort_unstable(); + assert_eq!(found, expected, "query {query:?}"); + assert!(!expected.is_empty(), "the query should hit something"); + } +} + +/// An oriented polyline keeps its one-sided behaviour after `set_vertices` (pseudo-normals are +/// recomputed from the moved vertices). +#[test] +fn oriented_polyline_recomputes_pseudo_normals() { + // Counter-clockwise square: outward is away from the center. + let vertices = vec![ + Vector::new(-1.0, -1.0), + Vector::new(1.0, -1.0), + Vector::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + ]; + let indices = Some(vec![[0, 1], [1, 2], [2, 3], [3, 0]]); + let mut polyline = Polyline::with_flags( + vertices, + indices, + PolylineFlags::ORIENTED | PolylineFlags::DEFORMABLE, + ); + let bottom0 = polyline.segment_normal_constraints(0).unwrap().face; + assert!(bottom0.abs_diff_eq(Vector::new(0.0, -1.0), 1.0e-5)); + + // Shear the square: the bottom edge now slopes. + polyline.set_vertices(&[ + Vector::new(-1.0, -1.0), + Vector::new(1.0, 0.0), + Vector::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + ]); + let bottom1 = polyline.segment_normal_constraints(0).unwrap().face; + assert!( + bottom1.abs_diff_eq(Vector::new(1.0, -2.0).normalize(), 1.0e-5), + "bottom1 = {bottom1:?}" + ); +} diff --git a/crates/parry2d/tests/geometry/mod.rs b/crates/parry2d/tests/geometry/mod.rs index 05907e8a..d21757b6 100644 --- a/crates/parry2d/tests/geometry/mod.rs +++ b/crates/parry2d/tests/geometry/mod.rs @@ -5,3 +5,4 @@ mod epa2; mod epa_convergence; mod ray_cast; mod time_of_impact2; +mod volume_mesh; diff --git a/crates/parry2d/tests/geometry/volume_mesh.rs b/crates/parry2d/tests/geometry/volume_mesh.rs new file mode 100644 index 00000000..77d19147 --- /dev/null +++ b/crates/parry2d/tests/geometry/volume_mesh.rs @@ -0,0 +1,136 @@ +use parry2d::math::{Real, Vector}; +use parry2d::transformation::{volume_mesh, VolumeMesh, VolumeMeshParameters}; + +/// A counter-clockwise circle, as a closed polyline. +fn disk(radius: Real, num_points: usize) -> (Vec, Vec<[u32; 2]>) { + let vertices = (0..num_points) + .map(|i| { + let angle = i as Real / num_points as Real * core::f64::consts::TAU as Real; + Vector::new(angle.cos(), angle.sin()) * radius + }) + .collect(); + let indices = (0..num_points as u32) + .map(|i| [i, (i + 1) % num_points as u32]) + .collect(); + (vertices, indices) +} + +fn area(mesh: &VolumeMesh) -> Real { + mesh.cells + .iter() + .map(|cell| { + let [a, b, c] = cell.map(|i| mesh.vertices[i as usize]); + (b - a).perp_dot(c - a) / 2.0 + }) + .sum() +} + +/// The smallest angle of the mesh, in degrees. +fn min_angle(mesh: &VolumeMesh) -> Real { + let mut min = Real::MAX; + + for cell in &mesh.cells { + let pts = cell.map(|i| mesh.vertices[i as usize]); + + for k in 0..3 { + let (a, b, c) = (pts[k], pts[(k + 1) % 3], pts[(k + 2) % 3]); + let angle = (b - a) + .normalize() + .dot((c - a).normalize()) + .clamp(-1.0, 1.0) + .acos(); + min = min.min(angle.to_degrees()); + } + } + + min +} + +#[test] +fn volume_mesh_disk() { + let (vertices, indices) = disk(1.0, 64); + let mesh = volume_mesh(&vertices, &indices, &VolumeMeshParameters::new(0.15)).unwrap(); + + for cell in &mesh.cells { + let [a, b, c] = cell.map(|i| mesh.vertices[i as usize]); + assert!( + (b - a).perp_dot(c - a) > 0.0, + "inverted or degenerate cell {cell:?}" + ); + } + + // The boundary is reproduced exactly, so the mesh covers the polygon and nothing else. + let expected: Real = (0..64) + .map(|i| { + let (a, b) = (vertices[i], vertices[(i + 1) % 64]); + a.perp_dot(b) / 2.0 + }) + .sum(); + assert!( + (area(&mesh) - expected).abs() < expected * 1.0e-3, + "disk area {} vs {expected}", + area(&mesh) + ); + + // The Delaunay refinement targets 30 degrees; it may stop a bit short near the boundary. + let min = min_angle(&mesh); + assert!(min > 20.0, "smallest angle {min}"); + + // The elements are sized as requested. + let mean_area = area(&mesh) / mesh.cells.len() as Real; + assert!(mean_area < 0.15 * 0.15, "mean cell area {mean_area}"); +} + +#[test] +fn volume_mesh_disk_with_hole() { + let (mut vertices, mut indices) = disk(1.0, 48); + let (inner_vertices, inner_indices) = disk(0.4, 24); + let offset = vertices.len() as u32; + vertices.extend(inner_vertices); + indices.extend(inner_indices.iter().map(|e| [e[0] + offset, e[1] + offset])); + + let mesh = volume_mesh(&vertices, &indices, &VolumeMeshParameters::new(0.1)).unwrap(); + + // The hole is left empty, whichever way it winds. + let ring: Real = core::f64::consts::PI as Real * (1.0 - 0.4 * 0.4); + assert!( + (area(&mesh) - ring).abs() < ring * 0.02, + "ring area {} vs {ring}", + area(&mesh) + ); + + for cell in &mesh.cells { + let center = cell + .iter() + .map(|i| mesh.vertices[*i as usize]) + .sum::() + / 3.0; + assert!(center.length() > 0.35, "cell inside the hole"); + } +} + +/// Asking for smaller elements must not make them worse: the refinement's vertex budget has to +/// follow the requested element size, or it gives up mid-refinement. +#[test] +fn volume_mesh_quality_holds_at_a_fine_resolution() { + let (vertices, indices) = disk(1.0, 64); + + for cell_size in [0.4, 0.2, 0.1, 0.05] { + let mesh = volume_mesh(&vertices, &indices, &VolumeMeshParameters::new(cell_size)).unwrap(); + let min = min_angle(&mesh); + assert!(min > 20.0, "smallest angle {min} at cell size {cell_size}"); + + let mean_area = area(&mesh) / mesh.cells.len() as Real; + assert!( + mean_area < cell_size * cell_size, + "mean cell area {mean_area} at cell size {cell_size}" + ); + } +} + +#[test] +fn volume_mesh_rejects_invalid_parameters() { + let (vertices, indices) = disk(1.0, 16); + assert!(volume_mesh(&vertices, &indices, &VolumeMeshParameters::new(0.0)).is_none()); + assert!(volume_mesh(&[], &[], &VolumeMeshParameters::new(0.1)).is_none()); +} diff --git a/crates/parry3d/examples/bench_cover_smoothing.rs b/crates/parry3d/examples/bench_cover_smoothing.rs new file mode 100644 index 00000000..95f876a3 --- /dev/null +++ b/crates/parry3d/examples/bench_cover_smoothing.rs @@ -0,0 +1,34 @@ +use parry3d::shape::Ball; +use parry3d::transformation::{volume_mesh, MeshEnclosure, VolumeMeshParameters}; +use std::time::Instant; + +fn main() { + let (vertices, indices) = Ball::new(1.0).to_trimesh(30, 30); + let mut params = VolumeMeshParameters::new(0.2); + params.enclosure = MeshEnclosure::Cover; + params.cover_subdivisions = 2; + + let raw = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + let _warmup = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + let start = Instant::now(); + let raw2 = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + let build = start.elapsed(); + let _ = raw2; + + params.cover_smoothing = 30; + let start = Instant::now(); + let smoothed = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + let total = start.elapsed(); + + let checksum: f64 = smoothed + .vertices + .iter() + .map(|v| (v.x as f64) + (v.y as f64) * 3.0 + (v.z as f64) * 7.0) + .sum(); + println!( + "{} cells; build {build:?}, build+smooth {total:?}, smoothing {:?}, checksum {checksum:.9}", + smoothed.cells.len(), + total - build + ); + let _ = raw; +} diff --git a/crates/parry3d/tests/deformable_trimesh.rs b/crates/parry3d/tests/deformable_trimesh.rs new file mode 100644 index 00000000..af9c3322 --- /dev/null +++ b/crates/parry3d/tests/deformable_trimesh.rs @@ -0,0 +1,248 @@ +//! A `TriMesh` flagged `DEFORMABLE` and updated with `set_vertices` must give correct contact +//! manifolds at a fixed relative pose: neither the interfering-triangle cache nor the cached +//! contact points of the rigid path may be reused. + +use parry3d::bounding_volume::{Aabb, BoundingVolume}; +use parry3d::math::{Pose, Real, Vector}; +use parry3d::query::{ + ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, PersistentQueryDispatcher, +}; +use parry3d::shape::{Ball, Cuboid, Shape, TriMesh, TriMeshFlags}; + +/// A flat `n x n` quad grid at `y = 0` spanning `[-half, half]` on x and z. +fn grid(n: usize, half: Real) -> (Vec, Vec<[u32; 3]>) { + let mut vertices = Vec::new(); + let mut indices = Vec::new(); + let step = 2.0 * half / n as Real; + for i in 0..=n { + for j in 0..=n { + vertices.push(Vector::new( + -half + i as Real * step, + 0.0, + -half + j as Real * step, + )); + } + } + let row = (n + 1) as u32; + for i in 0..n as u32 { + for j in 0..n as u32 { + let a = i * row + j; + indices.push([a, a + 1, a + row + 1]); + indices.push([a, a + row + 1, a + row]); + } + } + (vertices, indices) +} + +struct Pair { + manifolds: Vec>, + workspace: Option, +} + +impl Pair { + fn new() -> Self { + Self { + manifolds: Vec::new(), + workspace: None, + } + } + + fn update(&mut self, mesh: &TriMesh, other: &dyn Shape, pos12: &Pose) { + DefaultQueryDispatcher + .contact_manifolds( + pos12, + mesh, + other, + 0.1, + &mut self.manifolds, + &mut self.workspace, + ) + .unwrap(); + } + + fn deepest(&self) -> Option { + self.manifolds + .iter() + .flat_map(|m| m.points.iter().map(|p| p.dist)) + .fold(None, |acc: Option, d| { + Some(acc.map_or(d, |a| a.min(d))) + }) + } +} + +fn translated(vertices: &[Vector], shift: Vector) -> Vec { + vertices.iter().map(|v| *v + shift).collect() +} + +/// The ball starts above the middle of the mesh; the mesh then slides sideways so different +/// triangles end up under the ball (same `pos12`): the rigid path keeps its cached interfering +/// triangles (now far away) and finds nothing, the deformable one finds the new triangles. +#[test] +fn ball_finds_the_triangles_moved_under_it() { + let (vertices, indices) = grid(8, 4.0); + let ball = Ball::new(0.5); + let pos12 = Pose::translation(0.0, 0.4, 0.0); + + for deformable in [false, true] { + let flags = if deformable { + TriMeshFlags::DEFORMABLE + } else { + TriMeshFlags::empty() + }; + let mut mesh = TriMesh::with_flags(vertices.clone(), indices.clone(), flags).unwrap(); + let mut pair = Pair::new(); + + pair.update(&mesh, &ball, &pos12); + assert!(pair.deepest().unwrap() < 0.0, "initial contact expected"); + + // The vertices under the ball move 3 m along x: other triangles are now under it. + mesh.set_vertices(&translated(&vertices, Vector::new(3.0, 0.0, 0.0))); + pair.update(&mesh, &ball, &pos12); + let deepest = pair.deepest(); + + if deformable { + let dist = deepest.expect("the deformable mesh must find the new triangles"); + assert!((dist + 0.1).abs() < 1.0e-4, "dist = {dist}"); + } else { + assert!( + deepest.is_none(), + "the rigid path is expected to keep its stale interfering triangles" + ); + } + } +} + +/// A cuboid rests on the mesh; the mesh then rises into it at the same `pos12`: the cuboid vs +/// triangle generator reuses its cached points when the pose did not move, so the rigid path +/// reports the stale penetration and the deformable one the new one. +#[test] +fn cuboid_penetration_follows_the_moved_vertices() { + let (vertices, indices) = grid(4, 2.0); + let cuboid = Cuboid::new(Vector::splat(0.5)); + let pos12 = Pose::translation(0.0, 0.45, 0.0); + + for deformable in [false, true] { + let flags = if deformable { + TriMeshFlags::DEFORMABLE + } else { + TriMeshFlags::empty() + }; + let mut mesh = TriMesh::with_flags(vertices.clone(), indices.clone(), flags).unwrap(); + let mut pair = Pair::new(); + + pair.update(&mesh, &cuboid, &pos12); + let dist0 = pair.deepest().unwrap(); + assert!((dist0 + 0.05).abs() < 1.0e-4, "dist0 = {dist0}"); + + mesh.set_vertices(&translated(&vertices, Vector::new(0.0, 0.2, 0.0))); + pair.update(&mesh, &cuboid, &pos12); + let dist1 = pair.deepest().unwrap(); + + if deformable { + assert!((dist1 + 0.25).abs() < 1.0e-4, "dist1 = {dist1}"); + } else { + assert!( + (dist1 - dist0).abs() < 1.0e-6, + "the rigid path is expected to keep its stale points (dist1 = {dist1})" + ); + } + } +} + +/// The ball ends up on the other side of the mesh after the vertices moved past it: the contact +/// normal must flip with the geometry. +#[test] +fn ball_on_the_other_side_after_the_mesh_moved_past_it() { + let (vertices, indices) = grid(4, 2.0); + let ball = Ball::new(0.5); + let pos12 = Pose::translation(0.0, 0.4, 0.0); + let mut mesh = + TriMesh::with_flags(vertices.clone(), indices.clone(), TriMeshFlags::DEFORMABLE).unwrap(); + let mut pair = Pair::new(); + + pair.update(&mesh, &ball, &pos12); + let normal0 = pair + .manifolds + .iter() + .find(|m| !m.points.is_empty()) + .unwrap() + .local_n1; + assert!(normal0.y > 0.99, "normal0 = {normal0:?}"); + + // The mesh moves up through the ball: the ball is now below it. + mesh.set_vertices(&translated(&vertices, Vector::new(0.0, 0.8, 0.0))); + pair.update(&mesh, &ball, &pos12); + let normal1 = pair + .manifolds + .iter() + .find(|m| !m.points.is_empty()) + .unwrap() + .local_n1; + assert!(normal1.y < -0.99, "normal1 = {normal1:?}"); + let dist = pair.deepest().unwrap(); + assert!((dist + 0.1).abs() < 1.0e-4, "dist = {dist}"); +} + +/// After `set_vertices`, the refitted BVH must answer AABB queries exactly like a rebuilt one. +#[test] +fn bvh_refit_after_set_vertices_matches_brute_force() { + let (vertices, indices) = grid(10, 5.0); + let mut mesh = TriMesh::new(vertices.clone(), indices.clone()).unwrap(); + + // A smooth deformation: a wave along x plus a shear along z. + let deformed: Vec = vertices + .iter() + .map(|v| Vector::new(v.x + 0.3 * v.z, (v.x * 1.3).sin() * 1.5, v.z)) + .collect(); + mesh.set_vertices(&deformed); + assert_eq!(mesh.vertices(), &deformed[..]); + + let rebuilt = TriMesh::new(deformed.clone(), indices.clone()).unwrap(); + assert!(mesh + .local_aabb() + .mins + .abs_diff_eq(rebuilt.local_aabb().mins, 1.0e-6)); + assert!(mesh + .local_aabb() + .maxs + .abs_diff_eq(rebuilt.local_aabb().maxs, 1.0e-6)); + + let queries = [ + Aabb::new(Vector::new(-1.0, -0.5, -1.0), Vector::new(1.0, 0.5, 1.0)), + Aabb::new(Vector::new(2.0, 0.5, -6.0), Vector::new(6.0, 2.0, 6.0)), + Aabb::new(Vector::new(-6.0, -2.0, 3.0), Vector::new(-3.0, 0.0, 6.0)), + Aabb::new(Vector::new(-0.2, 1.4, -0.2), Vector::new(0.2, 1.6, 0.2)), + ]; + for query in &queries { + let mut found: Vec = mesh.bvh().intersect_aabb(query).collect(); + found.sort_unstable(); + let mut expected: Vec = (0..mesh.num_triangles() as u32) + .filter(|i| mesh.triangle(*i).local_aabb().intersects(query)) + .collect(); + expected.sort_unstable(); + assert_eq!(found, expected, "query {query:?}"); + assert!(!expected.is_empty(), "the query should hit something"); + } +} + +/// `update_vertices` is the in-place variant of `set_vertices`. +#[test] +fn update_vertices_refits_too() { + let (vertices, indices) = grid(3, 1.5); + let mut mesh = TriMesh::new(vertices, indices).unwrap(); + mesh.update_vertices(|vtx| { + for v in vtx { + v.y = 2.0; + } + }); + let aabb = mesh.local_aabb(); + assert!((aabb.mins.y - 2.0).abs() < 1.0e-6 && (aabb.maxs.y - 2.0).abs() < 1.0e-6); + let hits: Vec = mesh + .bvh() + .intersect_aabb(&Aabb::new( + Vector::new(-2.0, 1.9, -2.0), + Vector::new(2.0, 2.1, 2.0), + )) + .collect(); + assert_eq!(hits.len(), mesh.num_triangles()); +} diff --git a/crates/parry3d/tests/geometry/mod.rs b/crates/parry3d/tests/geometry/mod.rs index 25a4e755..7eb925aa 100644 --- a/crates/parry3d/tests/geometry/mod.rs +++ b/crates/parry3d/tests/geometry/mod.rs @@ -10,3 +10,4 @@ mod time_of_impact3; mod trimesh_connected_components; mod trimesh_intersection; mod trimesh_trimesh_toi; +mod volume_mesh; diff --git a/crates/parry3d/tests/geometry/volume_mesh.rs b/crates/parry3d/tests/geometry/volume_mesh.rs new file mode 100644 index 00000000..f3b16c0f --- /dev/null +++ b/crates/parry3d/tests/geometry/volume_mesh.rs @@ -0,0 +1,456 @@ +use parry3d::math::{Real, Vector}; +use parry3d::shape::{Ball, Cone, Cuboid}; +use parry3d::transformation::{volume_mesh, VolumeMesh, VolumeMeshParameters}; +use std::collections::HashMap; + +fn tet_volume(mesh: &VolumeMesh, cell: [u32; 4]) -> Real { + let [a, b, c, d] = cell.map(|i| mesh.vertices[i as usize]); + (b - a).cross(c - a).dot(d - a) / 6.0 +} + +fn total_volume(mesh: &VolumeMesh) -> Real { + mesh.cells.iter().map(|c| tet_volume(mesh, *c)).sum() +} + +/// The faces shared by a single cell: the boundary of the volume mesh. +fn boundary(mesh: &VolumeMesh) -> Vec<[u32; 3]> { + let mut faces: HashMap<[u32; 3], (usize, [u32; 3])> = HashMap::new(); + + for cell in &mesh.cells { + for face in [ + [cell[0], cell[2], cell[1]], + [cell[0], cell[1], cell[3]], + [cell[1], cell[2], cell[3]], + [cell[0], cell[3], cell[2]], + ] { + let mut key = face; + key.sort_unstable(); + let entry = faces.entry(key).or_insert((0, face)); + entry.0 += 1; + } + } + + for (count, _) in faces.values() { + assert!(*count <= 2, "non-manifold face shared by {count} cells"); + } + + faces + .values() + .filter(|(count, _)| *count == 1) + .map(|(_, face)| *face) + .collect() +} + +/// The extreme dihedral angles of the mesh, in degrees. +fn dihedral_angle_range(mesh: &VolumeMesh) -> (Real, Real) { + let (mut min, mut max): (Real, Real) = (Real::MAX, 0.0); + + for cell in &mesh.cells { + let pts = cell.map(|i| mesh.vertices[i as usize]); + // The outward normal of the face opposite to each vertex. + let normals: Vec<_> = [[1, 2, 3], [0, 3, 2], [0, 1, 3], [0, 2, 1]] + .iter() + .map(|f| { + let [a, b, c] = f.map(|i| pts[i]); + (b - a).cross(c - a).normalize() + }) + .collect(); + + for i in 0..4 { + for j in i + 1..4 { + // The dihedral angle along the edge shared by the two faces. + let angle = normals[i].dot(normals[j]).clamp(-1.0, 1.0).acos(); + let angle = (core::f64::consts::PI as Real - angle).to_degrees(); + min = min.min(angle); + max = max.max(angle); + } + } + } + + (min, max) +} + +fn check_mesh(mesh: &VolumeMesh, name: &str) { + assert!(!mesh.cells.is_empty(), "{name}: no cell"); + + for cell in &mesh.cells { + assert!( + tet_volume(mesh, *cell) > 0.0, + "{name}: inverted or degenerate cell {cell:?}" + ); + } + + let (min_angle, max_angle) = dihedral_angle_range(mesh); + // The angle bounds of the pristine lattice cells (the cover cuts and warps nothing). + assert!( + min_angle > 9.05, + "{name}: minimum dihedral angle {min_angle} is too small" + ); + assert!( + max_angle < 160.54, + "{name}: maximum dihedral angle {max_angle} is too large" + ); +} + +#[test] +fn volume_mesh_open_mesh_is_rejected() { + // A single triangle encloses nothing, so the cover, which needs a sign for its + // interior, refuses it; the crust is the mode that takes open input. + let vertices = vec![Vector::ZERO, Vector::X, Vector::Y]; + let indices = vec![[0, 1, 2]]; + assert!(volume_mesh(&vertices, &indices, &VolumeMeshParameters::new(0.1)).is_none()); +} + +#[test] +fn volume_mesh_rejects_invalid_parameters() { + let (vertices, indices) = Ball::new(1.0).to_trimesh(10, 10); + assert!(volume_mesh(&vertices, &indices, &VolumeMeshParameters::new(0.0)).is_none()); + assert!(volume_mesh(&[], &[], &VolumeMeshParameters::new(0.1)).is_none()); +} + +#[test] +fn volume_mesh_welds_duplicated_vertices() { + // The same cuboid, with every triangle carrying its own copy of its vertices: closed + // geometrically, but not topologically. + let (vertices, indices) = Cuboid::new(Vector::splat(0.5)).to_trimesh(); + let mut split_vertices = Vec::new(); + let split_indices: Vec<[u32; 3]> = indices + .iter() + .map(|tri| { + let first = split_vertices.len() as u32; + split_vertices.extend(tri.iter().map(|i| vertices[*i as usize])); + [first, first + 1, first + 2] + }) + .collect(); + + let params = VolumeMeshParameters::new(0.2); + let mesh = volume_mesh(&split_vertices, &split_indices, ¶ms).unwrap(); + check_mesh(&mesh, "welded cuboid"); + + let reference = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + assert_eq!(mesh.cells.len(), reference.cells.len()); +} + +/// Whether the cell (positively oriented) contains the point, within a relative tolerance. +fn cell_contains(mesh: &VolumeMesh, cell: [u32; 4], pt: Vector) -> bool { + let [a, b, c, d] = cell.map(|i| mesh.vertices[i as usize]); + let volume = (b - a).cross(c - a).dot(d - a); + let tol = -volume.abs() * 1.0e-4; + (b - pt).cross(c - pt).dot(d - pt) >= tol + && (pt - a).cross(c - a).dot(d - a) >= tol + && (b - a).cross(pt - a).dot(d - a) >= tol + && (b - a).cross(c - a).dot(pt - a) >= tol +} + +/// Every vertex, edge midpoint and triangle centroid of the boundary is inside some cell. +fn assert_encloses(mesh: &VolumeMesh, vertices: &[Vector], indices: &[[u32; 3]], name: &str) { + let samples = vertices + .iter() + .copied() + .chain(indices.iter().flat_map(|idx| { + let [a, b, c] = idx.map(|i| vertices[i as usize]); + [ + (a + b) * 0.5, + (b + c) * 0.5, + (c + a) * 0.5, + (a + b + c) / 3.0, + ] + })); + let aabbs: Vec<(Vector, Vector)> = mesh + .cells + .iter() + .map(|cell| { + let pts = cell.map(|i| mesh.vertices[i as usize]); + ( + pts.iter().fold(Vector::splat(Real::MAX), |a, b| a.min(*b)), + pts.iter().fold(Vector::splat(-Real::MAX), |a, b| a.max(*b)), + ) + }) + .collect(); + + 'samples: for (k, pt) in samples.enumerate() { + for (cell, (mins, maxs)) in mesh.cells.iter().zip(&aabbs) { + if pt.clamp(*mins, *maxs) == pt && cell_contains(mesh, *cell, pt) { + continue 'samples; + } + } + panic!("{name}: boundary sample {k} at {pt:?} is outside the mesh"); + } +} + +/// The cover keeps whole lattice cells, so it contains the shape, subdivided or not, and +/// the unsubdivided cells keep the lattice's own angle bounds (nothing was warped or cut). +#[test] +fn volume_mesh_cover_encloses() { + use parry3d::transformation::MeshEnclosure; + + let (vertices, indices) = Ball::new(1.0).to_trimesh(20, 20); + let mut params = VolumeMeshParameters::new(0.25); + + for subdivisions in [0, 1] { + params.cover_subdivisions = subdivisions; + params.enclosure = MeshEnclosure::Cover; + let cover = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + + let name = format!("cover ball ({subdivisions} subdivisions)"); + if subdivisions == 0 { + check_mesh(&cover, &name); + } else { + for cell in &cover.cells { + assert!(tet_volume(&cover, *cell) > 0.0, "inverted cell {cell:?}"); + } + } + assert_encloses(&cover, &vertices, &indices, &name); + + // Fatter than the ball it contains, by less than the cell-sized shell around it. + let expected: Real = 4.0 / 3.0 * core::f64::consts::PI as Real; + let volume = total_volume(&cover); + assert!( + volume > expected && volume < expected * 2.5, + "cover volume {volume} vs the ball's {expected}" + ); + } +} + +/// The mean distance from the mesh's boundary vertices to the unit sphere. +fn mean_boundary_distance_to_unit_sphere(mesh: &VolumeMesh) -> Real { + let mut on_boundary = vec![false; mesh.vertices.len()]; + for face in boundary(mesh) { + for v in face { + on_boundary[v as usize] = true; + } + } + let (mut sum, mut count) = (0.0, 0); + for (v, pt) in mesh.vertices.iter().enumerate() { + if on_boundary[v] { + sum += (pt.length() - 1.0).abs(); + count += 1; + } + } + sum / count.max(1) as Real +} + +/// The shrink-wrap flattens the cover's staircase (its boundary ends up far closer to the +/// surface) without giving up the containment guarantee or inverting a cell. +#[test] +fn volume_mesh_cover_smoothing() { + use parry3d::transformation::MeshEnclosure; + + let (vertices, indices) = Ball::new(1.0).to_trimesh(20, 20); + let mut params = VolumeMeshParameters::new(0.25); + params.enclosure = MeshEnclosure::Cover; + let raw = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + params.cover_smoothing = 20; + let smoothed = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + + for cell in &smoothed.cells { + assert!(tet_volume(&smoothed, *cell) > 0.0, "inverted cell {cell:?}"); + } + assert_encloses(&smoothed, &vertices, &indices, "smoothed cover ball"); + assert!( + total_volume(&smoothed) < total_volume(&raw), + "the wrap did not tighten the cover" + ); + + let raw_distance = mean_boundary_distance_to_unit_sphere(&raw); + let smoothed_distance = mean_boundary_distance_to_unit_sphere(&smoothed); + println!( + "cover boundary distance to the sphere: raw {raw_distance:.4}, smoothed {smoothed_distance:.4}" + ); + assert!( + smoothed_distance < raw_distance * 0.5, + "the wrap left the boundary {smoothed_distance} from the sphere against {raw_distance} raw" + ); + + // The demo's configuration: subdivided, smoothed cover. + let (vertices, indices) = Cone::new(1.5, 1.0).to_trimesh(40); + let mut params = VolumeMeshParameters::new(0.16); + params.enclosure = MeshEnclosure::Cover; + params.cover_subdivisions = 1; + params.cover_smoothing = 20; + let cone = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + for cell in &cone.cells { + assert!(tet_volume(&cone, *cell) > 0.0, "inverted cell {cell:?}"); + } + assert_encloses(&cone, &vertices, &indices, "smoothed adaptive cover cone"); +} + +/// Every edge of the mesh's boundary belongs to an even number of boundary faces: a +/// hanging vertex (a crack between subdivision levels) would leave the subdivided side's +/// half-edges unmatched, which a simulation mesh cannot afford. +fn assert_conforming_boundary(mesh: &VolumeMesh, name: &str) { + let mut edges: HashMap<[u32; 2], u32> = HashMap::new(); + for face in boundary(mesh) { + for k in 0..3 { + let (a, b) = (face[k], face[(k + 1) % 3]); + *edges.entry([a.min(b), a.max(b)]).or_insert(0) += 1; + } + } + for (edge, count) in &edges { + assert!( + count % 2 == 0, + "{name}: boundary edge {edge:?} borders {count} faces" + ); + } +} + +/// Boundary-crossing cells refine below the cell size, outside sub-cells are discarded, +/// and the mesh stays conforming: the staircase shrinks with each subdivision, containment +/// holds throughout, and the shrink-wrap on top hugs tighter than without subdivision. +#[test] +fn volume_mesh_cover_subdivision() { + use parry3d::transformation::MeshEnclosure; + + let (vertices, indices) = Ball::new(1.0).to_trimesh(16, 16); + let mut params = VolumeMeshParameters::new(0.35); + params.enclosure = MeshEnclosure::Cover; + + let mut distances = Vec::new(); + let mut volumes = Vec::new(); + for subdivisions in [0, 1, 2] { + params.cover_subdivisions = subdivisions; + let mesh = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + + for cell in &mesh.cells { + assert!(tet_volume(&mesh, *cell) > 0.0, "inverted cell {cell:?}"); + } + let name = format!("cover ball, {subdivisions} subdivisions"); + assert_encloses(&mesh, &vertices, &indices, &name); + assert_conforming_boundary(&mesh, &name); + + distances.push(mean_boundary_distance_to_unit_sphere(&mesh)); + volumes.push(total_volume(&mesh)); + println!( + "{subdivisions} subdivisions: {} cells, boundary distance {:.4}, volume {:.4}", + mesh.cells.len(), + distances[subdivisions as usize], + volumes[subdivisions as usize], + ); + } + for s in 1..3 { + assert!( + distances[s] < distances[s - 1] * 0.7, + "subdivision {s} left the staircase at {} against {}", + distances[s], + distances[s - 1] + ); + assert!(volumes[s] < volumes[s - 1]); + } + + // The wrap on the subdivided cover: its guard scales with the finer boundary cells, so + // it ends up closer to the surface than the wrap on the unsubdivided one. + params.cover_smoothing = 20; + params.cover_subdivisions = 0; + let smoothed = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + params.cover_subdivisions = 1; + let subdivided = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + + assert_encloses( + &subdivided, + &vertices, + &indices, + "smoothed subdivided cover", + ); + assert_conforming_boundary(&subdivided, "smoothed subdivided cover"); + let coarse = mean_boundary_distance_to_unit_sphere(&smoothed); + let fine = mean_boundary_distance_to_unit_sphere(&subdivided); + println!("smoothed boundary distance: unsubdivided {coarse:.4}, subdivided {fine:.4}"); + assert!( + fine < coarse, + "the subdivided wrap ({fine}) is no tighter than the unsubdivided one ({coarse})" + ); +} + +/// The crust covers the surface alone: an open mesh, which every other lattice mode +/// refuses, comes back as a shell of cells hugging its surface, hollow inside, conforming, +/// and the shrink-wrap still applies. +#[test] +fn volume_mesh_crust_covers_open_meshes() { + use parry3d::transformation::MeshEnclosure; + + // A ball with its cap cut off: boundary edges, so the closed-mesh modes refuse it. + let (vertices, indices) = Ball::new(1.0).to_trimesh(20, 20); + let open_indices: Vec<[u32; 3]> = indices + .iter() + .copied() + .filter(|tri| tri.iter().all(|v| vertices[*v as usize].y < 0.8)) + .collect(); + assert!(open_indices.len() < indices.len()); + + let mut params = VolumeMeshParameters::new(0.25); + params.cover_subdivisions = 1; + assert!( + volume_mesh(&vertices, &open_indices, ¶ms).is_none(), + "the cover was expected to refuse the open mesh" + ); + + params.enclosure = MeshEnclosure::Crust; + params.cover_smoothing = 20; + let crust = volume_mesh(&vertices, &open_indices, ¶ms).unwrap(); + + for cell in &crust.cells { + assert!(tet_volume(&crust, *cell) > 0.0, "inverted cell {cell:?}"); + } + assert_conforming_boundary(&crust, "open crust"); + + // Every cell hugs the surface; none sits deep inside or far outside. + for cell in &crust.cells { + let center = cell + .map(|i| crust.vertices[i as usize]) + .iter() + .copied() + .sum::() + / 4.0; + let distance = (center.length() - 1.0).abs(); + assert!( + center.y > 0.7 || distance < 0.5, + "a crust cell sits {distance} from the surface" + ); + } + + // The open surface is still enclosed: remap it to its own vertex list first. + let mut remap = vec![u32::MAX; vertices.len()]; + let mut kept_vertices: Vec = Vec::new(); + let kept_indices: Vec<[u32; 3]> = open_indices + .iter() + .map(|tri| { + tri.map(|v| { + if remap[v as usize] == u32::MAX { + remap[v as usize] = kept_vertices.len() as u32; + kept_vertices.push(vertices[v as usize]); + } + remap[v as usize] + }) + }) + .collect(); + assert_encloses(&crust, &kept_vertices, &kept_indices, "open crust"); +} + +/// On a closed mesh the crust is the cover minus its interior: hollow by design. +#[test] +fn volume_mesh_crust_is_hollow() { + use parry3d::transformation::MeshEnclosure; + + let (vertices, indices) = Ball::new(1.0).to_trimesh(20, 20); + let mut params = VolumeMeshParameters::new(0.25); + params.enclosure = MeshEnclosure::Crust; + let crust = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + params.enclosure = MeshEnclosure::Cover; + let cover = volume_mesh(&vertices, &indices, ¶ms).unwrap(); + + assert!(crust.cells.len() < cover.cells.len()); + for cell in &crust.cells { + let center = cell + .map(|i| crust.vertices[i as usize]) + .iter() + .copied() + .sum::() + / 4.0; + assert!( + center.length() > 0.5, + "a crust cell sits at depth {}", + 1.0 - center.length() + ); + } + assert_encloses(&crust, &vertices, &indices, "closed crust"); +} diff --git a/crates/parry3d/tests/two_sided_internal_edges.rs b/crates/parry3d/tests/two_sided_internal_edges.rs new file mode 100644 index 00000000..4e3d00b4 --- /dev/null +++ b/crates/parry3d/tests/two_sided_internal_edges.rs @@ -0,0 +1,75 @@ +//! `TriMeshFlags::FIX_INTERNAL_EDGES_TWO_SIDED` keeps the internal-edge fix on both faces of the +//! mesh instead of discarding the contacts coming from the back. + +use parry3d::math::{Pose, Real, Vector}; +use parry3d::query::{ + ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, PersistentQueryDispatcher, +}; +use parry3d::shape::{Ball, TriMesh, TriMeshFlags}; + +/// A flat two-quad strip at `y = 0`, normals up. +fn strip() -> (Vec, Vec<[u32; 3]>) { + let vertices = vec![ + Vector::new(-2.0, 0.0, -1.0), + Vector::new(0.0, 0.0, -1.0), + Vector::new(2.0, 0.0, -1.0), + Vector::new(-2.0, 0.0, 1.0), + Vector::new(0.0, 0.0, 1.0), + Vector::new(2.0, 0.0, 1.0), + ]; + let indices = vec![[0, 4, 1], [0, 3, 4], [1, 5, 2], [1, 4, 5]]; + (vertices, indices) +} + +/// Deepest contact (normal on the mesh, distance) of a ball at `ball_pos`. +fn deepest(mesh: &TriMesh, ball: &Ball, ball_pos: Vector) -> Option<(Vector, Real)> { + let pos12 = Pose::from_translation(ball_pos); + let mut manifolds: Vec> = Vec::new(); + let mut workspace: Option = None; + DefaultQueryDispatcher + .contact_manifolds(&pos12, mesh, ball, 0.0, &mut manifolds, &mut workspace) + .unwrap(); + let mut best: Option<(Vector, Real)> = None; + for m in &manifolds { + for pt in &m.points { + if best.is_none_or(|(_, d)| pt.dist < d) { + best = Some((m.local_n1, pt.dist)); + } + } + } + best +} + +#[test] +fn two_sided_internal_edges_keep_back_contacts() { + let (vertices, indices) = strip(); + let ball = Ball::new(0.5); + let above = Vector::new(0.0, 0.4, 0.0); + let below = Vector::new(0.0, -0.4, 0.0); + + let one_sided = TriMesh::with_flags( + vertices.clone(), + indices.clone(), + TriMeshFlags::FIX_INTERNAL_EDGES, + ) + .unwrap(); + let two_sided = TriMesh::with_flags( + vertices, + indices, + TriMeshFlags::FIX_INTERNAL_EDGES_TWO_SIDED, + ) + .unwrap(); + assert!(two_sided.flags().contains(TriMeshFlags::FIX_INTERNAL_EDGES)); + + // Front contacts are identical. + let (n1, d1) = deepest(&one_sided, &ball, above).unwrap(); + let (n2, d2) = deepest(&two_sided, &ball, above).unwrap(); + assert!(n1.y > 0.99 && n2.y > 0.99); + assert!((d1 - d2).abs() < 1.0e-6 && (d1 + 0.1).abs() < 1.0e-4); + + // Back contacts are discarded by the one-sided fix and kept (normal flipped) by the two-sided one. + assert!(deepest(&one_sided, &ball, below).is_none()); + let (n3, d3) = deepest(&two_sided, &ball, below).unwrap(); + assert!(n3.y < -0.99, "n3 = {n3:?}"); + assert!((d3 + 0.1).abs() < 1.0e-4, "d3 = {d3}"); +} diff --git a/src/partitioning/bvh/bvh_queries.rs b/src/partitioning/bvh/bvh_queries.rs index d1123771..065e05c3 100644 --- a/src/partitioning/bvh/bvh_queries.rs +++ b/src/partitioning/bvh/bvh_queries.rs @@ -1,5 +1,5 @@ use super::{Bvh, BvhNode}; -use crate::bounding_volume::{Aabb, BoundingVolume}; +use crate::bounding_volume::Aabb; use crate::math::Real; use crate::math::Vector; use crate::query::PointProjection; @@ -201,7 +201,9 @@ impl Bvh { /// - [`traverse`](Self::traverse) - Custom traversal logic /// - [`leaves`](Self::leaves) - General leaf iteration with predicate pub fn intersect_aabb<'a>(&'a self, aabb: &'a Aabb) -> impl Iterator + 'a { - self.leaves(|node: &BvhNode| node.aabb().intersects(aabb)) + // The query as a node: the node-vs-node test is the SIMD one (same inclusive semantics). + let query = BvhNode::leaf(*aabb, 0); + self.leaves(move |node: &BvhNode| node.intersects(&query)) } /// Projects a point on this BVH using the provided leaf projection function. diff --git a/src/query/contact_manifolds/contact_manifold.rs b/src/query/contact_manifolds/contact_manifold.rs index 15e702df..dcdfa7a3 100644 --- a/src/query/contact_manifolds/contact_manifold.rs +++ b/src/query/contact_manifolds/contact_manifold.rs @@ -747,6 +747,15 @@ impl ContactManifold) { ))); } +/// Leaves reached by the query from which the sub-shape manifolds are computed in parallel +/// (`parallel` feature). +#[cfg(feature = "parallel")] +const PARALLEL_LEAVES: usize = 256; + /// Computes the contact manifolds between a composite shape and an abstract shape. +/// +/// The manifolds are in the order of the composite's leaves reached by the query. Under the +/// `parallel` feature, a query reaching many leaves computes their manifolds across threads +/// (same manifolds, same order). pub fn contact_manifolds_composite_shape_shape( dispatcher: &dyn PersistentQueryDispatcher, pos12: &Pose, - composite1: &dyn CompositeShape, + composite1: &(dyn CompositeShape + Sync), shape2: &dyn Shape, prediction: Real, manifolds: &mut Vec>, workspace: &mut Option, flipped: bool, ) where - ManifoldData: Default + Clone, - ContactData: Default + Copy, + ManifoldData: Default + Clone + Send + Sync, + ContactData: Default + Copy + Send + Sync, { ensure_workspace_exists(workspace); let workspace: &mut CompositeShapeShapeContactManifoldsWorkspace = @@ -78,73 +87,109 @@ pub fn contact_manifolds_composite_shape_shape( let pos12 = *pos12; let pos21 = pos12.inverse(); + let deformable = composite1.is_deformable(); // Traverse bvh1 first. let ls_aabb2_1 = shape2.compute_aabb(&pos12).loosened(prediction); let mut old_manifolds = core::mem::take(manifolds); - let mut leaf1_fn = |leaf1: u32| { - composite1.map_part_at(leaf1, &mut |part_pos1, part_shape1, normal_constraints1| { - let sub_detector = match workspace.sub_detectors.entry(leaf1) { + // The manifold of a leaf: the one kept from the last query, or a fresh one (its sub-shape + // ids and pose set by the first computation), pushed in leaf order. + let mut bookkeep = + |leaf1: u32, manifolds: &mut Vec>| { + match workspace.sub_detectors.entry(leaf1) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); - let manifold = old_manifolds[sub_detector.manifold_id].take(); + let mut manifold = old_manifolds[sub_detector.manifold_id].take(); sub_detector.manifold_id = manifolds.len(); sub_detector.timestamp = new_timestamp; + if deformable { + manifold.mark_shapes_deformed(); + } manifolds.push(manifold); - sub_detector + false } Entry::Vacant(entry) => { - let sub_detector = SubDetector { + let _ = entry.insert(SubDetector { manifold_id: manifolds.len(), timestamp: new_timestamp, - }; - + }); let mut manifold = ContactManifold::new(); - if flipped { manifold.subshape1 = 0; manifold.subshape2 = leaf1; - manifold.set_subshape_pos2(part_pos1.copied()); } else { manifold.subshape1 = leaf1; manifold.subshape2 = 0; - manifold.set_subshape_pos1(part_pos1.copied()); - }; - + } + if deformable { + manifold.mark_shapes_deformed(); + } manifolds.push(manifold); - entry.insert(sub_detector) + true } - }; - - let manifold = &mut manifolds[sub_detector.manifold_id]; - - if flipped { - let _ = dispatcher.contact_manifold_convex_convex( - &part_pos1.prepend_to(&pos21), - shape2, - part_shape1, - None, - normal_constraints1, - prediction, - manifold, - ); - } else { - let _ = dispatcher.contact_manifold_convex_convex( - &part_pos1.inv_mul(&pos12), - part_shape1, - shape2, - normal_constraints1, - None, - prediction, - manifold, - ); } - }); - }; + }; + // The manifold's sub-shape pose (a fresh manifold) and contacts. + let compute = + |leaf1: u32, fresh: bool, manifold: &mut ContactManifold| { + composite1.map_part_at(leaf1, &mut |part_pos1, part_shape1, normal_constraints1| { + if fresh { + if flipped { + manifold.set_subshape_pos2(part_pos1.copied()); + } else { + manifold.set_subshape_pos1(part_pos1.copied()); + } + } + if flipped { + let _ = dispatcher.contact_manifold_convex_convex( + &part_pos1.prepend_to(&pos21), + shape2, + part_shape1, + None, + normal_constraints1, + prediction, + manifold, + ); + } else { + let _ = dispatcher.contact_manifold_convex_convex( + &part_pos1.inv_mul(&pos12), + part_shape1, + shape2, + normal_constraints1, + None, + prediction, + manifold, + ); + } + }); + }; - for leaf_id in composite1.bvh().intersect_aabb(&ls_aabb2_1) { - leaf1_fn(leaf_id); + #[cfg(feature = "parallel")] + { + let leaves: Vec = composite1.bvh().intersect_aabb(&ls_aabb2_1).collect(); + if leaves.len() >= PARALLEL_LEAVES { + use rayon::prelude::*; + let fresh: Vec = leaves + .iter() + .map(|&leaf1| bookkeep(leaf1, manifolds)) + .collect(); + manifolds + .par_iter_mut() + .zip(leaves.par_iter()) + .zip(fresh.par_iter()) + .for_each(|((manifold, &leaf1), &fresh)| compute(leaf1, fresh, manifold)); + } else { + for leaf1 in leaves { + let fresh = bookkeep(leaf1, manifolds); + compute(leaf1, fresh, manifolds.last_mut().unwrap()); + } + } + } + #[cfg(not(feature = "parallel"))] + for leaf1 in composite1.bvh().intersect_aabb(&ls_aabb2_1) { + let fresh = bookkeep(leaf1, manifolds); + compute(leaf1, fresh, manifolds.last_mut().unwrap()); } workspace diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs index 01749f43..3e1a961c 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs @@ -81,6 +81,7 @@ pub fn contact_manifolds_heightfield_composite_shape( let bvh2 = composite2.bvh(); let ls_aabb2_1 = bvh2.root_aabb().transform_by(pos12).loosened(prediction); let mut old_manifolds = core::mem::take(manifolds); + let deformable = composite2.is_deformable(); heightfield1.map_elements_in_local_aabb(&ls_aabb2_1, &mut |leaf1, part1| { #[cfg(feature = "dim2")] @@ -124,6 +125,9 @@ pub fn contact_manifolds_heightfield_composite_shape( }; let manifold = &mut manifolds[sub_detector.manifold_id]; + if deformable { + manifold.mark_shapes_deformed(); + } #[cfg(feature = "dim2")] let triangle_normals = None::<()>; diff --git a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs index 1217fbae..902de962 100644 --- a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs @@ -5,11 +5,13 @@ use crate::math::{Pose, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; -use crate::query::contact_manifolds::ContactManifoldsWorkspace; +use crate::query::contact_manifolds::{ + contact_manifolds_composite_shape_shape, ContactManifoldsWorkspace, +}; use crate::query::details::NormalConstraints; use crate::query::query_dispatcher::PersistentQueryDispatcher; use crate::query::ContactManifold; -use crate::shape::{Shape, TriMesh}; +use crate::shape::{CompositeShape, Shape, TriMesh, TriMeshFlags}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( @@ -49,8 +51,8 @@ pub fn contact_manifolds_trimesh_shape_shapes( manifolds: &mut Vec>, workspace: &mut Option, ) where - ManifoldData: Default, - ContactData: Default + Copy, + ManifoldData: Default + Clone + Send + Sync, + ContactData: Default + Copy + Send + Sync, { if let Some(trimesh1) = shape1.as_trimesh() { contact_manifolds_trimesh_shape( @@ -85,6 +87,10 @@ fn ensure_workspace_exists(workspace: &mut Option) { } /// Computes the contact manifold between a triangle-mesh and a shape. +/// +/// A mesh flagged [`TriMeshFlags::DEFORMABLE`] goes through +/// [`contact_manifolds_composite_shape_shape`](fn@contact_manifolds_composite_shape_shape) instead: +/// the interfering-triangle cache kept in this function's workspace assumes a rigid mesh. pub fn contact_manifolds_trimesh_shape( dispatcher: &dyn PersistentQueryDispatcher, pos12: &Pose, @@ -95,9 +101,22 @@ pub fn contact_manifolds_trimesh_shape( workspace: &mut Option, flipped: bool, ) where - ManifoldData: Default, - ContactData: Default + Copy, + ManifoldData: Default + Clone + Send + Sync, + ContactData: Default + Copy + Send + Sync, { + if trimesh1.flags().contains(TriMeshFlags::DEFORMABLE) { + return contact_manifolds_composite_shape_shape( + dispatcher, + pos12, + trimesh1 as &(dyn CompositeShape + Sync), + shape2, + prediction, + manifolds, + workspace, + flipped, + ); + } + ensure_workspace_exists(workspace); let workspace: &mut TriMeshShapeContactManifoldsWorkspace = workspace.as_mut().unwrap().0.downcast_mut().unwrap(); diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs index af948a09..157da874 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs @@ -77,6 +77,7 @@ pub fn contact_manifolds_voxels_composite_shape( // TODO: avoid reallocating the new `manifolds` vec at each step. let mut old_manifolds = core::mem::take(manifolds); let bvh2 = shape2.bvh(); + let deformable = shape2.is_deformable(); let radius1 = voxels1.voxel_size() / 2.0; @@ -160,6 +161,10 @@ pub fn contact_manifolds_voxels_composite_shape( let relative_pos12 = part_pos2.prepend_to(&canonical_pose12); if !manifold_updated { + if deformable { + manifold.mark_shapes_deformed(); + } + // If we already computed contacts in the previous simulation step, their // local points are relative to the previously calculated canonical shape // which might not have the same local center as the one computed in this diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 675c337a..2984bc7b 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -614,8 +614,8 @@ impl QueryDispatcher for DefaultQueryDispatcher { impl PersistentQueryDispatcher for DefaultQueryDispatcher where - ManifoldData: Default + Clone, - ContactData: Default + Copy, + ManifoldData: Default + Clone + Send + Sync, + ContactData: Default + Copy + Send + Sync, { fn contact_manifolds( &self, diff --git a/src/shape/composite_shape.rs b/src/shape/composite_shape.rs index 25f3002d..be3c70ac 100644 --- a/src/shape/composite_shape.rs +++ b/src/shape/composite_shape.rs @@ -32,6 +32,18 @@ pub trait CompositeShape { /// Gets the acceleration structure of the composite shape. fn bvh(&self) -> &Bvh; + + /// Whether the parts of this shape may change between two queries made at the same pose (a + /// deformable mesh whose vertices move). + /// + /// The contact-manifold queries then never reuse the contact points cached from a previous + /// call (see [`ContactManifold::mark_shapes_deformed`]); the caller must still make sure the + /// [`Self::bvh`] is up to date. Defaults to `false`. + /// + /// [`ContactManifold::mark_shapes_deformed`]: crate::query::ContactManifold::mark_shapes_deformed + fn is_deformable(&self) -> bool { + false + } } #[cfg(feature = "alloc")] @@ -55,36 +67,49 @@ pub trait TypedCompositeShape: CompositeShape { ) -> Option; } -#[cfg(feature = "alloc")] -impl TypedCompositeShape for dyn CompositeShape + '_ { - type PartShape = dyn Shape; - type PartNormalConstraints = dyn NormalConstraints; +/// The [`TypedCompositeShape`] view of a [`CompositeShape`] trait object (with or without the +/// `Sync` marker: distinct trait-object types). +macro_rules! impl_typed_composite_shape_for_dyn { + ($($object:tt)+) => { + #[cfg(feature = "alloc")] + impl TypedCompositeShape for $($object)+ { + type PartShape = dyn Shape; + type PartNormalConstraints = dyn NormalConstraints; - fn map_typed_part_at( - &self, - shape_id: u32, - mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, - ) -> Option { - let mut result = None; - self.map_part_at(shape_id, &mut |pose, part, normals| { - result = Some(f(pose, part, normals)); - }); - result - } + fn map_typed_part_at( + &self, + shape_id: u32, + mut f: impl FnMut( + Option<&Pose>, + &Self::PartShape, + Option<&Self::PartNormalConstraints>, + ) -> T, + ) -> Option { + let mut result = None; + self.map_part_at(shape_id, &mut |pose, part, normals| { + result = Some(f(pose, part, normals)); + }); + result + } - fn map_untyped_part_at( - &self, - shape_id: u32, - mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, - ) -> Option { - let mut result = None; - self.map_part_at(shape_id, &mut |pose, part, normals| { - result = Some(f(pose, part, normals)); - }); - result - } + fn map_untyped_part_at( + &self, + shape_id: u32, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + ) -> Option { + let mut result = None; + self.map_part_at(shape_id, &mut |pose, part, normals| { + result = Some(f(pose, part, normals)); + }); + result + } + } + }; } +impl_typed_composite_shape_for_dyn!(dyn CompositeShape + '_); +impl_typed_composite_shape_for_dyn!(dyn CompositeShape + Sync + '_); + /// A helper struct that implements scene queries on any composite shapes. /// /// For example, the `RayCast` implementation of a composite shape can use this wrapper or diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 1d1e59a5..8b19a5d4 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -496,6 +496,7 @@ impl HeightField { // TODO: the normals are given in no particular order. So they are **not** // guaranteed to be provided in the same order as the triangle’s edge. edges: [pseudo_normal1, pseudo_normal2, pseudo_normal3], + two_sided: false, }) } else { None diff --git a/src/shape/mod.rs b/src/shape/mod.rs index e3b0c2d6..00ad6aec 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -25,8 +25,7 @@ pub use self::{ voxels::{AxisMask, OctantPattern, VoxelData, VoxelState, VoxelType, Voxels, VoxelsChunkRef}, }; -// `PolylineFlags` is a 2D-only feature. -#[cfg(all(feature = "dim2", feature = "alloc"))] +#[cfg(feature = "alloc")] pub use self::polyline::PolylineFlags; #[cfg(feature = "dim2")] diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index 15a23a4e..a0cec209 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -12,7 +12,6 @@ use alloc::vec::Vec; use crate::query::details::NormalConstraints; -#[cfg(feature = "dim2")] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", @@ -23,7 +22,6 @@ use crate::query::details::NormalConstraints; /// Controls how a [`Polyline`] is loaded. pub struct PolylineFlags(u8); -#[cfg(feature = "dim2")] bitflags::bitflags! { impl PolylineFlags: u8 { /// If set, the polyline is treated as one-sided: a pseudo-normal is computed at every @@ -33,7 +31,14 @@ bitflags::bitflags! { /// a double-sided polyline. This one flag covers what `TriMesh` splits across /// `TriMeshFlags::ORIENTED` (compute pseudo-normals) and `TriMeshFlags::FIX_INTERNAL_EDGES` /// (use them to clamp contacts). + #[cfg(feature = "dim2")] const ORIENTED = 1; + /// If set, the vertices of this polyline are expected to move between queries while its + /// pose stays fixed (see [`Polyline::set_vertices`]). + /// + /// The contact-manifold queries then never reuse cached contact points (see + /// [`CompositeShape::is_deformable`]). + const DEFORMABLE = 1 << 1; } } @@ -130,7 +135,6 @@ pub struct Polyline { /// normals are then clamped to one side so the polyline acts as a one-sided surface. #[cfg(feature = "dim2")] pseudo_normals: Option>, - #[cfg(feature = "dim2")] flags: PolylineFlags, } @@ -218,13 +222,12 @@ impl Polyline { indices, #[cfg(feature = "dim2")] pseudo_normals: None, - #[cfg(feature = "dim2")] flags: PolylineFlags::empty(), } } /// Creates a new polyline with the given [`PolylineFlags`] controlling its optional associated - /// data, e.g. orientation via [`PolylineFlags::ORIENTED`]. + /// data, e.g. orientation via `PolylineFlags::ORIENTED` (2D only). /// /// # Example /// @@ -248,7 +251,6 @@ impl Polyline { /// assert!(bottom.face.abs_diff_eq(Vector::new(0.0, -1.0), 1.0e-5)); /// # } /// ``` - #[cfg(feature = "dim2")] pub fn with_flags( vertices: Vec, indices: Option>, @@ -260,10 +262,10 @@ impl Polyline { } /// Sets the [`PolylineFlags`], computing or discarding the polyline's optional associated data. - #[cfg(feature = "dim2")] pub fn set_flags(&mut self, flags: PolylineFlags) { self.flags = flags; + #[cfg(feature = "dim2")] if flags.contains(PolylineFlags::ORIENTED) { self.compute_pseudo_normals(); } else { @@ -272,11 +274,65 @@ impl Polyline { } /// The [`PolylineFlags`] controlling this polyline's optional associated data. - #[cfg(feature = "dim2")] pub fn flags(&self) -> PolylineFlags { self.flags } + /// Replaces the vertex positions in place, keeping the index buffer. + /// + /// The BVH is refitted (not rebuilt) and the pseudo-normals, if any, are recomputed. This + /// is the update path of a deformable polyline: `vertices.len()` must equal + /// [`Self::vertices`]`.len()`. + /// + /// # Panics + /// + /// Panics if `vertices.len()` differs from the current number of vertices. + /// + /// # Example + /// + /// ``` + /// # #[cfg(all(feature = "dim2", feature = "f32"))] { + /// use parry2d::shape::{Polyline, PolylineFlags}; + /// use parry2d::math::Vector; + /// + /// let vertices = vec![Vector::ZERO, Vector::new(1.0, 0.0), Vector::new(2.0, 0.0)]; + /// let mut polyline = Polyline::with_flags(vertices, None, PolylineFlags::DEFORMABLE); + /// + /// // Lift the middle vertex. + /// polyline.set_vertices(&[Vector::ZERO, Vector::new(1.0, 1.0), Vector::new(2.0, 0.0)]); + /// assert_eq!(polyline.local_aabb().maxs, Vector::new(2.0, 1.0)); + /// # } + /// ``` + pub fn set_vertices(&mut self, vertices: &[Vector]) { + assert_eq!( + vertices.len(), + self.vertices.len(), + "Polyline::set_vertices: the number of vertices must not change." + ); + self.update_vertices(|vtx| vtx.copy_from_slice(vertices)); + } + + /// Modifies the vertex positions in place through `f`, then refits the BVH and recomputes + /// the pseudo-normals, if any (see [`Self::set_vertices`]). + pub fn update_vertices(&mut self, f: impl FnOnce(&mut [Vector])) { + f(&mut self.vertices); + + for (i, idx) in self.indices.iter().enumerate() { + let aabb = Segment::new( + self.vertices[idx[0] as usize], + self.vertices[idx[1] as usize], + ) + .local_aabb(); + let _ = self.bvh.insert_or_update_partially(aabb, i as u32, 0.0); + } + self.bvh.refit_without_opt(); + + #[cfg(feature = "dim2")] + if self.pseudo_normals.is_some() { + self.compute_pseudo_normals(); + } + } + /// Computes the outward pseudo-normal at every vertex (the normalized sum of its incident /// segments' outward normals) for the one-sided behavior of [`PolylineFlags::ORIENTED`]. #[cfg(feature = "dim2")] @@ -302,6 +358,15 @@ impl Polyline { self.pseudo_normals = Some(vertex_normals); } + /// The outward pseudo-normal of every vertex, if they have been computed (i.e. if this + /// polyline was built with [`PolylineFlags::ORIENTED`]). + /// + /// The returned slice is indexed by vertex index, like [`Self::vertices`]. + #[cfg(feature = "dim2")] + pub fn pseudo_normals(&self) -> Option<&[Vector]> { + self.pseudo_normals.as_deref() + } + /// Returns the [`SegmentPseudoNormals`] for the segment with index `i`, or `None` unless this /// polyline was built with [`PolylineFlags::ORIENTED`]. /// @@ -764,6 +829,7 @@ impl Polyline { bvh, vertices: self.vertices, indices: self.indices, + flags: self.flags, } } } @@ -987,6 +1053,10 @@ impl CompositeShape for Polyline { fn bvh(&self) -> &Bvh { &self.bvh } + + fn is_deformable(&self) -> bool { + self.flags.contains(PolylineFlags::DEFORMABLE) + } } impl TypedCompositeShape for Polyline { diff --git a/src/shape/shape.rs b/src/shape/shape.rs index 0889f61c..45a48c38 100644 --- a/src/shape/shape.rs +++ b/src/shape/shape.rs @@ -404,7 +404,7 @@ pub trait Shape: RayCast + PointQuery + Any + Send + Sync { } #[cfg(feature = "alloc")] - fn as_composite_shape(&self) -> Option<&dyn CompositeShape> { + fn as_composite_shape(&self) -> Option<&(dyn CompositeShape + Sync)> { None } @@ -1054,8 +1054,8 @@ impl Shape for Compound { } #[cfg(feature = "alloc")] - fn as_composite_shape(&self) -> Option<&dyn CompositeShape> { - Some(self as &dyn CompositeShape) + fn as_composite_shape(&self) -> Option<&(dyn CompositeShape + Sync)> { + Some(self as &(dyn CompositeShape + Sync)) } } @@ -1104,8 +1104,8 @@ impl Shape for Polyline { } #[cfg(feature = "alloc")] - fn as_composite_shape(&self) -> Option<&dyn CompositeShape> { - Some(self as &dyn CompositeShape) + fn as_composite_shape(&self) -> Option<&(dyn CompositeShape + Sync)> { + Some(self as &(dyn CompositeShape + Sync)) } } @@ -1168,8 +1168,8 @@ impl Shape for TriMesh { } #[cfg(feature = "alloc")] - fn as_composite_shape(&self) -> Option<&dyn CompositeShape> { - Some(self as &dyn CompositeShape) + fn as_composite_shape(&self) -> Option<&(dyn CompositeShape + Sync)> { + Some(self as &(dyn CompositeShape + Sync)) } } diff --git a/src/shape/triangle_pseudo_normals.rs b/src/shape/triangle_pseudo_normals.rs index 09bd9569..a74f4fc7 100644 --- a/src/shape/triangle_pseudo_normals.rs +++ b/src/shape/triangle_pseudo_normals.rs @@ -25,6 +25,9 @@ pub struct TrianglePseudoNormals { // triangle_pseudo_normals code. /// The edges pseudo-normals, in no particular order. pub edges: [Vector; 3], + /// If `true`, a direction pointing to the back of the triangle is projected into the + /// mirrored cone (`-face`, `-edges`) and kept, instead of being discarded. + pub two_sided: bool, } #[cfg(feature = "alloc")] @@ -32,6 +35,21 @@ impl NormalConstraints for TrianglePseudoNormals { /// Projects the given direction to it is contained in the polygonal /// cone defined `self`. fn project_local_normal_mut(&self, dir: &mut Vector) -> bool { + if self.two_sided && dir.dot(self.face) < 0.0 { + // The back cone is the front cone mirrored through the triangle's plane. + let mut mirrored = -*dir; + let _ = self.project_front(&mut mirrored); + *dir = -mirrored; + return true; + } + + self.project_front(dir) + } +} + +#[cfg(feature = "alloc")] +impl TrianglePseudoNormals { + fn project_front(&self, dir: &mut Vector) -> bool { // Find the closest pseudo-normal. let dots = Vector3::new( dir.dot(self.edges[0]), @@ -63,6 +81,7 @@ mod test { let pn = TrianglePseudoNormals { face: Vector::Y, edges: [Vector::Y; 3], + two_sided: false, }; assert_eq!( @@ -89,6 +108,7 @@ mod test { let pn = TrianglePseudoNormals { face: Vector::Y, edges: cones_axes.map(|v| v.normalize()), + two_sided: false, }; for i in 0..3 { @@ -147,6 +167,7 @@ mod test { let pn = TrianglePseudoNormals { face: Vector::Y, edges: cones_axes.map(|v| v.normalize()), + two_sided: false, }; for i in 0..3 { @@ -172,4 +193,47 @@ mod test { } } } + + #[test] + fn two_sided_pseudo_normals_mirror_the_cone() { + let one_sided = TrianglePseudoNormals { + face: Vector::Y, + edges: [Vector::Y; 3], + two_sided: false, + }; + let two_sided = TrianglePseudoNormals { + two_sided: true, + ..one_sided.clone() + }; + + // Front directions behave the same. + let front = Vector::new(1.0, 1.0, 1.0); + assert_eq!(two_sided.project_local_normal(front), Some(Vector::Y)); + + // Back directions are kept and constrained to the mirrored cone. + let back = Vector::new(1.0, -1.0, 1.0); + assert!(one_sided.project_local_normal(back).is_none()); + assert_eq!(two_sided.project_local_normal(back), Some(-Vector::Y)); + + // The mirrored projection is the negation of the front projection. + let cone_axes = [ + bisector_y(-Vector::Z), + bisector_y(-Vector::X), + bisector_y(Vector::new(1.0, 0.0, 1.0).normalize()), + ]; + let pn = TrianglePseudoNormals { + face: Vector::Y, + edges: cone_axes, + two_sided: true, + }; + for dir in [ + Vector::new(0.3, 1.0, -0.8).normalize(), + Vector::new(-1.0, 0.2, 0.1).normalize(), + Vector::new(0.9, 0.5, 0.9).normalize(), + ] { + let front = pn.project_local_normal(dir).unwrap(); + let back = pn.project_local_normal(-dir).unwrap(); + assert!(back.abs_diff_eq(-front, 1.0e-6)); + } + } } diff --git a/src/shape/trimesh.rs b/src/shape/trimesh.rs index 5962cf22..82152146 100644 --- a/src/shape/trimesh.rs +++ b/src/shape/trimesh.rs @@ -499,6 +499,17 @@ bitflags::bitflags! { /// This is achieved by taking into account adjacent triangle normals when computing contact /// points for a given triangle. const FIX_INTERNAL_EDGES = (1 << 7) | Self::MERGE_DUPLICATE_VERTICES.bits(); + /// If set, the vertices of this mesh are expected to move between queries while its pose + /// stays fixed (see [`TriMesh::set_vertices`]). + /// + /// The contact-manifold queries then recompute the triangles interfering with the other + /// shape at every call and never reuse cached contact points (see + /// [`CompositeShape::is_deformable`]). + const DEFORMABLE = 1 << 8; + /// Same as [`Self::FIX_INTERNAL_EDGES`] but treating the mesh as two-sided: a contact + /// coming from the back of a triangle is kept and its normal is constrained by the + /// mirrored pseudo-normal cone instead of being discarded. + const FIX_INTERNAL_EDGES_TWO_SIDED = (1 << 9) | Self::FIX_INTERNAL_EDGES.bits(); } } @@ -928,6 +939,78 @@ impl TriMesh { } } + /// Replaces the vertex positions in place, keeping the index buffer and topology. + /// + /// The BVH is refitted (not rebuilt) and the pseudo-normals, if any, are recomputed. This + /// is the update path of a deformable mesh: `vertices.len()` must equal + /// [`Self::vertices`]`.len()`. + /// + /// The refit keeps the tree structure built for the original vertices; after very large + /// deformations, rebuilding the mesh gives a tighter tree. + /// + /// # Panics + /// + /// Panics if `vertices.len()` differs from the current number of vertices. + /// + /// # Example + /// + /// ``` + /// # #[cfg(all(feature = "dim3", feature = "f32"))] { + /// use parry3d::shape::{TriMesh, TriMeshFlags}; + /// use parry3d::math::Vector; + /// + /// let vertices = vec![ + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// ]; + /// let indices = vec![[0, 1, 2]]; + /// let mut mesh = TriMesh::with_flags(vertices, indices, TriMeshFlags::DEFORMABLE).unwrap(); + /// + /// // Lift the third vertex. + /// mesh.set_vertices(&[ + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 2.0), + /// ]); + /// assert_eq!(mesh.local_aabb().maxs, Vector::new(1.0, 1.0, 2.0)); + /// # } + /// ``` + pub fn set_vertices(&mut self, vertices: &[Vector]) { + assert_eq!( + vertices.len(), + self.vertices.len(), + "TriMesh::set_vertices: the number of vertices must not change." + ); + self.update_vertices(|vtx| vtx.copy_from_slice(vertices)); + } + + /// Modifies the vertex positions in place through `f`, then refits the BVH and recomputes + /// the pseudo-normals, if any (see [`Self::set_vertices`]). + pub fn update_vertices(&mut self, f: impl FnOnce(&mut [Vector])) { + f(&mut self.vertices); + self.refit_bvh(); + + #[cfg(feature = "dim3")] + if self.pseudo_normals.is_some() { + self.compute_pseudo_normals(); + } + } + + /// Updates every triangle's leaf AABB and refits the BVH in place, keeping its structure. + fn refit_bvh(&mut self) { + for (i, idx) in self.indices.iter().enumerate() { + let aabb = Triangle::new( + self.vertices[idx[0] as usize], + self.vertices[idx[1] as usize], + self.vertices[idx[2] as usize], + ) + .local_aabb(); + let _ = self.bvh.insert_or_update_partially(aabb, i as u32, 0.0); + } + self.bvh.refit_without_opt(); + } + /// Returns a scaled version of this triangle mesh. /// /// Creates a new mesh with all vertices scaled by the given per-axis scale factors. @@ -1921,6 +2004,9 @@ impl TriMesh { (edges_pseudo_normals[1]).try_normalize()?, (edges_pseudo_normals[2]).try_normalize()?, ], + two_sided: self + .flags + .contains(TriMeshFlags::FIX_INTERNAL_EDGES_TWO_SIDED), }) } else { None @@ -2135,6 +2221,10 @@ impl CompositeShape for TriMesh { fn bvh(&self) -> &Bvh { &self.bvh } + + fn is_deformable(&self) -> bool { + self.flags.contains(TriMeshFlags::DEFORMABLE) + } } impl TypedCompositeShape for TriMesh { diff --git a/src/transformation/mod.rs b/src/transformation/mod.rs index 8d02d726..cae58a18 100644 --- a/src/transformation/mod.rs +++ b/src/transformation/mod.rs @@ -13,6 +13,11 @@ pub use self::mesh_intersection::{ intersect_meshes, intersect_meshes_with_tolerances, MeshIntersectionError, MeshIntersectionTolerances, }; +#[cfg(all(feature = "dim3", feature = "alloc"))] +pub use self::volume_mesh::MeshEnclosure; +#[cfg(all(feature = "alloc", any(feature = "dim3", feature = "spade")))] +pub use self::volume_mesh::{volume_mesh, VolumeMesh, VolumeMeshParameters}; + pub use self::polygon_intersection::{ convex_polygons_intersection, convex_polygons_intersection_points, convex_polygons_intersection_points_with_tolerances, @@ -29,6 +34,8 @@ pub(crate) mod convex_hull_utils; mod polygon_intersection; /// Approximate convex decomposition using the VHACD algorithm. pub mod vhacd; +#[cfg(all(feature = "alloc", any(feature = "dim3", feature = "spade")))] +mod volume_mesh; /// Voxelization of a 2D polyline or 3D triangle mesh. pub mod voxelization; diff --git a/src/transformation/volume_mesh/cover_octree.rs b/src/transformation/volume_mesh/cover_octree.rs new file mode 100644 index 00000000..20f6258c --- /dev/null +++ b/src/transformation/volume_mesh/cover_octree.rs @@ -0,0 +1,422 @@ +//! The cover's subdivided background grid: an octree refined below the cell size where octants +//! cross the shape's boundary, turned into a conforming grid of cells (no hanging vertex) by the +//! balance rule and the transition cells. + +use super::isosurface_stuffing::{par_map, BackgroundGrid, MeshOracle}; +use super::VolumeMeshParameters; +use crate::bounding_volume::Aabb; +#[cfg(not(feature = "std"))] +use crate::math::ComplexField; +use crate::math::{Real, Vector}; +use crate::utils::hashmap::HashMap; +use alloc::vec::Vec; + +/// The eight corners of an octant, as offsets in units of its width. +const CORNERS: [[i32; 3]; 8] = [ + [0, 0, 0], + [1, 0, 0], + [0, 1, 0], + [1, 1, 0], + [0, 0, 1], + [1, 0, 1], + [0, 1, 1], + [1, 1, 1], +]; + +/// The octants the boundary passes through, refined down to the finest level, and the coarser ones +/// filling the rest. +/// +/// Octant coordinates are indices at their own level; vertex coordinates are integers in units of +/// half the finest octant's width, which makes every corner, center, face center and edge midpoint +/// of every level integral. +struct Octree { + origin: Vector, + /// Half the width of a finest octant, the unit of the vertex coordinates. + half: Real, + /// The level of the coarsest octants: they are `1 << levels` finest octants wide. + levels: u32, + /// The number of coarsest octants along each axis. + dims: [i32; 3], + /// The octants that have children. + subdivided: HashMap<(u32, [i32; 3]), ()>, +} + +impl Octree { + /// The width of an octant, in vertex coordinates: a finest octant is two of them wide. + fn width(&self, level: u32) -> i32 { + 2 << level + } + + /// The number of octants of a level along each axis. + fn count(&self, level: u32) -> [i32; 3] { + core::array::from_fn(|k| self.dims[k] << (self.levels - level)) + } + + fn in_range(&self, level: u32, c: [i32; 3]) -> bool { + let count = self.count(level); + (0..3).all(|k| c[k] >= 0 && c[k] < count[k]) + } + + fn is_subdivided(&self, level: u32, c: [i32; 3]) -> bool { + self.subdivided.contains_key(&(level, c)) + } + + /// A corner of an octant, in vertex coordinates. + fn corner(&self, level: u32, c: [i32; 3], offset: [i32; 3]) -> [i32; 3] { + let width = self.width(level); + core::array::from_fn(|k| (c[k] + offset[k]) * width) + } + + /// The center of an octant, in vertex coordinates. + fn center(&self, level: u32, c: [i32; 3]) -> [i32; 3] { + let width = self.width(level); + core::array::from_fn(|k| c[k] * width + width / 2) + } + + fn point(&self, v: [i32; 3]) -> Vector { + self.origin + Vector::new(v[0] as Real, v[1] as Real, v[2] as Real) * self.half + } + + /// The leaf covering a given octant: itself when it is one, an ancestor when it sits inside a + /// coarser leaf, `None` when it is subdivided (several leaves cover it) or out of range. + fn covering_leaf(&self, level: u32, c: [i32; 3]) -> Option<(u32, [i32; 3])> { + if !self.in_range(level, c) { + return None; + } + + let mut current = self.levels; + loop { + let shifted: [i32; 3] = core::array::from_fn(|k| c[k] >> (current - level)); + if !self.is_subdivided(current, shifted) { + return Some((current, shifted)); + } + if current == level { + return None; + } + current -= 1; + } + } + + /// Gives an octant children, so that its own children become leaves. + fn split(&mut self, level: u32, c: [i32; 3]) { + debug_assert!(level > 0, "a finest octant has no children"); + let _ = self.subdivided.insert((level, c), ()); + } + + /// Every leaf of the octree. + fn leaves(&self) -> Vec<(u32, [i32; 3])> { + let mut leaves = Vec::new(); + let mut stack: Vec<(u32, [i32; 3])> = Vec::new(); + let roots = self.count(self.levels); + + for k in 0..roots[2] { + for j in 0..roots[1] { + for i in 0..roots[0] { + stack.push((self.levels, [i, j, k])); + } + } + } + + while let Some((level, c)) = stack.pop() { + if level > 0 && self.is_subdivided(level, c) { + for offset in CORNERS { + stack.push((level - 1, core::array::from_fn(|k| c[k] * 2 + offset[k]))); + } + } else { + leaves.push((level, c)); + } + } + + leaves + } + + /// Whether an octant finer than `level` puts a vertex at the given coordinates. + fn has_finer_vertex(&self, level: u32, v: [i32; 3]) -> bool { + if level == 0 { + return false; + } + + let finer = level - 1; + let width = self.width(finer); + if (0..3).any(|k| v[k].rem_euclid(width) != 0) { + return false; + } + + let base: [i32; 3] = core::array::from_fn(|k| v[k] / width); + CORNERS.iter().any(|offset| { + let c: [i32; 3] = core::array::from_fn(|k| base[k] - offset[k]); + matches!(self.covering_leaf(finer, c), Some((leaf, _)) if leaf <= finer) + }) + } +} + +/// The center of a face of an octant, in vertex coordinates. +fn face_center(octree: &Octree, level: u32, c: [i32; 3], axis: usize, positive: bool) -> [i32; 3] { + let corners = face_corners(axis, positive).map(|offset| octree.corner(level, c, offset)); + core::array::from_fn(|k| (corners[0][k] + corners[2][k]) / 2) +} + +/// The four corners of a face of an octant, walking around it, as offsets in units of its width. +fn face_corners(axis: usize, positive: bool) -> [[i32; 3]; 4] { + let (u, v) = ((axis + 1) % 3, (axis + 2) % 3); + let base = i32::from(positive); + [[0, 0], [1, 0], [1, 1], [0, 1]].map(|[du, dv]| { + let mut corner = [0; 3]; + corner[axis] = base; + corner[u] = du; + corner[v] = dv; + corner + }) +} + +/// The background grid of a cover ([`MeshEnclosure::Cover`](super::MeshEnclosure::Cover)): an +/// octant that may cross the shape's boundary is refined +/// [`VolumeMeshParameters::cover_subdivisions`] halvings below the cell size and one that cannot +/// is not; balancing and transition cells keep the grid conforming. +pub(super) fn cover_octree_grid( + oracle: &MeshOracle, + aabb: Aabb, + params: &VolumeMeshParameters, +) -> Option { + let cell_size = params.cell_size; + let fine = cell_size / (1 << params.cover_subdivisions) as Real; + let levels = params.cover_subdivisions; + + // The domain, in cell-sized octants, with margin so the boundary never reaches its + // border. + let coarse_width = cell_size; + let margin = cell_size * 2.0; + let origin = aabb.mins - Vector::splat(margin); + let dims: [i32; 3] = core::array::from_fn(|k| { + let extent = aabb.maxs[k] - aabb.mins[k] + margin * 2.0; + ((extent / coarse_width).ceil() as i32).max(1) + }); + + let mut octree = Octree { + origin, + half: fine * 0.5, + levels, + dims, + subdivided: HashMap::default(), + }; + + let roots = octree.count(levels); + let mut frontier: Vec<[i32; 3]> = Vec::new(); + for k in 0..roots[2] { + for j in 0..roots[1] { + for i in 0..roots[0] { + frontier.push([i, j, k]); + } + } + } + + // Level-synchronous refinement: every octant of a level tests independently whether the + // boundary reaches into it (an exact, early-exit BVH existence query), the splits are applied + // in one pass, and the children become the next level's frontier. + for level in (1..=levels).rev() { + let decisions: Vec = par_map(&frontier, |c| { + let octant = Aabb::new( + octree.point(octree.corner(level, *c, [0, 0, 0])), + octree.point(octree.corner(level, *c, [1, 1, 1])), + ); + oracle.crosses_region(&octant) + }); + + let mut next = Vec::new(); + for (c, split) in frontier.iter().zip(&decisions) { + if *split { + octree.split(level, *c); + for offset in CORNERS { + next.push(core::array::from_fn(|k| c[k] * 2 + offset[k])); + } + } + } + frontier = next; + if frontier.is_empty() { + break; + } + } + + balance(&mut octree); + Some(background_grid(&octree)) +} + +/// The Weak Balance Condition: two octants sharing so much as an edge may not differ by more than +/// one level, or there is no bridging them. +fn balance(octree: &mut Octree) { + loop { + let leaves = octree.leaves(); + + // Each round decides every leaf against the same snapshot of the octree, in + // parallel; balancing is monotone (splitting only ever demands more splits), so + // the rounds converge on the same closure the one-by-one sweep would. + let decisions: Vec = par_map(&leaves, |&(level, c)| { + if level == 0 { + return false; + } + + // Every finest octant one step outside this leaf must be covered by a leaf no more + // than one level coarser than it. Only the shell around the leaf is walked: the + // octants inside it are its own. + let span = 1 << level; + let mut split = false; + + 'shell: for axis in 0..3 { + for side in [-1, span] { + for a in -1..=span { + for b in -1..=span { + let mut offset = [0; 3]; + offset[axis] = side; + offset[(axis + 1) % 3] = a; + offset[(axis + 2) % 3] = b; + let neighbor: [i32; 3] = + core::array::from_fn(|k| c[k] * span + offset[k]); + + let Some((neighbor_level, _)) = octree.covering_leaf(0, neighbor) + else { + continue; + }; + // A neighbor two or more levels finer: this leaf is the one that has + // to give, or nothing can bridge them. + if neighbor_level + 1 < level { + split = true; + break 'shell; + } + } + } + } + } + + split + }); + + let mut changed = false; + for (&(level, c), split) in leaves.iter().zip(&decisions) { + if *split { + octree.split(level, c); + changed = true; + } + } + + if !changed { + break; + } + } +} + +/// The background grid of a balanced octree: figure 10 of the paper. +/// +/// A cell spanning two octants would be created from both of them; the same-size case, the only +/// symmetric one, is built from the positive side alone. +fn background_grid(octree: &Octree) -> BackgroundGrid { + // Every leaf emits its cells independently, as vertex coordinates: the octree is only + // read, so the leaves are one parallel pass, and the coordinates are deduplicated into + // ids afterward. + let leaves = octree.leaves(); + let per_leaf: Vec> = + par_map(&leaves, |&(level, c)| leaf_cells(octree, level, c)); + let coord_cells: Vec<[[i32; 3]; 4]> = per_leaf.into_iter().flatten().collect(); + + let mut coords: Vec<[i32; 3]> = coord_cells.iter().flatten().copied().collect(); + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + coords.par_sort_unstable(); + } + #[cfg(not(feature = "parallel"))] + coords.sort_unstable(); + coords.dedup(); + + let mut ids: HashMap<[i32; 3], u32> = HashMap::default(); + for (id, v) in coords.iter().enumerate() { + let _ = ids.insert(*v, id as u32); + } + + let points = par_map(&coords, |v| octree.point(*v)); + let cells = par_map(&coord_cells, |quad| quad.map(|v| ids[&v])); + + BackgroundGrid { points, cells } +} + +/// The cells of one leaf, as vertex coordinates: figure 10 of Labelle and Shewchuk. +/// +/// A cell spanning two octants would be created from both of them; the same-size case, the +/// only symmetric one, is built from the positive side alone. +fn leaf_cells(octree: &Octree, level: u32, c: [i32; 3]) -> Vec<[[i32; 3]; 4]> { + let mut cells = Vec::new(); + let center = octree.center(level, c); + + for axis in 0..3 { + for positive in [false, true] { + let face: [[i32; 3]; 4] = + face_corners(axis, positive).map(|offset| octree.corner(level, c, offset)); + + let mut neighbor = c; + neighbor[axis] += if positive { 1 } else { -1 }; + + if octree.in_range(level, neighbor) && octree.is_subdivided(level, neighbor) { + // Finer octants across the face put a vertex at its center: quadrisected cells. + let middle: [i32; 3] = core::array::from_fn(|k| (face[0][k] + face[2][k]) / 2); + + for e in 0..4 { + let (a, b) = (face[e], face[(e + 1) % 4]); + let midpoint: [i32; 3] = core::array::from_fn(|k| (a[k] + b[k]) / 2); + + if octree.has_finer_vertex(level, midpoint) { + cells.push([middle, center, a, midpoint]); + cells.push([middle, center, midpoint, b]); + } else { + cells.push([middle, center, a, b]); + } + } + continue; + } + + match octree.covering_leaf(level, neighbor) { + Some((neighbor_level, neighbor_c)) if neighbor_level == level => { + // The same size: the lattice cells, built once per shared face. + if !positive { + continue; + } + let opposite = octree.center(neighbor_level, neighbor_c); + + for e in 0..4 { + let (a, b) = (face[e], face[(e + 1) % 4]); + let midpoint: [i32; 3] = core::array::from_fn(|k| (a[k] + b[k]) / 2); + + if octree.has_finer_vertex(level, midpoint) { + // A finer octant split this edge: two bisected cells, which the + // boundary never crosses. + cells.push([center, opposite, a, midpoint]); + cells.push([center, opposite, midpoint, b]); + } else { + cells.push([center, opposite, a, b]); + } + } + } + // A coarser neighbor, or the domain's border: two half-pyramids over the face, + // whose diagonal runs through the coarse face's center so that its two triangles + // are among the eight the coarse side fans out from that center. + other => { + let diagonal = other + .and_then(|(neighbor_level, neighbor_c)| { + let middle = + face_center(octree, neighbor_level, neighbor_c, axis, !positive); + face.iter().position(|v| *v == middle) + }) + .unwrap_or(0); + let (a, b, cc, d) = ( + face[diagonal], + face[(diagonal + 1) % 4], + face[(diagonal + 2) % 4], + face[(diagonal + 3) % 4], + ); + cells.push([center, a, b, cc]); + cells.push([center, a, cc, d]); + } + } + } + } + + cells +} diff --git a/src/transformation/volume_mesh/cover_smoothing.rs b/src/transformation/volume_mesh/cover_smoothing.rs new file mode 100644 index 00000000..6a6fb071 --- /dev/null +++ b/src/transformation/volume_mesh/cover_smoothing.rs @@ -0,0 +1,354 @@ +//! Shrink-wrap of a [`super::MeshEnclosure::Cover`] mesh: flattens its staircase while keeping its +//! containment guarantee, a boundary vertex moving only if its cells keep a volume floor and its +//! boundary faces still do not cross the shape. Vertices relax color by color in a fixed order. + +use super::isosurface_stuffing::{par_map, MeshOracle}; +use super::{VolumeMesh, VolumeMeshParameters}; +use crate::bounding_volume::{Aabb, BoundingVolume}; +use crate::math::{Real, Vector}; +use alloc::vec::Vec; + +/// How much of its lattice volume a cell must keep for a move to commit. +const VOLUME_FLOOR: Real = 0.2; + +/// How far a boundary vertex may travel from where the lattice put it, in local cell +/// sizes. The wrap's targets all lie within a cell, so this hardly ever binds; what it +/// buys is a bound that makes each vertex's neighborhood of the shape collectable once. +const TRAVEL_BUDGET: Real = 1.0; + +/// The half-extent of the region a vertex's shape primitives are collected in, in local +/// cell sizes: the incident faces' own extent, plus every incident vertex's travel budget, +/// with margin. +const REGION: Real = 2.5; + +/// The committed step, as a fraction of the guard, under which a vertex goes to sleep. +const SLEEP: Real = 0.05; + +/// The four faces of a tetrahedron, as index triples into the cell. +const FACES: [[usize; 3]; 4] = [[0, 1, 2], [0, 1, 3], [0, 2, 3], [1, 2, 3]]; + +fn cell_volume(positions: &[Vector], cell: [u32; 4]) -> Real { + let [a, b, c, d] = cell.map(|i| positions[i as usize]); + (b - a).cross(c - a).dot(d - a) / 6.0 +} + +/// The cell's volume, with vertex `v` read at `at` instead of its stored position. +fn cell_volume_at(positions: &[Vector], cell: [u32; 4], v: u32, at: Vector) -> Real { + let p = cell.map(|i| if i == v { at } else { positions[i as usize] }); + (p[1] - p[0]).cross(p[2] - p[0]).dot(p[3] - p[0]) / 6.0 +} + +/// The face's points, with vertex `v` read at `at` instead of its stored position. +fn face_at(positions: &[Vector], face: [u32; 3], v: u32, at: Vector) -> [Vector; 3] { + face.map(|i| if i == v { at } else { positions[i as usize] }) +} + +pub(super) fn smooth_cover( + mesh: &mut VolumeMesh, + oracle: &MeshOracle, + params: &VolumeMeshParameters, +) { + if params.cover_smoothing == 0 || params.cover_guard <= 0.0 || mesh.cells.is_empty() { + return; + } + + /* + * The cover's boundary: the faces one cell shares with no other (found by sorting the + * face keys, which is parallel-friendly and gives a deterministic order), their + * vertices, and the adjacency the relaxation reads. + */ + let mut keys: Vec<[u32; 3]> = Vec::with_capacity(mesh.cells.len() * 4); + for cell in &mesh.cells { + for face in FACES { + let mut key = face.map(|k| cell[k]); + key.sort_unstable(); + keys.push(key); + } + } + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + keys.par_sort_unstable(); + } + #[cfg(not(feature = "parallel"))] + keys.sort_unstable(); + + let mut boundary_faces: Vec<[u32; 3]> = Vec::new(); + let mut at = 0; + while at < keys.len() { + let mut next = at + 1; + while next < keys.len() && keys[next] == keys[at] { + next += 1; + } + if next - at == 1 { + boundary_faces.push(keys[at]); + } + at = next; + } + + let n = mesh.vertices.len(); + let mut is_boundary = alloc::vec![false; n]; + let mut vertex_faces: Vec> = alloc::vec![Vec::new(); n]; + let mut neighbors: Vec> = alloc::vec![Vec::new(); n]; + + for (id, face) in boundary_faces.iter().enumerate() { + for k in 0..3 { + let v = face[k] as usize; + is_boundary[v] = true; + vertex_faces[v].push(id as u32); + for other in [face[(k + 1) % 3], face[(k + 2) % 3]] { + if !neighbors[v].contains(&other) { + neighbors[v].push(other); + } + } + } + } + + let mut vertex_cells: Vec> = alloc::vec![Vec::new(); n]; + for (id, cell) in mesh.cells.iter().enumerate() { + for v in cell { + vertex_cells[*v as usize].push(id as u32); + } + } + + /* + * What the pristine lattice grants each cell and vertex: the volume floor, and the + * guard and step cap scaled by the local cell size. + */ + let floors: Vec = mesh + .cells + .iter() + .map(|cell| cell_volume(&mesh.vertices, *cell) * VOLUME_FLOOR) + .collect(); + let scales: Vec = (0..n) + .map(|v| { + if !is_boundary[v] { + return 0.0; + } + vertex_cells[v] + .iter() + .flat_map(|c| { + let pts = mesh.cells[*c as usize].map(|i| mesh.vertices[i as usize]); + (0..4).flat_map(move |i| (i + 1..4).map(move |j| (pts[i] - pts[j]).length())) + }) + .fold(0.0, Real::max) + }) + .collect(); + let guards: Vec = scales.iter().map(|s| params.cover_guard * s).collect(); + + let boundary_vertices: Vec = (0..n as u32).filter(|v| is_boundary[*v as usize]).collect(); + + /* + * Each vertex's and face's neighborhood of the shape, collected once: the shape is static and + * vertex travel is bounded, so the per-move barrier and projection scan these short lists + * instead of the whole shape. + */ + let origins: Vec = mesh.vertices.clone(); + let neighborhoods: Vec> = par_map(&boundary_vertices, |&v| { + let vid = v as usize; + let region = Aabb::from_half_extents(origins[vid], Vector::splat(scales[vid] * REGION)); + let mut primitives = Vec::new(); + oracle.collect(®ion, &mut primitives); + primitives + }); + let neighborhood_of = { + let mut ids = alloc::vec![u32::MAX; n]; + for (k, v) in boundary_vertices.iter().enumerate() { + ids[*v as usize] = k as u32; + } + ids + }; + let face_lists: Vec> = par_map(&boundary_faces, |face| { + let sweep = face + .iter() + .map(|v| scales[*v as usize]) + .fold(0.0, Real::max) + * (TRAVEL_BUDGET * 1.1); + let region = Aabb::from_points(face.iter().map(|v| origins[*v as usize])).loosened(sweep); + let mut primitives = Vec::new(); + oracle.collect(®ion, &mut primitives); + primitives + }); + + /* + * Greedy coloring of boundary vertices, no two of a color sharing a cell: a vertex's target + * and validity read only its incident cells and faces, so each color is one parallel batch. + */ + let mut colors = alloc::vec![u32::MAX; n]; + let mut palette: Vec> = Vec::new(); + for &v in &boundary_vertices { + let mut used = alloc::vec![false; palette.len()]; + for &c in &vertex_cells[v as usize] { + for o in mesh.cells[c as usize] { + let color = colors[o as usize]; + if color != u32::MAX { + used[color as usize] = true; + } + } + } + let color = used.iter().position(|u| !*u).unwrap_or(palette.len()); + if color == palette.len() { + palette.push(Vec::new()); + } + colors[v as usize] = color as u32; + palette[color].push(v); + } + + /* + * The wrap: color by color, each vertex validates its move against the batch's starting state + * and proposals apply in order; the step is capped at the guard and tested at its midpoint, so + * no sweep exceeds half a guard between exact tests; idle vertices wake on a neighbor's move. + */ + let mut active = is_boundary.clone(); + + for _ in 0..params.cover_smoothing { + let mut moved_any = false; + + for group in &palette { + let members: Vec = group + .iter() + .copied() + .filter(|v| active[*v as usize]) + .collect(); + if members.is_empty() { + continue; + } + + let positions = &mesh.vertices; + let proposals: Vec<(Vector, Real)> = par_map(&members, |&v| { + let vid = v as usize; + let pos = positions[vid]; + let guard = guards[vid]; + let primitives = &neighborhoods[neighborhood_of[vid] as usize]; + if neighbors[vid].is_empty() || primitives.is_empty() { + return (pos, 0.0); + } + + let average = neighbors[vid] + .iter() + .map(|o| positions[*o as usize]) + .sum::() + / neighbors[vid].len() as Real; + let projection = oracle.project_among(pos, primitives).unwrap_or(pos); + let Some(outward) = (pos - projection).try_normalize() else { + return (pos, 0.0); + }; + let held_off = projection + outward * guard; + let target = (average + held_off) * 0.5; + + // The mid-step barrier test keeps the effective sweep at half of this, so + // a full-guard cap converges twice as fast at the same tunneling + // granularity. + let mut step = target - pos; + if step.length() > guard { + step = step.normalize() * guard; + } + // The travel budget: what makes the collected neighborhoods complete. + let budget = scales[vid] * TRAVEL_BUDGET; + let strayed = pos + step - origins[vid]; + if strayed.length() > budget { + step = origins[vid] + strayed.normalize() * budget - pos; + } + // A step already below the sleep threshold has nothing to buy: skip the + // barrier work it would cost. + if step.length() < guard * SLEEP { + return (pos, 0.0); + } + + let valid = |to: Vector| -> bool { + for &c in &vertex_cells[vid] { + if cell_volume_at(positions, mesh.cells[c as usize], v, to) + < floors[c as usize] + { + return false; + } + } + let middle = (pos + to) * 0.5; + for &f in &vertex_faces[vid] { + let face = boundary_faces[f as usize]; + let barrier = &face_lists[f as usize]; + if oracle.crosses_among(&face_at(positions, face, v, to), barrier) + || oracle.crosses_among(&face_at(positions, face, v, middle), barrier) + { + return false; + } + } + true + }; + + for factor in [1.0, 0.5, 0.25] { + let candidate = pos + step * factor; + if valid(candidate) { + return (candidate, step.length() * factor); + } + } + (pos, 0.0) + }); + + for (v, (to, committed)) in members.iter().zip(&proposals) { + let vid = *v as usize; + mesh.vertices[vid] = *to; + if *committed < guards[vid] * SLEEP { + active[vid] = false; + } else { + moved_any = true; + for o in &neighbors[vid] { + active[*o as usize] = true; + } + } + } + } + + if !moved_any { + break; + } + } + + /* + * A light interior relaxation: the ring of interior vertices next to the wrapped boundary + * absorbed the squeeze, so they are eased toward their neighbors' average; the volume floor + * still guards the cage's region against these moves. + */ + let ring: Vec = (0..n as u32) + .filter(|v| { + !is_boundary[*v as usize] + && vertex_cells[*v as usize].iter().any(|c| { + mesh.cells[*c as usize] + .iter() + .any(|o| is_boundary[*o as usize]) + }) + }) + .collect(); + + for _ in 0..2 { + for &v in &ring { + let vid = v as usize; + let mut sum = Vector::ZERO; + let mut count = 0; + for &c in &vertex_cells[vid] { + for o in mesh.cells[c as usize] { + if o != v { + sum += mesh.vertices[o as usize]; + count += 1; + } + } + } + if count == 0 { + continue; + } + + let pos = mesh.vertices[vid]; + let step = sum / count as Real - pos; + for factor in [0.5, 0.25] { + let candidate = pos + step * factor; + mesh.vertices[vid] = candidate; + if vertex_cells[vid].iter().all(|c| { + cell_volume(&mesh.vertices, mesh.cells[*c as usize]) >= floors[*c as usize] + }) { + break; + } + mesh.vertices[vid] = pos; + } + } + } +} diff --git a/src/transformation/volume_mesh/delaunay_refinement.rs b/src/transformation/volume_mesh/delaunay_refinement.rs new file mode 100644 index 00000000..a1bf6d84 --- /dev/null +++ b/src/transformation/volume_mesh/delaunay_refinement.rs @@ -0,0 +1,131 @@ +//! Triangulation of the interior of a closed polyline, by Delaunay refinement (Ruppert's and +//! Chew's algorithms, as implemented by `spade`). + +use super::{VolumeMesh, VolumeMeshParameters}; +use crate::math::{Real, Vector}; +use crate::utils::hashmap::{Entry, HashMap}; +use crate::utils::sanitize_spade_point; +use alloc::vec::Vec; +use spade::{ + AngleLimit, ConstrainedDelaunayTriangulation, Point2, RefinementParameters, Triangulation as _, +}; + +pub fn triangulate( + vertices: &[Vector], + indices: &[[u32; 2]], + params: &VolumeMeshParameters, +) -> Option { + let cell_size = params.cell_size; + + if vertices.is_empty() || indices.is_empty() || cell_size <= 0.0 || cell_size.is_nan() { + return None; + } + + /* + * The boundary, as constraint edges: only the vertices it references take part in the + * triangulation. + */ + let mut cdt: ConstrainedDelaunayTriangulation> = + ConstrainedDelaunayTriangulation::new(); + let mut handles = HashMap::default(); + + for edge in indices { + let mut endpoints = [None; 2]; + + for (k, vid) in edge.iter().enumerate() { + let pt = *vertices.get(*vid as usize)?; + let handle = match handles.entry(*vid) { + Entry::Occupied(entry) => *entry.get(), + Entry::Vacant(entry) => { + let pt = sanitize_spade_point(Point2::new(pt.x, pt.y)); + *entry.insert(cdt.insert(pt).ok()?) + } + }; + endpoints[k] = Some(handle); + } + + if let [Some(from), Some(to)] = endpoints { + if from != to { + // Intersecting constraint edges are left out instead of panicking; the boundary + // shouldn't self-intersect in the first place. + let _ = cdt.try_add_constraint(from, to); + } + } + } + + /* + * Refinement: split the triangles that are too large or too sharp, and the boundary edges that + * stand in the way, until every triangle inside the boundary is fit for simulation. + */ + #[cfg_attr(feature = "f64", expect(clippy::unnecessary_cast))] + let angle_limit = AngleLimit::from_rad(params.min_angle as f64); + // The area of the equilateral triangle of side `cell_size`. + let max_area = cell_size * cell_size * Real::sqrt(0.75) / 2.0; + // The refinement gives up after a fixed number of added points, which defaults to ten times + // the boundary's: far too few for a small `cell_size`, and a mesh that stops mid-refinement is + // worse than a coarse one. Budget for a few times the elements the area target asks for. + let enclosed_area = indices + .iter() + .map(|e| vertices[e[0] as usize].perp_dot(vertices[e[1] as usize]) / 2.0) + .sum::() + .abs(); + let budget = (enclosed_area / max_area * 4.0) as usize + 1000; + let refinement = cdt.refine( + RefinementParameters::new() + .with_angle_limit(angle_limit) + .with_max_allowed_area(max_area) + .with_max_additional_vertices(budget) + .exclude_outer_faces(true), + ); + + if !refinement.refinement_complete { + log::warn!( + "volume_mesh: the refinement ran out of vertices after {budget}; some elements are \ + badly shaped. Try a larger cell size or a smaller minimum angle." + ); + } + + let outer: HashMap<_, ()> = refinement + .excluded_faces + .into_iter() + .map(|face| (face.index(), ())) + .collect(); + let mut vertices = alloc::vec![Vector::ZERO; cdt.num_vertices()]; + + for vertex in cdt.vertices() { + let pt = vertex.position(); + vertices[vertex.fix().index()] = Vector::new(pt.x, pt.y); + } + + let mut cells = Vec::new(); + + for face in cdt.inner_faces() { + if outer.contains_key(&face.fix().index()) { + continue; + } + + let cell = face.vertices().map(|v| v.fix().index() as u32); + let [a, b, c] = cell.map(|v| vertices[v as usize]); + + // Positively oriented, and no degenerate cell. + let area = (b - a).perp_dot(c - a) / 2.0; + if area.abs() < max_area * 1.0e-6 { + continue; + } + + cells.push(if area > 0.0 { + cell + } else { + [cell[0], cell[2], cell[1]] + }); + } + + if cells.is_empty() { + return None; + } + + let mut result = VolumeMesh { vertices, cells }; + result.compact(); + + Some(result) +} diff --git a/src/transformation/volume_mesh/isosurface_stuffing.rs b/src/transformation/volume_mesh/isosurface_stuffing.rs new file mode 100644 index 00000000..90f5b58c --- /dev/null +++ b/src/transformation/volume_mesh/isosurface_stuffing.rs @@ -0,0 +1,531 @@ +//! Covering a shape with the cells of a body-centered cubic lattice (the lattice of Labelle and +//! Shewchuk's isosurface stuffing, SIGGRAPH 2007, kept whole instead of cut). Every cell the shape +//! reaches is kept by exact intersection tests, so the mesh always contains what it covers. + +use super::{MeshEnclosure, VolumeMesh, VolumeMeshParameters}; +use crate::bounding_volume::{Aabb, BoundingVolume}; +#[cfg(not(feature = "std"))] +use crate::math::ComplexField; +use crate::math::{Pose, Real, Vector}; +use crate::query::PointQueryWithLocation; +use crate::shape::{SupportMap, TriMesh, TriMeshFlags}; +use crate::utils::hashmap::HashMap; +use alloc::vec::Vec; + +/// An order-preserving map over a slice, in parallel when the `parallel` feature is on. +/// +/// Everything the cover pipeline hands this is a pure per-item computation, and rayon's +/// collect keeps item order, so the parallel and sequential paths produce identical +/// results, bit for bit. +#[cfg(feature = "parallel")] +pub(super) fn par_map R + Sync + Send>(items: &[T], f: F) -> Vec { + use rayon::prelude::*; + items.par_iter().map(f).collect() +} + +/// The sequential twin of the parallel [`par_map`]. +#[cfg(not(feature = "parallel"))] +pub(super) fn par_map R>(items: &[T], f: F) -> Vec { + items.iter().map(f).collect() +} + +/// A convex point set as a support map, which is all GJK asks of a shape: the tetrahedra +/// and convex hulls the enclosure tests intersect never need more structure than this. +pub(super) struct PointsSupportMap<'a>(pub &'a [Vector]); + +impl SupportMap for PointsSupportMap<'_> { + fn local_support_point(&self, dir: Vector) -> Vector { + let mut best = self.0[0]; + for pt in &self.0[1..] { + if pt.dot(dir) > best.dot(dir) { + best = *pt; + } + } + best + } +} + +/// Whether the hull of a point set (a tetrahedron, a cage triangle) and a convex +/// support-mapped shape intersect (GJK). +pub(super) fn convex_points_intersect(points: &[Vector], other: &impl SupportMap) -> bool { + crate::query::details::intersection_test_support_map_support_map( + &Pose::IDENTITY, + &PointsSupportMap(points), + other, + ) +} + +/// The oracle of [`tetrahedralize`]: the boundary mesh's own pseudo-normal signed distance +/// and exact intersection tests, which is everything the lattice reads of the shape. +pub(super) struct MeshOracle<'a> { + mesh: &'a TriMesh, +} + +impl<'a> MeshOracle<'a> { + pub fn new(mesh: &'a TriMesh) -> Self { + Self { mesh } + } +} + +impl MeshOracle<'_> { + /// The signed distance to the boundary: positive inside. + pub fn signed_distance(&self, pt: Vector) -> Real { + signed_distance(self.mesh, pt) + } + + /// Whether the shape's surface reaches into the box: exact (the BVH prunes to candidate + /// triangles, GJK decides each, the first hit exits), which is what the cover's octree needs + /// to decide a subdivision. + pub fn crosses_region(&self, region: &Aabb) -> bool { + let corners = [ + region.mins, + Vector::new(region.maxs.x, region.mins.y, region.mins.z), + Vector::new(region.mins.x, region.maxs.y, region.mins.z), + Vector::new(region.maxs.x, region.maxs.y, region.mins.z), + Vector::new(region.mins.x, region.mins.y, region.maxs.z), + Vector::new(region.maxs.x, region.mins.y, region.maxs.z), + Vector::new(region.mins.x, region.maxs.y, region.maxs.z), + region.maxs, + ]; + let crossed = self + .mesh + .bvh() + .intersect_aabb(region) + .any(|tri| convex_points_intersect(&corners, &self.mesh.triangle(tri))); + crossed + } + + /// Whether the shape's surface passes through the tetrahedron ([`MeshEnclosure::Crust`]): + /// a buried tetrahedron is not reached and no orientation is read, so the crust stays + /// hollow and an open mesh can be crusted. + pub fn surface_intersects(&self, tet: &[Vector; 4]) -> bool { + let aabb = Aabb::from_points(tet.iter().copied()); + // Only the triangles the tetrahedron's Aabb can see are worth testing. + let crossed = self + .mesh + .bvh() + .intersect_aabb(&aabb) + .any(|tri| convex_points_intersect(tet, &self.mesh.triangle(tri))); + crossed + } + + /// The triangles reaching into a region, collected once per boundary vertex or face for the + /// smoothing's `_among` queries, so the per-move tests scan a few local triangles instead of + /// re-walking the BVH. + pub fn collect(&self, region: &Aabb, out: &mut Vec) { + out.extend(self.mesh.bvh().intersect_aabb(region)); + } + + /// Whether the shape forbids a cage triangle passing there, tested against the listed + /// triangles only: containment breaks exactly when the cage's boundary crosses the + /// shape, so this is the smoothing's barrier. + pub fn crosses_among(&self, triangle: &[Vector; 3], primitives: &[u32]) -> bool { + // The Aabb reject is what keeps the listed neighborhood cheap: GJK only runs on + // the one or two triangles overlapping the cage face's box. + let aabb = Aabb::from_points(triangle.iter().copied()); + primitives.iter().any(|tri| { + let candidate = self.mesh.triangle(*tri); + let lows = candidate.a.min(candidate.b).min(candidate.c); + let highs = candidate.a.max(candidate.b).max(candidate.c); + lows.cmple(aabb.maxs).all() + && highs.cmpge(aabb.mins).all() + && convex_points_intersect(triangle, &candidate) + }) + } + + /// The point of the shape's boundary closest to `pt` among the listed triangles, or + /// `None` if the list is empty. + pub fn project_among(&self, pt: Vector, primitives: &[u32]) -> Option { + use crate::query::PointQuery; + + // A triangle whose box is already farther than the best projection cannot beat it. + let mut best = None; + let mut best_dist = Real::MAX; + for tri in primitives { + let candidate = self.mesh.triangle(*tri); + let lows = candidate.a.min(candidate.b).min(candidate.c); + let highs = candidate.a.max(candidate.b).max(candidate.c); + if (pt.clamp(lows, highs) - pt).length() >= best_dist { + continue; + } + let projection = candidate.project_local_point(pt, true).point; + let dist = (projection - pt).length(); + if dist < best_dist { + best = Some(projection); + best_dist = dist; + } + } + best + } +} + +/// The body-centered cubic lattice covering an Aabb: the cube corners first, then the cube centers. +/// +/// Coordinates are doubled so that both sublattices are integral: the lattice point with +/// coordinates `c` sits at `origin + c * cell_size / 2`, corners having only even coordinates and +/// centers only odd ones. +struct Lattice { + origin: Vector, + half_cell: Real, + dims: [i32; 3], + /// Number of even (resp. odd) coordinates along each axis. + num_even: [i32; 3], + num_odd: [i32; 3], + /// Number of cube corners, i.e. the index of the first cube center. + corners_len: usize, +} + +impl Lattice { + fn covering(aabb: Aabb, cell_size: Real) -> Self { + let half_cell = cell_size * 0.5; + // One cell of margin all around, so that every edge crossing the boundary has both of its + // endpoints sampled. + let origin = aabb.mins - Vector::splat(cell_size); + let dims = core::array::from_fn(|i| { + let extent = aabb.maxs[i] - aabb.mins[i] + cell_size * 2.0; + ((extent / half_cell).ceil() as i32).max(2) + }); + let num_even = core::array::from_fn(|i: usize| dims[i] / 2 + 1); + let num_odd = core::array::from_fn(|i: usize| (dims[i] + 1) / 2); + let corners_len = (num_even[0] * num_even[1] * num_even[2]) as usize; + + Self { + origin, + half_cell, + dims, + num_even, + num_odd, + corners_len, + } + } + + fn len(&self) -> usize { + self.corners_len + (self.num_odd[0] * self.num_odd[1] * self.num_odd[2]) as usize + } + + /// The index of the lattice point with the given doubled coordinates, if it is one and it is + /// in range. + fn index(&self, c: [i32; 3]) -> Option { + if (0..3).any(|i| c[i] < 0 || c[i] > self.dims[i]) { + return None; + } + + let parity = c[0].rem_euclid(2); + if c[1].rem_euclid(2) != parity || c[2].rem_euclid(2) != parity { + return None; + } + + let (base, counts) = if parity == 0 { + (0, self.num_even) + } else { + (self.corners_len as i32, self.num_odd) + }; + let [i, j, k] = [c[0] / 2, c[1] / 2, c[2] / 2]; + Some((base + i + (j + k * counts[1]) * counts[0]) as u32) + } + + fn point(&self, c: [i32; 3]) -> Vector { + self.origin + Vector::new(c[0] as Real, c[1] as Real, c[2] as Real) * self.half_cell + } + + /// The doubled coordinates of every lattice point, in index order. + fn coords(&self) -> Vec<[i32; 3]> { + let mut coords = Vec::with_capacity(self.len()); + for (parity, counts) in [(0, self.num_even), (1, self.num_odd)] { + for k in 0..counts[2] { + for j in 0..counts[1] { + for i in 0..counts[0] { + coords.push([i * 2 + parity, j * 2 + parity, k * 2 + parity]); + } + } + } + } + coords + } +} + +/// The lattice tetrahedra, each of them stored so that its two long edges are `[0, 1]` and +/// `[2, 3]`. +/// +/// The lattice decomposes into octahedra: two neighboring cube centers plus the four corners of the +/// cube face between them. Splitting each octahedron along its two centers gives four tetrahedra, +/// one per edge of that face. +fn lattice_tetrahedra(lattice: &Lattice, coords: &[[i32; 3]]) -> Vec<[u32; 4]> { + let mut tets = Vec::new(); + + for (center, &c) in coords.iter().enumerate().skip(lattice.corners_len) { + let center = center as u32; + + for axis in 0..3 { + let mut step = [0; 3]; + step[axis] = 2; + let Some(opposite) = lattice.index(core::array::from_fn(|i| c[i] + step[i])) else { + continue; + }; + + // The corners of the cube face between the two centers, in cyclic order. + let (u, v) = ((axis + 1) % 3, (axis + 2) % 3); + let mut corners = [0; 4]; + let mut complete = true; + + for (k, [su, sv]) in [[1, 1], [-1, 1], [-1, -1], [1, -1]].into_iter().enumerate() { + let mut corner = c; + corner[axis] += 1; + corner[u] += su; + corner[v] += sv; + + match lattice.index(corner) { + Some(corner) => corners[k] = corner, + None => { + complete = false; + break; + } + } + } + + if !complete { + continue; + } + + for k in 0..4 { + tets.push([center, opposite, corners[k], corners[(k + 1) % 4]]); + } + } + } + + tets +} + +/// Whether every edge of the mesh is shared by exactly two triangles. +fn is_closed(indices: &[[u32; 3]]) -> bool { + let mut edges: HashMap<[u32; 2], u32> = HashMap::default(); + + for tri in indices { + for k in 0..3 { + *edges.entry(edge_key(tri[k], tri[(k + 1) % 3])).or_insert(0) += 1; + } + } + + edges.values().all(|count| *count == 2) +} + +/// The signed distance to the boundary mesh: positive inside. +fn signed_distance(mesh: &TriMesh, pt: Vector) -> Real { + // The projection has to keep its location for the inside test to use the pseudo-normals. + let (proj, _) = mesh.project_local_point_and_get_location(pt, false); + let dist = (pt - proj.point).length(); + if proj.is_inside { + dist + } else { + -dist + } +} + +fn edge_key(a: u32, b: u32) -> [u32; 2] { + if a < b { + [a, b] + } else { + [b, a] + } +} + +/// The tetrahedron flipped positive, or nothing if it is degenerate. +fn orient_tet(pts: &[Vector], mut tet: [u32; 4], min_volume: Real) -> Option<[u32; 4]> { + let [a, b, c, d] = tet.map(|i| pts[i as usize]); + let volume = (b - a).cross(c - a).dot(d - a) / 6.0; + + if volume.abs() < min_volume { + return None; + } + + if volume < 0.0 { + tet.swap(0, 1); + } + + Some(tet) +} + +/// A grid of cells filling the domain: the uniform lattice, or the cover's subdivided +/// octree grid. +pub(super) struct BackgroundGrid { + pub points: Vec, + pub cells: Vec<[u32; 4]>, +} + +/// The domain the grid has to cover: the shape's Aabb, grown by the whole cells the cover +/// keeps around the shape. +pub(super) fn domain(aabb: Aabb, cell_size: Real) -> Aabb { + aabb.loosened(cell_size) +} + +/// The body-centered cubic lattice covering an Aabb, as a background grid. +pub(super) fn uniform_grid(aabb: Aabb, cell_size: Real) -> BackgroundGrid { + let lattice = Lattice::covering(aabb, cell_size); + let coords = lattice.coords(); + let points = coords.iter().map(|&c| lattice.point(c)).collect(); + let cells = lattice_tetrahedra(&lattice, &coords); + + BackgroundGrid { points, cells } +} + +/// Which non-crossing cells are inside the shape, by connectivity: cells untouched by the surface +/// that share a vertex lie on the same side of it, so their vertex-connected components are inside +/// or outside as a whole and one signed-distance probe per component classifies every cell in it. +fn flood_fill_inside( + oracle: &MeshOracle, + points: &[Vector], + grid_cells: &[[u32; 4]], + crossing: &[bool], +) -> Vec { + // Union-find over the non-crossing cells, joined through shared vertices. + let mut parent: Vec = (0..grid_cells.len() as u32).collect(); + fn root(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + parent[i as usize] = parent[parent[i as usize] as usize]; + i = parent[i as usize]; + } + i + } + + let mut last_at_vertex = alloc::vec![u32::MAX; points.len()]; + for (id, cell) in grid_cells.iter().enumerate() { + if crossing[id] { + continue; + } + for v in cell { + let previous = last_at_vertex[*v as usize]; + last_at_vertex[*v as usize] = id as u32; + if previous != u32::MAX { + let (a, b) = (root(&mut parent, previous), root(&mut parent, id as u32)); + parent[a as usize] = b; + } + } + } + + // One probe per component, at the centroid of its representative cell. + let mut inside_root: HashMap = HashMap::default(); + let mut inside = alloc::vec![false; grid_cells.len()]; + for id in 0..grid_cells.len() { + if crossing[id] { + continue; + } + let component = root(&mut parent, id as u32); + let is_inside = *inside_root.entry(component).or_insert_with(|| { + let centroid = grid_cells[component as usize] + .iter() + .map(|v| points[*v as usize]) + .sum::() + / 4.0; + oracle.signed_distance(centroid) > 0.0 + }); + inside[id] = is_inside; + } + + inside +} + +/// Keeps every whole cell of the background grid the shape reaches, cutting and warping +/// nothing: the mesh contains the shape by construction ([`MeshEnclosure::Cover`]). +/// +/// A cell is kept when a vertex of it is inside the (dilated) shape, or when the exact +/// intersection test says the shape passes through it; the latter is what catches a feature +/// that slips between the sampled vertices, which the sampled field alone would lose. +fn cover_grid( + grid: BackgroundGrid, + oracle: &MeshOracle, + params: &VolumeMeshParameters, + min_volume: Real, +) -> Option { + let BackgroundGrid { + points, + cells: grid_cells, + } = grid; + + // The cells the surface exactly passes through: the BVH prunes to the band along the + // boundary, so the convex tests are only ever paid there. + let crossing: Vec = par_map(&grid_cells, |tet| { + let pts = tet.map(|v| points[v as usize]); + oracle.surface_intersects(&pts) + }); + + // What else is kept: nothing for the crust (hollow by design); for the cover, the + // cells inside the shape, classified by connectivity with one signed-distance probe + // per component. + let keep: Vec = if params.enclosure == MeshEnclosure::Crust { + crossing + } else { + let inside = flood_fill_inside(oracle, &points, &grid_cells, &crossing); + crossing + .iter() + .zip(&inside) + .map(|(crossing, inside)| *crossing || *inside) + .collect() + }; + + let ids: Vec = (0..grid_cells.len() as u32).collect(); + let kept = par_map(&ids, |id| { + if keep[*id as usize] { + orient_tet(&points, grid_cells[*id as usize], min_volume) + } else { + None + } + }); + let cells: Vec<[u32; 4]> = kept.into_iter().flatten().collect(); + + if cells.is_empty() { + return None; + } + + let mut result = VolumeMesh { + vertices: points, + cells, + }; + result.compact(); + + Some(result) +} + +pub fn tetrahedralize( + vertices: &[Vector], + indices: &[[u32; 3]], + params: &VolumeMeshParameters, +) -> Option { + let cell_size = params.cell_size; + + if vertices.is_empty() || indices.is_empty() || cell_size <= 0.0 || cell_size.is_nan() { + return None; + } + + // The cut function is the signed distance to the boundary, so the boundary must be closed and + // consistently oriented (duplicated vertices are welded first); the crust only keeps the cells + // the surface passes through and never reads the sign, so it accepts an open mesh. + let crust = params.enclosure == MeshEnclosure::Crust; + let flags = if crust { + TriMeshFlags::MERGE_DUPLICATE_VERTICES + } else { + TriMeshFlags::ORIENTED | TriMeshFlags::MERGE_DUPLICATE_VERTICES + }; + let mesh = TriMesh::with_flags(vertices.to_vec(), indices.to_vec(), flags) + .ok() + .filter(|mesh| crust || (mesh.pseudo_normals().is_some() && is_closed(mesh.indices())))?; + + let oracle = MeshOracle::new(&mesh); + let grid = if params.cover_subdivisions > 0 { + super::cover_octree::cover_octree_grid( + &oracle, + domain(mesh.local_aabb(), cell_size), + params, + )? + } else { + uniform_grid(domain(mesh.local_aabb(), cell_size), cell_size) + }; + + let mut result = cover_grid( + grid, + &oracle, + params, + cell_size * cell_size * cell_size * 1.0e-6, + )?; + super::cover_smoothing::smooth_cover(&mut result, &oracle, params); + Some(result) +} diff --git a/src/transformation/volume_mesh/mod.rs b/src/transformation/volume_mesh/mod.rs new file mode 100644 index 00000000..61ad7cc9 --- /dev/null +++ b/src/transformation/volume_mesh/mod.rs @@ -0,0 +1,175 @@ +//! Filling a closed boundary with a simulation-grade simplex mesh. + +use crate::math::{Real, Vector, DIM}; +use alloc::vec::Vec; + +#[cfg(feature = "dim3")] +mod cover_octree; +#[cfg(feature = "dim3")] +mod cover_smoothing; +#[cfg(feature = "dim2")] +mod delaunay_refinement; +#[cfg(feature = "dim3")] +mod isosurface_stuffing; + +/// A simplicial mesh filling the interior of a closed boundary: triangles in 2D, tetrahedra in 3D. +#[derive(Clone, Debug, Default)] +#[cfg_attr( + feature = "serde-serialize", + derive(serde::Serialize, serde::Deserialize) +)] +pub struct VolumeMesh { + /// The mesh vertices. + pub vertices: Vec, + /// The mesh cells, positively oriented (positive signed area/volume). + pub cells: Vec<[u32; DIM + 1]>, +} + +/// Which cover the mesh is (3D only): of the shape's volume, or of its surface alone. +/// +/// Either way the mesh is made of whole lattice cells the shape reaches, so it *contains* +/// what it covers, which is what an embedding wants: every point of the shape (a skin +/// vertex, say) interpolates inside a cell instead of extrapolating outside all of them. +#[cfg(feature = "dim3")] +#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] +pub enum MeshEnclosure { + /// Every lattice cell the shape reaches is kept whole: a solid fill containing the shape by + /// construction, blocky at the cell size (see [`VolumeMeshParameters::cover_smoothing`] and + /// [`VolumeMeshParameters::cover_subdivisions`]). Needs a closed, consistently oriented mesh. + #[default] + Cover, + /// The cover of the shape's surface alone: only the cells the surface crosses are kept, + /// the interior stays empty, and the mesh need not be closed or oriented. The result is a + /// hollow shell that deforms like a shell, not like a solid. + Crust, +} + +/// Parameters of [`volume_mesh`]. +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct VolumeMeshParameters { + /// Target size of the generated elements. + pub cell_size: Real, + /// Smallest angle, in radians, the refinement aims for (2D only). + /// + /// Above 30 degrees the refinement is not guaranteed to terminate, so it may stop early and + /// leave some elements badly shaped. + #[cfg(feature = "dim2")] + pub min_angle: Real, + /// Whether the whole shape is covered or its surface alone (3D only); see + /// [`MeshEnclosure`]. + #[cfg(feature = "dim3")] + pub enclosure: MeshEnclosure, + /// How many shrink-wrap iterations smooth the staircase of a [`MeshEnclosure::Cover`] mesh, + /// zero leaving it raw (3D only, cover only). Each iteration pulls the boundary toward + /// the shape, held off by [`Self::cover_guard`], and commits a move only if containment holds. + #[cfg(feature = "dim3")] + pub cover_smoothing: u32, + /// How close to the shape the smoothed cover may pull its boundary, as a fraction of + /// the local cell size (3D only, read by the cover's smoothing only). + #[cfg(feature = "dim3")] + pub cover_guard: Real, + /// How many halvings below [`Self::cell_size`] a [`MeshEnclosure::Cover`] cell crossing the + /// shape's boundary may be refined, zero keeping the boundary at the cell size (3D only, cover + /// only). The refinement is on the octree (no hanging vertex) and runs before the smoothing. + #[cfg(feature = "dim3")] + pub cover_subdivisions: u32, +} + +impl VolumeMeshParameters { + /// Parameters generating elements of size `cell_size`: in 3D a raw cover (no + /// smoothing, no subdivision), in 2D a refinement aiming for 30 degree angles. + pub fn new(cell_size: Real) -> Self { + Self { + cell_size, + #[cfg(feature = "dim2")] + #[cfg_attr(feature = "f64", expect(clippy::unnecessary_cast))] + min_angle: core::f64::consts::PI as Real / 6.0, + #[cfg(feature = "dim3")] + enclosure: MeshEnclosure::Cover, + #[cfg(feature = "dim3")] + cover_smoothing: 0, + #[cfg(feature = "dim3")] + cover_guard: 0.15, + #[cfg(feature = "dim3")] + cover_subdivisions: 0, + } + } +} + +/// Fills a boundary `(vertices, indices)` (2D polyline, 3D triangle mesh) with a simplex mesh +/// for finite-element simulation: 2D refines a constrained Delaunay triangulation of the boundary, +/// 3D builds a lattice cover containing it. Returns `None` if the boundary encloses nothing. +pub fn volume_mesh( + vertices: &[Vector], + indices: &[[u32; DIM]], + params: &VolumeMeshParameters, +) -> Option { + #[cfg(feature = "dim2")] + { + delaunay_refinement::triangulate(vertices, indices, params) + } + #[cfg(feature = "dim3")] + { + isosurface_stuffing::tetrahedralize(vertices, indices, params) + } +} + +impl VolumeMesh { + /// The connected component of each cell: two cells sharing a vertex are in the same one. + /// + /// Components are numbered from zero, so the number of them is the largest index plus one. + /// A mesh in more than one piece means the input's own surface is in several pieces: the + /// cover keeps every cell the surface reaches, however thin the feature. + pub fn connected_components(&self) -> Vec { + let mut parent: Vec = (0..self.vertices.len() as u32).collect(); + + fn root(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + parent[i as usize] = parent[parent[i as usize] as usize]; + i = parent[i as usize]; + } + i + } + + for cell in &self.cells { + for vid in &cell[1..] { + let (a, b) = (root(&mut parent, cell[0]), root(&mut parent, *vid)); + parent[a as usize] = b; + } + } + + let mut ids = alloc::vec![u32::MAX; self.vertices.len()]; + let mut count = 0; + + self.cells + .iter() + .map(|cell| { + let id = &mut ids[root(&mut parent, cell[0]) as usize]; + if *id == u32::MAX { + *id = count; + count += 1; + } + *id + }) + .collect() + } + + /// Removes the vertices no cell references, and reindexes the cells accordingly. + pub fn compact(&mut self) { + let mut remap = alloc::vec![u32::MAX; self.vertices.len()]; + let mut vertices = Vec::with_capacity(self.vertices.len()); + + for cell in &mut self.cells { + for vid in cell { + let remapped = &mut remap[*vid as usize]; + if *remapped == u32::MAX { + *remapped = vertices.len() as u32; + vertices.push(self.vertices[*vid as usize]); + } + *vid = *remapped; + } + } + + self.vertices = vertices; + } +} diff --git a/src/utils/mod.rs b/src/utils/mod.rs index 12552ada..809c597c 100644 --- a/src/utils/mod.rs +++ b/src/utils/mod.rs @@ -37,7 +37,7 @@ pub use self::segments_intersection::{segments_intersection2d, SegmentsIntersect pub use self::sort::sort2; pub use self::sort::sort3; pub use self::sorted_pair::SortedPair; -#[cfg(all(feature = "dim3", feature = "spade"))] +#[cfg(feature = "spade")] pub(crate) use self::spade::sanitize_spade_point; pub(crate) use self::wops::{WBasis, WCross, WSign}; @@ -78,7 +78,7 @@ mod sdp_matrix; mod segments_intersection; mod sort; mod sorted_pair; -#[cfg(all(feature = "dim3", feature = "spade"))] +#[cfg(feature = "spade")] mod spade; mod wops;