Add an area tree to bvh for fixing "inmonitorable area VS static body…

…" detection.

core/math/bvh.h

@@ -181,6 +181,11 @@ class BVH_Manager {
 		h.set(p_handle);
 		return item_get_tree_id(h);
 	}
+	uint32_t get_tree_collision_mask(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_get_tree_collision_mask(h);
+	}
 	int get_subindex(uint32_t p_handle) const {
 		BVHHandle h;
 		h.set(p_handle);
@@ -451,6 +456,9 @@ class BVH_Manager {
 			abb.from(expanded_aabb);
 
 			tree.item_fill_cullparams(h, params);
+			if (params.tree_collision_mask == 0) {
+				continue;
+			}
 
 			// find all the existing paired aabbs that are no longer
 			// paired, and send callbacks
@@ -491,6 +499,7 @@ class BVH_Manager {
 private:
 	// supplemental funcs
 	uint32_t item_get_tree_id(BVHHandle p_handle) const { return _get_extra(p_handle).tree_id; }
+	uint32_t item_get_tree_collision_mask(BVHHandle p_handle) const { return _get_extra(p_handle).tree_collision_mask; }
 	T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; }
 	int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; }
 

modules/godot_physics_2d/godot_area_2d.cpp

@@ -80,6 +80,8 @@ void GodotArea2D::set_space(GodotSpace2D *p_space) {
 void GodotArea2D::set_monitor_callback(const Callable &p_callback) {
 	_unregister_shapes();
 
+	bool prev_monitoring = is_monitoring();
+
 	monitor_callback = p_callback;
 
 	monitored_bodies.clear();
@@ -90,11 +92,18 @@ void GodotArea2D::set_monitor_callback(const Callable &p_callback) {
 	if (!moved_list.in_list() && get_space()) {
 		get_space()->area_add_to_moved_list(&moved_list);
 	}
+
+	if (prev_monitoring != is_monitoring()) {
+		_set_type(!monitorable, is_monitoring(), false);
+		_shapes_changed();
+	}
 }
 
 void GodotArea2D::set_area_monitor_callback(const Callable &p_callback) {
 	_unregister_shapes();
 
+	bool prev_monitoring = is_monitoring();
+
 	area_monitor_callback = p_callback;
 
 	monitored_bodies.clear();
@@ -105,6 +114,11 @@ void GodotArea2D::set_area_monitor_callback(const Callable &p_callback) {
 	if (!moved_list.in_list() && get_space()) {
 		get_space()->area_add_to_moved_list(&moved_list);
 	}
+
+	if (prev_monitoring != is_monitoring()) {
+		_set_type(!monitorable, is_monitoring(), false);
+		_shapes_changed();
+	}
 }
 
 void GodotArea2D::_set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode &r_mode, PhysicsServer2D::AreaSpaceOverrideMode p_new_mode) {
@@ -193,7 +207,7 @@ void GodotArea2D::set_monitorable(bool p_monitorable) {
 	}
 
 	monitorable = p_monitorable;
-	_set_static(!monitorable);
+	_set_type(!p_monitorable, is_monitoring(), false);
 	_shapes_changed();
 }
 
@@ -307,7 +321,7 @@ GodotArea2D::GodotArea2D() :
 		GodotCollisionObject2D(TYPE_AREA),
 		monitor_query_list(this),
 		moved_list(this) {
-	_set_static(true); //areas are not active by default
+	_set_type(false, false, false); //areas are not active by default
 }
 
 GodotArea2D::~GodotArea2D() {

modules/godot_physics_2d/godot_area_2d.h

@@ -144,6 +144,8 @@ class GodotArea2D : public GodotCollisionObject2D {
 	void set_monitorable(bool p_monitorable);
 	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
 
+	_FORCE_INLINE_ bool is_monitoring() const {return monitor_callback.is_valid() || area_monitor_callback.is_valid();}
+
 	void set_transform(const Transform2D &p_transform);
 
 	void set_space(GodotSpace2D *p_space) override;

modules/godot_physics_2d/godot_broad_phase_2d.h

@@ -48,13 +48,16 @@ class GodotBroadPhase2D {
 	typedef void (*UnpairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0;
+	virtual ID create(GodotCollisionObject2D *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false, bool p_area = false, bool p_dynamic = true) = 0;
 	virtual void move(ID p_id, const Rect2 &p_aabb) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
+	virtual void set_type(ID p_id, bool p_static, bool p_area, bool p_dynamic) = 0;
 	virtual void remove(ID p_id) = 0;
 
 	virtual GodotCollisionObject2D *get_object(ID p_id) const = 0;
 	virtual bool is_static(ID p_id) const = 0;
+	virtual bool is_area(ID p_id) const = 0;
+	virtual bool is_dynamic(ID p_id) const = 0;
 	virtual int get_subindex(ID p_id) const = 0;
 
 	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;

modules/godot_physics_2d/godot_broad_phase_2d_bvh.cpp

@@ -31,9 +31,33 @@
 #include "godot_broad_phase_2d_bvh.h"
 #include "godot_collision_object_2d.h"
 
-GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
-	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);
+void GodotBroadPhase2DBVH::get_tree_and_collition_mask(bool p_static, bool p_area, bool p_dynamic, uint32_t &r_tree_id, uint32_t &p_tree_collision_mask) {
+	if (p_static) {
+		if (p_area) {
+			// Active and monitorable Area (monitorable = false, monitoring = true).
+			r_tree_id = TREE_AREA;
+			p_tree_collision_mask = TREE_FLAG_STATIC | TREE_FLAG_DYNAMIC;
+		} else {
+			// Static body.
+			r_tree_id = TREE_STATIC;
+			p_tree_collision_mask = TREE_FLAG_AREA | TREE_FLAG_DYNAMIC;
+		}
+	} else if (p_dynamic || p_area) {
+		// Rigid body or active but inmonitorable area (monitorable = true, monitoring = true).
+		r_tree_id = TREE_DYNAMIC;
+		p_tree_collision_mask = TREE_FLAG_STATIC | TREE_FLAG_AREA | TREE_FLAG_DYNAMIC;
+	} else {
+		// Inactiave area (monitorable = false, monitoring = false), set it's mask to 0, and add additional check in BVH_Manager<>::_check_for_collisions().
+		r_tree_id = TREE_STATIC;
+		p_tree_collision_mask = 0;
+	}
+}
+
+GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static, bool p_area, bool p_dynamic) {
+	uint32_t tree_id = 0;
+	uint32_t tree_collision_mask = 0;
+	get_tree_and_collition_mask(p_static, p_area, p_dynamic, tree_id, tree_collision_mask);
+
 	ID oid = bvh.create(p_object, true, tree_id, tree_collision_mask, p_aabb, p_subindex); // Pair everything, don't care?
 	return oid + 1;
 }
@@ -45,8 +69,18 @@ void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &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);
+	uint32_t tree_id = 0;
+	uint32_t tree_collision_mask = 0;
+	get_tree_and_collition_mask(p_static, false, !p_static, tree_id, tree_collision_mask);
+	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
+}
+
+void GodotBroadPhase2DBVH::set_type(ID p_id, bool p_static, bool p_area, bool p_dynamic) {
+	ERR_FAIL_COND(!p_id);
+	uint32_t tree_id = 0;
+	uint32_t tree_collision_mask = 0;
+	get_tree_and_collition_mask(p_static, p_area, p_dynamic, tree_id, tree_collision_mask);
+
 	bvh.set_tree(p_id - 1, tree_id, tree_collision_mask, false);
 }
 
@@ -64,8 +98,17 @@ GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const {
 
 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;
+	return bvh.get_tree_collision_mask(p_id - 1) & TREE_FLAG_STATIC;
+}
+
+bool GodotBroadPhase2DBVH::is_area(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, false);
+	return bvh.get_tree_collision_mask(p_id - 1) & TREE_FLAG_AREA;
+}
+
+bool GodotBroadPhase2DBVH::is_dynamic(ID p_id) const {
+	ERR_FAIL_COND_V(!p_id, false);
+	return bvh.get_tree_collision_mask(p_id - 1) & TREE_FLAG_DYNAMIC;
 }
 
 int GodotBroadPhase2DBVH::get_subindex(ID p_id) const {

modules/godot_physics_2d/godot_broad_phase_2d_bvh.h

@@ -57,15 +57,20 @@ class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
 
 	enum Tree {
 		TREE_STATIC = 0,
-		TREE_DYNAMIC = 1,
+		TREE_AREA = 1,
+		TREE_DYNAMIC = 2,
+		TREE_MAX = 3,
 	};
 
 	enum TreeFlag {
 		TREE_FLAG_STATIC = 1 << TREE_STATIC,
+		TREE_FLAG_AREA = 1 << TREE_AREA,
 		TREE_FLAG_DYNAMIC = 1 << TREE_DYNAMIC,
 	};
 
-	BVH_Manager<GodotCollisionObject2D, 2, true, 128, UserPairTestFunction<GodotCollisionObject2D>, UserCullTestFunction<GodotCollisionObject2D>, Rect2, Vector2> bvh;
+	BVH_Manager<GodotCollisionObject2D, TREE_MAX, true, 128, UserPairTestFunction<GodotCollisionObject2D>, UserCullTestFunction<GodotCollisionObject2D>, Rect2, Vector2> bvh;
+
+	void get_tree_and_collition_mask(bool p_static, bool p_area, bool p_dynamic, uint32_t &r_tree_id, uint32_t &p_tree_collision_mask);
 
 	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 *);
@@ -77,13 +82,16 @@ class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) override;
+	virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false, bool p_area = false, bool p_dynamic = true) override;
 	virtual void move(ID p_id, const Rect2 &p_aabb) override;
 	virtual void set_static(ID p_id, bool p_static) override;
+	virtual void set_type(ID p_id, bool p_static, bool p_area, bool p_dynamic) override;
 	virtual void remove(ID p_id) override;
 
 	virtual GodotCollisionObject2D *get_object(ID p_id) const override;
 	virtual bool is_static(ID p_id) const override;
+	virtual bool is_area(ID p_id) const override;
+	virtual bool is_dynamic(ID p_id) const override;
 	virtual int get_subindex(ID p_id) const override;
 
 	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr) override;

modules/godot_physics_2d/godot_collision_object_2d.cpp

@@ -131,18 +131,42 @@ void GodotCollisionObject2D::remove_shape(int p_index) {
 }
 
 void GodotCollisionObject2D::_set_static(bool p_static) {
-	if (_static == p_static) {
+	if (_static == p_static && _area == false && _dynamic == !_static) {
 		return;
 	}
 	_static = p_static;
+	_area = false;
+	_dynamic = !_static;
 
 	if (!space) {
 		return;
 	}
 	for (int i = 0; i < get_shape_count(); i++) {
 		const Shape &s = shapes[i];
 		if (s.bpid > 0) {
-			space->get_broadphase()->set_static(s.bpid, _static);
+			space->get_broadphase()->set_type(s.bpid, _static, _area, _dynamic);
+		}
+	}
+}
+
+void GodotCollisionObject2D::_set_type(bool p_static, bool p_area, bool p_dynamic) {
+	if (_static == p_static && _area == p_area && _dynamic == p_dynamic) {
+		return;
+	}
+#ifdef DEV_ENABLED
+	ERR_FAIL_COND(p_static && p_dynamic);
+#endif // DEV_ENABLED
+	_static = p_static;
+	_area = p_area;
+	_dynamic = p_dynamic;
+
+	if (!space) {
+		return;
+	}
+	for (int i = 0; i < get_shape_count(); i++) {
+		const Shape &s = shapes[i];
+		if (s.bpid > 0) {
+			space->get_broadphase()->set_type(s.bpid, _static, _area, _dynamic);
 		}
 	}
 }
@@ -176,8 +200,8 @@ void GodotCollisionObject2D::_update_shapes() {
 		s.aabb_cache = shape_aabb;
 
 		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
-			space->get_broadphase()->set_static(s.bpid, _static);
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static, _dynamic);
+			space->get_broadphase()->set_type(s.bpid, _static, _area, _dynamic);
 		}
 
 		space->get_broadphase()->move(s.bpid, shape_aabb);
@@ -203,8 +227,8 @@ void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion)
 		s.aabb_cache = shape_aabb;
 
 		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
-			space->get_broadphase()->set_static(s.bpid, _static);
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static, _dynamic);
+			space->get_broadphase()->set_type(s.bpid, _static, _area, _dynamic);
 		}
 
 		space->get_broadphase()->move(s.bpid, shape_aabb);

modules/godot_physics_2d/godot_collision_object_2d.h

@@ -72,6 +72,8 @@ class GodotCollisionObject2D : public GodotShapeOwner2D {
 	uint32_t collision_layer = 1;
 	real_t collision_priority = 1.0;
 	bool _static = true;
+	bool _area = false;
+	bool _dynamic = false;
 
 	SelfList<GodotCollisionObject2D> pending_shape_update_list;
 
@@ -89,6 +91,7 @@ class GodotCollisionObject2D : public GodotShapeOwner2D {
 	}
 	_FORCE_INLINE_ void _set_inv_transform(const Transform2D &p_transform) { inv_transform = p_transform; }
 	void _set_static(bool p_static);
+	void _set_type(bool p_static, bool p_area, bool p_dynamic);
 
 	virtual void _shapes_changed() = 0;
 	void _set_space(GodotSpace2D *p_space);

modules/godot_physics_3d/godot_area_3d.cpp

@@ -89,6 +89,8 @@ void GodotArea3D::set_space(GodotSpace3D *p_space) {
 void GodotArea3D::set_monitor_callback(const Callable &p_callback) {
 	_unregister_shapes();
 
+	bool prev_monitoring = is_monitoring();
+
 	monitor_callback = p_callback;
 
 	monitored_bodies.clear();
@@ -99,11 +101,18 @@ void GodotArea3D::set_monitor_callback(const Callable &p_callback) {
 	if (!moved_list.in_list() && get_space()) {
 		get_space()->area_add_to_moved_list(&moved_list);
 	}
+
+	if (prev_monitoring != is_monitoring()) {
+		_set_type(!monitorable, is_monitoring(), false);
+		_shapes_changed();
+	}
 }
 
 void GodotArea3D::set_area_monitor_callback(const Callable &p_callback) {
 	_unregister_shapes();
 
+	bool prev_monitoring = is_monitoring();
+
 	area_monitor_callback = p_callback;
 
 	monitored_bodies.clear();
@@ -114,6 +123,11 @@ void GodotArea3D::set_area_monitor_callback(const Callable &p_callback) {
 	if (!moved_list.in_list() && get_space()) {
 		get_space()->area_add_to_moved_list(&moved_list);
 	}
+
+	if (prev_monitoring != is_monitoring()) {
+		_set_type(!monitorable, is_monitoring(), false);
+		_shapes_changed();
+	}
 }
 
 void GodotArea3D::_set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode) {
@@ -224,7 +238,7 @@ void GodotArea3D::set_monitorable(bool p_monitorable) {
 	}
 
 	monitorable = p_monitorable;
-	_set_static(!monitorable);
+	_set_type(!p_monitorable, is_monitoring(), false);
 	_shapes_changed();
 }
 
@@ -338,7 +352,7 @@ GodotArea3D::GodotArea3D() :
 		GodotCollisionObject3D(TYPE_AREA),
 		monitor_query_list(this),
 		moved_list(this) {
-	_set_static(true); //areas are never active
+	_set_type(false, false, false); //areas are never active
 	set_ray_pickable(false);
 }
 

modules/godot_physics_3d/godot_area_3d.h

@@ -165,6 +165,8 @@ class GodotArea3D : public GodotCollisionObject3D {
 	void set_monitorable(bool p_monitorable);
 	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
 
+	_FORCE_INLINE_ bool is_monitoring() const { return monitor_callback.is_valid() || area_monitor_callback.is_valid(); }
+
 	void set_transform(const Transform3D &p_transform);
 
 	void set_space(GodotSpace3D *p_space) override;