diff --git a/CHANGELOG.md b/CHANGELOG.md index ec4c721d..bc48c1e9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,13 @@ ## Unreleased +### Fixed + +- GJK no longer reports a stalled simplex as "touching" when the penetration bound is merely within + `sqrt(eps)` of the largest CSO support magnitude. With a large shape (e.g. a 200x200 ground cuboid) + that tolerance exceeded the size of the other shape, and the stale search direction was returned as + the contact normal. The tolerance is now `10 * eps` of that magnitude, so such stalls are + perturbed and, failing that, resolved by EPA. + ### Modified - Deserializing a `ContactManifoldsWorkspace` that was serialized by a custom contact-manifolds diff --git a/crates/parry3d/tests/geometry/cylinder_large_cuboid_normals.rs b/crates/parry3d/tests/geometry/cylinder_large_cuboid_normals.rs new file mode 100644 index 00000000..c3dc2fa0 --- /dev/null +++ b/crates/parry3d/tests/geometry/cylinder_large_cuboid_normals.rs @@ -0,0 +1,140 @@ +use parry3d::math::{Pose, Rotation, Vector}; +use parry3d::query::details::contact_manifold_pfm_pfm_shapes; +use parry3d::query::ContactManifold; +use parry3d::shape::{Cuboid, Cylinder}; + +// A wheel (cylinder, radius 0.1, half-height 0.05) spinning at ~43 rad/s and rolling on a +// 200x200x0.5 ground cuboid. The poses are consecutive simulation steps, so the manifold's +// cached normal is carried from one step to the next, as in a physics engine. +// +// The wheel hovers ~1e-4 above the ground. With the ground's far corners, the CSO support +// points reach ~144, and GJK stalled with a lower bound of -3.3e-2 on the penetration. That +// was accepted as "touching" because the tolerance scaled with `sqrt(eps) * support_scale` +// (~0.15), more than the whole wheel, and the stale `old_dir` was returned as the contact +// normal: (-0.51, 0.69, 0.51) instead of (0, 1, 0). +#[test] +fn cylinder_rolling_on_large_cuboid_keeps_a_vertical_normal() { + // (body translation, body rotation as xyzw) for 9 consecutive steps. + const POSES: [([f32; 3], [f32; 4]); 9] = [ + ( + [2.1498301029205322, 0.12448107451200485, 0.3996926546096802], + [ + 0.2489599585533142, + -0.6622065305709839, + 0.24911077320575714, + 0.6613963842391968, + ], + ), + ( + [2.1495745182037354, 0.12121988832950592, 0.3993242681026459], + [ + 0.46536311507225037, + -0.5327503085136414, + 0.46630358695983887, + 0.5312016010284424, + ], + ), + ( + [2.149324655532837, 0.11523353308439255, 0.39895594120025635], + [ + 0.6219001412391663, + -0.335325688123703, + 0.6241053342819214, + 0.333600789308548, + ], + ), + ( + [2.149080514907837, 0.10652196407318115, 0.398587703704834], + [ + 0.698938250541687, + -0.0951799601316452, + 0.7025531530380249, + 0.09405004233121872, + ], + ), + ( + [2.1422312259674072, 0.10064999759197235, 0.3972579836845398], + [ + 0.6860884428024292, + 0.16269077360630035, + 0.6905015707015991, + -0.16131402552127838, + ], + ), + ( + [2.1318469047546387, 0.10105381906032562, 0.3973941504955292], + [ + 0.5856216549873352, + 0.39615002274513245, + 0.5881026387214661, + -0.39274439215660095, + ], + ), + ( + [2.120213747024536, 0.10016228258609772, 0.3976336121559143], + [ + 0.409687876701355, + 0.5779147148132324, + 0.4103825092315674, + -0.5742447972297668, + ], + ), + ( + [2.105894088745117, 0.1001090407371521, 0.3976670503616333], + [ + 0.1809343695640564, + 0.6852918267250061, + 0.1802733689546585, + -0.6820118427276611, + ], + ), + ( + [2.0811259746551514, 0.11391204595565796, 0.40611711144447327], + [ + -0.03625503554940224, + 0.7045886516571045, + -0.047442466020584106, + -0.7070997357368469, + ], + ), + ]; + const PREDICTION: f32 = 0.02; + + let ground = Cuboid::new(Vector::new(100.0, 0.25, 100.0)); + let ground_pose = Pose::from_translation(Vector::new(0.0, -0.25, 0.0)); + let wheel = Cylinder::new(0.05, 0.1); + // The cylinder's axis (local Y) is laid along the body's local X. + let axle_to_body_x = Rotation::from_xyzw( + 0.0, + 0.0, + core::f32::consts::FRAC_1_SQRT_2, + core::f32::consts::FRAC_1_SQRT_2, + ); + + let mut manifold = ContactManifold::<(), ()>::new(); + for (step, (t, q)) in POSES.iter().enumerate() { + let body = Pose::from_parts( + Vector::new(t[0], t[1], t[2]), + Rotation::from_xyzw(q[0], q[1], q[2], q[3]), + ); + let wheel_pose = body * Pose::from_parts(Vector::ZERO, axle_to_body_x); + let pos12 = ground_pose.inv_mul(&wheel_pose); + + contact_manifold_pfm_pfm_shapes( + &pos12, + &ground, + &wheel, + None, + None, + PREDICTION, + &mut manifold, + ); + + assert!(!manifold.points.is_empty(), "step {step}: no contact"); + assert!( + manifold.local_n1.y > 0.99, + "step {step}: contact normal {:?} is not vertical", + manifold.local_n1 + ); + } +} diff --git a/crates/parry3d/tests/geometry/mod.rs b/crates/parry3d/tests/geometry/mod.rs index 7eb925aa..5eacd54e 100644 --- a/crates/parry3d/tests/geometry/mod.rs +++ b/crates/parry3d/tests/geometry/mod.rs @@ -4,6 +4,7 @@ mod ball_triangle_toi; mod convex_hull; mod cuboid_ray_cast; mod cylinder_cuboid_contact; +mod cylinder_large_cuboid_normals; mod epa3; mod still_objects_toi; mod time_of_impact3; diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 56f4caea..9cb64cb3 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -412,7 +412,7 @@ where } else { return GJKResult::Proximity(old_dir); } - } else if exact_dist && best_min_bound >= -_eps_rel * support_scale { + } else if exact_dist && best_min_bound >= -_eps_tol * support_scale { // Origin on the CSO boundary: the pair is touching, so the witnesses // describe the contact. let (p1, p2) = result(simplex, true); @@ -457,7 +457,7 @@ where } else { return GJKResult::Proximity(dir); } - } else if exact_dist && best_min_bound >= -_eps_rel * support_scale { + } else if exact_dist && best_min_bound >= -_eps_tol * support_scale { let (p1, p2) = result(simplex, false); return GJKResult::ClosestPoints(p1, p2, dir); } else if nb_perturbs < MAX_PERTURBATIONS {