diff --git a/crates/parry3d/tests/geometry/time_of_impact3.rs b/crates/parry3d/tests/geometry/time_of_impact3.rs index 8bf314a0..537025a9 100644 --- a/crates/parry3d/tests/geometry/time_of_impact3.rs +++ b/crates/parry3d/tests/geometry/time_of_impact3.rs @@ -59,3 +59,72 @@ fn ball_cuboid_toi() { )); assert_eq!(toi_wont_touch, None); } + +#[test] +fn shape_cast_toi_accuracy_does_not_scale_with_shape_extent() { + const BALL_RADIUS: Real = 0.5166; + const MAX_TOI_ERROR: Real = 2.0e-4; // 0.2 mm. + const MAX_WITNESS_ERROR: Real = 1.0e-6; + const MAX_NORMAL_ERROR: Real = 5.0e-4; + + let ball = Ball::new(BALL_RADIUS); + let direction = -Vector::Y; + let mut toi_errors = Vec::new(); + + for half_extent in [5.0, 50.0, 500.0] { + let ground = Cuboid::new(Vector::new(half_extent, 0.5, half_extent)); + let ground_pose = Pose::translation(0.0, -0.5, 0.0); + let mut max_toi_error: Real = 0.0; + let mut max_witness_error: Real = 0.0; + let mut max_normal_error: Real = 0.0; + + // Sweep sub-millimetre start offsets at several lateral positions. This mirrors + // suspension probes near their resting pose while exercising large support points. + for i in 0..200 { + let y = 1.177 + i as Real * 5.0e-5; + for (x, z) in [(1.4, 2.8), (-1.4, -0.4), (1.4, -2.0), (-1.4, 2.0)] { + let ball_pose = Pose::translation(x, y, z); + let hit = query::cast_shapes( + &ground_pose, + Vector::ZERO, + &ground, + &ball_pose, + direction, + &ball, + ShapeCastOptions::with_max_time_of_impact(2.0), + ) + .unwrap() + .expect("the downward cast should hit the cuboid"); + + let expected_toi = y - BALL_RADIUS; + max_toi_error = max_toi_error.max((hit.time_of_impact - expected_toi).abs()); + + let witness1 = ground_pose.transform_point(hit.witness1); + max_witness_error = max_witness_error.max(witness1.y.abs()); + max_normal_error = max_normal_error.max((hit.normal1 - Vector::Y).length()); + } + } + + assert!( + max_witness_error <= MAX_WITNESS_ERROR, + "shape-cast witness error {max_witness_error} exceeded {MAX_WITNESS_ERROR} for half-extent {half_extent}", + ); + assert!( + max_normal_error <= MAX_NORMAL_ERROR, + "shape-cast normal error {max_normal_error} exceeded {MAX_NORMAL_ERROR} for half-extent {half_extent}", + ); + println!( + "half-extent {half_extent}: max TOI error = {} mm, max witness error = {} mm", + max_toi_error * 1000.0, + max_witness_error * 1000.0, + ); + toi_errors.push((half_extent, max_toi_error)); + } + + for (half_extent, max_toi_error) in toi_errors { + assert!( + max_toi_error <= MAX_TOI_ERROR, + "shape-cast TOI error {max_toi_error} exceeded {MAX_TOI_ERROR} for half-extent {half_extent}", + ); + } +} diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 25009a72..290e8a3d 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -710,7 +710,8 @@ where } let support_point = if max_bound >= old_max_bound { - // Upper bounds inconsistencies. Consider the projection as a valid support point. + // Upper bounds inconsistencies. Keep the projection as a valid support point + // for the last-chance path below. last_chance = true; CsoPoint::single_point(proj + curr_ray.origin) } else { @@ -718,7 +719,19 @@ where }; if last_chance && ltoi > 0.0 { - // last_chance && ltoi > 0.0 && (support_point.point - curr_ray.origin).dot(ldir) >= 0.0 { + // The simplex witnesses remain precise even when large support coordinates + // prevent the upper bound from decreasing. Use their separation along the + // cast direction to refine the lower bound before accepting the impact. + let (witness1, witness2) = result(simplex, simplex.dimension() == DIM); + let witness_ltoi = (witness1 - witness2 - ray.origin).dot(curr_ray.dir); + if witness_ltoi.is_finite() && witness_ltoi > ltoi { + ltoi = witness_ltoi; + + if ltoi / ray_length > max_time_of_impact { + return None; + } + } + return Some((ltoi / ray_length, ldir)); }