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

@@ -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.

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() {

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++;

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);

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);

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);

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;
 }

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);

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;

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);
 	}
 }

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);