From 67991f6fbc75ca8c7fa517619d72d2198a95d039 Mon Sep 17 00:00:00 2001
From: alexpardes <alexpardes@gmail.com>
Date: Sun, 23 Jun 2024 07:23:22 -0700
Subject: [PATCH] fix: respect stop_on_penetration flag in ball-ball shape
 casting (#206)

---
 .../parry2d/tests/geometry/time_of_impact2.rs | 64 +++++++++++++++++++
 src/query/shape_cast/shape_cast_ball_ball.rs  |  4 ++
 2 files changed, 68 insertions(+)

diff --git a/crates/parry2d/tests/geometry/time_of_impact2.rs b/crates/parry2d/tests/geometry/time_of_impact2.rs
index bcb47ec2..bdfafeda 100644
--- a/crates/parry2d/tests/geometry/time_of_impact2.rs
+++ b/crates/parry2d/tests/geometry/time_of_impact2.rs
@@ -4,6 +4,70 @@ use parry2d::query;
 use parry2d::query::details::ShapeCastOptions;
 use parry2d::shape::{Ball, Cuboid, Polyline, Segment};
 
+#[test]
+fn ball_ball_intersecting_toi() {
+    let ball1 = Ball::new(1.0);
+    let ball2 = Ball::new(2.0);
+
+    let ball1_pos_intersecting = Isometry2::new(Vector2::new(1.0, 1.0), 0.0);
+    let ball2_pos = Isometry2::identity();
+
+    let ball1_vel_separating = Vector2::new(1.0, 2.0);
+    let ball1_vel_penetrating = Vector2::new(1.0, -2.0);
+    let ball2_vel = Vector2::zeros();
+
+    let toi_separating = query::cast_shapes(
+        &ball1_pos_intersecting,
+        &ball1_vel_separating,
+        &ball1,
+        &ball2_pos,
+        &ball2_vel,
+        &ball2,
+        ShapeCastOptions::default(),
+    )
+    .unwrap();
+
+    let toi_penetrating_ignore_pen = query::cast_shapes(
+        &ball1_pos_intersecting,
+        &ball1_vel_penetrating,
+        &ball1,
+        &ball2_pos,
+        &ball2_vel,
+        &ball2,
+        ShapeCastOptions {
+            stop_at_penetration: false,
+            ..Default::default()
+        },
+    )
+    .unwrap();
+
+    let toi_separating_ignore_pen = query::cast_shapes(
+        &ball1_pos_intersecting,
+        &ball1_vel_separating,
+        &ball1,
+        &ball2_pos,
+        &ball2_vel,
+        &ball2,
+        ShapeCastOptions {
+            stop_at_penetration: false,
+            ..Default::default()
+        },
+    )
+    .unwrap();
+
+    assert_eq!(toi_separating.map(|hit| hit.time_of_impact), Some(0.0));
+
+    assert_eq!(
+        toi_penetrating_ignore_pen.map(|hit| hit.time_of_impact),
+        Some(0.0),
+    );
+
+    assert_eq!(
+        toi_separating_ignore_pen.map(|hit| hit.time_of_impact),
+        None
+    );
+}
+
 #[test]
 fn ball_cuboid_toi() {
     let cuboid = Cuboid::new(Vector2::new(1.0, 1.0));
diff --git a/src/query/shape_cast/shape_cast_ball_ball.rs b/src/query/shape_cast/shape_cast_ball_ball.rs
index f41744e6..29d5204f 100644
--- a/src/query/shape_cast/shape_cast_ball_ball.rs
+++ b/src/query/shape_cast/shape_cast_ball_ball.rs
@@ -45,6 +45,10 @@ pub fn cast_shapes_ball_ball(
             witness2 = Point::from(*normal2 * b2.radius);
         }
 
+        if !options.stop_at_penetration && time_of_impact < 1.0e-5 && normal1.dot(vel12) >= 0.0 {
+            return None;
+        }
+
         let status = if inside && center.coords.norm_squared() < rsum * rsum {
             ShapeCastStatus::PenetratingOrWithinTargetDist
         } else {