From 5794a4e11e7a8ade3916622c030af6cc1a553a37 Mon Sep 17 00:00:00 2001
From: Marcel Admiraal <madmiraal@users.noreply.github.com>
Date: Mon, 8 Jun 2020 17:18:23 +0100
Subject: [PATCH] Test collision mask before creating constraint pair in Godot
 physics broadphase 2D and 3D.

---
 servers/physics/broad_phase_basic.cpp           | 7 +++++--
 servers/physics/space_sw.cpp                    | 6 ++++++
 servers/physics_2d/broad_phase_2d_basic.cpp     | 7 +++++--
 servers/physics_2d/broad_phase_2d_hash_grid.cpp | 5 ++++-
 servers/physics_2d/space_2d_sw.cpp              | 6 ++++++
 5 files changed, 26 insertions(+), 5 deletions(-)

diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp
index f49bf9d4cc0..dc8091bfa2c 100644
--- a/servers/physics/broad_phase_basic.cpp
+++ b/servers/physics/broad_phase_basic.cpp
@@ -202,9 +202,12 @@ void BroadPhaseBasic::update() {
 			if (pair_ok && !E) {
 
 				void *data = NULL;
-				if (pair_callback)
+				if (pair_callback) {
 					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
-				pair_map.insert(key, data);
+					if (data) {
+						pair_map.insert(key, data);
+					}
+				}
 			}
 		}
 	}
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 222da7ed584..ba1070a3cb8 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -996,6 +996,9 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 }
 
 void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) {
+	if (!A->test_collision_mask(B)) {
+		return nullptr;
+	}
 
 	CollisionObjectSW::Type type_A = A->get_type();
 	CollisionObjectSW::Type type_B = B->get_type();
@@ -1034,6 +1037,9 @@ void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, Collisio
 }
 
 void SpaceSW::_broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self) {
+	if (!p_data) {
+		return;
+	}
 
 	SpaceSW *self = (SpaceSW *)p_self;
 	self->collision_pairs--;
diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp
index 11bf8712ac4..84db0b13c6a 100644
--- a/servers/physics_2d/broad_phase_2d_basic.cpp
+++ b/servers/physics_2d/broad_phase_2d_basic.cpp
@@ -159,9 +159,12 @@ void BroadPhase2DBasic::update() {
 			if (pair_ok && !E) {
 
 				void *data = NULL;
-				if (pair_callback)
+				if (pair_callback) {
 					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
-				pair_map.insert(key, data);
+					if (data) {
+						pair_map.insert(key, data);
+					}
+				}
 			}
 		}
 	}
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 711ff9f1f78..4c0740dc0e6 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -83,7 +83,10 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
 			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 {
 
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 08a261da2ae..4748094fe26 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -1122,6 +1122,9 @@ 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();
@@ -1160,6 +1163,9 @@ 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--;