Dynamic BVH broadphase in 2D & 3D Godot Physics

Port lawnjelly's dynamic BVH implementation from 3.x to be used in
both 2D and 3D broadphases.

Removed alternative broadphase implementations which are not meant to be
used anymore since they are much slower.

Includes changes in Rect2, Vector2, Vector3 that help with the template
implementation of the dynamic BVH by uniformizing the interface between
2D and 3D math.

Co-authored-by: lawnjelly <lawnjelly@gmail.com>

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;