Fix GodotPhysics solver with kinematic body set to report contacts

In 3D, collision is disabled between kinematic/static bodies when
contacts are generated only to report them.

In 2D, this case was already fixed but the code is cleaned to make it
easier to follow.

1 files changed, 16 insertions, 4 deletions

diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index 6012ff1522..0703007736 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -211,11 +211,21 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) {
 
 bool BodyPair3DSW::setup(real_t p_step) {
 	//cannot collide
-	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
+	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
 		collided = false;
 		return false;
 	}
 
+	bool report_contacts_only = false;
+	if ((A->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC) && (B->get_mode() <= PhysicsServer3D::BODY_MODE_KINEMATIC)) {
+		if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
+			report_contacts_only = true;
+		} else {
+			collided = false;
+			return false;
+		}
+	}
+
 	if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
 		collided = false;
 		return false;
@@ -279,12 +289,9 @@ bool BodyPair3DSW::setup(real_t p_step) {
 		real_t depth = c.normal.dot(global_A - global_B);
 
 		if (depth <= 0) {
-			c.active = false;
 			continue;
 		}
 
-		c.active = true;
-
 #ifdef DEBUG_ENABLED
 
 		if (space->is_debugging_contacts()) {
@@ -308,6 +315,11 @@ bool BodyPair3DSW::setup(real_t p_step) {
 			B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
 		}
 
+		if (report_contacts_only) {
+			collided = false;
+			continue;
+		}
+
 		c.active = true;
 
 		// Precompute normal mass, tangent mass, and bias.