Merge pull request #46917 from nekomatata/solver-kinematic-bug-fix

Fix GodotPhysics solver with kinematic body set to report contacts

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 06bafb1624..36114c0c91 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -213,11 +213,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;
@@ -281,12 +291,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()) {
@@ -310,6 +317,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.