BVH - Sync BVH with 3.x

Templated mask checks and generic NUM_TREES
Fix leaking leaves

8 files changed, 81 insertions, 25 deletions

diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
index 5a96dae8ca..6d46f6074e 100644
--- a/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
+++ b/servers/physics_2d/godot_broad_phase_2d_bvh.cpp
@@ -32,7 +32,9 @@
 #include "godot_collision_object_2d.h"
 
 GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *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;
 }
 
@@ -41,8 +43,9 @@ void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
 }
 
 void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) {
-	GodotCollisionObject2D *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 GodotBroadPhase2DBVH::remove(ID p_id) {
@@ -56,7 +59,8 @@ GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const {
 }
 
 bool GodotBroadPhase2DBVH::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 GodotBroadPhase2DBVH::get_subindex(ID p_id) const {
@@ -64,11 +68,11 @@ int GodotBroadPhase2DBVH::get_subindex(ID p_id) const {
 }
 
 int GodotBroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **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 GodotBroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **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 *GodotBroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject2D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject2D *p_object_B, int subindex_B) {
diff --git a/servers/physics_2d/godot_broad_phase_2d_bvh.h b/servers/physics_2d/godot_broad_phase_2d_bvh.h
index d77e0574eb..b11ad0e75e 100644
--- a/servers/physics_2d/godot_broad_phase_2d_bvh.h
+++ b/servers/physics_2d/godot_broad_phase_2d_bvh.h
@@ -38,7 +38,34 @@
 #include "core/math/vector2.h"
 
 class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
-	BVH_Manager<GodotCollisionObject2D, 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->interacts_with(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<GodotCollisionObject2D, 2, true, 128, UserPairTestFunction<GodotCollisionObject2D>, UserCullTestFunction<GodotCollisionObject2D>, Rect2, Vector2> bvh;
 
 	static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int);
 	static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *);
diff --git a/servers/physics_2d/godot_collision_object_2d.h b/servers/physics_2d/godot_collision_object_2d.h
index 1e9baad8d9..19d6e91561 100644
--- a/servers/physics_2d/godot_collision_object_2d.h
+++ b/servers/physics_2d/godot_collision_object_2d.h
@@ -180,7 +180,7 @@ public:
 		return p_other->collision_layer & collision_mask;
 	}
 
-	_FORCE_INLINE_ bool interacts_with(GodotCollisionObject2D *p_other) const {
+	_FORCE_INLINE_ bool interacts_with(const GodotCollisionObject2D *p_other) const {
 		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
 	}
 
diff --git a/servers/physics_2d/godot_space_2d.cpp b/servers/physics_2d/godot_space_2d.cpp
index 5c2bda340b..53a82d18c3 100644
--- a/servers/physics_2d/godot_space_2d.cpp
+++ b/servers/physics_2d/godot_space_2d.cpp
@@ -996,11 +996,8 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
 	return collided;
 }
 
+// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
 void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) {
-	if (!A->interacts_with(B)) {
-		return nullptr;
-	}
-
 	GodotCollisionObject2D::Type type_A = A->get_type();
 	GodotCollisionObject2D::Type type_B = B->get_type();
 	if (type_A > type_B) {
diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
index 9a6b96c411..d947892751 100644
--- a/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
+++ b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
@@ -33,7 +33,9 @@
 #include "godot_collision_object_3d.h"
 
 GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *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;
 }
 
@@ -42,8 +44,9 @@ void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
 }
 
 void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) {
-	GodotCollisionObject3D *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 GodotBroadPhase3DBVH::remove(ID p_id) {
@@ -57,7 +60,8 @@ GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const {
 }
 
 bool GodotBroadPhase3DBVH::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 GodotBroadPhase3DBVH::get_subindex(ID p_id) const {
@@ -65,15 +69,15 @@ int GodotBroadPhase3DBVH::get_subindex(ID p_id) const {
 }
 
 int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **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 GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **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 GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **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 *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) {
diff --git a/servers/physics_3d/godot_broad_phase_3d_bvh.h b/servers/physics_3d/godot_broad_phase_3d_bvh.h
index 7138019a9c..7660030195 100644
--- a/servers/physics_3d/godot_broad_phase_3d_bvh.h
+++ b/servers/physics_3d/godot_broad_phase_3d_bvh.h
@@ -36,7 +36,34 @@
 #include "core/math/bvh.h"
 
 class GodotBroadPhase3DBVH : public GodotBroadPhase3D {
-	BVH_Manager<GodotCollisionObject3D, 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->interacts_with(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<GodotCollisionObject3D, 2, true, 128, UserPairTestFunction<GodotCollisionObject3D>, UserCullTestFunction<GodotCollisionObject3D>> bvh;
 
 	static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int);
 	static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int, void *);
diff --git a/servers/physics_3d/godot_collision_object_3d.h b/servers/physics_3d/godot_collision_object_3d.h
index 0178838a25..515b945564 100644
--- a/servers/physics_3d/godot_collision_object_3d.h
+++ b/servers/physics_3d/godot_collision_object_3d.h
@@ -169,7 +169,7 @@ public:
 		return p_other->collision_layer & collision_mask;
 	}
 
-	_FORCE_INLINE_ bool interacts_with(GodotCollisionObject3D *p_other) const {
+	_FORCE_INLINE_ bool interacts_with(const GodotCollisionObject3D *p_other) const {
 		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
 	}
 
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index ed756a7f9d..fa5e08db48 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -1008,11 +1008,8 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 	return collided;
 }
 
+// Assumes a valid collision pair, this should have been checked beforehand in the BVH or octree.
 void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self) {
-	if (!A->interacts_with(B)) {
-		return nullptr;
-	}
-
 	GodotCollisionObject3D::Type type_A = A->get_type();
 	GodotCollisionObject3D::Type type_B = B->get_type();
 	if (type_A > type_B) {