Fix physics BVH broadphase update when changing collision layer/mask

The BVH implementation is not checking collision layers on existing
pairs on move like other physics broadphases do.

This is solved by adding a new call to trigger pair callbacks again so
the physics engine can check layers again (specific to the BVH version,
other broadphase implementations just trigger a move like before).

core/math/bvh.h

@@ -59,6 +59,7 @@ public:
 	// is for compatibility with octree
 	typedef void *(*PairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int);
 	typedef void (*UnpairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
+	typedef void *(*CheckPairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
 
 	// allow locally toggling thread safety if the template has been compiled with BVH_THREAD_SAFE
 	void params_set_thread_safe(bool p_enable) {
@@ -97,6 +98,11 @@ public:
 		unpair_callback = p_callback;
 		unpair_callback_userdata = p_userdata;
 	}
+	void set_check_pair_callback(CheckPairCallback p_callback, void *p_userdata) {
+		BVH_LOCKED_FUNCTION
+		check_pair_callback = p_callback;
+		check_pair_callback_userdata = p_userdata;
+	}
 
 	BVHHandle create(T *p_userdata, bool p_active, const Bounds &p_aabb = Bounds(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) {
 		BVH_LOCKED_FUNCTION
@@ -142,6 +148,12 @@ public:
 		move(h, p_aabb);
 	}
 
+	void recheck_pairs(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		recheck_pairs(h);
+	}
+
 	void erase(uint32_t p_handle) {
 		BVHHandle h;
 		h.set(p_handle);
@@ -200,6 +212,13 @@ public:
 		}
 	}
 
+	void recheck_pairs(BVHHandle p_handle) {
+		BVH_LOCKED_FUNCTION
+		if (USE_PAIRS) {
+			_recheck_pairs(p_handle);
+		}
+	}
+
 	void erase(BVHHandle p_handle) {
 		BVH_LOCKED_FUNCTION
 		// call unpair and remove all references to the item
@@ -517,6 +536,23 @@ private:
 		}
 	}
 
+	void _recheck_pair(BVHHandle p_from, BVHHandle p_to, void *p_pair_data) {
+		tree._handle_sort(p_from, p_to);
+
+		typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
+		typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
+
+		// if the userdata is the same, no collisions should occur
+		if ((exa.userdata == exb.userdata) && exa.userdata) {
+			return;
+		}
+
+		// callback
+		if (check_pair_callback) {
+			check_pair_callback(check_pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, p_pair_data);
+		}
+	}
+
 	// returns true if unpair
 	bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVHABB_CLASS &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) {
 		BVHABB_CLASS abb_to;
@@ -620,6 +656,18 @@ private:
 		}
 	}
 
+	// Send pair callbacks again for all existing pairs for the given handle.
+	void _recheck_pairs(BVHHandle p_handle) {
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
+
+		// checking pair for every partner.
+		for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
+			const typename BVHTREE_CLASS::ItemPairs::Link &pair = p_from.extended_pairs[n];
+			BVHHandle h_to = pair.handle;
+			_recheck_pair(p_handle, h_to, pair.userdata);
+		}
+	}
+
 private:
 	const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const {
 		return tree._extra[p_handle.id()];
@@ -696,8 +744,10 @@ private:
 
 	PairCallback pair_callback;
 	UnpairCallback unpair_callback;
+	CheckPairCallback check_pair_callback;
 	void *pair_callback_userdata;
 	void *unpair_callback_userdata;
+	void *check_pair_callback_userdata;
 
 	BVHTREE_CLASS tree;
 

core/math/octree_definition.inc

@@ -445,6 +445,7 @@ public:
 	bool is_pairable(OctreeElementID p_id) const;
 	T *get(OctreeElementID p_id) const;
 	int get_subindex(OctreeElementID p_id) const;
+	AABB get_aabb(OctreeElementID p_id) const;
 
 	int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF);
 	int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
@@ -498,6 +499,12 @@ OCTREE_FUNC(int)::get_subindex(OctreeElementID p_id) const {
 	return E->get().subindex;
 }
 
+OCTREE_FUNC(AABB)::get_aabb(OctreeElementID p_id) const {
+	const typename ElementMap::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, AABB());
+	return E->get().aabb;
+}
+
 #define OCTREE_DIVISOR 4
 
 OCTREE_FUNC(void)::_insert_element(Element *p_element, Octant *p_octant) {

servers/physics/broad_phase_basic.cpp

@@ -51,11 +51,17 @@ void BroadPhaseBasic::move(ID p_id, const AABB &p_aabb) {
 	ERR_FAIL_COND(!E);
 	E->get().aabb = p_aabb;
 }
+
+void BroadPhaseBasic::recheck_pairs(ID p_id) {
+	// Not supported.
+}
+
 void BroadPhaseBasic::set_static(ID p_id, bool p_static) {
 	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 	E->get()._static = p_static;
 }
+
 void BroadPhaseBasic::remove(ID p_id) {
 	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
@@ -183,7 +189,7 @@ void BroadPhaseBasic::update() {
 			if (pair_ok && !E) {
 				void *data = nullptr;
 				if (pair_callback) {
-					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
+					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, nullptr, unpair_userdata);
 					if (data) {
 						pair_map.insert(key, data);
 					}

servers/physics/broad_phase_basic.h

@@ -82,6 +82,7 @@ public:
 	// 0 is an invalid ID
 	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
+	virtual void recheck_pairs(ID p_id);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 

servers/physics/broad_phase_bvh.cpp

@@ -41,10 +41,15 @@ void BroadPhaseBVH::move(ID p_id, const AABB &p_aabb) {
 	bvh.move(p_id - 1, p_aabb);
 }
 
+void BroadPhaseBVH::recheck_pairs(ID p_id) {
+	bvh.recheck_pairs(p_id - 1);
+}
+
 void BroadPhaseBVH::set_static(ID p_id, bool p_static) {
 	CollisionObjectSW *it = bvh.get(p_id - 1);
 	bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
 }
+
 void BroadPhaseBVH::remove(ID p_id) {
 	bvh.erase(p_id - 1);
 }
@@ -54,9 +59,11 @@ CollisionObjectSW *BroadPhaseBVH::get_object(ID p_id) const {
 	ERR_FAIL_COND_V(!it, nullptr);
 	return it;
 }
+
 bool BroadPhaseBVH::is_static(ID p_id) const {
 	return !bvh.is_pairable(p_id - 1);
 }
+
 int BroadPhaseBVH::get_subindex(ID p_id) const {
 	return bvh.get_subindex(p_id - 1);
 }
@@ -73,28 +80,38 @@ int BroadPhaseBVH::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results,
 	return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
 }
 
-void *BroadPhaseBVH::_pair_callback(void *self, uint32_t p_A, CollisionObjectSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObjectSW *p_object_B, int subindex_B) {
-	BroadPhaseBVH *bpo = (BroadPhaseBVH *)(self);
+void *BroadPhaseBVH::_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B) {
+	BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self);
 	if (!bpo->pair_callback) {
 		return nullptr;
 	}
 
-	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
+	return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, nullptr, bpo->pair_userdata);
 }
 
-void BroadPhaseBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObjectSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) {
-	BroadPhaseBVH *bpo = (BroadPhaseBVH *)(self);
+void BroadPhaseBVH::_unpair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data) {
+	BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self);
 	if (!bpo->unpair_callback) {
 		return;
 	}
 
-	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
+	bpo->unpair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->unpair_userdata);
+}
+
+void *BroadPhaseBVH::_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data) {
+	BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self);
+	if (!bpo->pair_callback) {
+		return nullptr;
+	}
+
+	return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->pair_userdata);
 }
 
 void BroadPhaseBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 	pair_callback = p_pair_callback;
 	pair_userdata = p_userdata;
 }
+
 void BroadPhaseBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
 	unpair_callback = p_unpair_callback;
 	unpair_userdata = p_userdata;
@@ -112,6 +129,7 @@ BroadPhaseBVH::BroadPhaseBVH() {
 	bvh.params_set_thread_safe(GLOBAL_GET("rendering/threads/thread_safe_bvh"));
 	bvh.set_pair_callback(_pair_callback, this);
 	bvh.set_unpair_callback(_unpair_callback, this);
+	bvh.set_check_pair_callback(_check_pair_callback, this);
 	pair_callback = nullptr;
 	pair_userdata = nullptr;
 	unpair_userdata = nullptr;

servers/physics/broad_phase_bvh.h

@@ -37,8 +37,9 @@
 class BroadPhaseBVH : public BroadPhaseSW {
 	BVH_Manager<CollisionObjectSW, true, 128> bvh;
 
-	static void *_pair_callback(void *, uint32_t, CollisionObjectSW *, int, uint32_t, CollisionObjectSW *, int);
-	static void _unpair_callback(void *, uint32_t, CollisionObjectSW *, int, uint32_t, CollisionObjectSW *, int, void *);
+	static void *_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B);
+	static void _unpair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data);
+	static void *_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data);
 
 	PairCallback pair_callback;
 	void *pair_userdata;
@@ -49,6 +50,7 @@ public:
 	// 0 is an invalid ID
 	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
+	virtual void recheck_pairs(ID p_id);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 

servers/physics/broad_phase_octree.cpp

@@ -40,10 +40,16 @@ void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) {
 	octree.move(p_id, p_aabb);
 }
 
+void BroadPhaseOctree::recheck_pairs(ID p_id) {
+	AABB aabb = octree.get_aabb(p_id);
+	octree.move(p_id, aabb);
+}
+
 void BroadPhaseOctree::set_static(ID p_id, bool p_static) {
 	CollisionObjectSW *it = octree.get(p_id);
 	octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
 }
+
 void BroadPhaseOctree::remove(ID p_id) {
 	octree.erase(p_id);
 }
@@ -78,7 +84,7 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio
 		return nullptr;
 	}
 
-	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
+	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, nullptr, bpo->pair_userdata);
 }
 
 void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) {

servers/physics/broad_phase_octree.h

@@ -49,6 +49,7 @@ public:
 	// 0 is an invalid ID
 	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
+	virtual void recheck_pairs(ID p_id);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 

servers/physics/broad_phase_sw.h

@@ -44,12 +44,13 @@ public:
 
 	typedef uint32_t ID;
 
-	typedef void *(*PairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_userdata);
-	typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata);
+	typedef void *(*PairCallback)(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
+	typedef void (*UnpairCallback)(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
 
 	// 0 is an invalid ID
 	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
 	virtual void move(ID p_id, const AABB &p_aabb) = 0;
+	virtual void recheck_pairs(ID p_id) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
 	virtual void remove(ID p_id) = 0;
 

servers/physics/collision_object_sw.cpp

@@ -180,6 +180,23 @@ void CollisionObjectSW::_update_shapes() {
 	}
 }
 
+void CollisionObjectSW::_recheck_shapes() {
+	if (!space) {
+		return;
+	}
+
+	for (int i = 0; i < shapes.size(); i++) {
+		Shape &s = shapes.write[i];
+		if (s.disabled) {
+			continue;
+		}
+
+		if (s.bpid != 0) {
+			space->get_broadphase()->recheck_pairs(s.bpid);
+		}
+	}
+}
+
 void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 	if (!space) {
 		return;

servers/physics/collision_object_sw.h

@@ -79,6 +79,7 @@ private:
 	SelfList<CollisionObjectSW> pending_shape_update_list;
 
 	void _update_shapes();
+	void _recheck_shapes();
 
 protected:
 	void _update_shapes_with_motion(const Vector3 &p_motion);
@@ -155,13 +156,15 @@ public:
 
 	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
 		collision_layer = p_layer;
-		_shape_changed();
+		_recheck_shapes();
+		_shapes_changed();
 	}
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
 
 	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
 		collision_mask = p_mask;
-		_shape_changed();
+		_recheck_shapes();
+		_shapes_changed();
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 

servers/physics/space_sw.cpp

@@ -1079,15 +1079,29 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 	return collided;
 }
 
-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)) {
+void *SpaceSW::_broadphase_pair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
+	bool valid_collision_pair = p_object_A->test_collision_mask(p_object_B);
+
+	if (p_pair_data) {
+		// Checking an existing pair.
+		if (valid_collision_pair) {
+			// Nothing to do, pair is still valid.
+			return p_pair_data;
+		} else {
+			// Logical collision not valid anymore, unpair.
+			_broadphase_unpair(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, p_self);
+			return nullptr;
+		}
+	}
+
+	if (!valid_collision_pair) {
 		return nullptr;
 	}
 
-	CollisionObjectSW::Type type_A = A->get_type();
-	CollisionObjectSW::Type type_B = B->get_type();
+	CollisionObjectSW::Type type_A = p_object_A->get_type();
+	CollisionObjectSW::Type type_B = p_object_B->get_type();
 	if (type_A > type_B) {
-		SWAP(A, B);
+		SWAP(p_object_A, p_object_B);
 		SWAP(p_subindex_A, p_subindex_B);
 		SWAP(type_A, type_B);
 	}
@@ -1097,32 +1111,34 @@ void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, Collisio
 	self->collision_pairs++;
 
 	if (type_A == CollisionObjectSW::TYPE_AREA) {
-		AreaSW *area = static_cast<AreaSW *>(A);
+		AreaSW *area_a = static_cast<AreaSW *>(p_object_A);
 		if (type_B == CollisionObjectSW::TYPE_AREA) {
-			AreaSW *area_b = static_cast<AreaSW *>(B);
-			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A));
+			AreaSW *area_b = static_cast<AreaSW *>(p_object_B);
+			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area_a, p_subindex_A));
 			return area2_pair;
 		} else {
-			BodySW *body = static_cast<BodySW *>(B);
-			AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A));
+			BodySW *body_b = static_cast<BodySW *>(p_object_B);
+			AreaPairSW *area_pair = memnew(AreaPairSW(body_b, p_subindex_B, area_a, p_subindex_A));
 			return area_pair;
 		}
 	} else {
-		BodyPairSW *b = memnew(BodyPairSW((BodySW *)A, p_subindex_A, (BodySW *)B, p_subindex_B));
-		return b;
+		BodySW *body_a = static_cast<BodySW *>(p_object_A);
+		BodySW *body_b = static_cast<BodySW *>(p_object_B);
+		BodyPairSW *body_pair = memnew(BodyPairSW(body_a, p_subindex_A, body_b, p_subindex_B));
+		return body_pair;
 	}
 
 	return nullptr;
 }
 
-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) {
+void SpaceSW::_broadphase_unpair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
+	if (!p_pair_data) {
 		return;
 	}
 
 	SpaceSW *self = (SpaceSW *)p_self;
 	self->collision_pairs--;
-	ConstraintSW *c = (ConstraintSW *)p_data;
+	ConstraintSW *c = (ConstraintSW *)p_pair_data;
 	memdelete(c);
 }
 

servers/physics/space_sw.h

@@ -83,8 +83,8 @@ private:
 	SelfList<AreaSW>::List monitor_query_list;
 	SelfList<AreaSW>::List area_moved_list;
 
-	static void *_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self);
-	static void _broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self);
+	static void *_broadphase_pair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
+	static void _broadphase_unpair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
 
 	Set<CollisionObjectSW *> objects;
 

servers/physics_2d/broad_phase_2d_basic.cpp

@@ -47,11 +47,17 @@ void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) {
 	ERR_FAIL_COND(!E);
 	E->get().aabb = p_aabb;
 }
+
+void BroadPhase2DBasic::recheck_pairs(ID p_id) {
+	// Not supported.
+}
+
 void BroadPhase2DBasic::set_static(ID p_id, bool p_static) {
 	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 	E->get()._static = p_static;
 }
+
 void BroadPhase2DBasic::remove(ID p_id) {
 	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
@@ -145,7 +151,7 @@ void BroadPhase2DBasic::update() {
 			if (pair_ok && !E) {
 				void *data = nullptr;
 				if (pair_callback) {
-					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
+					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, nullptr, unpair_userdata);
 					if (data) {
 						pair_map.insert(key, data);
 					}

servers/physics_2d/broad_phase_2d_basic.h

@@ -81,6 +81,7 @@ public:
 	// 0 is an invalid ID
 	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
 	virtual void move(ID p_id, const Rect2 &p_aabb);
+	virtual void recheck_pairs(ID p_id);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 

servers/physics_2d/broad_phase_2d_bvh.cpp

@@ -41,10 +41,15 @@ void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
 	bvh.move(p_id - 1, p_aabb);
 }
 
+void BroadPhase2DBVH::recheck_pairs(ID p_id) {
+	bvh.recheck_pairs(p_id - 1);
+}
+
 void BroadPhase2DBVH::set_static(ID p_id, bool p_static) {
 	CollisionObject2DSW *it = bvh.get(p_id - 1);
 	bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
 }
+
 void BroadPhase2DBVH::remove(ID p_id) {
 	bvh.erase(p_id - 1);
 }
@@ -54,9 +59,11 @@ CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const {
 	ERR_FAIL_COND_V(!it, nullptr);
 	return it;
 }
+
 bool BroadPhase2DBVH::is_static(ID p_id) const {
 	return !bvh.is_pairable(p_id - 1);
 }
+
 int BroadPhase2DBVH::get_subindex(ID p_id) const {
 	return bvh.get_subindex(p_id - 1);
 }
@@ -69,22 +76,31 @@ int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_resu
 	return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
 }
 
-void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) {
-	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self);
+void *BroadPhase2DBVH::_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B) {
+	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(p_self);
 	if (!bpo->pair_callback) {
 		return nullptr;
 	}
 
-	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
+	return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, nullptr, bpo->pair_userdata);
 }
 
-void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) {
-	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self);
+void BroadPhase2DBVH::_unpair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data) {
+	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(p_self);
 	if (!bpo->unpair_callback) {
 		return;
 	}
 
-	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
+	bpo->unpair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->unpair_userdata);
+}
+
+void *BroadPhase2DBVH::_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data) {
+	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(p_self);
+	if (!bpo->pair_callback) {
+		return nullptr;
+	}
+
+	return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->pair_userdata);
 }
 
 void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
@@ -109,6 +125,7 @@ BroadPhase2DBVH::BroadPhase2DBVH() {
 	bvh.params_set_thread_safe(GLOBAL_GET("rendering/threads/thread_safe_bvh"));
 	bvh.set_pair_callback(_pair_callback, this);
 	bvh.set_unpair_callback(_unpair_callback, this);
+	bvh.set_check_pair_callback(_check_pair_callback, this);
 	pair_callback = nullptr;
 	pair_userdata = nullptr;
 	unpair_userdata = nullptr;

servers/physics_2d/broad_phase_2d_bvh.h

@@ -39,8 +39,9 @@
 class BroadPhase2DBVH : public BroadPhase2DSW {
 	BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh;
 
-	static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int);
-	static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *);
+	static void *_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B);
+	static void _unpair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data);
+	static void *_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data);
 
 	PairCallback pair_callback;
 	void *pair_userdata;
@@ -51,6 +52,7 @@ public:
 	// 0 is an invalid ID
 	virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
 	virtual void move(ID p_id, const Rect2 &p_aabb);
+	virtual void recheck_pairs(ID p_id);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 

servers/physics_2d/broad_phase_2d_hash_grid.cpp

@@ -88,7 +88,7 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
 
 		if (physical_collision && logical_collision) {
 			if (!E->get()->colliding && pair_callback) {
-				E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
+				E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, nullptr, pair_userdata);
 			}
 			E->get()->colliding = true;
 		} else { // No collision
@@ -336,6 +336,14 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
 	_check_motion(&e);
 }
 
+void BroadPhase2DHashGrid::recheck_pairs(ID p_id) {
+	Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND(!E);
+
+	Element &e = E->get();
+	move(p_id, e.aabb);
+}
+
 void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
 	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);

servers/physics_2d/broad_phase_2d_hash_grid.h

@@ -165,11 +165,13 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 
 	void _pair_attempt(Element *p_elem, Element *p_with);
 	void _unpair_attempt(Element *p_elem, Element *p_with);
+	void _move_internal(Element *p_elem, const Rect2 &p_aabb);
 	void _check_motion(Element *p_elem);
 
 public:
 	virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
 	virtual void move(ID p_id, const Rect2 &p_aabb);
+	virtual void recheck_pairs(ID p_id);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 

servers/physics_2d/broad_phase_2d_sw.h

@@ -44,12 +44,13 @@ public:
 
 	typedef uint32_t ID;
 
-	typedef void *(*PairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_userdata);
-	typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
+	typedef void *(*PairCallback)(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
+	typedef void (*UnpairCallback)(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
 
 	// 0 is an invalid ID
 	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0;
 	virtual void move(ID p_id, const Rect2 &p_aabb) = 0;
+	virtual void recheck_pairs(ID p_id) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
 	virtual void remove(ID p_id) = 0;
 

servers/physics_2d/collision_object_2d_sw.cpp

@@ -187,6 +187,23 @@ void CollisionObject2DSW::_update_shapes() {
 	}
 }
 
+void CollisionObject2DSW::_recheck_shapes() {
+	if (!space) {
+		return;
+	}
+
+	for (int i = 0; i < shapes.size(); i++) {
+		Shape &s = shapes.write[i];
+		if (s.disabled) {
+			continue;
+		}
+
+		if (s.bpid != 0) {
+			space->get_broadphase()->recheck_pairs(s.bpid);
+		}
+	}
+}
+
 void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
 	if (!space) {
 		return;

servers/physics_2d/collision_object_2d_sw.h

@@ -80,6 +80,7 @@ private:
 	SelfList<CollisionObject2DSW> pending_shape_update_list;
 
 	void _update_shapes();
+	void _recheck_shapes();
 
 protected:
 	void _update_shapes_with_motion(const Vector2 &p_motion);
@@ -166,13 +167,15 @@ public:
 
 	void set_collision_mask(uint32_t p_mask) {
 		collision_mask = p_mask;
-		_shape_changed();
+		_recheck_shapes();
+		_shapes_changed();
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
 	void set_collision_layer(uint32_t p_layer) {
 		collision_layer = p_layer;
-		_shape_changed();
+		_recheck_shapes();
+		_shapes_changed();
 	}
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
 

servers/physics_2d/space_2d_sw.cpp

@@ -1196,15 +1196,29 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	return collided;
 }
 
-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)) {
+void *Space2DSW::_broadphase_pair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
+	bool valid_collision_pair = p_object_A->test_collision_mask(p_object_B);
+
+	if (p_pair_data) {
+		// Checking an existing pair.
+		if (valid_collision_pair) {
+			// Nothing to do, pair is still valid.
+			return p_pair_data;
+		} else {
+			// Logical collision not valid anymore, unpair.
+			_broadphase_unpair(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, p_self);
+			return nullptr;
+		}
+	}
+
+	if (!valid_collision_pair) {
 		return nullptr;
 	}
 
-	CollisionObject2DSW::Type type_A = A->get_type();
-	CollisionObject2DSW::Type type_B = B->get_type();
+	CollisionObject2DSW::Type type_A = p_object_A->get_type();
+	CollisionObject2DSW::Type type_B = p_object_B->get_type();
 	if (type_A > type_B) {
-		SWAP(A, B);
+		SWAP(p_object_A, p_object_B);
 		SWAP(p_subindex_A, p_subindex_B);
 		SWAP(type_A, type_B);
 	}
@@ -1213,33 +1227,35 @@ void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, Coll
 	self->collision_pairs++;
 
 	if (type_A == CollisionObject2DSW::TYPE_AREA) {
-		Area2DSW *area = static_cast<Area2DSW *>(A);
+		Area2DSW *area_a = static_cast<Area2DSW *>(p_object_A);
 		if (type_B == CollisionObject2DSW::TYPE_AREA) {
-			Area2DSW *area_b = static_cast<Area2DSW *>(B);
-			Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A));
+			Area2DSW *area_b = static_cast<Area2DSW *>(p_object_B);
+			Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area_a, p_subindex_A));
 			return area2_pair;
 		} else {
-			Body2DSW *body = static_cast<Body2DSW *>(B);
-			AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A));
+			Body2DSW *body_b = static_cast<Body2DSW *>(p_object_B);
+			AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body_b, p_subindex_B, area_a, p_subindex_A));
 			return area_pair;
 		}
 
 	} else {
-		BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B));
-		return b;
+		Body2DSW *body_a = static_cast<Body2DSW *>(p_object_A);
+		Body2DSW *body_b = static_cast<Body2DSW *>(p_object_B);
+		BodyPair2DSW *body_pair = memnew(BodyPair2DSW(body_a, p_subindex_A, body_b, p_subindex_B));
+		return body_pair;
 	}
 
 	return nullptr;
 }
 
-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) {
+void Space2DSW::_broadphase_unpair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
+	if (!p_pair_data) {
 		return;
 	}
 
 	Space2DSW *self = (Space2DSW *)p_self;
 	self->collision_pairs--;
-	Constraint2DSW *c = (Constraint2DSW *)p_data;
+	Constraint2DSW *c = (Constraint2DSW *)p_pair_data;
 	memdelete(c);
 }
 

servers/physics_2d/space_2d_sw.h

@@ -91,8 +91,8 @@ private:
 	SelfList<Area2DSW>::List monitor_query_list;
 	SelfList<Area2DSW>::List area_moved_list;
 
-	static void *_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self);
-	static void _broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self);
+	static void *_broadphase_pair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
+	static void _broadphase_unpair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
 
 	Set<CollisionObject2DSW *> objects;