From 0dacc681b651ae6e7cf47ec6f36b5df56be853ee Mon Sep 17 00:00:00 2001
From: PouleyKetchoupp <pouleyketchoup@gmail.com>
Date: Fri, 30 Apr 2021 11:45:41 -0700
Subject: [PATCH] Fixed unnecessary bvh tree updates when calling set_pairable

Dynamic BVH doesn't update the tree anymore when calling set_pairable
with no parameter change.

Also modified Godot Physics broadphase to create objects directly with
pairable (static) set correctly to make use of this optimization for the
BVH broadphase.

Note: Octree broadphase doesn't use this optimization because it forces
an update on move, so passing the proper AABB and static parameters on
creation would cause the tree to update twice.
---
 core/math/bvh.h                           | 11 ++++++-----
 core/math/bvh_public.inc                  | 11 +++++++----
 servers/physics/broad_phase_basic.cpp     |  2 +-
 servers/physics/broad_phase_basic.h       |  2 +-
 servers/physics/broad_phase_bvh.cpp       |  6 +++---
 servers/physics/broad_phase_bvh.h         |  2 +-
 servers/physics/broad_phase_octree.cpp    |  2 +-
 servers/physics/broad_phase_octree.h      |  2 +-
 servers/physics/broad_phase_sw.h          |  2 +-
 servers/physics/collision_object_sw.cpp   | 12 +++++++-----
 servers/physics_2d/broad_phase_2d_bvh.cpp |  2 +-
 11 files changed, 30 insertions(+), 24 deletions(-)

diff --git a/core/math/bvh.h b/core/math/bvh.h
index c75f1d291aa..28f81b40a0a 100644
--- a/core/math/bvh.h
+++ b/core/math/bvh.h
@@ -153,10 +153,10 @@ public:
 		return deactivate(h);
 	}
 
-	void set_pairable(uint32_t p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
+	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) {
 		BVHHandle h;
 		h.set(p_handle);
-		set_pairable(h, p_pairable, p_pairable_type, p_pairable_mask);
+		set_pairable(h, p_pairable, p_pairable_type, p_pairable_mask, p_force_collision_check);
 	}
 
 	bool is_pairable(uint32_t p_handle) const {
@@ -277,15 +277,16 @@ 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) {
-		tree.item_set_pairable(p_handle, p_pairable, p_pairable_type, p_pairable_mask);
+	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) {
+		// 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);
 
 		if (USE_PAIRS) {
 			// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
 			// of waiting for update, so only uncomment this if there are bugs.
 			//_check_for_collisions();
 
-			if (get_active(p_handle)) {
+			if ((p_force_collision_check || state_changed) && get_active(p_handle)) {
 				// when the pairable state changes, we need to force a collision check because newly pairable
 				// items may be in collision, and unpairable items might move out of collision.
 				// We cannot depend on waiting for the next update, because that may come much later.
diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc
index a35668be1ba..9d53d367e7a 100644
--- a/core/math/bvh_public.inc
+++ b/core/math/bvh_public.inc
@@ -280,18 +280,19 @@ void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) {
 	r_abb = leaf.get_aabb(ref.item_id);
 }
 
-void item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
+bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
 	// change tree?
 	uint32_t ref_id = p_handle.id();
 
 	ItemExtra &ex = _extra[ref_id];
 	ItemRef &ref = _refs[ref_id];
 
-	ex.pairable_type = p_pairable_type;
-	ex.pairable_mask = p_pairable_mask;
-
 	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);
+
+	ex.pairable_type = p_pairable_type;
+	ex.pairable_mask = p_pairable_mask;
 
 	if (active && pairable_changed) {
 		// record abb
@@ -328,6 +329,8 @@ void item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 		// always keep this up to date
 		ex.pairable = p_pairable;
 	}
+
+	return state_changed;
 }
 
 void incremental_optimize() {
diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp
index 9ffab4eca3d..dbd6f327984 100644
--- a/servers/physics/broad_phase_basic.cpp
+++ b/servers/physics/broad_phase_basic.cpp
@@ -32,7 +32,7 @@
 #include "core/list.h"
 #include "core/print_string.h"
 
-BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb) {
+BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
 	ERR_FAIL_COND_V(p_object == nullptr, 0);
 
 	current++;
diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h
index 05394994c50..085a48cfbee 100644
--- a/servers/physics/broad_phase_basic.h
+++ b/servers/physics/broad_phase_basic.h
@@ -80,7 +80,7 @@ class BroadPhaseBasic : public BroadPhaseSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB());
+	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
diff --git a/servers/physics/broad_phase_bvh.cpp b/servers/physics/broad_phase_bvh.cpp
index 93ba0a169e5..134fc6a186a 100644
--- a/servers/physics/broad_phase_bvh.cpp
+++ b/servers/physics/broad_phase_bvh.cpp
@@ -32,8 +32,8 @@
 #include "collision_object_sw.h"
 #include "core/project_settings.h"
 
-BroadPhaseSW::ID BroadPhaseBVH::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb) {
-	ID oid = bvh.create(p_object, true, p_aabb, p_subindex, false, 1 << p_object->get_type(), 0);
+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?
 	return oid + 1;
 }
 
@@ -43,7 +43,7 @@ void BroadPhaseBVH::move(ID p_id, const AABB &p_aabb) {
 
 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); //pair everything, don't care 1?
+	bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
 }
 void BroadPhaseBVH::remove(ID p_id) {
 	bvh.erase(p_id - 1);
diff --git a/servers/physics/broad_phase_bvh.h b/servers/physics/broad_phase_bvh.h
index c662bb50778..4e8890829d9 100644
--- a/servers/physics/broad_phase_bvh.h
+++ b/servers/physics/broad_phase_bvh.h
@@ -47,7 +47,7 @@ class BroadPhaseBVH : public BroadPhaseSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB());
+	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp
index 1cbb32bb8ce..955768360f9 100644
--- a/servers/physics/broad_phase_octree.cpp
+++ b/servers/physics/broad_phase_octree.cpp
@@ -31,7 +31,7 @@
 #include "broad_phase_octree.h"
 #include "collision_object_sw.h"
 
-BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb) {
+BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
 	ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
 	return oid;
 }
diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h
index 4188899e5f4..e1529e11159 100644
--- a/servers/physics/broad_phase_octree.h
+++ b/servers/physics/broad_phase_octree.h
@@ -47,7 +47,7 @@ class BroadPhaseOctree : public BroadPhaseSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB());
+	virtual ID create(CollisionObjectSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h
index 497a958dabc..9e2606b2418 100644
--- a/servers/physics/broad_phase_sw.h
+++ b/servers/physics/broad_phase_sw.h
@@ -48,7 +48,7 @@ public:
 	typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB()) = 0;
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
 	virtual void move(ID p_id, const AABB &p_aabb) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
 	virtual void remove(ID p_id) = 0;
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index 440f1823afd..239cf1dd147 100644
--- a/servers/physics/collision_object_sw.cpp
+++ b/servers/physics/collision_object_sw.cpp
@@ -146,17 +146,18 @@ void CollisionObjectSW::_update_shapes() {
 		AABB shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
 		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
 		s.aabb_cache = shape_aabb;
-		s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
 
 		Vector3 scale = xform.get_basis().get_scale();
 		s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
 
 		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i, s.aabb_cache);
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
 			space->get_broadphase()->set_static(s.bpid, _static);
 		}
-		space->get_broadphase()->move(s.bpid, s.aabb_cache);
+
+		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
 }
 
@@ -171,13 +172,14 @@ void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 		AABB shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
 		shape_aabb = xform.xform(shape_aabb);
-		shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
+		shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
 		s.aabb_cache = shape_aabb;
 
 		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i, s.aabb_cache);
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
 			space->get_broadphase()->set_static(s.bpid, _static);
 		}
+
 		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
 }
diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp
index 7aeae9b2935..5a60de6c8a6 100644
--- a/servers/physics_2d/broad_phase_2d_bvh.cpp
+++ b/servers/physics_2d/broad_phase_2d_bvh.cpp
@@ -43,7 +43,7 @@ void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
 
 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); // Pair everything, don't care?
+	bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
 }
 void BroadPhase2DBVH::remove(ID p_id) {
 	bvh.erase(p_id - 1);