From c9a760f9b8c0e324c16c87e6a03e52c5c556a8cd Mon Sep 17 00:00:00 2001 From: Michael Herzog <8261669+VitaminCpp@users.noreply.github.com> Date: Fri, 6 Nov 2026 10:07:54 +0100 Subject: [PATCH 1/2] perf: skip the heightfield triangles a shape-cast can't hit Shape-casting against a `HeightField` ran GJK on every triangle (segment in 2D) of the cells it walked through. It now skips a triangle when the moving shape stays clear of it until the nearest hit found so far, or until `max_time_of_impact` before any hit: either its swept Aabb misses the triangle's Aabb, or, for a support map, it stays on one side of the triangle's plane over the whole sweep. The clearance is the target distance plus a margin chosen from measurements, relative to the length of the sweep and the target distance. In random shape-casts the result is unchanged, except for some hits GJK reports on shapes a few 1e-3 across that stay clear of the triangle by about their own size. Co-authored-by: Roland <11982367+MinimalCpp@users.noreply.github.com> Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.md | 5 + .../parry2d/tests/heightfield_shape_cast.rs | 360 +++++++++++++++++ .../parry3d/tests/heightfield_shape_cast.rs | 369 ++++++++++++++++++ .../shape_cast_heightfield_shape.rs | 111 +++++- 4 files changed, 839 insertions(+), 6 deletions(-) create mode 100644 crates/parry2d/tests/heightfield_shape_cast.rs create mode 100644 crates/parry3d/tests/heightfield_shape_cast.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index ec4c721d..15ea225a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,11 @@ ### Modified +- Shape-casting against a `HeightField` no longer shape-casts the triangles (segments in 2D) that + the moving shape can't hit within `max_time_of_impact`, or before the nearest hit found so far, + which skips running GJK on most of the triangles it walks through. The results are unchanged, + except for some hits GJK reports on shapes a few 1e-3 across that stay clear of the triangle + by about their own size ([#PR_NUMBER](https://github.com/dimforge/parry/pull/PR_NUMBER)). - Deserializing a `ContactManifoldsWorkspace` that was serialized by a custom contact-manifolds algorithm no longer fails with `"Cannot deserialize custom shape."`. It now yields the new `UnknownContactManifoldsWorkspace`, which the relevant algorithm can replace with a workspace diff --git a/crates/parry2d/tests/heightfield_shape_cast.rs b/crates/parry2d/tests/heightfield_shape_cast.rs new file mode 100644 index 00000000..022fcb63 --- /dev/null +++ b/crates/parry2d/tests/heightfield_shape_cast.rs @@ -0,0 +1,360 @@ +// Shape-casting against a `HeightField` skips the segments the moving shape can't hit within +// `max_time_of_impact`, or before the nearest hit found so far. These check that the result is +// still the nearest of the hits of a shape-cast against each segment on its own. + +use approx::assert_relative_eq; +use parry2d::math::{Pose, Real, Vector}; +use parry2d::query::details::cast_shapes_heightfield_shape; +use parry2d::query::{ + self, ClosestPoints, Contact, DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, + ShapeCastHit, ShapeCastOptions, ShapeDistance, ShapeIntersection, Unsupported, +}; +use parry2d::shape::{Ball, Capsule, Cuboid, HeightField, RoundCuboid, Segment, Shape}; +use std::sync::Mutex; + +/// A heightfield `size` wide with `samples` samples, with the given height function. +fn heightfield(samples: usize, size: Real, height: impl Fn(Real) -> Real) -> HeightField { + let heights = (0..samples).map(|i| height(i as Real)).collect(); + HeightField::new(heights, Vector::new(size, 1.0)) +} + +/// The shape-casts against each segment of `heightfield` on its own that hit. +fn segment_hits( + pos1: &Pose, + heightfield: &HeightField, + pos2: &Pose, + vel2: Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> Vec { + heightfield + .segments() + .filter_map(|seg| { + query::cast_shapes(pos1, Vector::ZERO, &seg, pos2, vel2, g2, options).unwrap() + }) + .collect() +} + +/// Shape-casts `g2` from `pos2` along `vel2` against `heightfield` at `pos1`, asserts that the +/// result is the nearest hit of the shape-casts against each segment on its own (any one among +/// exact ties), and returns whether it hits. +fn assert_nearest_segment_hit( + pos1: &Pose, + heightfield: &HeightField, + pos2: &Pose, + vel2: Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> bool { + let hit = query::cast_shapes(pos1, Vector::ZERO, heightfield, pos2, vel2, g2, options).unwrap(); + let hits = segment_hits(pos1, heightfield, pos2, vel2, g2, options); + let nearest = hits.iter().map(|hit| hit.time_of_impact).reduce(Real::min); + let context = || format!("{pos2:?}, {vel2:?}, {options:?}"); + + assert_eq!(hit.map(|hit| hit.time_of_impact), nearest, "{}", context()); + if let Some(hit) = hit { + // `ShapeCastHit` has no `PartialEq`; `Debug` prints each float so that it reads back + // exactly. + let hit = format!("{hit:?}"); + assert!( + hits.iter().any(|seg_hit| format!("{seg_hit:?}") == hit), + "{hit} is no segment's hit: {}", + context() + ); + } + hit.is_some() +} + +fn shapes() -> [Box; 4] { + [ + Box::new(Ball::new(0.4)), + Box::new(Cuboid::new(Vector::new(0.6, 0.3))), + Box::new(Capsule::new_y(0.5, 0.4)), + Box::new(RoundCuboid { + inner_shape: Cuboid::new(Vector::new(0.3, 0.2)), + border_radius: 0.15, + }), + ] +} + +fn options( + max_time_of_impact: Real, + target_distance: Real, + stop_at_penetration: bool, +) -> ShapeCastOptions { + ShapeCastOptions { + max_time_of_impact, + target_distance, + stop_at_penetration, + compute_impact_geometry_on_penetration: true, + } +} + +/// Shape-casts over rolling and rough ground, standing on it, hovering over it and sunk in it, +/// down onto it, along it and up from it, short and long, with and without a target distance. +#[test] +fn heightfield_shape_cast_gives_the_nearest_segment_hit() { + // Waves about 10 m long and 0.5 m high, and ground that rises and falls by up to about 1 m + // from one sample to the next. + let grounds = [ + heightfield(41, 40.0, |i| 0.5 * (0.6 * i).cos()), + heightfield(41, 40.0, |i| 0.6 * ((1.3 * i).sin() + (0.7 * i).cos())), + ]; + // The heightfield is posed off the origin and turned, so that the cast runs in its frame. + let pos1 = Pose::new(Vector::new(3.0, -1.0), 0.3); + // In the heightfield's frame. + let directions = [ + Vector::new(1.0, 0.0), + Vector::new(-1.0, -0.01), + Vector::new(1.0, 0.02), + -Vector::Y, + Vector::new(0.3, -1.0), + Vector::new(-1.0, -1.0), + Vector::new(1.0, -0.5), + Vector::new(-0.5, 0.4), + ] + .map(|dir| pos1.rotation * dir.normalize()); + let options = [ + options(0.31, 0.0, false), + options(3.0, 0.0, true), + options(3.0, 0.05, false), + options(100.0, 0.0, true), + ]; + let shapes = shapes(); + + let (mut hits, mut misses) = (0, 0); + for ground in &grounds { + for (k, shape) in shapes.iter().enumerate() { + for x in (-19..=19).step_by(2) { + for lift in [0.5, 0.95, 1.8] { + let origin = pos1 * Vector::new(x as Real + 0.37, lift); + let pos2 = Pose::new(origin, 0.1 * (x + k as i32) as Real); + for dir in directions { + for options in options { + if assert_nearest_segment_hit( + &pos1, + ground, + &pos2, + dir * 2.0, + &**shape, + options, + ) { + hits += 1; + } else { + misses += 1; + } + } + } + } + } + } + } + assert!( + hits > 1_000 && misses > 1_000, + "{hits} hits, {misses} misses" + ); +} + +/// Parry's dispatcher, recording each segment it shape-casts against. +#[derive(Default)] +struct SegmentRecorder(Mutex>); + +impl QueryDispatcher for SegmentRecorder { + fn intersection_test( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + DefaultQueryDispatcher.intersection_test(pos12, g1, g2) + } + + fn distance( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + DefaultQueryDispatcher.distance(pos12, g1, g2) + } + + fn contact( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + ) -> Result, Unsupported> { + DefaultQueryDispatcher.contact(pos12, g1, g2, prediction) + } + + fn closest_points( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + max_dist: Real, + ) -> Result { + DefaultQueryDispatcher.closest_points(pos12, g1, g2, max_dist) + } + + fn cast_shapes( + &self, + pos12: &Pose, + local_vel12: Vector, + g1: &dyn Shape, + g2: &dyn Shape, + options: ShapeCastOptions, + ) -> Result, Unsupported> { + if let Some(seg) = g1.as_segment() { + self.0.lock().unwrap().push(*seg); + } + DefaultQueryDispatcher.cast_shapes(pos12, local_vel12, g1, g2, options) + } + + fn cast_shapes_nonlinear( + &self, + motion1: &NonlinearRigidMotion, + g1: &dyn Shape, + motion2: &NonlinearRigidMotion, + g2: &dyn Shape, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, + ) -> Result, Unsupported> { + DefaultQueryDispatcher.cast_shapes_nonlinear( + motion1, + g1, + motion2, + g2, + start_time, + end_time, + stop_at_penetration, + ) + } +} + +/// The segments of `heightfield` that a shape-cast of `g2` from `pos12` along `vel12` shape-casts +/// against, and its result. +fn cast_segments( + heightfield: &HeightField, + pos12: &Pose, + vel12: Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> (Vec, Option) { + let recorder = SegmentRecorder::default(); + let hit = + cast_shapes_heightfield_shape(&recorder, pos12, vel12, heightfield, g2, options).unwrap(); + (recorder.0.into_inner().unwrap(), hit) +} + +/// A ball dropping by 0.1 toward the bottom of a valley, from 0.2 above it. +#[test] +fn heightfield_shape_cast_skips_the_segments_out_of_reach() { + let valley = heightfield(41, 40.0, |i| 0.02 * (i - 20.0).powi(2)); + let ball = Ball::new(0.4); + let pos12 = Pose::translation(0.3, 0.2 + 0.4); + + let (segments, hit) = cast_segments( + &valley, + &pos12, + -Vector::Y, + &ball, + ShapeCastOptions::with_max_time_of_impact(0.1), + ); + + assert!(hit.is_none()); + assert!(segments.is_empty(), "{} segments cast", segments.len()); +} + +/// A ball skimming down onto flat ground along -X hits it 10 along X from its start, at X = 5. +#[test] +fn heightfield_shape_cast_skips_the_segments_past_the_nearest_hit() { + let flat = heightfield(41, 40.0, |_| 0.0); + let ball = Ball::new(0.4); + let pos12 = Pose::translation(15.0, 1.4); + let vel12 = Vector::new(-1.0, -0.1); + + let (segments, hit) = cast_segments( + &flat, + &pos12, + vel12, + &ball, + ShapeCastOptions::with_max_time_of_impact(30.0), + ); + + let hit = hit.expect("the ball lands on the ground"); + assert_relative_eq!(hit.time_of_impact, 10.0, epsilon = 1.0e-3); + // The ball's Aabb reaches X = 4.6 by then, short of the segment [3, 4]. + let past_the_hit = segments + .iter() + .filter(|seg| seg.a.x.max(seg.b.x) <= 4.0) + .count(); + assert_eq!(past_the_hit, 0, "{} segments cast", segments.len()); +} + +/// A ball dropping onto flat ground lands at the very end of its shape-cast: the cast's +/// `max_time_of_impact` is the time of impact against the segments on their own. +#[test] +fn heightfield_shape_cast_hits_at_the_end_of_the_cast() { + let flat = heightfield(11, 10.0, |_| 0.0); + let ball = Ball::new(0.4); + let pos2 = Pose::translation(0.3, 1.4); + let landing = segment_hits( + &Pose::IDENTITY, + &flat, + &pos2, + -Vector::Y, + &ball, + ShapeCastOptions::with_max_time_of_impact(2.0), + ) + .iter() + .map(|hit| hit.time_of_impact) + .reduce(Real::min) + .expect("the ball lands on the ground"); + + let hits = assert_nearest_segment_hit( + &Pose::IDENTITY, + &flat, + &pos2, + -Vector::Y, + &ball, + ShapeCastOptions::with_max_time_of_impact(landing), + ); + + assert!(hits); +} + +/// Shapes standing on flat ground, or a hair above it, where GJK may report a hit on a segment the +/// shape stays clear of, slide along it. +#[test] +fn heightfield_shape_cast_gives_the_nearest_segment_hit_within_gjks_tolerance() { + let flat = heightfield(11, 10.0, |_| 0.0); + let shapes = shapes(); + let half_heights = [0.4, 0.3, 0.9, 0.35]; + let directions = [ + Vector::X, + -Vector::X, + Vector::new(1.0, 0.01), + Vector::new(-1.0, -0.001), + ]; + + for (shape, half_height) in shapes.iter().zip(half_heights) { + for hair in [0.0, 1.0e-6, 1.0e-4] { + let pos2 = Pose::translation(0.3, half_height + hair); + for dir in directions { + for options in [options(1.0, 0.0, true), options(1.0, 0.0, false)] { + assert_nearest_segment_hit( + &Pose::IDENTITY, + &flat, + &pos2, + dir.normalize(), + &**shape, + options, + ); + } + } + } + } +} diff --git a/crates/parry3d/tests/heightfield_shape_cast.rs b/crates/parry3d/tests/heightfield_shape_cast.rs new file mode 100644 index 00000000..133311c2 --- /dev/null +++ b/crates/parry3d/tests/heightfield_shape_cast.rs @@ -0,0 +1,369 @@ +// Shape-casting against a `HeightField` skips the triangles the moving shape can't hit within +// `max_time_of_impact`, or before the nearest hit found so far. These check that the result is +// still the nearest of the hits of a shape-cast against each triangle on its own. + +use approx::assert_relative_eq; +use parry3d::math::{Pose, Real, Rotation, Vector}; +use parry3d::query::details::cast_shapes_heightfield_shape; +use parry3d::query::{ + self, ClosestPoints, Contact, DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, + ShapeCastHit, ShapeCastOptions, ShapeDistance, ShapeIntersection, Unsupported, +}; +use parry3d::shape::{ + Ball, Capsule, Cone, Cuboid, HeightField, HeightFieldFlags, RoundCuboid, Shape, Triangle, +}; +use parry3d::utils::Array2; +use std::sync::Mutex; + +/// A heightfield `size` wide on a grid of `samples` × `samples`, with the given height function. +fn heightfield(samples: usize, size: Real, height: impl Fn(Real, Real) -> Real) -> HeightField { + let heights = (0..samples) + .flat_map(|i| (0..samples).map(move |j| (i as Real, j as Real))) + .map(|(i, j)| height(i, j)) + .collect(); + HeightField::with_flags( + Array2::new(samples, samples, heights), + Vector::new(size, 1.0, size), + HeightFieldFlags::FIX_INTERNAL_EDGES, + ) +} + +/// The shape-casts against each triangle of `heightfield` on its own that hit. +fn triangle_hits( + pos1: &Pose, + heightfield: &HeightField, + pos2: &Pose, + vel2: Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> Vec { + heightfield + .triangles() + .filter_map(|tri| { + query::cast_shapes(pos1, Vector::ZERO, &tri, pos2, vel2, g2, options).unwrap() + }) + .collect() +} + +/// Shape-casts `g2` from `pos2` along `vel2` against `heightfield` at `pos1`, asserts that the +/// result is the nearest hit of the shape-casts against each triangle on its own (any one among +/// exact ties), and returns whether it hits. +fn assert_nearest_triangle_hit( + pos1: &Pose, + heightfield: &HeightField, + pos2: &Pose, + vel2: Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> bool { + let hit = query::cast_shapes(pos1, Vector::ZERO, heightfield, pos2, vel2, g2, options).unwrap(); + let hits = triangle_hits(pos1, heightfield, pos2, vel2, g2, options); + let nearest = hits.iter().map(|hit| hit.time_of_impact).reduce(Real::min); + let context = || format!("{pos2:?}, {vel2:?}, {options:?}"); + + assert_eq!(hit.map(|hit| hit.time_of_impact), nearest, "{}", context()); + if let Some(hit) = hit { + // `ShapeCastHit` has no `PartialEq`; `Debug` prints each float so that it reads back + // exactly. + let hit = format!("{hit:?}"); + assert!( + hits.iter().any(|tri_hit| format!("{tri_hit:?}") == hit), + "{hit} is no triangle's hit: {}", + context() + ); + } + hit.is_some() +} + +fn shapes() -> [Box; 5] { + [ + Box::new(Ball::new(0.4)), + Box::new(Cuboid::new(Vector::new(0.6, 0.3, 0.4))), + Box::new(Capsule::new_y(0.5, 0.4)), + Box::new(Cone::new(0.4, 0.3)), + Box::new(RoundCuboid { + inner_shape: Cuboid::new(Vector::new(0.3, 0.2, 0.25)), + border_radius: 0.15, + }), + ] +} + +fn options( + max_time_of_impact: Real, + target_distance: Real, + stop_at_penetration: bool, +) -> ShapeCastOptions { + ShapeCastOptions { + max_time_of_impact, + target_distance, + stop_at_penetration, + compute_impact_geometry_on_penetration: true, + } +} + +/// Shape-casts over rolling and rough ground, standing on it, hovering over it and sunk in it, +/// down onto it, along it and up from it, short and long, with and without a target distance. +#[test] +fn heightfield_shape_cast_gives_the_nearest_triangle_hit() { + // Waves about 10 m long and 0.5 m high, and ground that rises and falls by up to about 1 m + // from one sample to the next. + let grounds = [ + heightfield(11, 10.0, |i, j| 0.25 * ((0.6 * i).cos() + (0.6 * j).cos())), + heightfield(11, 10.0, |i, j| 0.6 * ((1.3 * i).sin() + (1.1 * j).cos())), + ]; + // The heightfield is posed off the origin and turned, so that the cast runs in its frame. + let pos1 = Pose::from_parts(Vector::new(3.0, -1.0, 2.0), Rotation::from_rotation_y(0.3)); + // In the heightfield's frame. The traversal of the heightfield misses triangles on its own, + // before any of them is skipped, when the shape moves along +X or +Z, or with no vertical + // motion, so these don't. + let directions = [ + Vector::new(-1.0, -0.01, 0.0), + Vector::new(-1.0, 0.02, -1.0), + Vector::new(-0.1, -0.03, -1.0), + -Vector::Y, + Vector::new(-0.3, -1.0, -0.1), + Vector::new(-1.0, -0.5, -0.2), + Vector::new(0.0, -0.5, -1.0), + Vector::new(-0.5, 0.4, -1.0), + ] + .map(|dir| pos1.rotation * dir.normalize()); + let options = [ + options(0.31, 0.0, false), + options(3.0, 0.0, true), + options(3.0, 0.05, false), + options(100.0, 0.0, true), + ]; + let shapes = shapes(); + + let (mut hits, mut misses) = (0, 0); + for ground in &grounds { + for (k, shape) in shapes.iter().enumerate() { + for x in [-4, -1, 2] { + for z in [-3, 2] { + for lift in [0.5, 0.95, 1.8] { + let origin = pos1 * Vector::new(x as Real + 0.37, lift, z as Real + 0.21); + let rotation = Rotation::from_rotation_x(0.1 * (x + k as i32) as Real); + let pos2 = Pose::from_parts(origin, rotation); + for dir in directions { + for options in options { + if assert_nearest_triangle_hit( + &pos1, + ground, + &pos2, + dir * 2.0, + &**shape, + options, + ) { + hits += 1; + } else { + misses += 1; + } + } + } + } + } + } + } + } + assert!( + hits > 1_000 && misses > 1_000, + "{hits} hits, {misses} misses" + ); +} + +/// A box some 100 across, cast at a target distance of about 9 over a cell 200 wide, at a +/// time of impact where GJK leaves the box about 8e-3 farther than the target distance from one +/// triangle: GJK's tolerance grows with the target distance and the length of the cast. +#[test] +fn heightfield_shape_cast_gives_the_nearest_triangle_hit_at_a_large_scale() { + let heights = vec![-51.100224, 14.935101, -0.4987359, -7.1095943]; + let cell = HeightField::new(Array2::new(2, 2, heights), Vector::new(200.0, 1.0, 200.0)); + let cuboid = Cuboid::new(Vector::new(13.917611, 40.03146, 25.72583)); + let pos2 = Pose::from_parts( + Vector::new(34.971046, 98.25103, -77.62439), + Rotation::from_xyzw(-0.28899258, -0.5181429, -0.7841668, 0.18191668), + ); + let vel2 = Vector::new(68.49045, -97.044716, -51.755314); + + let hits = assert_nearest_triangle_hit( + &Pose::IDENTITY, + &cell, + &pos2, + vel2, + &cuboid, + options(3.7495742, 9.270905, true), + ); + + assert!(hits); +} + +/// Parry's dispatcher, recording each triangle it shape-casts against. +#[derive(Default)] +struct TriangleRecorder(Mutex>); + +impl QueryDispatcher for TriangleRecorder { + fn intersection_test( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + DefaultQueryDispatcher.intersection_test(pos12, g1, g2) + } + + fn distance( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result { + DefaultQueryDispatcher.distance(pos12, g1, g2) + } + + fn contact( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + prediction: Real, + ) -> Result, Unsupported> { + DefaultQueryDispatcher.contact(pos12, g1, g2, prediction) + } + + fn closest_points( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + max_dist: Real, + ) -> Result { + DefaultQueryDispatcher.closest_points(pos12, g1, g2, max_dist) + } + + fn cast_shapes( + &self, + pos12: &Pose, + local_vel12: Vector, + g1: &dyn Shape, + g2: &dyn Shape, + options: ShapeCastOptions, + ) -> Result, Unsupported> { + if let Some(tri) = g1.as_triangle() { + self.0.lock().unwrap().push(*tri); + } + DefaultQueryDispatcher.cast_shapes(pos12, local_vel12, g1, g2, options) + } + + fn cast_shapes_nonlinear( + &self, + motion1: &NonlinearRigidMotion, + g1: &dyn Shape, + motion2: &NonlinearRigidMotion, + g2: &dyn Shape, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, + ) -> Result, Unsupported> { + DefaultQueryDispatcher.cast_shapes_nonlinear( + motion1, + g1, + motion2, + g2, + start_time, + end_time, + stop_at_penetration, + ) + } +} + +/// The triangles of `heightfield` that a shape-cast of `g2` from `pos12` along `vel12` shape-casts +/// against, and its result. +fn cast_triangles( + heightfield: &HeightField, + pos12: &Pose, + vel12: Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> (Vec, Option) { + let recorder = TriangleRecorder::default(); + let hit = + cast_shapes_heightfield_shape(&recorder, pos12, vel12, heightfield, g2, options).unwrap(); + (recorder.0.into_inner().unwrap(), hit) +} + +/// A ball dropping by 0.1 toward the bottom of a valley, from 0.2 above it. +#[test] +fn heightfield_shape_cast_skips_the_triangles_out_of_reach() { + let valley = heightfield(41, 40.0, |i, _| 0.02 * (i - 20.0).powi(2)); + let ball = Ball::new(0.4); + let pos12 = Pose::translation(0.3, 0.2 + 0.4, 0.0); + + let (triangles, hit) = cast_triangles( + &valley, + &pos12, + -Vector::Y, + &ball, + ShapeCastOptions::with_max_time_of_impact(0.1), + ); + + assert!(hit.is_none()); + assert!(triangles.is_empty(), "{} triangles cast", triangles.len()); +} + +/// A ball skimming down onto flat ground along -X hits it 10 along X from its start, at X = 5. +#[test] +fn heightfield_shape_cast_skips_the_triangles_past_the_nearest_hit() { + let flat = heightfield(41, 40.0, |_, _| 0.0); + let ball = Ball::new(0.4); + let pos12 = Pose::translation(15.0, 1.4, 0.3); + let vel12 = Vector::new(-1.0, -0.1, 0.0); + + let (triangles, hit) = cast_triangles( + &flat, + &pos12, + vel12, + &ball, + ShapeCastOptions::with_max_time_of_impact(30.0), + ); + + let hit = hit.expect("the ball lands on the ground"); + assert_relative_eq!(hit.time_of_impact, 10.0, epsilon = 1.0e-3); + // The ball's Aabb reaches X = 4.6 by then, short of the cell [3, 4]. + let past_the_hit = triangles + .iter() + .filter(|tri| tri.a.x.max(tri.b.x).max(tri.c.x) <= 4.0) + .count(); + assert_eq!(past_the_hit, 0, "{} triangles cast", triangles.len()); +} + +/// A ball dropping onto flat ground lands at the very end of its shape-cast: the cast's +/// `max_time_of_impact` is the time of impact against the triangles on their own. +#[test] +fn heightfield_shape_cast_hits_at_the_end_of_the_cast() { + let flat = heightfield(11, 10.0, |_, _| 0.0); + let ball = Ball::new(0.4); + let pos2 = Pose::translation(0.3, 1.4, 0.2); + let landing = triangle_hits( + &Pose::IDENTITY, + &flat, + &pos2, + -Vector::Y, + &ball, + ShapeCastOptions::with_max_time_of_impact(2.0), + ) + .iter() + .map(|hit| hit.time_of_impact) + .reduce(Real::min) + .expect("the ball lands on the ground"); + + let hits = assert_nearest_triangle_hit( + &Pose::IDENTITY, + &flat, + &pos2, + -Vector::Y, + &ball, + ShapeCastOptions::with_max_time_of_impact(landing), + ); + + assert!(hits); +} diff --git a/src/query/shape_cast/shape_cast_heightfield_shape.rs b/src/query/shape_cast/shape_cast_heightfield_shape.rs index 83182034..eeae18b0 100644 --- a/src/query/shape_cast/shape_cast_heightfield_shape.rs +++ b/src/query/shape_cast/shape_cast_heightfield_shape.rs @@ -1,10 +1,101 @@ -use crate::bounding_volume::BoundingVolume; +use crate::bounding_volume::{Aabb, BoundingVolume}; use crate::math::{Pose, Real, Vector}; use crate::query::details::ShapeCastOptions; +#[cfg(feature = "dim3")] +use crate::query::RayCast; use crate::query::{QueryDispatcher, Ray, ShapeCastHit, Unsupported}; -use crate::shape::{HeightField, Shape}; +#[cfg(feature = "dim2")] +use crate::shape::Segment as HeightFieldPart; #[cfg(feature = "dim3")] -use crate::{bounding_volume::Aabb, query::RayCast}; +use crate::shape::Triangle as HeightFieldPart; +use crate::shape::{HeightField, Shape, SupportMap}; + +/// How much farther than the target distance a shape may stay from a triangle that GJK reports +/// as hit, relative to the length of the sweep and the target distance, and in absolute terms +/// below 1. Chosen from measurements, not derived from GJK: random shape-casts with shapes from +/// 1e-4 to 100 across found no hit beyond it, except on shapes a few 1e-3 across that GJK +/// reports as hit while they stay clear of the triangle by about their own size. +const GJK_TOLERANCE: Real = 2.0e-3; + +/// The rounding of the coordinates the clearance is computed from, relative to their magnitude. +const ROUNDING: Real = 1.0e-5; + +/// The shape moving in a shape-cast against a heightfield, to tell which of the heightfield's +/// triangles (segments in 2D) it can't hit. +struct CastShape<'a> { + pos12: &'a Pose, + vel12: Vector, + aabb: Aabb, + support_map: Option<&'a dyn SupportMap>, + target_distance: Real, +} + +impl<'a> CastShape<'a> { + fn new(pos12: &'a Pose, vel12: Vector, g2: &'a dyn Shape, target_distance: Real) -> Self { + Self { + pos12, + vel12, + aabb: g2.compute_aabb(pos12), + support_map: g2.as_support_map(), + target_distance, + } + } + + /// Whether the shape stays farther than the target distance from `part` until the time + /// `reach`, so that the shape-cast against `part` can't hit before `reach`. + /// + /// The shape must stay farther by a margin above GJK's tolerance and the rounding of the + /// coordinates involved. A value that isn't finite, or a negative target distance, keeps the + /// part within reach. + fn misses(&self, part: &HeightFieldPart, reach: Real) -> bool { + let sweep = self.vel12 * reach; + let part_aabb = part.local_aabb(); + let corners = [ + part_aabb.mins, + part_aabb.maxs, + self.aabb.mins, + self.aabb.maxs, + sweep, + ]; + if !corners.iter().all(|corner| corner.is_finite()) + || !self.target_distance.is_finite() + || self.target_distance < 0.0 + { + return false; + } + let scale = corners + .iter() + .map(|corner| corner.abs().max_element()) + .fold(0.0, Real::max); + let clearance = self.target_distance + + GJK_TOLERANCE * (1.0 + sweep.length() + self.target_distance) + + ROUNDING * scale; + + // Apart along an axis over the whole sweep. + let start = self.aabb.loosened(clearance); + if !start + .merged(&start.translated(sweep)) + .intersects(&part_aabb) + { + return true; + } + + // On one side of the part's line (2D) or plane (3D) over the whole sweep. + let (Some(support_map), Some(normal)) = (self.support_map, part.normal()) else { + return false; + }; + let nearest = normal.dot(support_map.support_point(self.pos12, -normal) - part.a); + let farthest = normal.dot(support_map.support_point(self.pos12, normal) - part.a); + let approach = normal.dot(sweep); + nearest + approach.min(0.0) > clearance || farthest + approach.max(0.0) < -clearance + } +} + +/// The time up to which a triangle (segment in 2D) of a heightfield must be shape-cast against: +/// one the shape can't hit before can't yield a hit nearer than the nearest one so far. +fn reach(best_hit: Option, options: &ShapeCastOptions) -> Real { + best_hit.map_or(options.max_time_of_impact, |hit| hit.time_of_impact) +} /// Time Of Impact between a moving shape and a heightfield. #[cfg(feature = "dim2")] @@ -30,6 +121,7 @@ pub fn cast_shapes_heightfield_shape( } let mut best_hit = None::; + let cast_shape = CastShape::new(pos12, vel12, g2, options.target_distance); /* * Test the segment under the ray. @@ -38,7 +130,9 @@ pub fn cast_shapes_heightfield_shape( ..curr_range.end.clamp(0, heightfield1.num_cells() as isize) as usize; for curr in clamped_curr_range { if let Some(seg) = heightfield1.segment_at(curr) { - // TODO: pre-check using a ray-cast on the Aabbs first? + if cast_shape.misses(&seg, reach(best_hit, &options)) { + continue; + } if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &seg, g2, options)? { if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) { best_hit = Some(hit); @@ -79,7 +173,9 @@ pub fn cast_shapes_heightfield_shape( } if let Some(seg) = heightfield1.segment_at(curr_elt as usize) { - // TODO: pre-check using a ray-cast on the Aabbs first? + if cast_shape.misses(&seg, reach(best_hit, &options)) { + continue; + } if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &seg, g2, options)? { if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) { best_hit = Some(hit); @@ -120,6 +216,7 @@ pub fn cast_shapes_heightfield_shape( heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1); let (ncells_i, ncells_j) = heightfield1.num_cells_ij(); let mut best_hit = None::; + let cast_shape = CastShape::new(pos12, vel12, g2, options.target_distance); /* * Enlarge the ranges by 1 to account for any movement within one cell. @@ -148,7 +245,9 @@ pub fn cast_shapes_heightfield_shape( if i >= 0 && j >= 0 { let (tri_a, tri_b) = heightfield1.triangles_at(i as usize, j as usize); for tri in [tri_a, tri_b].into_iter().flatten() { - // TODO: pre-check using a ray-cast on the Aabbs first? + if cast_shape.misses(&tri, reach(best_hit, &options)) { + continue; + } if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &tri, g2, options)? { if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) { From c203a46a692d2d3f306362ea0a49a06aae3f4342 Mon Sep 17 00:00:00 2001 From: Michael Herzog <8261669+VitaminCpp@users.noreply.github.com> Date: Fri, 6 Nov 2026 10:26:53 +0100 Subject: [PATCH 2/2] chore: add the pull request number to the CHANGELOG Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 15ea225a..739a36d0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ the moving shape can't hit within `max_time_of_impact`, or before the nearest hit found so far, which skips running GJK on most of the triangles it walks through. The results are unchanged, except for some hits GJK reports on shapes a few 1e-3 across that stay clear of the triangle - by about their own size ([#PR_NUMBER](https://github.com/dimforge/parry/pull/PR_NUMBER)). + by about their own size ([#464](https://github.com/dimforge/parry/pull/464)). - Deserializing a `ContactManifoldsWorkspace` that was serialized by a custom contact-manifolds algorithm no longer fails with `"Cannot deserialize custom shape."`. It now yields the new `UnknownContactManifoldsWorkspace`, which the relevant algorithm can replace with a workspace