Add protective checks for invalid handle use in BVH

Adds DEV_ASSERTS that will halt at runtime if the BVH is misused with invalid IDs, and adds ERR_FAIL macros to prevent calling with invalid IDs.

Any such misuse is a bug in the physics, but this should flag any errors quickly.

core/math/bvh.h

@@ -196,6 +196,7 @@ public:
 	////////////////////////////////////////////////////
 
 	void move(BVHHandle p_handle, const BOUNDS &p_aabb) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		if (tree.item_move(p_handle, p_aabb)) {
 			if (USE_PAIRS) {
@@ -205,10 +206,12 @@ public:
 	}
 
 	void recheck_pairs(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		force_collision_check(p_handle);
 	}
 
 	void erase(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		// call unpair and remove all references to the item
 		// before deleting from the tree
@@ -225,6 +228,7 @@ public:
 	// set pairable has never been called.
 	// (deferred collision checks are a workaround for visual server for historical reasons)
 	void force_collision_check(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		if (USE_PAIRS) {
 			// the aabb should already be up to date in the BVH
@@ -243,6 +247,7 @@ public:
 	// but generically this makes items add or remove from the
 	// tree internally, to speed things up by ignoring inactive items
 	bool activate(BVHHandle p_handle, const BOUNDS &p_aabb, bool p_delay_collision_check = false) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		// sending the aabb here prevents the need for the BVH to maintain
 		// a redundant copy of the aabb.
@@ -267,6 +272,7 @@ public:
 	}
 
 	bool deactivate(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		// returns success
 		if (tree.item_deactivate(p_handle)) {
@@ -285,6 +291,7 @@ public:
 	}
 
 	bool get_active(BVHHandle p_handle) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		return tree.item_get_active(p_handle);
 	}
@@ -307,6 +314,7 @@ public:
 
 	// prefer calling this directly as type safe
 	void set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVH_LOCKED_FUNCTION
 		// Returns true if the pairing state has changed.
 		bool state_changed = tree.item_set_tree(p_handle, p_tree_id, p_tree_collision_mask);
@@ -465,13 +473,6 @@ private:
 					continue;
 				}
 
-#ifdef BVH_CHECKS
-				// if neither are pairable, they should ignore each other
-				// THIS SHOULD NEVER HAPPEN .. now we only test the pairable tree
-				// if the changed item is not pairable
-				CRASH_COND(params.test_pairable_only && !tree._extra[ref_id].pairable);
-#endif
-
 				// checkmasks is already done in the cull routine.
 				BVHHandle h_collidee;
 				h_collidee.set_id(ref_id);
@@ -485,6 +486,7 @@ private:
 
 public:
 	void item_get_AABB(BVHHandle p_handle, BOUNDS &r_aabb) {
+		DEV_ASSERT(!p_handle.is_invalid());
 		BVHABB_CLASS abb;
 		tree.item_get_ABB(p_handle, abb);
 		abb.to(r_aabb);

core/math/bvh_structs.inc

@@ -60,11 +60,23 @@ private:
 
 public:
 	// accessors
-	BVHABB_CLASS &get_aabb(uint32_t p_id) { return aabbs[p_id]; }
+	BVHABB_CLASS &get_aabb(uint32_t p_id) {
-	const BVHABB_CLASS &get_aabb(uint32_t p_id) const { return aabbs[p_id]; }
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return aabbs[p_id];
+	}
+	const BVHABB_CLASS &get_aabb(uint32_t p_id) const {
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return aabbs[p_id];
+	}
 
-	uint32_t &get_item_ref_id(uint32_t p_id) { return item_ref_ids[p_id]; }
+	uint32_t &get_item_ref_id(uint32_t p_id) {
-	const uint32_t &get_item_ref_id(uint32_t p_id) const { return item_ref_ids[p_id]; }
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return item_ref_ids[p_id];
+	}
+	const uint32_t &get_item_ref_id(uint32_t p_id) const {
+		BVH_ASSERT(p_id < MAX_ITEMS);
+		return item_ref_ids[p_id];
+	}
 
 	bool is_dirty() const { return dirty; }
 	void set_dirty(bool p) { dirty = p; }

core/math/bvh_tree.h

@@ -54,7 +54,7 @@
 #define BVH_EXPAND_LEAF_AABBS
 
 // never do these checks in release
-#if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED)
+#ifdef DEV_ENABLED
 //#define BVH_VERBOSE
 //#define BVH_VERBOSE_TREE
 //#define BVH_VERBOSE_PAIRING
@@ -217,7 +217,7 @@ private:
 		BVH_ASSERT(!parent.is_leaf());
 
 		int child_num = parent.find_child(p_old_child_id);
-		BVH_ASSERT(child_num != BVHCommon::INVALID);
+		BVH_ASSERT(child_num != -1);
 		parent.children[child_num] = p_new_child_id;
 
 		TNode &new_child = _nodes[p_new_child_id];
@@ -229,7 +229,7 @@ private:
 		BVH_ASSERT(!parent.is_leaf());
 
 		int child_num = parent.find_child(p_child_id);
-		BVH_ASSERT(child_num != BVHCommon::INVALID);
+		BVH_ASSERT(child_num != -1);
 
 		parent.remove_child_internal(child_num);
 

servers/physics_2d/godot_broad_phase_2d_bvh.cpp

@@ -39,31 +39,37 @@ GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_obj
 }
 
 void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
+	ERR_FAIL_COND(!p_id);
 	bvh.move(p_id - 1, p_aabb);
 }
 
 void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) {
+	ERR_FAIL_COND(!p_id);
 	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
 	uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
 	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
 }
 
 void GodotBroadPhase2DBVH::remove(ID p_id) {
+	ERR_FAIL_COND(!p_id);
 	bvh.erase(p_id - 1);
 }
 
 GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, nullptr);
 	GodotCollisionObject2D *it = bvh.get(p_id - 1);
 	ERR_FAIL_COND_V(!it, nullptr);
 	return it;
 }
 
 bool GodotBroadPhase2DBVH::is_static(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, false);
 	uint32_t tree_id = bvh.get_tree_id(p_id - 1);
 	return tree_id == 0;
 }
 
 int GodotBroadPhase2DBVH::get_subindex(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, 0);
 	return bvh.get_subindex(p_id - 1);
 }
 

servers/physics_3d/godot_broad_phase_3d_bvh.cpp

@@ -40,31 +40,37 @@ GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_
 }
 
 void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
+	ERR_FAIL_COND(!p_id);
 	bvh.move(p_id - 1, p_aabb);
 }
 
 void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) {
+	ERR_FAIL_COND(!p_id);
 	uint32_t tree_id = p_static ? TREE_STATIC : TREE_DYNAMIC;
 	uint32_t tree_collision_mask = p_static ? TREE_FLAG_DYNAMIC : (TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC);
 	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
 }
 
 void GodotBroadPhase3DBVH::remove(ID p_id) {
+	ERR_FAIL_COND(!p_id);
 	bvh.erase(p_id - 1);
 }
 
 GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, nullptr);
 	GodotCollisionObject3D *it = bvh.get(p_id - 1);
 	ERR_FAIL_COND_V(!it, nullptr);
 	return it;
 }
 
 bool GodotBroadPhase3DBVH::is_static(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, false);
 	uint32_t tree_id = bvh.get_tree_id(p_id - 1);
 	return tree_id == 0;
 }
 
 int GodotBroadPhase3DBVH::get_subindex(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, 0);
 	return bvh.get_subindex(p_id - 1);
 }