Merge pull request #48629 from nekomatata/dynamic-bvh-broadphase-4.0

Dynamic BVH broadphase in 2D & 3D Godot Physics

8 files changed, 145 insertions, 1167 deletions

diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp
deleted file mode 100644
index 17424629a9..0000000000
--- a/servers/physics_2d/broad_phase_2d_basic.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/*************************************************************************/
-/*  broad_phase_2d_basic.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_basic.h"
-
-BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) {
-	current++;
-
-	Element e;
-	e.owner = p_object_;
-	e._static = false;
-	e.subindex = p_subindex;
-
-	element_map[current] = e;
-	return current;
-}
-
-void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-	E->get().aabb = p_aabb;
-}
-
-void BroadPhase2DBasic::set_static(ID p_id, bool p_static) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-	E->get()._static = p_static;
-}
-
-void BroadPhase2DBasic::remove(ID p_id) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-	element_map.erase(E);
-}
-
-CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const {
-	const Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND_V(!E, nullptr);
-	return E->get().owner;
-}
-
-bool BroadPhase2DBasic::is_static(ID p_id) const {
-	const Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND_V(!E, false);
-	return E->get()._static;
-}
-
-int BroadPhase2DBasic::get_subindex(ID p_id) const {
-	const Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND_V(!E, -1);
-	return E->get().subindex;
-}
-
-int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
-	int rc = 0;
-
-	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
-		const Rect2 aabb = E->get().aabb;
-		if (aabb.intersects_segment(p_from, p_to)) {
-			p_results[rc] = E->get().owner;
-			p_result_indices[rc] = E->get().subindex;
-			rc++;
-			if (rc >= p_max_results) {
-				break;
-			}
-		}
-	}
-
-	return rc;
-}
-
-int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
-	int rc = 0;
-
-	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
-		const Rect2 aabb = E->get().aabb;
-		if (aabb.intersects(p_aabb)) {
-			p_results[rc] = E->get().owner;
-			p_result_indices[rc] = E->get().subindex;
-			rc++;
-			if (rc >= p_max_results) {
-				break;
-			}
-		}
-	}
-
-	return rc;
-}
-
-void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
-	pair_userdata = p_userdata;
-	pair_callback = p_pair_callback;
-}
-
-void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
-	unpair_userdata = p_userdata;
-	unpair_callback = p_unpair_callback;
-}
-
-void BroadPhase2DBasic::update() {
-	// recompute pairs
-	for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
-		for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) {
-			Element *elem_A = &I->get();
-			Element *elem_B = &J->get();
-
-			if (elem_A->owner == elem_B->owner) {
-				continue;
-			}
-
-			bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static);
-
-			PairKey key(I->key(), J->key());
-
-			Map<PairKey, void *>::Element *E = pair_map.find(key);
-
-			if (!pair_ok && E) {
-				if (unpair_callback) {
-					unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata);
-				}
-				pair_map.erase(key);
-			}
-
-			if (pair_ok && !E) {
-				void *data = nullptr;
-				if (pair_callback) {
-					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
-					if (data) {
-						pair_map.insert(key, data);
-					}
-				}
-			}
-		}
-	}
-}
-
-BroadPhase2DSW *BroadPhase2DBasic::_create() {
-	return memnew(BroadPhase2DBasic);
-}
-
-BroadPhase2DBasic::BroadPhase2DBasic() {
-	current = 1;
-	unpair_callback = nullptr;
-	unpair_userdata = nullptr;
-	pair_callback = nullptr;
-	pair_userdata = nullptr;
-}
diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp
new file mode 100644
index 0000000000..5f53f4a012
--- /dev/null
+++ b/servers/physics_2d/broad_phase_2d_bvh.cpp
@@ -0,0 +1,116 @@
+/*************************************************************************/
+/*  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"
+
+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, false); // 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, nullptr);
+	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 nullptr;
+	}
+
+	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 = nullptr;
+	pair_userdata = nullptr;
+	unpair_userdata = nullptr;
+}
diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_bvh.h
index ca1db360fb..6c11d2561b 100644
--- a/servers/physics_2d/broad_phase_2d_basic.h
+++ b/servers/physics_2d/broad_phase_2d_bvh.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_2d_basic.h                                               */
+/*  broad_phase_2d_bvh.h                                                 */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,49 +28,19 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BROAD_PHASE_2D_BASIC_H
-#define BROAD_PHASE_2D_BASIC_H
+#ifndef BROAD_PHASE_2D_BVH_H
+#define BROAD_PHASE_2D_BVH_H
 
-#include "core/templates/map.h"
-#include "space_2d_sw.h"
-class BroadPhase2DBasic : public BroadPhase2DSW {
-	struct Element {
-		CollisionObject2DSW *owner;
-		bool _static;
-		Rect2 aabb;
-		int subindex;
-	};
+#include "broad_phase_2d_sw.h"
+#include "core/math/bvh.h"
+#include "core/math/rect2.h"
+#include "core/math/vector2.h"
 
-	Map<ID, Element> element_map;
+class BroadPhase2DBVH : public BroadPhase2DSW {
+	BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh;
 
-	ID current;
-
-	struct PairKey {
-		union {
-			struct {
-				ID a;
-				ID b;
-			};
-			uint64_t key;
-		};
-
-		_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
-			return key < p_key.key;
-		}
-
-		PairKey() { key = 0; }
-		PairKey(ID p_a, ID p_b) {
-			if (p_a > p_b) {
-				a = p_b;
-				b = p_a;
-			} else {
-				a = p_a;
-				b = p_b;
-			}
-		}
-	};
-
-	Map<PairKey, void *> pair_map;
+	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;
@@ -79,7 +49,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);
@@ -97,7 +67,7 @@ public:
 	virtual void update();
 
 	static BroadPhase2DSW *_create();
-	BroadPhase2DBasic();
+	BroadPhase2DBVH();
 };
 
-#endif // BROAD_PHASE_2D_BASIC_H
+#endif // BROAD_PHASE_2D_BVH_H
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
deleted file mode 100644
index 35447c5389..0000000000
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ /dev/null
@@ -1,738 +0,0 @@
-/*************************************************************************/
-/*  broad_phase_2d_hash_grid.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_hash_grid.h"
-#include "collision_object_2d_sw.h"
-#include "core/config/project_settings.h"
-
-#define LARGE_ELEMENT_FI 1.01239812
-
-void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) {
-	if (p_elem->owner == p_with->owner) {
-		return;
-	}
-	if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) {
-		return;
-	}
-	Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
-
-	ERR_FAIL_COND(p_elem->_static && p_with->_static);
-
-	if (!E) {
-		PairData *pd = memnew(PairData);
-		p_elem->paired[p_with] = pd;
-		p_with->paired[p_elem] = pd;
-	} else {
-		E->get()->rc++;
-	}
-}
-
-void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) {
-	if (p_elem->owner == p_with->owner) {
-		return;
-	}
-	if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) {
-		return;
-	}
-	Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
-
-	ERR_FAIL_COND(!E); //this should really be paired..
-
-	E->get()->rc--;
-
-	if (E->get()->rc == 0) {
-		if (E->get()->colliding) {
-			//uncollide
-			if (unpair_callback) {
-				unpair_callback(p_elem->owner, p_elem->subindex, p_with->owner, p_with->subindex, E->get()->ud, unpair_userdata);
-			}
-		}
-
-		memdelete(E->get());
-		p_elem->paired.erase(E);
-		p_with->paired.erase(p_elem);
-	}
-}
-
-void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
-	for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {
-		bool physical_collision = p_elem->aabb.intersects(E->key()->aabb);
-		bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner);
-
-		if (physical_collision && logical_collision) {
-			if (!E->get()->colliding && pair_callback) {
-				E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
-			}
-			E->get()->colliding = true;
-		} else { // No collision
-			if (E->get()->colliding && unpair_callback) {
-				unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
-				E->get()->ud = nullptr;
-			}
-			E->get()->colliding = false;
-		}
-	}
-}
-
-void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter) {
-	Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
-	if (sz.width * sz.height > large_object_min_surface) {
-		//large object, do not use grid, must check against all elements
-		for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
-			if (E->key() == p_elem->self) {
-				continue; // do not pair against itself
-			}
-			if (E->get()._static && p_static) {
-				continue;
-			}
-
-			_pair_attempt(p_elem, &E->get());
-		}
-
-		large_elements[p_elem].inc();
-		return;
-	}
-
-	Point2i from = (p_rect.position / cell_size).floor();
-	Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor();
-
-	for (int i = from.x; i <= to.x; i++) {
-		for (int j = from.y; j <= to.y; j++) {
-			PosKey pk;
-			pk.x = i;
-			pk.y = j;
-
-			uint32_t idx = pk.hash() % hash_table_size;
-			PosBin *pb = hash_table[idx];
-
-			while (pb) {
-				if (pb->key == pk) {
-					break;
-				}
-
-				pb = pb->next;
-			}
-
-			bool entered = p_force_enter;
-
-			if (!pb) {
-				//does not exist, create!
-				pb = memnew(PosBin);
-				pb->key = pk;
-				pb->next = hash_table[idx];
-				hash_table[idx] = pb;
-			}
-
-			if (p_static) {
-				if (pb->static_object_set[p_elem].inc() == 1) {
-					entered = true;
-				}
-			} else {
-				if (pb->object_set[p_elem].inc() == 1) {
-					entered = true;
-				}
-			}
-
-			if (entered) {
-				for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
-					_pair_attempt(p_elem, E->key());
-				}
-
-				if (!p_static) {
-					for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
-						_pair_attempt(p_elem, E->key());
-					}
-				}
-			}
-		}
-	}
-
-	//pair separatedly with large elements
-
-	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
-		if (E->key() == p_elem) {
-			continue; // do not pair against itself
-		}
-		if (E->key()->_static && p_static) {
-			continue;
-		}
-		_pair_attempt(E->key(), p_elem);
-	}
-}
-
-void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit) {
-	Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI);
-	if (sz.width * sz.height > large_object_min_surface) {
-		//unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
-		Map<Element *, PairData *>::Element *E = p_elem->paired.front();
-		while (E) {
-			Map<Element *, PairData *>::Element *next = E->next();
-			_unpair_attempt(p_elem, E->key());
-			E = next;
-		}
-
-		if (large_elements[p_elem].dec() == 0) {
-			large_elements.erase(p_elem);
-		}
-		return;
-	}
-
-	Point2i from = (p_rect.position / cell_size).floor();
-	Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor();
-
-	for (int i = from.x; i <= to.x; i++) {
-		for (int j = from.y; j <= to.y; j++) {
-			PosKey pk;
-			pk.x = i;
-			pk.y = j;
-
-			uint32_t idx = pk.hash() % hash_table_size;
-			PosBin *pb = hash_table[idx];
-
-			while (pb) {
-				if (pb->key == pk) {
-					break;
-				}
-
-				pb = pb->next;
-			}
-
-			ERR_CONTINUE(!pb); //should exist!!
-
-			bool exited = p_force_exit;
-
-			if (p_static) {
-				if (pb->static_object_set[p_elem].dec() == 0) {
-					pb->static_object_set.erase(p_elem);
-					exited = true;
-				}
-			} else {
-				if (pb->object_set[p_elem].dec() == 0) {
-					pb->object_set.erase(p_elem);
-					exited = true;
-				}
-			}
-
-			if (exited) {
-				for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
-					_unpair_attempt(p_elem, E->key());
-				}
-
-				if (!p_static) {
-					for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
-						_unpair_attempt(p_elem, E->key());
-					}
-				}
-			}
-
-			if (pb->object_set.is_empty() && pb->static_object_set.is_empty()) {
-				if (hash_table[idx] == pb) {
-					hash_table[idx] = pb->next;
-				} else {
-					PosBin *px = hash_table[idx];
-
-					while (px) {
-						if (px->next == pb) {
-							px->next = pb->next;
-							break;
-						}
-
-						px = px->next;
-					}
-
-					ERR_CONTINUE(!px);
-				}
-
-				memdelete(pb);
-			}
-		}
-	}
-
-	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
-		if (E->key() == p_elem) {
-			continue; // do not pair against itself
-		}
-		if (E->key()->_static && p_static) {
-			continue;
-		}
-
-		//unpair from large elements
-		_unpair_attempt(p_elem, E->key());
-	}
-}
-
-BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
-	current++;
-
-	Element e;
-	e.owner = p_object;
-	e._static = false;
-	e.collision_mask = p_object->get_collision_mask();
-	e.collision_layer = p_object->get_collision_layer();
-	e.subindex = p_subindex;
-	e.self = current;
-	e.pass = 0;
-
-	element_map[current] = e;
-	return current;
-}
-
-void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-
-	Element &e = E->get();
-	bool layer_changed = e.collision_mask != e.owner->get_collision_mask() || e.collision_layer != e.owner->get_collision_layer();
-
-	if (p_aabb != e.aabb || layer_changed) {
-		uint32_t old_mask = e.collision_mask;
-		uint32_t old_layer = e.collision_layer;
-		if (p_aabb != Rect2()) {
-			e.collision_mask = e.owner->get_collision_mask();
-			e.collision_layer = e.owner->get_collision_layer();
-
-			_enter_grid(&e, p_aabb, e._static, layer_changed);
-		}
-		if (e.aabb != Rect2()) {
-			// Need _exit_grid to remove from cells based on the old layer values.
-			e.collision_mask = old_mask;
-			e.collision_layer = old_layer;
-
-			_exit_grid(&e, e.aabb, e._static, layer_changed);
-
-			e.collision_mask = e.owner->get_collision_mask();
-			e.collision_layer = e.owner->get_collision_layer();
-		}
-		e.aabb = p_aabb;
-	}
-
-	_check_motion(&e);
-}
-
-void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-
-	Element &e = E->get();
-
-	if (e._static == p_static) {
-		return;
-	}
-
-	if (e.aabb != Rect2()) {
-		_exit_grid(&e, e.aabb, e._static, false);
-	}
-
-	e._static = p_static;
-
-	if (e.aabb != Rect2()) {
-		_enter_grid(&e, e.aabb, e._static, false);
-		_check_motion(&e);
-	}
-}
-
-void BroadPhase2DHashGrid::remove(ID p_id) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-
-	Element &e = E->get();
-
-	if (e.aabb != Rect2()) {
-		_exit_grid(&e, e.aabb, e._static, false);
-	}
-
-	element_map.erase(p_id);
-}
-
-CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {
-	const Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND_V(!E, nullptr);
-	return E->get().owner;
-}
-
-bool BroadPhase2DHashGrid::is_static(ID p_id) const {
-	const Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND_V(!E, false);
-	return E->get()._static;
-}
-
-int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
-	const Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND_V(!E, -1);
-	return E->get().subindex;
-}
-
-template <bool use_aabb, bool use_segment>
-void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index) {
-	PosKey pk;
-	pk.x = p_cell.x;
-	pk.y = p_cell.y;
-
-	uint32_t idx = pk.hash() % hash_table_size;
-	PosBin *pb = hash_table[idx];
-
-	while (pb) {
-		if (pb->key == pk) {
-			break;
-		}
-
-		pb = pb->next;
-	}
-
-	if (!pb) {
-		return;
-	}
-
-	for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
-		if (index >= p_max_results) {
-			break;
-		}
-		if (E->key()->pass == pass) {
-			continue;
-		}
-
-		E->key()->pass = pass;
-
-		if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
-			continue;
-		}
-
-		if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) {
-			continue;
-		}
-
-		p_results[index] = E->key()->owner;
-		p_result_indices[index] = E->key()->subindex;
-		index++;
-	}
-
-	for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
-		if (index >= p_max_results) {
-			break;
-		}
-		if (E->key()->pass == pass) {
-			continue;
-		}
-
-		if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
-			continue;
-		}
-
-		if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) {
-			continue;
-		}
-
-		E->key()->pass = pass;
-		p_results[index] = E->key()->owner;
-		p_result_indices[index] = E->key()->subindex;
-		index++;
-	}
-}
-
-int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
-	pass++;
-
-	Vector2 dir = (p_to - p_from);
-	if (dir == Vector2()) {
-		return 0;
-	}
-	//avoid divisions by zero
-	dir.normalize();
-	if (dir.x == 0.0) {
-		dir.x = 0.000001;
-	}
-	if (dir.y == 0.0) {
-		dir.y = 0.000001;
-	}
-	Vector2 delta = dir.abs();
-
-	delta.x = cell_size / delta.x;
-	delta.y = cell_size / delta.y;
-
-	Point2i pos = (p_from / cell_size).floor();
-	Point2i end = (p_to / cell_size).floor();
-
-	Point2i step = Vector2(SGN(dir.x), SGN(dir.y));
-
-	Vector2 max;
-
-	if (dir.x < 0) {
-		max.x = (Math::floor((double)pos.x) * cell_size - p_from.x) / dir.x;
-	} else {
-		max.x = (Math::floor((double)pos.x + 1) * cell_size - p_from.x) / dir.x;
-	}
-
-	if (dir.y < 0) {
-		max.y = (Math::floor((double)pos.y) * cell_size - p_from.y) / dir.y;
-	} else {
-		max.y = (Math::floor((double)pos.y + 1) * cell_size - p_from.y) / dir.y;
-	}
-
-	int cullcount = 0;
-	_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);
-
-	bool reached_x = false;
-	bool reached_y = false;
-
-	while (true) {
-		if (max.x < max.y) {
-			max.x += delta.x;
-			pos.x += step.x;
-		} else {
-			max.y += delta.y;
-			pos.y += step.y;
-		}
-
-		if (step.x > 0) {
-			if (pos.x >= end.x) {
-				reached_x = true;
-			}
-		} else if (pos.x <= end.x) {
-			reached_x = true;
-		}
-
-		if (step.y > 0) {
-			if (pos.y >= end.y) {
-				reached_y = true;
-			}
-		} else if (pos.y <= end.y) {
-			reached_y = true;
-		}
-
-		_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);
-
-		if (reached_x && reached_y) {
-			break;
-		}
-	}
-
-	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
-		if (cullcount >= p_max_results) {
-			break;
-		}
-		if (E->key()->pass == pass) {
-			continue;
-		}
-
-		E->key()->pass = pass;
-
-		/*
-		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
-			continue;
-		*/
-
-		if (!E->key()->aabb.intersects_segment(p_from, p_to)) {
-			continue;
-		}
-
-		p_results[cullcount] = E->key()->owner;
-		p_result_indices[cullcount] = E->key()->subindex;
-		cullcount++;
-	}
-
-	return cullcount;
-}
-
-int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
-	pass++;
-
-	Point2i from = (p_aabb.position / cell_size).floor();
-	Point2i to = ((p_aabb.position + p_aabb.size) / cell_size).floor();
-	int cullcount = 0;
-
-	for (int i = from.x; i <= to.x; i++) {
-		for (int j = from.y; j <= to.y; j++) {
-			_cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount);
-		}
-	}
-
-	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
-		if (cullcount >= p_max_results) {
-			break;
-		}
-		if (E->key()->pass == pass) {
-			continue;
-		}
-
-		E->key()->pass = pass;
-
-		if (!p_aabb.intersects(E->key()->aabb)) {
-			continue;
-		}
-
-		/*
-		if (!E->key()->aabb.intersects_segment(p_from,p_to))
-			continue;
-		*/
-
-		p_results[cullcount] = E->key()->owner;
-		p_result_indices[cullcount] = E->key()->subindex;
-		cullcount++;
-	}
-	return cullcount;
-}
-
-void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
-	pair_callback = p_pair_callback;
-	pair_userdata = p_userdata;
-}
-
-void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
-	unpair_callback = p_unpair_callback;
-	unpair_userdata = p_userdata;
-}
-
-void BroadPhase2DHashGrid::update() {
-}
-
-BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
-	return memnew(BroadPhase2DHashGrid);
-}
-
-BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
-	hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096);
-	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);
-	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);
-	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++) {
-		hash_table[i] = nullptr;
-	}
-	pass = 1;
-
-	current = 0;
-}
-
-BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {
-	for (uint32_t i = 0; i < hash_table_size; i++) {
-		while (hash_table[i]) {
-			PosBin *pb = hash_table[i];
-			hash_table[i] = pb->next;
-			memdelete(pb);
-		}
-	}
-
-	memdelete_arr(hash_table);
-}
-
-/* 3D version of voxel traversal:
-
-public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
-{
-    // Implementation is based on:
-    // "A Fast Voxel Traversal Algorithm for Ray Tracing"
-    // John Amanatides, Andrew Woo
-    // http://www.cse.yorku.ca/~amana/research/grid.pdf
-    // https://web.archive.org/web/20100616193049/http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
-
-    // NOTES:
-    // * This code assumes that the ray's position and direction are in 'cell coordinates', which means
-    //   that one unit equals one cell in all directions.
-    // * When the ray doesn't start within the voxel grid, calculate the first position at which the
-    //   ray could enter the grid. If it never enters the grid, there is nothing more to do here.
-    // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
-    // * The Point3D structure is a simple structure having three integer fields (X, Y and Z).
-
-    // The cell in which the ray starts.
-    Point3D start = GetCellAt(ray.Position);        // Rounds the position's X, Y and Z down to the nearest integer values.
-    int x = start.X;
-    int y = start.Y;
-    int z = start.Z;
-
-    // Determine which way we go.
-    int stepX = Math.Sign(ray.Direction.X);
-    int stepY = Math.Sign(ray.Direction.Y);
-    int stepZ = Math.Sign(ray.Direction.Z);
-
-    // Calculate cell boundaries. When the step (i.e. direction sign) is positive,
-    // the next boundary is AFTER our current position, meaning that we have to add 1.
-    // Otherwise, it is BEFORE our current position, in which case we add nothing.
-    Point3D cellBoundary = new Point3D(
-	x + (stepX > 0 ? 1 : 0),
-	y + (stepY > 0 ? 1 : 0),
-	z + (stepZ > 0 ? 1 : 0));
-
-    // NOTE: For the following calculations, the result will be Single.PositiveInfinity
-    // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
-    // value of the division also equals zero, the result is Single.NaN, which is not OK.
-
-    // Determine how far we can travel along the ray before we hit a voxel boundary.
-    Vector3 tMax = new Vector3(
-	(cellBoundary.X - ray.Position.X) / ray.Direction.X,    // Boundary is a plane on the YZ axis.
-	(cellBoundary.Y - ray.Position.Y) / ray.Direction.Y,    // Boundary is a plane on the XZ axis.
-	(cellBoundary.Z - ray.Position.Z) / ray.Direction.Z);    // Boundary is a plane on the XY axis.
-    if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity;
-    if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity;
-    if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity;
-
-    // Determine how far we must travel along the ray before we have crossed a gridcell.
-    Vector3 tDelta = new Vector3(
-	stepX / ray.Direction.X,                    // Crossing the width of a cell.
-	stepY / ray.Direction.Y,                    // Crossing the height of a cell.
-	stepZ / ray.Direction.Z);                    // Crossing the depth of a cell.
-    if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity;
-    if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity;
-    if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity;
-
-    // For each step, determine which distance to the next voxel boundary is lowest (i.e.
-    // which voxel boundary is nearest) and walk that way.
-    for (int i = 0; i < maxDepth; i++)
-    {
-	// Return it.
-	yield return new Point3D(x, y, z);
-
-	// Do the next step.
-	if (tMax.X < tMax.Y && tMax.X < tMax.Z)
-	{
-	    // tMax.X is the lowest, an YZ cell boundary plane is nearest.
-	    x += stepX;
-	    tMax.X += tDelta.X;
-	}
-	else if (tMax.Y < tMax.Z)
-	{
-	    // tMax.Y is the lowest, an XZ cell boundary plane is nearest.
-	    y += stepY;
-	    tMax.Y += tDelta.Y;
-	}
-	else
-	{
-	    // tMax.Z is the lowest, an XY cell boundary plane is nearest.
-	    z += stepZ;
-	    tMax.Z += tDelta.Z;
-	}
-    }
-
-    */
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h
deleted file mode 100644
index bb7c03b989..0000000000
--- a/servers/physics_2d/broad_phase_2d_hash_grid.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/*************************************************************************/
-/*  broad_phase_2d_hash_grid.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_HASH_GRID_H
-#define BROAD_PHASE_2D_HASH_GRID_H
-
-#include "broad_phase_2d_sw.h"
-#include "core/templates/map.h"
-
-class BroadPhase2DHashGrid : public BroadPhase2DSW {
-	struct PairData {
-		bool colliding;
-		int rc;
-		void *ud;
-		PairData() {
-			colliding = false;
-			rc = 1;
-			ud = nullptr;
-		}
-	};
-
-	struct Element {
-		ID self;
-		CollisionObject2DSW *owner;
-		bool _static;
-		Rect2 aabb;
-		// Owner's collision_mask/layer, used to detect changes in layers.
-		uint32_t collision_mask;
-		uint32_t collision_layer;
-		int subindex;
-		uint64_t pass;
-		Map<Element *, PairData *> paired;
-	};
-
-	struct RC {
-		int ref;
-
-		_FORCE_INLINE_ int inc() {
-			ref++;
-			return ref;
-		}
-		_FORCE_INLINE_ int dec() {
-			ref--;
-			return ref;
-		}
-
-		_FORCE_INLINE_ RC() {
-			ref = 0;
-		}
-	};
-
-	Map<ID, Element> element_map;
-	Map<Element *, RC> large_elements;
-
-	ID current;
-
-	uint64_t pass;
-
-	struct PairKey {
-		union {
-			struct {
-				ID a;
-				ID b;
-			};
-			uint64_t key;
-		};
-
-		_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
-			return key < p_key.key;
-		}
-
-		PairKey() { key = 0; }
-		PairKey(ID p_a, ID p_b) {
-			if (p_a > p_b) {
-				a = p_b;
-				b = p_a;
-			} else {
-				a = p_a;
-				b = p_b;
-			}
-		}
-	};
-
-	Map<PairKey, PairData> pair_map;
-
-	int cell_size;
-	int large_object_min_surface;
-
-	PairCallback pair_callback;
-	void *pair_userdata;
-	UnpairCallback unpair_callback;
-	void *unpair_userdata;
-
-	static _FORCE_INLINE_ bool _test_collision_mask(uint32_t p_mask1, uint32_t p_layer1, uint32_t p_mask2, uint32_t p_layer2) {
-		return p_mask1 & p_layer2 || p_mask2 & p_layer1;
-	}
-
-	void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter);
-	void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit);
-	template <bool use_aabb, bool use_segment>
-	_FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index);
-
-	struct PosKey {
-		union {
-			struct {
-				int32_t x;
-				int32_t y;
-			};
-			uint64_t key;
-		};
-
-		_FORCE_INLINE_ uint32_t hash() const {
-			uint64_t k = key;
-			k = (~k) + (k << 18); // k = (k << 18) - k - 1;
-			k = k ^ (k >> 31);
-			k = k * 21; // k = (k + (k << 2)) + (k << 4);
-			k = k ^ (k >> 11);
-			k = k + (k << 6);
-			k = k ^ (k >> 22);
-			return k;
-		}
-
-		bool operator==(const PosKey &p_key) const { return key == p_key.key; }
-		_FORCE_INLINE_ bool operator<(const PosKey &p_key) const {
-			return key < p_key.key;
-		}
-	};
-
-	struct PosBin {
-		PosKey key;
-		Map<Element *, RC> object_set;
-		Map<Element *, RC> static_object_set;
-		PosBin *next;
-	};
-
-	uint32_t hash_table_size;
-	PosBin **hash_table;
-
-	void _pair_attempt(Element *p_elem, Element *p_with);
-	void _unpair_attempt(Element *p_elem, Element *p_with);
-	void _check_motion(Element *p_elem);
-
-public:
-	virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0);
-	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 = nullptr);
-	virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
-
-	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();
-
-	BroadPhase2DHashGrid();
-	~BroadPhase2DHashGrid();
-};
-
-#endif // BROAD_PHASE_2D_HASH_GRID_H
diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h
index d17ee6e2d6..0f82f06b9c 100644
--- a/servers/physics_2d/broad_phase_2d_sw.h
+++ b/servers/physics_2d/broad_phase_2d_sw.h
@@ -48,7 +48,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;
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 7a2f312263..fa87dc1f3f 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -182,19 +182,19 @@ void CollisionObject2DSW::_update_shapes() {
 			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);
 	}
 }
 
@@ -209,11 +209,6 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
 			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;
@@ -221,6 +216,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);
 	}
 }
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 6d64f4126c..425546e5ca 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -30,8 +30,7 @@
 
 #include "physics_server_2d_sw.h"
 
-#include "broad_phase_2d_basic.h"
-#include "broad_phase_2d_hash_grid.h"
+#include "broad_phase_2d_bvh.h"
 #include "collision_solver_2d_sw.h"
 #include "core/config/project_settings.h"
 #include "core/debugger/engine_debugger.h"
@@ -1356,8 +1355,7 @@ PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr;
 
 PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) {
 	singletonsw = this;
-	BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create;
-	//BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
+	BroadPhase2DSW::create_func = BroadPhase2DBVH::_create;
 
 	active = true;
 	island_count = 0;