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

servers/physics_2d/broad_phase_2d_basic.cpp

@@ -152,8 +152,10 @@ void BroadPhase2DBasic::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);
 			}
 		}
 	}

servers/physics_2d/broad_phase_2d_hash_grid.cpp

@@ -75,7 +75,10 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
 		if (pairing != E->get()->colliding) {
 			if (pairing) {
 				if (pair_callback) {
-					E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
+					void *ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
+					if (ud) {
+						E->get()->ud = ud;
+					}
 				}
 			} else {
 				if (unpair_callback) {

servers/physics_2d/space_2d_sw.cpp

@@ -1111,6 +1111,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 }
 
 void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) {
+	if (!A->test_collision_mask(B)) {
+		return nullptr;
+	}
+
 	CollisionObject2DSW::Type type_A = A->get_type();
 	CollisionObject2DSW::Type type_B = B->get_type();
 	if (type_A > type_B) {
@@ -1143,6 +1147,10 @@ void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, Coll
 }
 
 void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) {
+	if (!p_data) {
+		return;
+	}
+
 	Space2DSW *self = (Space2DSW *)p_self;
 	self->collision_pairs--;
 	Constraint2DSW *c = (Constraint2DSW *)p_data;

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);
 			}
 		}
 	}

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;