Merge pull request #54108 from nekomatata/fix-bvh-update-collision-la…

…yer-3.x

core/math/bvh.h

@@ -59,6 +59,7 @@ class BVH_Manager {
 	// 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 @@ class BVH_Manager {
 		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 @@ class BVH_Manager {
 		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 @@ class BVH_Manager {
 		}
 	}
 
+	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 @@ class BVH_Manager {
 		}
 	}
 
+	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 @@ class BVH_Manager {
 		}
 	}
 
+	// 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 @@ class BVH_Manager {
 
 	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 @@ class BroadPhaseBasic : public BroadPhaseSW {
 	// 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 @@ class BroadPhaseBVH : public BroadPhaseSW {
 	// 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 @@ class BroadPhaseOctree : public BroadPhaseSW {
 	// 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 @@ class BroadPhaseSW {
 
 	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 @@ class CollisionObjectSW : public ShapeOwnerSW {
 	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 @@ class CollisionObjectSW : public ShapeOwnerSW {
 
 	_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; }