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.

2 files changed, 12 insertions, 0 deletions

diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
index 06f035a506..2cc60fc765 100644
--- a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
+++ b/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);
 }
 
diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
index ecdf74fd41..b34f9d214f 100644
--- a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
+++ b/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);
 }