4 files changed, 27 insertions, 16 deletions

diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
index 7b7c67bbc2..1684835e76 100644
--- a/servers/physics_2d/godot_body_pair_2d.cpp
+++ b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -161,6 +161,11 @@ void GodotBodyPair2D::_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 GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
 	Vector2 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
@@ -180,24 +185,32 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
 		return false;
 	}
 
-	// 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.
+	// 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.
 	int a;
 	Vector2 s[2];
-	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
+	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a);
 	Vector2 from = p_xform_A.xform(s[0]);
+	// 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.
 	Vector2 to = from + motion;
 
 	Transform2D from_inv = p_xform_B.affine_inverse();
 
-	// Start from a little inside the bounding box.
-	Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
+	// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+	// At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. But it still works out.
+	Vector2 local_from = from_inv.xform(from - motion * 0.1);
 	Vector2 local_to = from_inv.xform(to);
 
 	Vector2 rpos, rnorm;
 	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
+		// 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;
 	}
 
@@ -215,7 +228,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
 	// next frame will hit softly or soft enough.
 	Vector2 hitpos = p_xform_B.xform(rpos);
 
-	real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+	real_t newlen = hitpos.distance_to(from) + (max - min) * 0.01; // adding 1% of body length to the distance between collision and support point should cause body A's support point to arrive just within B's collider next frame.
 	p_A->set_linear_velocity(mnormal * (newlen / p_step));
 
 	return true;
diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp
index da414ae233..6823cb32c1 100644
--- a/servers/physics_2d/godot_shape_2d.cpp
+++ b/servers/physics_2d/godot_shape_2d.cpp
@@ -369,8 +369,9 @@ void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_suppo
 	Vector2 n = p_normal;
 
 	real_t d = n.y;
+	real_t h = height * 0.5 - radius; // half-height of the rectangle part
 
-	if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) {
+	if (h > 0 && Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) {
 		// make it flat
 		n.y = 0.0;
 		n.normalize();
@@ -378,13 +379,10 @@ void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_suppo
 
 		r_amount = 2;
 		r_supports[0] = n;
-		r_supports[0].y += height * 0.5 - radius;
+		r_supports[0].y += h;
 		r_supports[1] = n;
-		r_supports[1].y -= height * 0.5 - radius;
-
+		r_supports[1].y -= h;
 	} else {
-		real_t h = height * 0.5 - radius;
-
 		n *= radius;
 		n.y += (d > 0) ? h : -h;
 		r_amount = 1;
diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp
index 46718c8819..b7d79bc0e3 100644
--- a/servers/physics_2d/godot_step_2d.cpp
+++ b/servers/physics_2d/godot_step_2d.cpp
@@ -71,7 +71,7 @@ void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D
 	}
 }
 
-void GodotStep2D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
+void GodotStep2D::_setup_constraint(uint32_t p_constraint_index, void *p_userdata) {
 	GodotConstraint2D *constraint = all_constraints[p_constraint_index];
 	constraint->setup(delta);
 }
@@ -238,8 +238,8 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
 
 	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
 
-	uint32_t total_contraint_count = all_constraints.size();
-	WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_contraint, nullptr, total_contraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
+	uint32_t total_constraint_count = all_constraints.size();
+	WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &GodotStep2D::_setup_constraint, nullptr, total_constraint_count, -1, true, SNAME("Physics2DConstraintSetup"));
 	WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
 
 	{ //profile
diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h
index 9f8fdd6ce3..4b3b6fc966 100644
--- a/servers/physics_2d/godot_step_2d.h
+++ b/servers/physics_2d/godot_step_2d.h
@@ -47,7 +47,7 @@ class GodotStep2D {
 	LocalVector<GodotConstraint2D *> all_constraints;
 
 	void _populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island);
-	void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
+	void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
 	void _pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const;
 	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
 	void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const;