feat: add deformable polylines/meshes

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<Vector> {
+    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<ContactManifold<(), ()>>,
+    workspace: Option<ContactManifoldsWorkspace>,
+}
+
+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<Real> {
+        self.manifolds
+            .iter()
+            .flat_map(|m| m.points.iter().map(|p| p.dist))
+            .fold(None, |acc: Option<Real>, d| {
+                Some(acc.map_or(d, |a| a.min(d)))
+            })
+    }
+}
+
+fn translated(vertices: &[Vector], shift: Vector) -> Vec<Vector> {
+    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<Vector> = 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<u32> = polyline.bvh().intersect_aabb(query).collect();
+        found.sort_unstable();
+        let mut expected: Vec<u32> = (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:?}"
+    );
+}

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;

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<Vector>, 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::<Vector>()
+            / 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());
+}

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, &params).unwrap();
+    let _warmup = volume_mesh(&vertices, &indices, &params).unwrap();
+    let start = Instant::now();
+    let raw2 = volume_mesh(&vertices, &indices, &params).unwrap();
+    let build = start.elapsed();
+    let _ = raw2;
+
+    params.cover_smoothing = 30;
+    let start = Instant::now();
+    let smoothed = volume_mesh(&vertices, &indices, &params).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;
+}