BVH templated mask checks and generic NUM_TREES

Refactors the BVH to make it more generic and customizable. Instead of hard coding the system of pairable_mask and pairable_type into the BVH, this information is no longer stored internally, and instead the BVH uses callbacks both for determining whether pairs of objects can pair with each other, and for filtering cull / intersection tests.

In addition, instead of hard coding the number of trees, the BVH now supports up to 32 trees, and each object can supply a tree collision mask to determine which trees it can collide against.

This enables the BVH to scale to either the two or 3 trees needed in physics, and the single tree used without pairing in Godot 4 render tree.

core/math/bvh.h

@@ -46,13 +46,18 @@
 // Layer masks are implemented in the renderers as a later step, and light_cull_mask appears to be
 // implemented in GLES3 but not GLES2. Layer masks are not yet implemented for directional lights.
 
+// In the physics, the pairable_type is based on 1 << p_object->get_type() where:
+// TYPE_AREA,
+// TYPE_BODY
+// and pairable_mask is either 0 if static, or set to all if non static
+
 #include "bvh_tree.h"
 #include "core/os/mutex.h"
 
-#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS, BOUNDS, POINT>
+#define BVHTREE_CLASS BVH_Tree<T, NUM_TREES, 2, MAX_ITEMS, USER_PAIR_TEST_FUNCTION, USER_CULL_TEST_FUNCTION, USE_PAIRS, BOUNDS, POINT>
 #define BVH_LOCKED_FUNCTION BVHLockedFunction(&_mutex, BVH_THREAD_SAFE &&_thread_safe);
 
-template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32, class BOUNDS = AABB, class POINT = Vector3, bool BVH_THREAD_SAFE = true>
+template <class T, int NUM_TREES = 1, bool USE_PAIRS = false, int MAX_ITEMS = 32, class USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, class USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, class BOUNDS = AABB, class POINT = Vector3, bool BVH_THREAD_SAFE = true>
 class BVH_Manager {
 public:
 	// note we are using uint32_t instead of BVHHandle, losing type safety, but this
@@ -99,7 +104,7 @@ public:
 		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) {
+	BVHHandle create(T *p_userdata, bool p_active = true, uint32_t p_tree_id = 0, uint32_t p_tree_collision_mask = 1, const BOUNDS &p_aabb = BOUNDS(), int p_subindex = 0) {
 		BVH_LOCKED_FUNCTION
 
 		// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
@@ -108,15 +113,7 @@ public:
 			//_check_for_collisions();
 		}
 
-#ifdef TOOLS_ENABLED
-		if (!USE_PAIRS) {
-			if (p_pairable) {
-				WARN_PRINT_ONCE("creating pairable item in BVH with USE_PAIRS set to false");
-			}
-		}
-#endif
-
-		BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_pairable, p_pairable_type, p_pairable_mask);
+		BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_tree_id, p_tree_collision_mask);
 
 		if (USE_PAIRS) {
 			// for safety initialize the expanded AABB
@@ -173,16 +170,16 @@ public:
 		return deactivate(h);
 	}
 
-	void set_pairable(uint32_t p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) {
+	void set_tree(uint32_t p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
 		BVHHandle h;
 		h.set(p_handle);
-		set_pairable(h, p_pairable, p_pairable_type, p_pairable_mask, p_force_collision_check);
+		set_tree(h, p_tree_id, p_tree_collision_mask, p_force_collision_check);
 	}
 
-	bool is_pairable(uint32_t p_handle) const {
+	uint32_t get_tree_id(uint32_t p_handle) const {
 		BVHHandle h;
 		h.set(p_handle);
-		return item_is_pairable(h);
+		return item_get_tree_id(h);
 	}
 	int get_subindex(uint32_t p_handle) const {
 		BVHHandle h;
@@ -208,10 +205,7 @@ public:
 	}
 
 	void recheck_pairs(BVHHandle p_handle) {
-		BVH_LOCKED_FUNCTION
-		if (USE_PAIRS) {
-			_recheck_pairs(p_handle);
-		}
+		force_collision_check(p_handle);
 	}
 
 	void erase(BVHHandle p_handle) {
@@ -312,10 +306,10 @@ public:
 	}
 
 	// prefer calling this directly as type safe
-	void set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) {
+	void set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
 		BVH_LOCKED_FUNCTION
 		// Returns true if the pairing state has changed.
-		bool state_changed = tree.item_set_pairable(p_handle, p_pairable, p_pairable_type, p_pairable_mask);
+		bool state_changed = tree.item_set_tree(p_handle, p_tree_id, p_tree_collision_mask);
 
 		if (USE_PAIRS) {
 			// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
@@ -343,7 +337,7 @@ public:
 	}
 
 	// cull tests
-	int cull_aabb(const BOUNDS &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_aabb(const BOUNDS &p_aabb, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
 		BVH_LOCKED_FUNCTION
 		typename BVHTREE_CLASS::CullParams params;
 
@@ -351,17 +345,16 @@ public:
 		params.result_max = p_result_max;
 		params.result_array = p_result_array;
 		params.subindex_array = p_subindex_array;
-		params.mask = p_mask;
-		params.pairable_type = 0;
-		params.test_pairable_only = false;
+		params.tree_collision_mask = p_tree_collision_mask;
 		params.abb.from(p_aabb);
+		params.tester = p_tester;
 
 		tree.cull_aabb(params);
 
 		return params.result_count_overall;
 	}
 
-	int cull_segment(const POINT &p_from, const POINT &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_segment(const POINT &p_from, const POINT &p_to, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
 		BVH_LOCKED_FUNCTION
 		typename BVHTREE_CLASS::CullParams params;
 
@@ -369,8 +362,8 @@ public:
 		params.result_max = p_result_max;
 		params.result_array = p_result_array;
 		params.subindex_array = p_subindex_array;
-		params.mask = p_mask;
-		params.pairable_type = 0;
+		params.tester = p_tester;
+		params.tree_collision_mask = p_tree_collision_mask;
 
 		params.segment.from = p_from;
 		params.segment.to = p_to;
@@ -380,7 +373,7 @@ public:
 		return params.result_count_overall;
 	}
 
-	int cull_point(const POINT &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_point(const POINT &p_point, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
 		BVH_LOCKED_FUNCTION
 		typename BVHTREE_CLASS::CullParams params;
 
@@ -388,8 +381,8 @@ public:
 		params.result_max = p_result_max;
 		params.result_array = p_result_array;
 		params.subindex_array = p_subindex_array;
-		params.mask = p_mask;
-		params.pairable_type = 0;
+		params.tester = p_tester;
+		params.tree_collision_mask = p_tree_collision_mask;
 
 		params.point = p_point;
 
@@ -397,7 +390,7 @@ public:
 		return params.result_count_overall;
 	}
 
-	int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF) {
 		BVH_LOCKED_FUNCTION
 		if (!p_convex.size()) {
 			return 0;
@@ -413,8 +406,8 @@ public:
 		params.result_max = p_result_max;
 		params.result_array = p_result_array;
 		params.subindex_array = nullptr;
-		params.mask = p_mask;
-		params.pairable_type = 0;
+		params.tester = p_tester;
+		params.tree_collision_mask = p_tree_collision_mask;
 
 		params.hull.planes = &p_convex[0];
 		params.hull.num_planes = p_convex.size();
@@ -442,8 +435,6 @@ private:
 		params.result_max = INT_MAX;
 		params.result_array = nullptr;
 		params.subindex_array = nullptr;
-		params.mask = 0xFFFFFFFF;
-		params.pairable_type = 0;
 
 		for (unsigned int n = 0; n < changed_items.size(); n++) {
 			const BVHHandle &h = changed_items[n];
@@ -453,17 +444,14 @@ private:
 			BVHABB_CLASS abb;
 			abb.from(expanded_aabb);
 
+			tree.item_fill_cullparams(h, params);
+
 			// find all the existing paired aabbs that are no longer
 			// paired, and send callbacks
 			_find_leavers(h, abb, p_full_check);
 
 			uint32_t changed_item_ref_id = h.id();
 
-			// set up the test from this item.
-			// this includes whether to test the non pairable tree,
-			// and the item mask.
-			tree.item_fill_cullparams(h, params);
-
 			params.abb = abb;
 
 			params.result_count_overall = 0; // might not be needed
@@ -504,7 +492,7 @@ public:
 
 private:
 	// supplemental funcs
-	bool item_is_pairable(BVHHandle p_handle) const { return _get_extra(p_handle).pairable; }
+	uint32_t item_get_tree_id(BVHHandle p_handle) const { return _get_extra(p_handle).tree_id; }
 	T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; }
 	int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; }
 
@@ -561,8 +549,8 @@ private:
 
 		// do they overlap?
 		if (p_abb_from.intersects(abb_to)) {
-			// the full check for pairable / non pairable and mask changes is extra expense
-			// this need not be done in most cases (for speed) except in the case where set_pairable is called
+			// the full check for pairable / non pairable (i.e. tree_id and tree_masks) and mask changes is extra expense
+			// this need not be done in most cases (for speed) except in the case where set_tree is called
 			// where the masks etc of the objects in question may have changed
 			if (!p_full_check) {
 				return false;
@@ -570,12 +558,13 @@ private:
 			const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_from);
 			const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_to);
 
-			// one of the two must be pairable to still pair
-			// if neither are pairable, we always unpair
-			if (exa.pairable || exb.pairable) {
+			// Checking tree_ids and tree_collision_masks
+			if (exa.are_item_trees_compatible(exb)) {
+				bool pair_allowed = USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata);
+
 				// the masks must still be compatible to pair
-				// i.e. if there is a hit between the two, then they should stay paired
-				if (tree._cull_pairing_mask_test_hit(exa.pairable_mask, exa.pairable_type, exb.pairable_mask, exb.pairable_type)) {
+				// i.e. if there is a hit between the two and they intersect, then they should stay paired
+				if (pair_allowed) {
 					return false;
 				}
 			}
@@ -613,6 +602,11 @@ private:
 		const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha);
 		const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb);
 
+		// user collision callback
+		if (!USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata)) {
+			return;
+		}
+
 		// if the userdata is the same, no collisions should occur
 		if ((exa.userdata == exb.userdata) && exa.userdata) {
 			return;

core/math/bvh_abb.h

@@ -212,6 +212,7 @@ struct BVH_ABB {
 		return true;
 	}
 
+	// Very hot in profiling, make sure optimized
 	bool intersects(const BVH_ABB &p_o) const {
 		if (_any_morethan(p_o.min, -neg_max)) {
 			return false;
@@ -222,6 +223,17 @@ struct BVH_ABB {
 		return true;
 	}
 
+	// for pre-swizzled tester (this object)
+	bool intersects_swizzled(const BVH_ABB &p_o) const {
+		if (_any_lessthan(min, p_o.min)) {
+			return false;
+		}
+		if (_any_lessthan(neg_max, p_o.neg_max)) {
+			return false;
+		}
+		return true;
+	}
+
 	bool is_other_within(const BVH_ABB &p_o) const {
 		if (_any_lessthan(p_o.neg_max, neg_max)) {
 			return false;

core/math/bvh_cull.inc

@@ -9,9 +9,12 @@ struct CullParams {
 	T **result_array;
 	int *subindex_array;
 
-	// nobody truly understands how masks are intended to work.
-	uint32_t mask;
-	uint32_t pairable_type;
+	// We now process masks etc in a user template function,
+	// and these for simplicity assume even for cull tests there is a
+	// testing object (which has masks etc) for the user cull checks.
+	// This means for cull tests on their own, the client will usually
+	// want to create a dummy object, just in order to specify masks etc.
+	const T *tester;
 
 	// optional components for different tests
 	POINT point;
@@ -19,10 +22,9 @@ struct CullParams {
 	typename BVHABB_CLASS::ConvexHull hull;
 	typename BVHABB_CLASS::Segment segment;
 
-	// when collision testing, non pairable moving items
-	// only need to be tested against the pairable tree.
-	// collisions with other non pairable items are irrelevant.
-	bool test_pairable_only;
+	// When collision testing, we can specify which tree ids
+	// to collide test against with the tree_collision_mask.
+	uint32_t tree_collision_mask;
 };
 
 private:
@@ -58,11 +60,22 @@ int cull_convex(CullParams &r_params, bool p_translate_hits = true) {
 	_cull_hits.clear();
 	r_params.result_count = 0;
 
+	uint32_t tree_test_mask = 0;
+
 	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
 		if (_root_node_id[n] == BVHCommon::INVALID) {
 			continue;
 		}
 
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
 		_cull_convex_iterative(_root_node_id[n], r_params);
 	}
 
@@ -77,11 +90,22 @@ int cull_segment(CullParams &r_params, bool p_translate_hits = true) {
 	_cull_hits.clear();
 	r_params.result_count = 0;
 
+	uint32_t tree_test_mask = 0;
+
 	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
 		if (_root_node_id[n] == BVHCommon::INVALID) {
 			continue;
 		}
 
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
 		_cull_segment_iterative(_root_node_id[n], r_params);
 	}
 
@@ -96,11 +120,22 @@ int cull_point(CullParams &r_params, bool p_translate_hits = true) {
 	_cull_hits.clear();
 	r_params.result_count = 0;
 
+	uint32_t tree_test_mask = 0;
+
 	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
 		if (_root_node_id[n] == BVHCommon::INVALID) {
 			continue;
 		}
 
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
+			continue;
+		}
+
 		_cull_point_iterative(_root_node_id[n], r_params);
 	}
 
@@ -115,12 +150,20 @@ int cull_aabb(CullParams &r_params, bool p_translate_hits = true) {
 	_cull_hits.clear();
 	r_params.result_count = 0;
 
+	uint32_t tree_test_mask = 0;
+
 	for (int n = 0; n < NUM_TREES; n++) {
+		tree_test_mask <<= 1;
+		if (!tree_test_mask) {
+			tree_test_mask = 1;
+		}
+
 		if (_root_node_id[n] == BVHCommon::INVALID) {
 			continue;
 		}
 
-		if ((n == 0) && r_params.test_pairable_only) {
+		// the tree collision mask determines which trees to collide test against
+		if (!(r_params.tree_collision_mask & tree_test_mask)) {
 			continue;
 		}
 
@@ -142,22 +185,6 @@ bool _cull_hits_full(const CullParams &p) {
 	return (int)_cull_hits.size() >= p.result_max;
 }
 
-// write this logic once for use in all routines
-// double check this as a possible source of bugs in future.
-bool _cull_pairing_mask_test_hit(uint32_t p_maskA, uint32_t p_typeA, uint32_t p_maskB, uint32_t p_typeB) const {
-	// double check this as a possible source of bugs in future.
-	bool A_match_B = p_maskA & p_typeB;
-
-	if (!A_match_B) {
-		bool B_match_A = p_maskB & p_typeA;
-		if (!B_match_A) {
-			return false;
-		}
-	}
-
-	return true;
-}
-
 void _cull_hit(uint32_t p_ref_id, CullParams &p) {
 	// take into account masks etc
 	// this would be more efficient to do before plane checks,
@@ -165,7 +192,8 @@ void _cull_hit(uint32_t p_ref_id, CullParams &p) {
 	if (USE_PAIRS) {
 		const ItemExtra &ex = _extra[p_ref_id];
 
-		if (!_cull_pairing_mask_test_hit(p.mask, p.pairable_type, ex.pairable_mask, ex.pairable_type)) {
+		// user supplied function (for e.g. pairable types and pairable masks in the render tree)
+		if (!USER_CULL_TEST_FUNCTION::user_cull_check(p.tester, ex.userdata)) {
 			return;
 		}
 	}
@@ -294,6 +322,7 @@ bool _cull_point_iterative(uint32_t p_node_id, CullParams &r_params) {
 	return true;
 }
 
+// Note: This is a very hot loop profiling wise. Take care when changing this and profile.
 bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
 	// our function parameters to keep on a stack
 	struct CullAABBParams {
@@ -336,16 +365,26 @@ bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully
 					_cull_hit(child_id, r_params);
 				}
 			} else {
-				for (int n = 0; n < leaf.num_items; n++) {
+				// This section is the hottest area in profiling, so
+				// is optimized highly
+				// get this into a local register and preconverted to correct type
+				int leaf_num_items = leaf.num_items;
+
+				BVHABB_CLASS swizzled_tester;
+				swizzled_tester.min = -r_params.abb.neg_max;
+				swizzled_tester.neg_max = -r_params.abb.min;
+
+				for (int n = 0; n < leaf_num_items; n++) {
 					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
 
-					if (aabb.intersects(r_params.abb)) {
+					if (swizzled_tester.intersects_swizzled(aabb)) {
 						uint32_t child_id = leaf.get_item_ref_id(n);
 
 						// register hit
 						_cull_hit(child_id, r_params);
 					}
 				}
+
 			} // not fully within
 		} else {
 			if (!cap.fully_within) {

core/math/bvh_misc.inc

@@ -1,11 +1,7 @@
 
 int _handle_get_tree_id(BVHHandle p_handle) const {
 	if (USE_PAIRS) {
-		int tree = 0;
-		if (_extra[p_handle.id()].pairable) {
-			tree = 1;
-		}
-		return tree;
+		return _extra[p_handle.id()].tree_id;
 	}
 	return 0;
 }

core/math/bvh_public.inc

@@ -1,5 +1,5 @@
 public:
-BVHHandle item_add(T *p_userdata, bool p_active, const BOUNDS &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) {
+BVHHandle item_add(T *p_userdata, bool p_active, const BOUNDS &p_aabb, int32_t p_subindex, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_invisible = false) {
 #ifdef BVH_VERBOSE_TREE
 	VERBOSE_PRINT("\nitem_add BEFORE");
 	_debug_recursive_print_tree(0);
@@ -47,29 +47,17 @@ BVHHandle item_add(T *p_userdata, bool p_active, const BOUNDS &p_aabb, int32_t p
 	extra->active_ref_id = _active_refs.size();
 	_active_refs.push_back(ref_id);
 
-	if (USE_PAIRS) {
-		extra->pairable_mask = p_pairable_mask;
-		extra->pairable_type = p_pairable_type;
-		extra->pairable = p_pairable;
-	} else {
-		// just for safety, in case this gets queried etc
-		extra->pairable = 0;
-		p_pairable = false;
-	}
+	extra->tree_id = p_tree_id;
+	extra->tree_collision_mask = p_tree_collision_mask;
 
 	// assign to handle to return
 	handle.set_id(ref_id);
 
-	uint32_t tree_id = 0;
-	if (p_pairable) {
-		tree_id = 1;
-	}
-
-	create_root_node(tree_id);
+	create_root_node(p_tree_id);
 
 	// we must choose where to add to tree
 	if (p_active) {
-		ref->tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
+		ref->tnode_id = _logic_choose_item_add_node(_root_node_id[p_tree_id], abb);
 
 		bool refit = _node_add_item(ref->tnode_id, ref_id, abb);
 
@@ -77,7 +65,7 @@ BVHHandle item_add(T *p_userdata, bool p_active, const BOUNDS &p_aabb, int32_t p
 			// only need to refit from the parent
 			const TNode &add_node = _nodes[ref->tnode_id];
 			if (add_node.parent_id != BVHCommon::INVALID) {
-				refit_upward_and_balance(add_node.parent_id, tree_id);
+				refit_upward_and_balance(add_node.parent_id, p_tree_id);
 			}
 		}
 	} else {
@@ -295,12 +283,14 @@ void item_fill_cullparams(BVHHandle p_handle, CullParams &r_params) const {
 	uint32_t ref_id = p_handle.id();
 	const ItemExtra &extra = _extra[ref_id];
 
-	// testing from a non pairable item, we only want to test pairable items
-	r_params.test_pairable_only = extra.pairable == 0;
+	// which trees does this item want to collide detect against?
+	r_params.tree_collision_mask = extra.tree_collision_mask;
 
-	// we take into account the mask of the item testing from
-	r_params.mask = extra.pairable_mask;
-	r_params.pairable_type = extra.pairable_type;
+	// The testing user defined object is passed to the user defined cull check function
+	// for masks etc. This is usually a dummy object of type T with masks set.
+	// However, if not using the cull_check callback (i.e. returning true), you can pass
+	// a nullptr instead of dummy object, as it will not be used.
+	r_params.tester = extra.userdata;
 }
 
 bool item_is_pairable(const BVHHandle &p_handle) {
@@ -320,7 +310,7 @@ void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) {
 	r_abb = leaf.get_aabb(ref.item_id);
 }
 
-bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
+bool item_set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask) {
 	// change tree?
 	uint32_t ref_id = p_handle.id();
 
@@ -328,13 +318,15 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 	ItemRef &ref = _refs[ref_id];
 
 	bool active = ref.is_active();
-	bool pairable_changed = (ex.pairable != 0) != p_pairable;
-	bool state_changed = pairable_changed || (ex.pairable_type != p_pairable_type) || (ex.pairable_mask != p_pairable_mask);
+	bool tree_changed = ex.tree_id != p_tree_id;
+	bool mask_changed = ex.tree_collision_mask != p_tree_collision_mask;
+	bool state_changed = tree_changed | mask_changed;
 
-	ex.pairable_type = p_pairable_type;
-	ex.pairable_mask = p_pairable_mask;
+	// Keep an eye on this for bugs of not noticing changes to objects,
+	// especially when changing client user masks that will not be detected as a change
+	// in the BVH. You may need to force a collision check in this case with recheck_pairs().
 
-	if (active && pairable_changed) {
+	if (active && (tree_changed | mask_changed)) {
 		// record abb
 		TNode &tnode = _nodes[ref.tnode_id];
 		TLeaf &leaf = _node_get_leaf(tnode);
@@ -348,7 +340,8 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 
 		// we must set the pairable AFTER getting the current tree
 		// because the pairable status determines which tree
-		ex.pairable = p_pairable;
+		ex.tree_id = p_tree_id;
+		ex.tree_collision_mask = p_tree_collision_mask;
 
 		// add to new tree
 		tree_id = _handle_get_tree_id(p_handle);
@@ -368,7 +361,8 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 		}
 	} else {
 		// always keep this up to date
-		ex.pairable = p_pairable;
+		ex.tree_id = p_tree_id;
+		ex.tree_collision_mask = p_tree_collision_mask;
 	}
 
 	return state_changed;

core/math/bvh_structs.inc

@@ -14,19 +14,38 @@ struct ItemRef {
 // extra info kept in separate parallel list to the references,
 // as this is less used as keeps cache better
 struct ItemExtra {
-	uint32_t last_updated_tick;
-	uint32_t pairable;
-	uint32_t pairable_mask;
-	uint32_t pairable_type;
+	// Before doing user defined pairing checks (especially in the find_leavers function),
+	// we may want to check that two items have compatible tree ids and tree masks,
+	// as if they are incompatible they should not pair / collide.
+	bool are_item_trees_compatible(const ItemExtra &p_other) const {
+		uint32_t other_type = 1 << p_other.tree_id;
+		if (tree_collision_mask & other_type) {
+			return true;
+		}
+		uint32_t our_type = 1 << tree_id;
+		if (p_other.tree_collision_mask & our_type) {
+			return true;
+		}
+		return false;
+	}
 
+	// There can be multiple user defined trees
+	uint32_t tree_id;
+
+	// Defines which trees this item should collision check against.
+	// 1 << tree_id, and normally items would collide against there own
+	// tree (but not always).
+	uint32_t tree_collision_mask;
+
+	uint32_t last_updated_tick;
 	int32_t subindex;
 
+	T *userdata;
+
 	// the active reference is a separate list of which references
 	// are active so that we can slowly iterate through it over many frames for
 	// slow optimize.
 	uint32_t active_ref_id;
-
-	T *userdata;
 };
 
 // tree leaf
@@ -146,15 +165,11 @@ uint32_t _current_active_ref = 0;
 // for pairing collision detection
 LocalVector<uint32_t, uint32_t, true> _cull_hits;
 
-// we now have multiple root nodes, allowing us to store
-// more than 1 tree. This can be more efficient, while sharing the same
-// common lists
-enum { NUM_TREES = 2,
-};
-
-// Tree 0 - Non pairable
-// Tree 1 - Pairable
-// This is more efficient because in physics we only need check non pairable against the pairable tree.
+// We can now have a user definable number of trees.
+// This allows using e.g. a non-pairable and pairable tree,
+// which can be more efficient for example, if we only need check non pairable against the pairable tree.
+// It also may be more efficient in terms of separating static from dynamic objects, by reducing housekeeping.
+// However this is a trade off, as there is a cost of traversing two trees.
 uint32_t _root_node_id[NUM_TREES];
 
 // these values may need tweaking according to the project

core/math/bvh_tree.h

@@ -153,7 +153,25 @@ public:
 	}
 };
 
-template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false, class BOUNDS = AABB, class POINT = Vector3>
+template <class T>
+class BVH_DummyPairTestFunction {
+public:
+	static bool user_collision_check(T *p_a, T *p_b) {
+		// return false if no collision, decided by masks etc
+		return true;
+	}
+};
+
+template <class T>
+class BVH_DummyCullTestFunction {
+public:
+	static bool user_cull_check(T *p_a, T *p_b) {
+		// return false if no collision
+		return true;
+	}
+};
+
+template <class T, int NUM_TREES, int MAX_CHILDREN, int MAX_ITEMS, class USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, class USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, bool USE_PAIRS = false, class BOUNDS = AABB, class POINT = Vector3>
 class BVH_Tree {
 	friend class BVH;
 

servers/physics/broad_phase_bvh.cpp

@@ -33,7 +33,9 @@
 #include "core/project_settings.h"
 
 BroadPhaseSW::ID BroadPhaseBVH::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
-	ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
+	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
+	uint32_t tree_collision_mask = p_static ? 0 : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
+	ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care?
 	return oid + 1;
 }
 
@@ -46,8 +48,9 @@ void BroadPhaseBVH::recheck_pairs(ID p_id) {
 }
 
 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?
+	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
+	uint32_t tree_collision_mask = p_static ? 0 : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
+	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
 }
 
 void BroadPhaseBVH::remove(ID p_id) {
@@ -61,7 +64,8 @@ CollisionObjectSW *BroadPhaseBVH::get_object(ID p_id) const {
 }
 
 bool BroadPhaseBVH::is_static(ID p_id) const {
-	return !bvh.is_pairable(p_id - 1);
+	uint32_t tree_id = bvh.get_tree_id(p_id - 1);
+	return tree_id == 0;
 }
 
 int BroadPhaseBVH::get_subindex(ID p_id) const {
@@ -69,15 +73,15 @@ int BroadPhaseBVH::get_subindex(ID p_id) const {
 }
 
 int BroadPhaseBVH::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
-	return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices);
+	return bvh.cull_point(p_point, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
 }
 
 int BroadPhaseBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
-	return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
+	return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
 }
 
 int BroadPhaseBVH::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
-	return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
+	return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
 }
 
 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) {

servers/physics/broad_phase_bvh.h

@@ -35,7 +35,34 @@
 #include "core/math/bvh.h"
 
 class BroadPhaseBVH : public BroadPhaseSW {
-	BVH_Manager<CollisionObjectSW, true, 128> bvh;
+	template <class T>
+	class UserPairTestFunction {
+	public:
+		static bool user_pair_check(const T *p_a, const T *p_b) {
+			// return false if no collision, decided by masks etc
+			return p_a->test_collision_mask(p_b);
+		}
+	};
+
+	template <class T>
+	class UserCullTestFunction {
+	public:
+		static bool user_cull_check(const T *p_a, const T *p_b) {
+			return true;
+		}
+	};
+
+	enum Tree {
+		TREE_STATIC = 0,
+		TREE_DYNAMIC = 1,
+	};
+
+	enum TreeFlag {
+		TREE_FLAG_STATIC = 1 << TREE_STATIC,
+		TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC,
+	};
+
+	BVH_Manager<CollisionObjectSW, 2, true, 128, UserPairTestFunction<CollisionObjectSW>, UserCullTestFunction<CollisionObjectSW>> bvh;
 
 	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);

servers/physics/broad_phase_octree.cpp

@@ -84,6 +84,25 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio
 		return nullptr;
 	}
 
+	bool valid_collision_pair = p_object_A->test_collision_mask(p_object_B);
+	void *pair_data = bpo->pair_userdata;
+
+	if (pair_data) {
+		// Checking an existing pair.
+		if (valid_collision_pair) {
+			// Nothing to do, pair is still valid.
+			return pair_data;
+		} else {
+			// Logical collision not valid anymore, unpair.
+			_unpair_callback(self, p_A, p_object_A, subindex_A, p_B, p_object_B, subindex_B, pair_data);
+			return nullptr;
+		}
+	}
+
+	if (!valid_collision_pair) {
+		return nullptr;
+	}
+
 	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, nullptr, bpo->pair_userdata);
 }
 

servers/physics/collision_object_sw.h

@@ -168,7 +168,7 @@ public:
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
-	_FORCE_INLINE_ bool test_collision_mask(CollisionObjectSW *p_other) const {
+	_FORCE_INLINE_ bool test_collision_mask(const CollisionObjectSW *p_other) const {
 		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
 	}
 

servers/physics/space_sw.cpp

@@ -1085,25 +1085,14 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 	return collided;
 }
 
+// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
 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);
-
+	// An existing pair - nothing to do, pair is still valid.
 	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;
+		return p_pair_data;
 	}
 
+	// New pair
 	CollisionObjectSW::Type type_A = p_object_A->get_type();
 	CollisionObjectSW::Type type_B = p_object_B->get_type();
 	if (type_A > type_B) {

servers/physics_2d/broad_phase_2d_bvh.cpp

@@ -33,7 +33,9 @@
 #include "core/project_settings.h"
 
 BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
-	ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
+	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
+	uint32_t tree_collision_mask = p_static ? 0 : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
+	ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care?
 	return oid + 1;
 }
 
@@ -46,8 +48,9 @@ void BroadPhase2DBVH::recheck_pairs(ID p_id) {
 }
 
 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?
+	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
+	uint32_t tree_collision_mask = p_static ? 0 : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
+	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
 }
 
 void BroadPhase2DBVH::remove(ID p_id) {
@@ -61,7 +64,8 @@ CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const {
 }
 
 bool BroadPhase2DBVH::is_static(ID p_id) const {
-	return !bvh.is_pairable(p_id - 1);
+	uint32_t tree_id = bvh.get_tree_id(p_id - 1);
+	return tree_id == 0;
 }
 
 int BroadPhase2DBVH::get_subindex(ID p_id) const {
@@ -69,11 +73,11 @@ int BroadPhase2DBVH::get_subindex(ID p_id) const {
 }
 
 int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
-	return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
+	return bvh.cull_segment(p_from, p_to, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
 }
 
 int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
-	return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
+	return bvh.cull_aabb(p_aabb, p_results, p_max_results, nullptr, 0xFFFFFFFF, p_result_indices);
 }
 
 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) {

servers/physics_2d/broad_phase_2d_bvh.h

@@ -37,7 +37,34 @@
 #include "core/math/vector2.h"
 
 class BroadPhase2DBVH : public BroadPhase2DSW {
-	BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh;
+	template <class T>
+	class UserPairTestFunction {
+	public:
+		static bool user_pair_check(const T *p_a, const T *p_b) {
+			// return false if no collision, decided by masks etc
+			return p_a->test_collision_mask(p_b);
+		}
+	};
+
+	template <class T>
+	class UserCullTestFunction {
+	public:
+		static bool user_cull_check(const T *p_a, const T *p_b) {
+			return true;
+		}
+	};
+
+	enum Tree {
+		TREE_STATIC = 0,
+		TREE_DYNAMIC = 1,
+	};
+
+	enum TreeFlag {
+		TREE_FLAG_STATIC = 1 << TREE_STATIC,
+		TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC,
+	};
+
+	BVH_Manager<CollisionObject2DSW, 2, true, 128, UserPairTestFunction<CollisionObject2DSW>, UserCullTestFunction<CollisionObject2DSW>, Rect2, Vector2> bvh;
 
 	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);

servers/physics_2d/collision_object_2d_sw.h

@@ -189,7 +189,7 @@ public:
 	void set_pickable(bool p_pickable) { pickable = p_pickable; }
 	_FORCE_INLINE_ bool is_pickable() const { return pickable; }
 
-	_FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW *p_other) const {
+	_FORCE_INLINE_ bool test_collision_mask(const CollisionObject2DSW *p_other) const {
 		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
 	}
 

servers/physics_2d/space_2d_sw.cpp

@@ -1203,25 +1203,14 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	return collided;
 }
 
+// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
 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);
-
+	// An existing pair - nothing to do, pair is still valid.
 	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;
+		return p_pair_data;
 	}
 
+	// New pair
 	CollisionObject2DSW::Type type_A = p_object_A->get_type();
 	CollisionObject2DSW::Type type_B = p_object_B->get_type();
 	if (type_A > type_B) {

servers/visual/visual_server_scene.cpp

@@ -101,6 +101,15 @@ void VisualServerScene::camera_set_use_vertical_aspect(RID p_camera, bool p_enab
 VisualServerScene::SpatialPartitioningScene_BVH::SpatialPartitioningScene_BVH() {
 	_bvh.params_set_thread_safe(GLOBAL_GET("rendering/threads/thread_safe_bvh"));
 	_bvh.params_set_pairing_expansion(GLOBAL_GET("rendering/quality/spatial_partitioning/bvh_collision_margin"));
+
+	_dummy_cull_object = memnew(Instance);
+}
+
+VisualServerScene::SpatialPartitioningScene_BVH::~SpatialPartitioningScene_BVH() {
+	if (_dummy_cull_object) {
+		memdelete(_dummy_cull_object);
+		_dummy_cull_object = nullptr;
+	}
 }
 
 VisualServerScene::SpatialPartitionID VisualServerScene::SpatialPartitioningScene_BVH::create(Instance *p_userdata, const AABB &p_aabb, int p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
@@ -109,7 +118,16 @@ VisualServerScene::SpatialPartitionID VisualServerScene::SpatialPartitioningScen
 	// the visible flag to the bvh.
 	DEV_ASSERT(p_userdata);
 #endif
-	return _bvh.create(p_userdata, p_userdata->visible, p_aabb, p_subindex, p_pairable, p_pairable_type, p_pairable_mask) + 1;
+
+	// cache the pairable mask and pairable type on the instance as it is needed for user callbacks from the BVH, and this is
+	// too complex to calculate each callback...
+	p_userdata->bvh_pairable_mask = p_pairable_mask;
+	p_userdata->bvh_pairable_type = p_pairable_type;
+
+	uint32_t tree_id = p_pairable ? 1 : 0;
+	uint32_t tree_collision_mask = 3;
+
+	return _bvh.create(p_userdata, p_userdata->visible, tree_id, tree_collision_mask, p_aabb, p_subindex) + 1;
 }
 
 void VisualServerScene::SpatialPartitioningScene_BVH::erase(SpatialPartitionID p_handle) {
@@ -143,20 +161,34 @@ void VisualServerScene::SpatialPartitioningScene_BVH::update_collisions() {
 	_bvh.update_collisions();
 }
 
-void VisualServerScene::SpatialPartitioningScene_BVH::set_pairable(SpatialPartitionID p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
-	_bvh.set_pairable(p_handle - 1, p_pairable, p_pairable_type, p_pairable_mask);
+void VisualServerScene::SpatialPartitioningScene_BVH::set_pairable(Instance *p_instance, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
+	SpatialPartitionID handle = p_instance->spatial_partition_id;
+
+	p_instance->bvh_pairable_mask = p_pairable_mask;
+	p_instance->bvh_pairable_type = p_pairable_type;
+
+	uint32_t tree_id = p_pairable ? 1 : 0;
+	uint32_t tree_collision_mask = 3;
+
+	_bvh.set_tree(handle - 1, tree_id, tree_collision_mask);
 }
 
 int VisualServerScene::SpatialPartitioningScene_BVH::cull_convex(const Vector<Plane> &p_convex, Instance **p_result_array, int p_result_max, uint32_t p_mask) {
-	return _bvh.cull_convex(p_convex, p_result_array, p_result_max, p_mask);
+	_dummy_cull_object->bvh_pairable_mask = p_mask;
+	_dummy_cull_object->bvh_pairable_type = 0;
+	return _bvh.cull_convex(p_convex, p_result_array, p_result_max, _dummy_cull_object);
 }
 
 int VisualServerScene::SpatialPartitioningScene_BVH::cull_aabb(const AABB &p_aabb, Instance **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
-	return _bvh.cull_aabb(p_aabb, p_result_array, p_result_max, p_subindex_array, p_mask);
+	_dummy_cull_object->bvh_pairable_mask = p_mask;
+	_dummy_cull_object->bvh_pairable_type = 0;
+	return _bvh.cull_aabb(p_aabb, p_result_array, p_result_max, _dummy_cull_object, 0xFFFFFFFF, p_subindex_array);
 }
 
 int VisualServerScene::SpatialPartitioningScene_BVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, Instance **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) {
-	return _bvh.cull_segment(p_from, p_to, p_result_array, p_result_max, p_subindex_array, p_mask);
+	_dummy_cull_object->bvh_pairable_mask = p_mask;
+	_dummy_cull_object->bvh_pairable_type = 0;
+	return _bvh.cull_segment(p_from, p_to, p_result_array, p_result_max, _dummy_cull_object, 0xFFFFFFFF, p_subindex_array);
 }
 
 void VisualServerScene::SpatialPartitioningScene_BVH::set_pair_callback(PairCallback p_callback, void *p_userdata) {
@@ -181,8 +213,9 @@ void VisualServerScene::SpatialPartitioningScene_Octree::move(SpatialPartitionID
 	_octree.move(p_handle, p_aabb);
 }
 
-void VisualServerScene::SpatialPartitioningScene_Octree::set_pairable(SpatialPartitionID p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
-	_octree.set_pairable(p_handle, p_pairable, p_pairable_type, p_pairable_mask);
+void VisualServerScene::SpatialPartitioningScene_Octree::set_pairable(Instance *p_instance, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
+	SpatialPartitionID handle = p_instance->spatial_partition_id;
+	_octree.set_pairable(handle, p_pairable, p_pairable_type, p_pairable_mask);
 }
 
 int VisualServerScene::SpatialPartitioningScene_Octree::cull_convex(const Vector<Plane> &p_convex, Instance **p_result_array, int p_result_max, uint32_t p_mask) {
@@ -782,25 +815,25 @@ void VisualServerScene::instance_set_visible(RID p_instance, bool p_visible) {
 	switch (instance->base_type) {
 		case VS::INSTANCE_LIGHT: {
 			if (VSG::storage->light_get_type(instance->base) != VS::LIGHT_DIRECTIONAL && instance->spatial_partition_id && instance->scenario) {
-				instance->scenario->sps->set_pairable(instance->spatial_partition_id, p_visible, 1 << VS::INSTANCE_LIGHT, p_visible ? VS::INSTANCE_GEOMETRY_MASK : 0);
+				instance->scenario->sps->set_pairable(instance, p_visible, 1 << VS::INSTANCE_LIGHT, p_visible ? VS::INSTANCE_GEOMETRY_MASK : 0);
 			}
 
 		} break;
 		case VS::INSTANCE_REFLECTION_PROBE: {
 			if (instance->spatial_partition_id && instance->scenario) {
-				instance->scenario->sps->set_pairable(instance->spatial_partition_id, p_visible, 1 << VS::INSTANCE_REFLECTION_PROBE, p_visible ? VS::INSTANCE_GEOMETRY_MASK : 0);
+				instance->scenario->sps->set_pairable(instance, p_visible, 1 << VS::INSTANCE_REFLECTION_PROBE, p_visible ? VS::INSTANCE_GEOMETRY_MASK : 0);
 			}
 
 		} break;
 		case VS::INSTANCE_LIGHTMAP_CAPTURE: {
 			if (instance->spatial_partition_id && instance->scenario) {
-				instance->scenario->sps->set_pairable(instance->spatial_partition_id, p_visible, 1 << VS::INSTANCE_LIGHTMAP_CAPTURE, p_visible ? VS::INSTANCE_GEOMETRY_MASK : 0);
+				instance->scenario->sps->set_pairable(instance, p_visible, 1 << VS::INSTANCE_LIGHTMAP_CAPTURE, p_visible ? VS::INSTANCE_GEOMETRY_MASK : 0);
 			}
 
 		} break;
 		case VS::INSTANCE_GI_PROBE: {
 			if (instance->spatial_partition_id && instance->scenario) {
-				instance->scenario->sps->set_pairable(instance->spatial_partition_id, p_visible, 1 << VS::INSTANCE_GI_PROBE, p_visible ? (VS::INSTANCE_GEOMETRY_MASK | (1 << VS::INSTANCE_LIGHT)) : 0);
+				instance->scenario->sps->set_pairable(instance, p_visible, 1 << VS::INSTANCE_GI_PROBE, p_visible ? (VS::INSTANCE_GEOMETRY_MASK | (1 << VS::INSTANCE_LIGHT)) : 0);
 			}
 
 		} break;

servers/visual/visual_server_scene.h

@@ -122,7 +122,7 @@ public:
 		virtual void force_collision_check(SpatialPartitionID p_handle) {}
 		virtual void update() {}
 		virtual void update_collisions() {}
-		virtual void set_pairable(SpatialPartitionID p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) = 0;
+		virtual void set_pairable(Instance *p_instance, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) = 0;
 		virtual int cull_convex(const Vector<Plane> &p_convex, Instance **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF) = 0;
 		virtual int cull_aabb(const AABB &p_aabb, Instance **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) = 0;
 		virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, Instance **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) = 0;
@@ -150,7 +150,7 @@ public:
 		SpatialPartitionID create(Instance *p_userdata, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1);
 		void erase(SpatialPartitionID p_handle);
 		void move(SpatialPartitionID p_handle, const AABB &p_aabb);
-		void set_pairable(SpatialPartitionID p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask);
+		void set_pairable(Instance *p_instance, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask);
 		int cull_convex(const Vector<Plane> &p_convex, Instance **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF);
 		int cull_aabb(const AABB &p_aabb, Instance **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
 		int cull_segment(const Vector3 &p_from, const Vector3 &p_to, Instance **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
@@ -160,11 +160,59 @@ public:
 	};
 
 	class SpatialPartitioningScene_BVH : public SpatialPartitioningScene {
+		template <class T>
+		class UserPairTestFunction {
+		public:
+			static bool user_pair_check(const T *p_a, const T *p_b) {
+				// return false if no collision, decided by masks etc
+				return true;
+			}
+		};
+
+		template <class T>
+		class UserCullTestFunction {
+			// write this logic once for use in all routines
+			// double check this as a possible source of bugs in future.
+			static bool _cull_pairing_mask_test_hit(uint32_t p_maskA, uint32_t p_typeA, uint32_t p_maskB, uint32_t p_typeB) {
+				// double check this as a possible source of bugs in future.
+				bool A_match_B = p_maskA & p_typeB;
+
+				if (!A_match_B) {
+					bool B_match_A = p_maskB & p_typeA;
+					if (!B_match_A) {
+						return false;
+					}
+				}
+
+				return true;
+			}
+
+		public:
+			static bool user_cull_check(const T *p_a, const T *p_b) {
+				DEV_ASSERT(p_a);
+				DEV_ASSERT(p_b);
+
+				uint32_t a_mask = p_a->bvh_pairable_mask;
+				uint32_t a_type = p_a->bvh_pairable_type;
+				uint32_t b_mask = p_b->bvh_pairable_mask;
+				uint32_t b_type = p_b->bvh_pairable_type;
+
+				if (!_cull_pairing_mask_test_hit(a_mask, a_type, b_mask, b_type)) {
+					return false;
+				}
+
+				return true;
+			}
+		};
+
+	private:
 		// Note that SpatialPartitionIDs are +1 based when stored in visual server, to enable 0 to indicate invalid ID.
-		BVH_Manager<Instance, true, 256> _bvh;
+		BVH_Manager<Instance, 2, true, 256, UserPairTestFunction<Instance>, UserCullTestFunction<Instance>> _bvh;
+		Instance *_dummy_cull_object;
 
 	public:
 		SpatialPartitioningScene_BVH();
+		~SpatialPartitioningScene_BVH();
 		SpatialPartitionID create(Instance *p_userdata, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1);
 		void erase(SpatialPartitionID p_handle);
 		void move(SpatialPartitionID p_handle, const AABB &p_aabb);
@@ -173,7 +221,7 @@ public:
 		void force_collision_check(SpatialPartitionID p_handle);
 		void update();
 		void update_collisions();
-		void set_pairable(SpatialPartitionID p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask);
+		void set_pairable(Instance *p_instance, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask);
 		int cull_convex(const Vector<Plane> &p_convex, Instance **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF);
 		int cull_aabb(const AABB &p_aabb, Instance **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
 		int cull_segment(const Vector3 &p_from, const Vector3 &p_to, Instance **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
@@ -251,6 +299,11 @@ public:
 		float lod_end_hysteresis;
 		RID lod_instance;
 
+		// These are used for the user cull testing function
+		// in the BVH, this is precached rather than recalculated each time.
+		uint32_t bvh_pairable_mask;
+		uint32_t bvh_pairable_type;
+
 		uint64_t last_render_pass;
 		uint64_t last_frame_pass;
 
@@ -288,6 +341,9 @@ public:
 			lod_begin_hysteresis = 0;
 			lod_end_hysteresis = 0;
 
+			bvh_pairable_mask = 0;
+			bvh_pairable_type = 0;
+
 			last_render_pass = 0;
 			last_frame_pass = 0;
 			version = 1;