Merge pull request #47084 from nekomatata/cylinder-capsule-collision-fix

Fix capsule-cylinder collision in godot physics

1 files changed, 45 insertions, 0 deletions

diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 651961433c..9c10c2a70b 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -1627,6 +1627,51 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p
 
 	SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	// Cylinder B end caps.
+	Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized();
+	if (!separator.test_axis(cylinder_B_axis)) {
+		return;
+	}
+
+	// Cylinder edge against capsule balls.
+
+	Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1);
+
+	Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5);
+	Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5);
+
+	if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_2).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) {
+		return;
+	}
+
+	// Cylinder edge against capsule edge.
+
+	Vector3 center_diff = p_transform_b.origin - p_transform_a.origin;
+
+	if (!separator.test_axis(capsule_A_axis.cross(center_diff).cross(capsule_A_axis).normalized())) {
+		return;
+	}
+
+	if (!separator.test_axis(cylinder_B_axis.cross(center_diff).cross(cylinder_B_axis).normalized())) {
+		return;
+	}
+
+	real_t proj = capsule_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(capsule_A_axis);
+	if (Math::is_zero_approx(proj)) {
+		// Parallel capsule and cylinder axes, handle with specific axes only.
+		// Note: GJKEPA with no margin can lead to degenerate cases in this situation.
+		separator.generate_contacts();
+		return;
+	}
+
 	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points;
 
 	// Fallback to generic algorithm to find the best separating axis.