diff --git a/CHANGELOG.md b/CHANGELOG.md index ec4c721d..739a36d0 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 ([#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 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) {