Merge pull request #55640 from lawnjelly/bvh5_multi_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;