Test collision mask before creating constraint pair in Godot physics broadphase 2D and 3D.

2 files changed, 11 insertions, 1 deletions

diff --git a/servers/physics_3d/broad_phase_3d_basic.cpp b/servers/physics_3d/broad_phase_3d_basic.cpp
index 0f271b33af..f5ea1897a9 100644
--- a/servers/physics_3d/broad_phase_3d_basic.cpp
+++ b/servers/physics_3d/broad_phase_3d_basic.cpp
@@ -190,8 +190,10 @@ void BroadPhase3DBasic::update() {
 				void *data = nullptr;
 				if (pair_callback) {
 					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
+					if (data) {
+						pair_map.insert(key, data);
+					}
 				}
-				pair_map.insert(key, data);
 			}
 		}
 	}
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 4d272bdabd..48f250ba35 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -987,6 +987,10 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 }
 
 void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) {
+	if (!A->test_collision_mask(B)) {
+		return nullptr;
+	}
+
 	CollisionObject3DSW::Type type_A = A->get_type();
 	CollisionObject3DSW::Type type_B = B->get_type();
 	if (type_A > type_B) {
@@ -1019,6 +1023,10 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
 }
 
 void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) {
+	if (!p_data) {
+		return;
+	}
+
 	Space3DSW *self = (Space3DSW *)p_self;
 	self->collision_pairs--;
 	Constraint3DSW *c = (Constraint3DSW *)p_data;