Merge pull request #49057 from lawnjelly/bvh_current_tree

BVH - fix stale current_tree in deactivate function

core/math/bvh_logic.inc

@@ -20,17 +20,17 @@ void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
 	// some overlay elaborate way to find out which tree the node is in!
 	BVHHandle temp_handle;
 	temp_handle.set_id(p_ref_id);
-	_current_tree = _handle_get_tree_id(temp_handle);
+	uint32_t tree_id = _handle_get_tree_id(temp_handle);
 
 	// remove and reinsert
 	BVHABB_CLASS abb;
-	node_remove_item(p_ref_id, &abb);
+	node_remove_item(p_ref_id, tree_id, &abb);
 
 	// we must choose where to add to tree
-	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
 	_node_add_item(ref.tnode_id, p_ref_id, abb);
 
-	refit_upward_and_balance(ref.tnode_id);
+	refit_upward_and_balance(ref.tnode_id, tree_id);
 }
 
 // from randy gaul balance function
@@ -66,7 +66,7 @@ BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) {
 // https://github.com/RandyGaul/qu3e
 // It is MODIFIED from qu3e version.
 // This is the only function used (and _logic_abb_merge helper function).
-int32_t _logic_balance(int32_t iA) {
+int32_t _logic_balance(int32_t iA, uint32_t p_tree_id) {
 	//	return iA; // uncomment this to bypass balance
 
 	TNode *A = &_nodes[iA];
@@ -107,7 +107,7 @@ int32_t _logic_balance(int32_t iA) {
 			}
 		} else {
 			// check this .. seems dodgy
-			change_root_node(iC);
+			change_root_node(iC, p_tree_id);
 		}
 
 		// Swap A and C
@@ -159,7 +159,7 @@ int32_t _logic_balance(int32_t iA) {
 
 		else {
 			// check this .. seems dodgy
-			change_root_node(iB);
+			change_root_node(iB, p_tree_id);
 		}
 
 		// Swap A and B

core/math/bvh_public.inc

@@ -53,16 +53,16 @@ BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p
 	// assign to handle to return
 	handle.set_id(ref_id);
 
-	_current_tree = 0;
+	uint32_t tree_id = 0;
 	if (p_pairable) {
-		_current_tree = 1;
+		tree_id = 1;
 	}
 
-	create_root_node(_current_tree);
+	create_root_node(tree_id);
 
 	// we must choose where to add to tree
 	if (p_active) {
-		ref->tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+		ref->tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
 
 		bool refit = _node_add_item(ref->tnode_id, ref_id, abb);
 
@@ -70,7 +70,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);
+				refit_upward_and_balance(add_node.parent_id, tree_id);
 			}
 		}
 	} else {
@@ -139,13 +139,13 @@ bool item_move(BVHHandle p_handle, const Bounds &p_aabb) {
 		return true;
 	}
 
-	_current_tree = _handle_get_tree_id(p_handle);
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
 
 	// remove and reinsert
-	node_remove_item(ref_id);
+	node_remove_item(ref_id, tree_id);
 
 	// we must choose where to add to tree
-	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
 
 	// add to the tree
 	bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
@@ -167,7 +167,7 @@ bool item_move(BVHHandle p_handle, const Bounds &p_aabb) {
 void item_remove(BVHHandle p_handle) {
 	uint32_t ref_id = p_handle.id();
 
-	_current_tree = _handle_get_tree_id(p_handle);
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
 
 	VERBOSE_PRINT("item_remove [" + itos(ref_id) + "] ");
 
@@ -187,7 +187,7 @@ void item_remove(BVHHandle p_handle) {
 
 	// remove the item from the node (only if active)
 	if (_refs[ref_id].is_active()) {
-		node_remove_item(ref_id);
+		node_remove_item(ref_id, tree_id);
 	}
 
 	// remove the item reference
@@ -198,10 +198,10 @@ void item_remove(BVHHandle p_handle) {
 	}
 
 	// don't think refit_all is necessary?
-	//refit_all(_current_tree);
+	//refit_all(_tree_id);
 
 #ifdef BVH_VERBOSE_TREE
-	_debug_recursive_print_tree(_current_tree);
+	_debug_recursive_print_tree(tree_id);
 #endif
 }
 
@@ -218,13 +218,13 @@ bool item_activate(BVHHandle p_handle, const Bounds &p_aabb) {
 	BVHABB_CLASS abb;
 	abb.from(p_aabb);
 
-	_current_tree = _handle_get_tree_id(p_handle);
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
 
 	// we must choose where to add to tree
-	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
 	_node_add_item(ref.tnode_id, ref_id, abb);
 
-	refit_upward_and_balance(ref.tnode_id);
+	refit_upward_and_balance(ref.tnode_id, tree_id);
 
 	return true;
 }
@@ -238,9 +238,11 @@ bool item_deactivate(BVHHandle p_handle) {
 		return false;
 	}
 
+	uint32_t tree_id = _handle_get_tree_id(p_handle);
+
 	// remove from tree
 	BVHABB_CLASS abb;
-	node_remove_item(ref_id, &abb);
+	node_remove_item(ref_id, tree_id, &abb);
 
 	// mark as inactive
 	ref.set_inactive();
@@ -304,21 +306,21 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 		BVHABB_CLASS abb = leaf.get_aabb(ref.item_id);
 
 		// make sure current tree is correct prior to changing
-		_current_tree = _handle_get_tree_id(p_handle);
+		uint32_t tree_id = _handle_get_tree_id(p_handle);
 
 		// remove from old tree
-		node_remove_item(ref_id);
+		node_remove_item(ref_id, tree_id);
 
 		// we must set the pairable AFTER getting the current tree
 		// because the pairable status determines which tree
 		ex.pairable = p_pairable;
 
 		// add to new tree
-		_current_tree = _handle_get_tree_id(p_handle);
-		create_root_node(_current_tree);
+		tree_id = _handle_get_tree_id(p_handle);
+		create_root_node(tree_id);
 
 		// we must choose where to add to tree
-		ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+		ref.tnode_id = _logic_choose_item_add_node(_root_node_id[tree_id], abb);
 		bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
 
 		// only need to refit from the PARENT
@@ -326,7 +328,7 @@ bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 			// 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);
+				refit_upward_and_balance(add_node.parent_id, tree_id);
 			}
 		}
 	} else {

core/math/bvh_refit.inc

@@ -66,10 +66,10 @@ void refit_upward(uint32_t p_node_id) {
 	}
 }
 
-void refit_upward_and_balance(uint32_t p_node_id) {
+void refit_upward_and_balance(uint32_t p_node_id, uint32_t p_tree_id) {
 	while (p_node_id != BVHCommon::INVALID) {
 		uint32_t before = p_node_id;
-		p_node_id = _logic_balance(p_node_id);
+		p_node_id = _logic_balance(p_node_id, p_tree_id);
 
 		if (before != p_node_id) {
 			VERBOSE_PRINT("REBALANCED!");

core/math/bvh_structs.inc

@@ -162,7 +162,6 @@ enum { NUM_TREES = 2,
 // Tree 1 - Pairable
 // This is more efficient because in physics we only need check non pairable against the pairable tree.
 uint32_t _root_node_id[NUM_TREES];
-int _current_tree = 0;
 
 // these values may need tweaking according to the project
 // the bound of the world, and the average velocities of the objects

core/math/bvh_tree.h

@@ -196,7 +196,7 @@ private:
 		new_child.parent_id = p_parent_id;
 	}
 
-	void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, bool p_prevent_sibling = false) {
+	void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, uint32_t p_tree_id, bool p_prevent_sibling = false) {
 		TNode &parent = _nodes[p_parent_id];
 		BVH_ASSERT(!parent.is_leaf());
 
@@ -231,7 +231,7 @@ private:
 		if (grandparent_id == BVHCommon::INVALID) {
 			if (sibling_present) {
 				// change the root node
-				change_root_node(sibling_id);
+				change_root_node(sibling_id, p_tree_id);
 
 				// delete the old root node as no longer needed
 				_nodes.free(p_parent_id);
@@ -243,16 +243,15 @@ private:
 		if (sibling_present) {
 			node_replace_child(grandparent_id, p_parent_id, sibling_id);
 		} else {
-			node_remove_child(grandparent_id, p_parent_id, true);
+			node_remove_child(grandparent_id, p_parent_id, p_tree_id, true);
 		}
 
 		// put the node on the free list to recycle
 		_nodes.free(p_parent_id);
 	}
 
-	// this relies on _current_tree being accurate
-	void change_root_node(uint32_t p_new_root_id) {
-		_root_node_id[_current_tree] = p_new_root_id;
+	void change_root_node(uint32_t p_new_root_id, uint32_t p_tree_id) {
+		_root_node_id[p_tree_id] = p_new_root_id;
 		TNode &root = _nodes[p_new_root_id];
 
 		// mark no parent
@@ -272,7 +271,7 @@ private:
 		node.neg_leaf_id = -(int)child_leaf_id;
 	}
 
-	void node_remove_item(uint32_t p_ref_id, BVHABB_CLASS *r_old_aabb = nullptr) {
+	void node_remove_item(uint32_t p_ref_id, uint32_t p_tree_id, BVHABB_CLASS *r_old_aabb = nullptr) {
 		// get the reference
 		ItemRef &ref = _refs[p_ref_id];
 		uint32_t owner_node_id = ref.tnode_id;
@@ -336,7 +335,7 @@ private:
 
 				uint32_t parent_id = tnode.parent_id;
 
-				node_remove_child(parent_id, owner_node_id);
+				node_remove_child(parent_id, owner_node_id, p_tree_id);
 				refit_upward(parent_id);
 
 				// put the node on the free list to recycle