Support for Dynamic BVH as 2D Physics broadphase

List of changes:
- Modified bvh class to handle 2D and 3D as a template
- Changes in Rect2, Vector2, Vector3 interface to uniformize template
calls
- New option in Project Settings to enable BVH for 2D Physics (enabled
by default like in 3D)

core/math/bvh.h

@@ -48,9 +48,9 @@
 
 #include "bvh_tree.h"
 
-#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS>
+#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS, Bounds, Point>
 
-template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32>
+template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32, class Bounds = AABB, class Point = Vector3>
 class BVH_Manager {
 
 public:
@@ -88,7 +88,7 @@ public:
 		unpair_callback_userdata = p_userdata;
 	}
 
-	BVHHandle create(T *p_userdata, bool p_active, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) {
+	BVHHandle create(T *p_userdata, bool p_active, const Bounds &p_aabb = Bounds(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) {
 
 		// 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.
@@ -108,7 +108,7 @@ public:
 
 		if (USE_PAIRS) {
 			// for safety initialize the expanded AABB
-			AABB &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
+			Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
 			expanded_aabb = p_aabb;
 			expanded_aabb.grow_by(tree._pairing_expansion);
 
@@ -125,7 +125,7 @@ public:
 	////////////////////////////////////////////////////
 	// wrapper versions that use uint32_t instead of handle
 	// for backward compatibility. Less type safe
-	void move(uint32_t p_handle, const AABB &p_aabb) {
+	void move(uint32_t p_handle, const Bounds &p_aabb) {
 		BVHHandle h;
 		h.set(p_handle);
 		move(h, p_aabb);
@@ -143,7 +143,7 @@ public:
 		force_collision_check(h);
 	}
 
-	bool activate(uint32_t p_handle, const AABB &p_aabb, bool p_delay_collision_check = false) {
+	bool activate(uint32_t p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) {
 		BVHHandle h;
 		h.set(p_handle);
 		return activate(h, p_aabb, p_delay_collision_check);
@@ -180,7 +180,7 @@ public:
 
 	////////////////////////////////////////////////////
 
-	void move(BVHHandle p_handle, const AABB &p_aabb) {
+	void move(BVHHandle p_handle, const Bounds &p_aabb) {
 
 		if (tree.item_move(p_handle, p_aabb)) {
 			if (USE_PAIRS) {
@@ -207,7 +207,7 @@ public:
 	void force_collision_check(BVHHandle p_handle) {
 		if (USE_PAIRS) {
 			// the aabb should already be up to date in the BVH
-			AABB aabb;
+			Bounds aabb;
 			item_get_AABB(p_handle, aabb);
 
 			// add it as changed even if aabb not different
@@ -221,7 +221,7 @@ public:
 	// these should be read as set_visible for render trees,
 	// but generically this makes items add or remove from the
 	// tree internally, to speed things up by ignoring inactive items
-	bool activate(BVHHandle p_handle, const AABB &p_aabb, bool p_delay_collision_check = false) {
+	bool activate(BVHHandle p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) {
 		// sending the aabb here prevents the need for the BVH to maintain
 		// a redundant copy of the aabb.
 		// returns success
@@ -294,7 +294,7 @@ public:
 				// 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.
-				AABB aabb;
+				Bounds aabb;
 				item_get_AABB(p_handle, aabb);
 
 				// passing false disables the optimization which prevents collision checks if
@@ -311,7 +311,7 @@ public:
 	}
 
 	// cull tests
-	int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_aabb(const Bounds &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
 		typename BVHTREE_CLASS::CullParams params;
 
 		params.result_count_overall = 0;
@@ -328,7 +328,7 @@ public:
 		return params.result_count_overall;
 	}
 
-	int cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_segment(const Point &p_from, const Point &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
 		typename BVHTREE_CLASS::CullParams params;
 
 		params.result_count_overall = 0;
@@ -346,7 +346,7 @@ public:
 		return params.result_count_overall;
 	}
 
-	int cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+	int cull_point(const Point &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
 		typename BVHTREE_CLASS::CullParams params;
 
 		params.result_count_overall = 0;
@@ -396,7 +396,7 @@ private:
 			return;
 		}
 
-		AABB bb;
+		Bounds bb;
 
 		typename BVHTREE_CLASS::CullParams params;
 
@@ -411,8 +411,8 @@ private:
 			const BVHHandle &h = changed_items[n];
 
 			// use the expanded aabb for pairing
-			const AABB &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
-			BVH_ABB abb;
+			const Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
+			BVHABB_CLASS abb;
 			abb.from(expanded_aabb);
 
 			// find all the existing paired aabbs that are no longer
@@ -457,8 +457,8 @@ private:
 	}
 
 public:
-	void item_get_AABB(BVHHandle p_handle, AABB &r_aabb) {
-		BVH_ABB abb;
+	void item_get_AABB(BVHHandle p_handle, Bounds &r_aabb) {
+		BVHABB_CLASS abb;
 		tree.item_get_ABB(p_handle, abb);
 		abb.to(r_aabb);
 	}
@@ -494,8 +494,8 @@ private:
 	}
 
 	// returns true if unpair
-	bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVH_ABB &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) {
-		BVH_ABB abb_to;
+	bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVHABB_CLASS &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) {
+		BVHABB_CLASS abb_to;
 		tree.item_get_ABB(p_to, abb_to);
 
 		// do they overlap?
@@ -526,10 +526,10 @@ private:
 
 	// find all the existing paired aabbs that are no longer
 	// paired, and send callbacks
-	void _find_leavers(BVHHandle p_handle, const BVH_ABB &expanded_abb_from, bool p_full_check) {
+	void _find_leavers(BVHHandle p_handle, const BVHABB_CLASS &expanded_abb_from, bool p_full_check) {
 		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
 
-		BVH_ABB abb_from = expanded_abb_from;
+		BVHABB_CLASS abb_from = expanded_abb_from;
 
 		// remove from pairing list for every partner
 		for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
@@ -608,7 +608,7 @@ private:
 		_tick++;
 	}
 
-	void _add_changed_item(BVHHandle p_handle, const AABB &aabb, bool p_check_aabb = true) {
+	void _add_changed_item(BVHHandle p_handle, const Bounds &aabb, bool p_check_aabb = true) {
 
 		// Note that non pairable items can pair with pairable,
 		// so all types must be added to the list
@@ -616,7 +616,7 @@ private:
 		// aabb check with expanded aabb. This greatly decreases processing
 		// at the cost of slightly less accurate pairing checks
 		// Note this pairing AABB is separate from the AABB in the actual tree
-		AABB &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
+		Bounds &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
 
 		// passing p_check_aabb false disables the optimization which prevents collision checks if
 		// the aabb hasn't changed. This is needed where set_pairable has been called, but the position

core/math/bvh_abb.h

@@ -32,6 +32,7 @@
 #define BVH_ABB_H
 
 // special optimized version of axis aligned bounding box
+template <class Bounds = AABB, class Point = Vector3>
 struct BVH_ABB {
 	struct ConvexHull {
 		// convex hulls (optional)
@@ -42,8 +43,8 @@ struct BVH_ABB {
 	};
 
 	struct Segment {
-		Vector3 from;
-		Vector3 to;
+		Point from;
+		Point to;
 	};
 
 	enum IntersectResult {
@@ -53,49 +54,50 @@ struct BVH_ABB {
 	};
 
 	// we store mins with a negative value in order to test them with SIMD
-	Vector3 min;
-	Vector3 neg_max;
+	Point min;
+	Point neg_max;
 
 	bool operator==(const BVH_ABB &o) const { return (min == o.min) && (neg_max == o.neg_max); }
 	bool operator!=(const BVH_ABB &o) const { return (*this == o) == false; }
 
-	void set(const Vector3 &_min, const Vector3 &_max) {
+	void set(const Point &_min, const Point &_max) {
 		min = _min;
 		neg_max = -_max;
 	}
 
 	// to and from standard AABB
-	void from(const AABB &p_aabb) {
+	void from(const Bounds &p_aabb) {
 		min = p_aabb.position;
 		neg_max = -(p_aabb.position + p_aabb.size);
 	}
 
-	void to(AABB &r_aabb) const {
+	void to(Bounds &r_aabb) const {
 		r_aabb.position = min;
 		r_aabb.size = calculate_size();
 	}
 
 	void merge(const BVH_ABB &p_o) {
-		neg_max.x = MIN(neg_max.x, p_o.neg_max.x);
-		neg_max.y = MIN(neg_max.y, p_o.neg_max.y);
-		neg_max.z = MIN(neg_max.z, p_o.neg_max.z);
-
-		min.x = MIN(min.x, p_o.min.x);
-		min.y = MIN(min.y, p_o.min.y);
-		min.z = MIN(min.z, p_o.min.z);
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			neg_max[axis] = MIN(neg_max[axis], p_o.neg_max[axis]);
+			min[axis] = MIN(min[axis], p_o.min[axis]);
+		}
 	}
 
-	Vector3 calculate_size() const {
+	Point calculate_size() const {
 		return -neg_max - min;
 	}
 
-	Vector3 calculate_centre() const {
-		return Vector3((calculate_size() * 0.5) + min);
+	Point calculate_centre() const {
+		return Point((calculate_size() * 0.5) + min);
 	}
 
 	real_t get_proximity_to(const BVH_ABB &p_b) const {
-		const Vector3 d = (min - neg_max) - (p_b.min - p_b.neg_max);
-		return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z));
+		const Point d = (min - neg_max) - (p_b.min - p_b.neg_max);
+		real_t proximity = 0.0;
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			proximity += Math::abs(d[axis]);
+		}
+		return proximity;
 	}
 
 	int select_by_proximity(const BVH_ABB &p_a, const BVH_ABB &p_b) const {
@@ -158,7 +160,7 @@ struct BVH_ABB {
 	}
 
 	bool intersects_convex_partial(const ConvexHull &p_hull) const {
-		AABB bb;
+		Bounds bb;
 		to(bb);
 		return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points);
 	}
@@ -178,7 +180,7 @@ struct BVH_ABB {
 
 	bool is_within_convex(const ConvexHull &p_hull) const {
 		// use half extents routine
-		AABB bb;
+		Bounds bb;
 		to(bb);
 		return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes);
 	}
@@ -192,59 +194,66 @@ struct BVH_ABB {
 	}
 
 	bool intersects_segment(const Segment &p_s) const {
-		AABB bb;
+		Bounds bb;
 		to(bb);
 		return bb.intersects_segment(p_s.from, p_s.to);
 	}
 
-	bool intersects_point(const Vector3 &p_pt) const {
-		if (_vector3_any_lessthan(-p_pt, neg_max)) return false;
-		if (_vector3_any_lessthan(p_pt, min)) return false;
+	bool intersects_point(const Point &p_pt) const {
+		if (_any_lessthan(-p_pt, neg_max)) return false;
+		if (_any_lessthan(p_pt, min)) return false;
 		return true;
 	}
 
 	bool intersects(const BVH_ABB &p_o) const {
-		if (_vector3_any_morethan(p_o.min, -neg_max)) return false;
-		if (_vector3_any_morethan(min, -p_o.neg_max)) return false;
+		if (_any_morethan(p_o.min, -neg_max)) return false;
+		if (_any_morethan(min, -p_o.neg_max)) return false;
 		return true;
 	}
 
 	bool is_other_within(const BVH_ABB &p_o) const {
-		if (_vector3_any_lessthan(p_o.neg_max, neg_max)) return false;
-		if (_vector3_any_lessthan(p_o.min, min)) return false;
+		if (_any_lessthan(p_o.neg_max, neg_max)) return false;
+		if (_any_lessthan(p_o.min, min)) return false;
 		return true;
 	}
 
-	void grow(const Vector3 &p_change) {
+	void grow(const Point &p_change) {
 		neg_max -= p_change;
 		min -= p_change;
 	}
 
 	void expand(real_t p_change) {
-		grow(Vector3(p_change, p_change, p_change));
+		Point change;
+		change.set_all(p_change);
+		grow(change);
 	}
 
-	float get_area() const // actually surface area metric
-	{
-		Vector3 d = calculate_size();
+	// Actually surface area metric.
+	float get_area() const {
+		Point d = calculate_size();
 		return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
 	}
+
 	void set_to_max_opposite_extents() {
-		neg_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
+		neg_max.set_all(FLT_MAX);
 		min = neg_max;
 	}
 
-	bool _vector3_any_morethan(const Vector3 &p_a, const Vector3 &p_b) const {
-		if (p_a.x > p_b.x) return true;
-		if (p_a.y > p_b.y) return true;
-		if (p_a.z > p_b.z) return true;
+	bool _any_morethan(const Point &p_a, const Point &p_b) const {
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			if (p_a[axis] > p_b[axis]) {
+				return true;
+			}
+		}
 		return false;
 	}
 
-	bool _vector3_any_lessthan(const Vector3 &p_a, const Vector3 &p_b) const {
-		if (p_a.x < p_b.x) return true;
-		if (p_a.y < p_b.y) return true;
-		if (p_a.z < p_b.z) return true;
+	bool _any_lessthan(const Point &p_a, const Point &p_b) const {
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			if (p_a[axis] < p_b[axis]) {
+				return true;
+			}
+		}
 		return false;
 	}
 };

core/math/bvh_cull.inc

@@ -15,9 +15,9 @@ struct CullParams {
 
 	// optional components for different tests
 	Vector3 point;
-	BVH_ABB abb;
-	typename BVH_ABB::ConvexHull hull;
-	typename BVH_ABB::Segment segment;
+	BVHABB_CLASS abb;
+	typename BVHABB_CLASS::ConvexHull hull;
+	typename BVHABB_CLASS::Segment segment;
 
 	// when collision testing, non pairable moving items
 	// only need to be tested against the pairable tree.
@@ -200,7 +200,7 @@ bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) {
 
 			// test children individually
 			for (int n = 0; n < leaf.num_items; n++) {
-				const BVH_ABB &aabb = leaf.get_aabb(n);
+				const BVHABB_CLASS &aabb = leaf.get_aabb(n);
 
 				if (aabb.intersects_segment(r_params.segment)) {
 					uint32_t child_id = leaf.get_item_ref_id(n);
@@ -214,7 +214,7 @@ bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) {
 			for (int n = 0; n < tnode.num_children; n++) {
 
 				uint32_t child_id = tnode.children[n];
-				const BVH_ABB &child_abb = _nodes[child_id].aabb;
+				const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
 
 				if (child_abb.intersects_segment(r_params.segment)) {
 
@@ -339,7 +339,7 @@ bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully
 				}
 			} else {
 				for (int n = 0; n < leaf.num_items; n++) {
-					const BVH_ABB &aabb = leaf.get_aabb(n);
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
 
 					if (aabb.intersects(r_params.abb)) {
 						uint32_t child_id = leaf.get_item_ref_id(n);
@@ -355,7 +355,7 @@ bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully
 				for (int n = 0; n < tnode.num_children; n++) {
 
 					uint32_t child_id = tnode.children[n];
-					const BVH_ABB &child_abb = _nodes[child_id].aabb;
+					const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
 
 					if (child_abb.intersects(r_params.abb)) {
 						// is the node totally within the aabb?
@@ -421,15 +421,15 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful
 
 		if (!ccp.fully_within) {
 
-			typename BVH_ABB::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull);
+			typename BVHABB_CLASS::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull);
 
 			switch (res) {
 				default: {
 					continue; // miss, just move on to the next node in the stack
 				} break;
-				case BVH_ABB::IR_PARTIAL: {
+				case BVHABB_CLASS::IR_PARTIAL: {
 				} break;
-				case BVH_ABB::IR_FULL: {
+				case BVHABB_CLASS::IR_FULL: {
 					ccp.fully_within = true;
 				} break;
 			}
@@ -478,7 +478,7 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful
 				// test children individually
 				for (int n = 0; n < leaf.num_items; n++) {
 					//const Item &item = leaf.get_item(n);
-					const BVH_ABB &aabb = leaf.get_aabb(n);
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
 
 					if (aabb.intersects_convex_optimized(r_params.hull, plane_ids, num_planes)) {
 						uint32_t child_id = leaf.get_item_ref_id(n);
@@ -496,7 +496,7 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful
 				uint32_t test_count = 0;
 
 				for (int n = 0; n < leaf.num_items; n++) {
-					const BVH_ABB &aabb = leaf.get_aabb(n);
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
 
 					if (aabb.intersects_convex_partial(r_params.hull)) {
 						uint32_t child_id = leaf.get_item_ref_id(n);
@@ -511,7 +511,7 @@ bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_ful
 				// not BVH_CONVEX_CULL_OPTIMIZED
 				// test children individually
 				for (int n = 0; n < leaf.num_items; n++) {
-					const BVH_ABB &aabb = leaf.get_aabb(n);
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
 
 					if (aabb.intersects_convex_partial(r_params.hull)) {
 						uint32_t child_id = leaf.get_item_ref_id(n);

core/math/bvh_debug.inc

@@ -5,7 +5,7 @@ void _debug_recursive_print_tree(int p_tree_id) const {
 		_debug_recursive_print_tree_node(_root_node_id[p_tree_id]);
 }
 
-String _debug_aabb_to_string(const BVH_ABB &aabb) const {
+String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const {
 	String sz = "(";
 	sz += itos(aabb.min.x);
 	sz += " ~ ";

core/math/bvh_integrity.inc

@@ -12,10 +12,10 @@ void _integrity_check_all() {
 void _integrity_check_up(uint32_t p_node_id) {
 	TNode &node = _nodes[p_node_id];
 
-	BVH_ABB abb = node.aabb;
+	BVHABB_CLASS abb = node.aabb;
 	node_update_aabb(node);
 
-	BVH_ABB abb2 = node.aabb;
+	BVHABB_CLASS abb2 = node.aabb;
 	abb2.expand(-_node_expansion);
 
 	CRASH_COND(!abb.is_other_within(abb2));

core/math/bvh_logic.inc

@@ -21,7 +21,7 @@ void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
 	_current_tree = _handle_get_tree_id(temp_handle);
 
 	// remove and reinsert
-	BVH_ABB abb;
+	BVHABB_CLASS abb;
 	node_remove_item(p_ref_id, &abb);
 
 	// we must choose where to add to tree
@@ -32,8 +32,8 @@ void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
 }
 
 // from randy gaul balance function
-BVH_ABB _logic_abb_merge(const BVH_ABB &a, const BVH_ABB &b) {
-	BVH_ABB c = a;
+BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) {
+	BVHABB_CLASS c = a;
 	c.merge(b);
 	return c;
 }
@@ -192,7 +192,7 @@ int32_t _logic_balance(int32_t iA) {
 }
 
 // either choose an existing node to add item to, or create a new node and return this
-uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVH_ABB &p_aabb) {
+uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
 
 	while (true) {
 		BVH_ASSERT(p_node_id != BVHCommon::INVALID);

core/math/bvh_pair.inc

@@ -14,10 +14,10 @@ struct ItemPairs {
 	void clear() {
 		num_pairs = 0;
 		extended_pairs.reset();
-		expanded_aabb = AABB();
+		expanded_aabb = Bounds();
 	}
 
-	AABB expanded_aabb;
+	Bounds expanded_aabb;
 
 	// maybe we can just use the number in the vector TODO
 	int32_t num_pairs;

core/math/bvh_public.inc

@@ -1,12 +1,12 @@
 public:
-BVHHandle item_add(T *p_userdata, bool p_active, const AABB &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) {
+BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) {
 #ifdef BVH_VERBOSE_TREE
 	VERBOSE_PRINT("\nitem_add BEFORE");
 	_debug_recursive_print_tree(0);
 	VERBOSE_PRINT("\n");
 #endif
 
-	BVH_ABB abb;
+	BVHABB_CLASS abb;
 	abb.from(p_aabb);
 
 	// handle to be filled with the new item ref
@@ -101,7 +101,7 @@ void _debug_print_refs() {
 }
 
 // returns false if noop
-bool item_move(BVHHandle p_handle, const AABB &p_aabb) {
+bool item_move(BVHHandle p_handle, const Bounds &p_aabb) {
 	uint32_t ref_id = p_handle.id();
 
 	// get the reference
@@ -110,7 +110,7 @@ bool item_move(BVHHandle p_handle, const AABB &p_aabb) {
 		return false;
 	}
 
-	BVH_ABB abb;
+	BVHABB_CLASS abb;
 	abb.from(p_aabb);
 
 	BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
@@ -124,7 +124,7 @@ bool item_move(BVHHandle p_handle, const AABB &p_aabb) {
 		// for accurate collision detection
 		TLeaf &leaf = _node_get_leaf(tnode);
 
-		BVH_ABB &leaf_abb = leaf.get_aabb(ref.item_id);
+		BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id);
 
 		// no change?
 		if (leaf_abb == abb) {
@@ -203,7 +203,7 @@ void item_remove(BVHHandle p_handle) {
 }
 
 // returns success
-bool item_activate(BVHHandle p_handle, const AABB &p_aabb) {
+bool item_activate(BVHHandle p_handle, const Bounds &p_aabb) {
 	uint32_t ref_id = p_handle.id();
 	ItemRef &ref = _refs[ref_id];
 	if (ref.is_active()) {
@@ -212,7 +212,7 @@ bool item_activate(BVHHandle p_handle, const AABB &p_aabb) {
 	}
 
 	// add to tree
-	BVH_ABB abb;
+	BVHABB_CLASS abb;
 	abb.from(p_aabb);
 
 	_current_tree = _handle_get_tree_id(p_handle);
@@ -236,7 +236,7 @@ bool item_deactivate(BVHHandle p_handle) {
 	}
 
 	// remove from tree
-	BVH_ABB abb;
+	BVHABB_CLASS abb;
 	node_remove_item(ref_id, &abb);
 
 	// mark as inactive
@@ -269,7 +269,7 @@ bool item_is_pairable(const BVHHandle &p_handle) {
 	return extra.pairable != 0;
 }
 
-void item_get_ABB(const BVHHandle &p_handle, BVH_ABB &r_abb) {
+void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) {
 	// change tree?
 	uint32_t ref_id = p_handle.id();
 	const ItemRef &ref = _refs[ref_id];
@@ -297,7 +297,7 @@ void item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pa
 		// record abb
 		TNode &tnode = _nodes[ref.tnode_id];
 		TLeaf &leaf = _node_get_leaf(tnode);
-		BVH_ABB abb = leaf.get_aabb(ref.item_id);
+		BVHABB_CLASS abb = leaf.get_aabb(ref.item_id);
 
 		// make sure current tree is correct prior to changing
 		_current_tree = _handle_get_tree_id(p_handle);
@@ -377,7 +377,7 @@ void update() {
 //#define BVH_ALLOW_AUTO_EXPANSION
 #ifdef BVH_ALLOW_AUTO_EXPANSION
 	if (_auto_node_expansion || _auto_pairing_expansion) {
-		BVH_ABB world_bound;
+		BVHABB_CLASS world_bound;
 		world_bound.set_to_max_opposite_extents();
 
 		bool bound_valid = false;
@@ -392,7 +392,7 @@ void update() {
 
 		// if there are no nodes, do nothing, but if there are...
 		if (bound_valid) {
-			AABB bb;
+			Bounds bb;
 			world_bound.to(bb);
 			real_t size = bb.get_longest_axis_size();
 

core/math/bvh_refit.inc

@@ -1,10 +1,10 @@
 void _debug_node_verify_bound(uint32_t p_node_id) {
 	TNode &node = _nodes[p_node_id];
-	BVH_ABB abb_before = node.aabb;
+	BVHABB_CLASS abb_before = node.aabb;
 
 	node_update_aabb(node);
 
-	BVH_ABB abb_after = node.aabb;
+	BVHABB_CLASS abb_after = node.aabb;
 	CRASH_COND(abb_before != abb_after);
 }
 

core/math/bvh_split.inc

@@ -11,7 +11,7 @@ void _split_inform_references(uint32_t p_node_id) {
 	}
 }
 
-void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVH_ABB *temp_bounds, const BVH_ABB full_bound) {
+void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds, const BVHABB_CLASS full_bound) {
 	// special case for low leaf sizes .. should static compile out
 	if (MAX_ITEMS < 4) {
 		uint32_t ind = group_a[0];
@@ -25,8 +25,8 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u
 		return;
 	}
 
-	Vector3 centre = full_bound.calculate_centre();
-	Vector3 size = full_bound.calculate_size();
+	Point centre = full_bound.calculate_centre();
+	Point size = full_bound.calculate_size();
 
 	int order[3];
 
@@ -135,16 +135,16 @@ void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, u
 	}
 }
 
-void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVH_ABB *temp_bounds) {
-	BVH_ABB groupb_aabb;
+void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds) {
+	BVHABB_CLASS groupb_aabb;
 	groupb_aabb.set_to_max_opposite_extents();
 	for (int n = 0; n < num_b; n++) {
 		int which = group_b[n];
 		groupb_aabb.merge(temp_bounds[which]);
 	}
-	BVH_ABB groupb_aabb_new;
+	BVHABB_CLASS groupb_aabb_new;
 
-	BVH_ABB rest_aabb;
+	BVHABB_CLASS rest_aabb;
 
 	float best_size = FLT_MAX;
 	int best_candidate = -1;
@@ -181,12 +181,12 @@ void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t
 	group_a[best_candidate] = group_a[num_a];
 }
 
-uint32_t split_leaf(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb) {
+uint32_t split_leaf(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
 	return split_leaf_complex(p_node_id, p_added_item_aabb);
 }
 
 // aabb is the new inserted node
-uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb) {
+uint32_t split_leaf_complex(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
 	VERBOSE_PRINT("split_leaf");
 
 	// note the tnode before and AFTER splitting may be a different address
@@ -231,7 +231,7 @@ uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb
 	uint16_t *group_b = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
 
 	// we are copying the ABBs. This is ugly, but we need one extra for the inserted item...
-	BVH_ABB *temp_bounds = (BVH_ABB *)alloca(sizeof(BVH_ABB) * max_children);
+	BVHABB_CLASS *temp_bounds = (BVHABB_CLASS *)alloca(sizeof(BVHABB_CLASS) * max_children);
 
 	int num_a = max_children;
 	int num_b = 0;
@@ -257,7 +257,7 @@ uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb
 		int which = group_a[n];
 
 		if (which != wildcard) {
-			const BVH_ABB &source_item_aabb = orig_leaf.get_aabb(which);
+			const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
 			uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
 			//const Item &source_item = orig_leaf.get_item(which);
 			_node_add_item(tnode.children[0], source_item_ref_id, source_item_aabb);
@@ -269,7 +269,7 @@ uint32_t split_leaf_complex(uint32_t p_node_id, const BVH_ABB &p_added_item_aabb
 		int which = group_b[n];
 
 		if (which != wildcard) {
-			const BVH_ABB &source_item_aabb = orig_leaf.get_aabb(which);
+			const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
 			uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
 			//const Item &source_item = orig_leaf.get_item(which);
 			_node_add_item(tnode.children[1], source_item_ref_id, source_item_aabb);

core/math/bvh_structs.inc

@@ -31,7 +31,7 @@ struct ItemExtra {
 
 // this is an item OR a child node depending on whether a leaf node
 struct Item {
-	BVH_ABB aabb;
+	BVHABB_CLASS aabb;
 	uint32_t item_ref_id;
 };
 
@@ -43,12 +43,12 @@ private:
 	uint16_t dirty;
 	// separate data orientated lists for faster SIMD traversal
 	uint32_t item_ref_ids[MAX_ITEMS];
-	BVH_ABB aabbs[MAX_ITEMS];
+	BVHABB_CLASS aabbs[MAX_ITEMS];
 
 public:
 	// accessors
-	BVH_ABB &get_aabb(uint32_t p_id) { return aabbs[p_id]; }
-	const BVH_ABB &get_aabb(uint32_t p_id) const { return aabbs[p_id]; }
+	BVHABB_CLASS &get_aabb(uint32_t p_id) { return aabbs[p_id]; }
+	const BVHABB_CLASS &get_aabb(uint32_t p_id) const { return aabbs[p_id]; }
 
 	uint32_t &get_item_ref_id(uint32_t p_id) { return item_ref_ids[p_id]; }
 	const uint32_t &get_item_ref_id(uint32_t p_id) const { return item_ref_ids[p_id]; }
@@ -81,7 +81,7 @@ public:
 
 // tree node
 struct TNode {
-	BVH_ABB aabb;
+	BVHABB_CLASS aabb;
 	// either number of children if positive
 	// or leaf id if negative (leaf id 0 is disallowed)
 	union {

core/math/bvh_tree.h

@@ -48,6 +48,8 @@
 #include "core/print_string.h"
 #include <limits.h>
 
+#define BVHABB_CLASS BVH_ABB<Bounds, Point>
+
 // never do these checks in release
 #if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED)
 //#define BVH_VERBOSE
@@ -146,7 +148,7 @@ public:
 	}
 };
 
-template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false>
+template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false, class Bounds = AABB, class Point = Vector3>
 class BVH_Tree {
 	friend class BVH;
 
@@ -269,7 +271,7 @@ private:
 		node.neg_leaf_id = -(int)child_leaf_id;
 	}
 
-	void node_remove_item(uint32_t p_ref_id, BVH_ABB *r_old_aabb = nullptr) {
+	void node_remove_item(uint32_t p_ref_id, BVHABB_CLASS *r_old_aabb = nullptr) {
 		// get the reference
 		ItemRef &ref = _refs[p_ref_id];
 		uint32_t owner_node_id = ref.tnode_id;
@@ -286,7 +288,7 @@ private:
 
 		// if the aabb is not determining the corner size, then there is no need to refit!
 		// (optimization, as merging AABBs takes a lot of time)
-		const BVH_ABB &old_aabb = leaf.get_aabb(ref.item_id);
+		const BVHABB_CLASS &old_aabb = leaf.get_aabb(ref.item_id);
 
 		// shrink a little to prevent using corner aabbs
 		// in order to miss the corners first we shrink by node_expansion
@@ -294,7 +296,7 @@ private:
 		// shrink by an epsilon, in order to miss out the very corner aabbs
 		// which are important in determining the bound. Any other aabb
 		// within this can be removed and not affect the overall bound.
-		BVH_ABB node_bound = tnode.aabb;
+		BVHABB_CLASS node_bound = tnode.aabb;
 		node_bound.expand(-_node_expansion - 0.001f);
 		bool refit = true;
 
@@ -348,7 +350,7 @@ private:
 
 	// returns true if needs refit of PARENT tree only, the node itself AABB is calculated
 	// within this routine
-	bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVH_ABB &p_aabb) {
+	bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVHABB_CLASS &p_aabb) {
 		ItemRef &ref = _refs[p_ref_id];
 		ref.tnode_id = p_node_id;
 
@@ -362,7 +364,7 @@ private:
 		bool needs_refit = true;
 
 		// expand bound now
-		BVH_ABB expanded = p_aabb;
+		BVHABB_CLASS expanded = p_aabb;
 		expanded.expand(_node_expansion);
 
 		// the bound will only be valid if there is an item in there already
@@ -390,7 +392,7 @@ private:
 		return needs_refit;
 	}
 
-	uint32_t _node_create_another_child(uint32_t p_node_id, const BVH_ABB &p_aabb) {
+	uint32_t _node_create_another_child(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
 		uint32_t child_node_id;
 		TNode *child_node = _nodes.request(child_node_id);
 		child_node->clear();

core/math/rect2.h

@@ -170,15 +170,18 @@ struct Rect2 {
 	bool operator!=(const Rect2 &p_rect) const { return position != p_rect.position || size != p_rect.size; }
 
 	inline Rect2 grow(real_t p_by) const {
-
 		Rect2 g = *this;
-		g.position.x -= p_by;
-		g.position.y -= p_by;
-		g.size.width += p_by * 2;
-		g.size.height += p_by * 2;
+		g.grow_by(p_by);
 		return g;
 	}
 
+	inline void grow_by(real_t p_by) {
+		position.x -= p_by;
+		position.y -= p_by;
+		size.width += p_by * 2;
+		size.height += p_by * 2;
+	}
+
 	inline Rect2 grow_margin(Margin p_margin, real_t p_amount) const {
 		Rect2 g = *this;
 		g = g.grow_individual((MARGIN_LEFT == p_margin) ? p_amount : 0,

core/math/vector2.h

@@ -37,6 +37,7 @@
 struct Vector2i;
 
 struct Vector2 {
+	static const int AXIS_COUNT = 2;
 
 	enum Axis {
 		AXIS_X,
@@ -44,12 +45,18 @@ struct Vector2 {
 	};
 
 	union {
-		real_t x;
-		real_t width;
-	};
-	union {
-		real_t y;
-		real_t height;
+		struct {
+			union {
+				real_t x;
+				real_t width;
+			};
+			union {
+				real_t y;
+				real_t height;
+			};
+		};
+
+		real_t coord[2];
 	};
 
 	_FORCE_INLINE_ real_t &operator[](int p_idx) {
@@ -59,6 +66,18 @@ struct Vector2 {
 		return p_idx ? y : x;
 	}
 
+	_FORCE_INLINE_ void set_all(real_t p_value) {
+		x = y = p_value;
+	}
+
+	_FORCE_INLINE_ int min_axis() const {
+		return x < y ? 0 : 1;
+	}
+
+	_FORCE_INLINE_ int max_axis() const {
+		return x < y ? 1 : 0;
+	}
+
 	void normalize();
 	Vector2 normalized() const;
 	bool is_normalized() const;

core/math/vector3.cpp

@@ -54,15 +54,6 @@ real_t Vector3::get_axis(int p_axis) const {
 	return operator[](p_axis);
 }
 
-int Vector3::min_axis() const {
-
-	return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2);
-}
-int Vector3::max_axis() const {
-
-	return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0);
-}
-
 void Vector3::snap(Vector3 p_val) {
 
 	x = Math::stepify(x, p_val.x);

core/math/vector3.h

@@ -37,6 +37,7 @@
 class Basis;
 
 struct Vector3 {
+	static const int AXIS_COUNT = 3;
 
 	enum Axis {
 		AXIS_X,
@@ -67,8 +68,17 @@ struct Vector3 {
 	void set_axis(int p_axis, real_t p_value);
 	real_t get_axis(int p_axis) const;
 
-	int min_axis() const;
-	int max_axis() const;
+	_FORCE_INLINE_ void set_all(real_t p_value) {
+		x = y = z = p_value;
+	}
+
+	_FORCE_INLINE_ int min_axis() const {
+		return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2);
+	}
+
+	_FORCE_INLINE_ int max_axis() const {
+		return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0);
+	}
 
 	_FORCE_INLINE_ real_t length() const;
 	_FORCE_INLINE_ real_t length_squared() const;

doc/classes/ProjectSettings.xml

@@ -935,9 +935,11 @@
 		</member>
 		<member name="physics/2d/bp_hash_table_size" type="int" setter="" getter="" default="4096">
 			Size of the hash table used for the broad-phase 2D hash grid algorithm.
+			[b]Note:[/b] Not used if [member ProjectSettings.physics/2d/use_bvh] is enabled.
 		</member>
 		<member name="physics/2d/cell_size" type="int" setter="" getter="" default="128">
 			Cell size used for the broad-phase 2D hash grid algorithm (in pixels).
+			[b]Note:[/b] Not used if [member ProjectSettings.physics/2d/use_bvh] is enabled.
 		</member>
 		<member name="physics/2d/default_angular_damp" type="float" setter="" getter="" default="1.0">
 			The default angular damp in 2D.
@@ -965,6 +967,7 @@
 		</member>
 		<member name="physics/2d/large_object_surface_threshold_in_cells" type="int" setter="" getter="" default="512">
 			Threshold defining the surface size that constitutes a large object with regard to cells in the broad-phase 2D hash grid algorithm.
+			[b]Note:[/b] Not used if [member ProjectSettings.physics/2d/use_bvh] is enabled.
 		</member>
 		<member name="physics/2d/physics_engine" type="String" setter="" getter="" default="&quot;DEFAULT&quot;">
 			Sets which physics engine to use for 2D physics.
@@ -983,6 +986,9 @@
 		<member name="physics/2d/time_before_sleep" type="float" setter="" getter="" default="0.5">
 			Time (in seconds) of inactivity before which a 2D physics body will put to sleep. See [constant Physics2DServer.SPACE_PARAM_BODY_TIME_TO_SLEEP].
 		</member>
+		<member name="physics/2d/use_bvh" type="bool" setter="" getter="" default="true">
+			Enables the use of bounding volume hierarchy instead of hash grid for 2D physics spatial partitioning. This may give better performance.
+		</member>
 		<member name="physics/3d/active_soft_world" type="bool" setter="" getter="" default="true">
 			Sets whether the 3D physics world will be created with support for [SoftBody] physics. Only applies to the Bullet physics engine.
 		</member>
@@ -1011,7 +1017,7 @@
 			[b]Note:[/b] Good values are in the range [code]0[/code] to [code]1[/code]. At value [code]0[/code] objects will keep moving with the same velocity. Values greater than [code]1[/code] will aim to reduce the velocity to [code]0[/code] in less than a second e.g. a value of [code]2[/code] will aim to reduce the velocity to [code]0[/code] in half a second. A value equal to or greater than the physics frame rate ([member ProjectSettings.physics/common/physics_fps], [code]60[/code] by default) will bring the object to a stop in one iteration.
 		</member>
 		<member name="physics/3d/godot_physics/use_bvh" type="bool" setter="" getter="" default="true">
-			Enables the use of bounding volume hierarchy instead of octree for physics spatial partitioning. This may give better performance.
+			Enables the use of bounding volume hierarchy instead of octree for 3D physics spatial partitioning. This may give better performance.
 		</member>
 		<member name="physics/3d/physics_engine" type="String" setter="" getter="" default="&quot;DEFAULT&quot;">
 			Sets which physics engine to use for 3D physics.

servers/physics_2d/broad_phase_2d_basic.cpp

@@ -30,7 +30,7 @@
 
 #include "broad_phase_2d_basic.h"
 
-BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) {
+BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex, const Rect2 &p_aabb, bool p_static) {
 
 	current++;
 

servers/physics_2d/broad_phase_2d_basic.h

@@ -82,7 +82,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0);
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
 	virtual void move(ID p_id, const Rect2 &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);

servers/physics_2d/broad_phase_2d_bvh.cpp

@@ -0,0 +1,126 @@
+/*************************************************************************/
+/*  broad_phase_2d_bvh.cpp                                               */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "broad_phase_2d_bvh.h"
+#include "collision_object_2d_sw.h"
+#include "core/project_settings.h"
+
+BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *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?
+	return oid + 1;
+}
+
+void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
+
+	bvh.move(p_id - 1, 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?
+}
+void BroadPhase2DBVH::remove(ID p_id) {
+
+	bvh.erase(p_id - 1);
+}
+
+CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const {
+
+	CollisionObject2DSW *it = bvh.get(p_id - 1);
+	ERR_FAIL_COND_V(!it, NULL);
+	return it;
+}
+bool BroadPhase2DBVH::is_static(ID p_id) const {
+
+	return !bvh.is_pairable(p_id - 1);
+}
+int BroadPhase2DBVH::get_subindex(ID p_id) const {
+
+	return bvh.get_subindex(p_id - 1);
+}
+
+int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **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);
+}
+
+int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
+
+	return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
+}
+
+void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) {
+
+	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self);
+	if (!bpo->pair_callback)
+		return NULL;
+
+	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
+}
+
+void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) {
+
+	BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self);
+	if (!bpo->unpair_callback)
+		return;
+
+	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
+}
+
+void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
+
+	pair_callback = p_pair_callback;
+	pair_userdata = p_userdata;
+}
+
+void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
+
+	unpair_callback = p_unpair_callback;
+	unpair_userdata = p_userdata;
+}
+
+void BroadPhase2DBVH::update() {
+	bvh.update();
+}
+
+BroadPhase2DSW *BroadPhase2DBVH::_create() {
+
+	return memnew(BroadPhase2DBVH);
+}
+
+BroadPhase2DBVH::BroadPhase2DBVH() {
+	bvh.set_pair_callback(_pair_callback, this);
+	bvh.set_unpair_callback(_unpair_callback, this);
+	pair_callback = NULL;
+	pair_userdata = NULL;
+	unpair_userdata = NULL;
+}

servers/physics_2d/broad_phase_2d_bvh.h

@@ -0,0 +1,74 @@
+/*************************************************************************/
+/*  broad_phase_2d_bvh.h                                                 */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BROAD_PHASE_2D_BVH_H
+#define BROAD_PHASE_2D_BVH_H
+
+#include "broad_phase_2d_sw.h"
+#include "core/math/bvh.h"
+#include "core/math/rect2.h"
+#include "core/math/vector2.h"
+
+class BroadPhase2DBVH : public BroadPhase2DSW {
+
+	BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh;
+
+	static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int);
+	static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *);
+
+	PairCallback pair_callback;
+	void *pair_userdata;
+	UnpairCallback unpair_callback;
+	void *unpair_userdata;
+
+public:
+	// 0 is an invalid ID
+	virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
+	virtual void move(ID p_id, const Rect2 &p_aabb);
+	virtual void set_static(ID p_id, bool p_static);
+	virtual void remove(ID p_id);
+
+	virtual CollisionObject2DSW *get_object(ID p_id) const;
+	virtual bool is_static(ID p_id) const;
+	virtual int get_subindex(ID p_id) const;
+
+	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL);
+	virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL);
+
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
+
+	virtual void update();
+
+	static BroadPhase2DSW *_create();
+	BroadPhase2DBVH();
+};
+
+#endif // BROAD_PHASE_2D_BVH_H

servers/physics_2d/broad_phase_2d_hash_grid.cpp

@@ -317,7 +317,7 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool
 	}
 }
 
-BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
+BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
 
 	current++;
 
@@ -633,15 +633,15 @@ BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
 
 BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
 
-	hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096);
+	hash_table_size = GLOBAL_GET("physics/2d/bp_hash_table_size");
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/bp_hash_table_size", PropertyInfo(Variant::INT, "physics/2d/bp_hash_table_size", PROPERTY_HINT_RANGE, "0,8192,1,or_greater"));
 	hash_table_size = Math::larger_prime(hash_table_size);
 	hash_table = memnew_arr(PosBin *, hash_table_size);
 
-	cell_size = GLOBAL_DEF("physics/2d/cell_size", 128);
+	cell_size = GLOBAL_GET("physics/2d/cell_size");
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/cell_size", PropertyInfo(Variant::INT, "physics/2d/cell_size", PROPERTY_HINT_RANGE, "0,512,1,or_greater"));
 
-	large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512);
+	large_object_min_surface = GLOBAL_GET("physics/2d/large_object_surface_threshold_in_cells");
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/large_object_surface_threshold_in_cells", PropertyInfo(Variant::INT, "physics/2d/large_object_surface_threshold_in_cells", PROPERTY_HINT_RANGE, "0,1024,1,or_greater"));
 
 	for (uint32_t i = 0; i < hash_table_size; i++)

servers/physics_2d/broad_phase_2d_hash_grid.h

@@ -168,7 +168,7 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 	void _check_motion(Element *p_elem);
 
 public:
-	virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0);
+	virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
 	virtual void move(ID p_id, const Rect2 &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);

servers/physics_2d/broad_phase_2d_sw.h

@@ -49,7 +49,7 @@ public:
 	typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0) = 0;
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 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 remove(ID p_id) = 0;

servers/physics_2d/collision_object_2d_sw.cpp

@@ -187,19 +187,19 @@ void CollisionObject2DSW::_update_shapes() {
 		if (s.disabled)
 			continue;
 
-		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i);
-			space->get_broadphase()->set_static(s.bpid, _static);
-		}
-
 		//not quite correct, should compute the next matrix..
 		Rect2 shape_aabb = s.shape->get_aabb();
 		Transform2D 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);
 
-		space->get_broadphase()->move(s.bpid, s.aabb_cache);
+		if (s.bpid == 0) {
+			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);
 	}
 }
 
@@ -214,11 +214,6 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
 		if (s.disabled)
 			continue;
 
-		if (s.bpid == 0) {
-			s.bpid = space->get_broadphase()->create(this, i);
-			space->get_broadphase()->set_static(s.bpid, _static);
-		}
-
 		//not quite correct, should compute the next matrix..
 		Rect2 shape_aabb = s.shape->get_aabb();
 		Transform2D xform = transform * s.xform;
@@ -226,6 +221,11 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
 		shape_aabb = shape_aabb.merge(Rect2(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, shape_aabb, _static);
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
+
 		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
 }

servers/physics_2d/physics_2d_server_sw.cpp

@@ -30,6 +30,7 @@
 
 #include "physics_2d_server_sw.h"
 #include "broad_phase_2d_basic.h"
+#include "broad_phase_2d_bvh.h"
 #include "broad_phase_2d_hash_grid.h"
 #include "collision_solver_2d_sw.h"
 #include "core/os/os.h"
@@ -1440,8 +1441,19 @@ Physics2DServerSW *Physics2DServerSW::singletonsw = NULL;
 Physics2DServerSW::Physics2DServerSW() {
 
 	singletonsw = this;
-	BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create;
-	//BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
+
+	GLOBAL_DEF("physics/2d/use_bvh", true);
+	GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096);
+	GLOBAL_DEF("physics/2d/cell_size", 128);
+	GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512);
+
+	bool use_bvh = GLOBAL_GET("physics/2d/use_bvh");
+
+	if (use_bvh) {
+		BroadPhase2DSW::create_func = BroadPhase2DBVH::_create;
+	} else {
+		BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create;
+	}
 
 	active = true;
 	island_count = 0;
@@ -1450,7 +1462,7 @@ Physics2DServerSW::Physics2DServerSW() {
 #ifdef NO_THREADS
 	using_threads = false;
 #else
-	using_threads = int(ProjectSettings::get_singleton()->get("physics/2d/thread_model")) == 2;
+	using_threads = int(GLOBAL_GET("physics/2d/thread_model")) == 2;
 #endif
 	flushing_queries = false;
 };