Fix Physics3D and Physics2D CCD sometimes adjusting velocity too much (preventing collision) or not enough (allowing tunneling)

1 files changed, 20 insertions, 9 deletions

diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 7e6cc6f834..981a7c502f 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -161,6 +161,11 @@ void GodotBodyPair3D::validate_contacts() {
 	}
 }
 
+// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
+// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
+// Process: only proceed if body A's motion is high relative to its size.
+// cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
+// adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
 bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
 	Vector3 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
@@ -177,33 +182,39 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
 	// Let's say it should move more than 1/3 the size of the object in that axis.
 	bool fast_object = mlen > (max - min) * 0.3;
 	if (!fast_object) {
-		return false;
+		return false; // moving slow enough that there's no chance of tunneling.
 	}
 
-	// Going too fast in that direction.
+	// A is moving fast enough that tunneling might occur. See if it's really about to collide.
 
 	// Cast a segment from support in motion normal, in the same direction of motion by motion length.
-	// Support is the worst case collision point, so real collision happened before.
-	Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
+	// Support point will the farthest forward collision point along the movement vector.
+	// i.e. the point that should hit B first if any collision does occur.
+
+	// convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
+	Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform_inv(mnormal).normalized());
 	Vector3 from = p_xform_A.xform(s);
+	// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+	// This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
+	from -= motion * 0.1;
 	Vector3 to = from + motion;
 
 	Transform3D from_inv = p_xform_B.affine_inverse();
 
-	// Start from a little inside the bounding box.
-	Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
+	Vector3 local_from = from_inv.xform(from);
 	Vector3 local_to = from_inv.xform(to);
 
 	Vector3 rpos, rnorm;
 	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, true)) {
+		// there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
+		// actually collide yet on next frame. We'll probably check again next frame once they're closer.
 		return false;
 	}
 
-	// Shorten the linear velocity so it does not hit, but gets close enough,
-	// next frame will hit softly or soft enough.
+	// Shorten the linear velocity so it will collide next frame.
 	Vector3 hitpos = p_xform_B.xform(rpos);
 
-	real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+	real_t newlen = hitpos.distance_to(from); // this length (speed) should cause the point we chose slightly behind A's support point to arrive right at B's collider next frame.
 	p_A->set_linear_velocity((mnormal * newlen) / p_step);
 
 	return true;