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>

core/math/bvh.h

@@ -0,0 +1,695 @@
+/*************************************************************************/
+/*  bvh.h                                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BVH_H
+#define BVH_H
+
+// BVH
+// This class provides a wrapper around BVH tree, which contains most of the functionality
+// for a dynamic BVH with templated leaf size.
+// However BVH also adds facilities for pairing, to maintain compatibility with Godot 3.2.
+// Pairing is a collision pairing system, on top of the basic BVH.
+
+// Some notes on the use of BVH / Octree from Godot 3.2.
+// This is not well explained elsewhere.
+// The rendering tree mask and types that are sent to the BVH are NOT layer masks.
+// They are INSTANCE_TYPES (defined in visual_server.h), e.g. MESH, MULTIMESH, PARTICLES etc.
+// Thus the lights do no cull by layer mask in the BVH.
+
+// Layer masks are implemented in the renderers as a later step, and light_cull_mask appears to be
+// implemented in GLES3 but not GLES2. Layer masks are not yet implemented for directional lights.
+
+#include "bvh_tree.h"
+
+#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS, Bounds, Point>
+
+template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32, class Bounds = AABB, class Point = Vector3>
+class BVH_Manager {
+public:
+	// note we are using uint32_t instead of BVHHandle, losing type safety, but this
+	// is for compatibility with octree
+	typedef void *(*PairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int);
+	typedef void (*UnpairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
+
+	// these 2 are crucial for fine tuning, and can be applied manually
+	// see the variable declarations for more info.
+	void params_set_node_expansion(real_t p_value) {
+		if (p_value >= 0.0) {
+			tree._node_expansion = p_value;
+			tree._auto_node_expansion = false;
+		} else {
+			tree._auto_node_expansion = true;
+		}
+	}
+
+	void params_set_pairing_expansion(real_t p_value) {
+		if (p_value >= 0.0) {
+			tree._pairing_expansion = p_value;
+			tree._auto_pairing_expansion = false;
+		} else {
+			tree._auto_pairing_expansion = true;
+		}
+	}
+
+	void set_pair_callback(PairCallback p_callback, void *p_userdata) {
+		pair_callback = p_callback;
+		pair_callback_userdata = p_userdata;
+	}
+	void set_unpair_callback(UnpairCallback p_callback, void *p_userdata) {
+		unpair_callback = p_callback;
+		unpair_callback_userdata = p_userdata;
+	}
+
+	BVHHandle create(T *p_userdata, bool p_active, const Bounds &p_aabb = Bounds(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) {
+		// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
+		// of waiting for update, so only uncomment this if there are bugs.
+		if (USE_PAIRS) {
+			//_check_for_collisions();
+		}
+
+#ifdef TOOLS_ENABLED
+		if (!USE_PAIRS) {
+			if (p_pairable) {
+				WARN_PRINT_ONCE("creating pairable item in BVH with USE_PAIRS set to false");
+			}
+		}
+#endif
+
+		BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_pairable, p_pairable_type, p_pairable_mask);
+
+		if (USE_PAIRS) {
+			// for safety initialize the expanded AABB
+			Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
+			expanded_aabb = p_aabb;
+			expanded_aabb.grow_by(tree._pairing_expansion);
+
+			// force a collision check no matter the AABB
+			if (p_active) {
+				_add_changed_item(h, p_aabb, false);
+				_check_for_collisions(true);
+			}
+		}
+
+		return h;
+	}
+
+	////////////////////////////////////////////////////
+	// wrapper versions that use uint32_t instead of handle
+	// for backward compatibility. Less type safe
+	void move(uint32_t p_handle, const Bounds &p_aabb) {
+		BVHHandle h;
+		h.set(p_handle);
+		move(h, p_aabb);
+	}
+
+	void erase(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		erase(h);
+	}
+
+	void force_collision_check(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		force_collision_check(h);
+	}
+
+	bool activate(uint32_t p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) {
+		BVHHandle h;
+		h.set(p_handle);
+		return activate(h, p_aabb, p_delay_collision_check);
+	}
+
+	bool deactivate(uint32_t p_handle) {
+		BVHHandle h;
+		h.set(p_handle);
+		return deactivate(h);
+	}
+
+	void set_pairable(uint32_t p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) {
+		BVHHandle h;
+		h.set(p_handle);
+		set_pairable(h, p_pairable, p_pairable_type, p_pairable_mask, p_force_collision_check);
+	}
+
+	bool is_pairable(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_is_pairable(h);
+	}
+	int get_subindex(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_get_subindex(h);
+	}
+
+	T *get(uint32_t p_handle) const {
+		BVHHandle h;
+		h.set(p_handle);
+		return item_get_userdata(h);
+	}
+
+	////////////////////////////////////////////////////
+
+	void move(BVHHandle p_handle, const Bounds &p_aabb) {
+		if (tree.item_move(p_handle, p_aabb)) {
+			if (USE_PAIRS) {
+				_add_changed_item(p_handle, p_aabb);
+			}
+		}
+	}
+
+	void erase(BVHHandle p_handle) {
+		// call unpair and remove all references to the item
+		// before deleting from the tree
+		if (USE_PAIRS) {
+			_remove_changed_item(p_handle);
+		}
+
+		tree.item_remove(p_handle);
+
+		_check_for_collisions(true);
+	}
+
+	// use in conjunction with activate if you have deferred the collision check, and
+	// set pairable has never been called.
+	// (deferred collision checks are a workaround for visual server for historical reasons)
+	void force_collision_check(BVHHandle p_handle) {
+		if (USE_PAIRS) {
+			// the aabb should already be up to date in the BVH
+			Bounds aabb;
+			item_get_AABB(p_handle, aabb);
+
+			// add it as changed even if aabb not different
+			_add_changed_item(p_handle, aabb, false);
+
+			// force an immediate full collision check, much like calls to set_pairable
+			_check_for_collisions(true);
+		}
+	}
+
+	// these should be read as set_visible for render trees,
+	// but generically this makes items add or remove from the
+	// tree internally, to speed things up by ignoring inactive items
+	bool activate(BVHHandle p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) {
+		// sending the aabb here prevents the need for the BVH to maintain
+		// a redundant copy of the aabb.
+		// returns success
+		if (tree.item_activate(p_handle, p_aabb)) {
+			if (USE_PAIRS) {
+				// in the special case of the render tree, when setting visibility we are using the combination of
+				// activate then set_pairable. This would case 2 sets of collision checks. For efficiency here we allow
+				// deferring to have a single collision check at the set_pairable call.
+				// Watch for bugs! This may cause bugs if set_pairable is not called.
+				if (!p_delay_collision_check) {
+					_add_changed_item(p_handle, p_aabb, false);
+
+					// force an immediate collision check, much like calls to set_pairable
+					_check_for_collisions(true);
+				}
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	bool deactivate(BVHHandle p_handle) {
+		// returns success
+		if (tree.item_deactivate(p_handle)) {
+			// call unpair and remove all references to the item
+			// before deleting from the tree
+			if (USE_PAIRS) {
+				_remove_changed_item(p_handle);
+
+				// force check for collisions, much like an erase was called
+				_check_for_collisions(true);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	bool get_active(BVHHandle p_handle) const {
+		return tree.item_get_active(p_handle);
+	}
+
+	// call e.g. once per frame (this does a trickle optimize)
+	void update() {
+		tree.update();
+		_check_for_collisions();
+#ifdef BVH_INTEGRITY_CHECKS
+		tree.integrity_check_all();
+#endif
+	}
+
+	// this can be called more frequently than per frame if necessary
+	void update_collisions() {
+		_check_for_collisions();
+	}
+
+	// prefer calling this directly as type safe
+	void set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) {
+		// Returns true if the pairing state has changed.
+		bool state_changed = tree.item_set_pairable(p_handle, p_pairable, p_pairable_type, p_pairable_mask);
+
+		if (USE_PAIRS) {
+			// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
+			// of waiting for update, so only uncomment this if there are bugs.
+			//_check_for_collisions();
+
+			if ((p_force_collision_check || state_changed) && get_active(p_handle)) {
+				// when the pairable state changes, we need to force a collision check because newly pairable
+				// items may be in collision, and unpairable items might move out of collision.
+				// We cannot depend on waiting for the next update, because that may come much later.
+				Bounds aabb;
+				item_get_AABB(p_handle, aabb);
+
+				// passing false disables the optimization which prevents collision checks if
+				// the aabb hasn't changed
+				_add_changed_item(p_handle, aabb, false);
+
+				// force an immediate collision check (probably just for this one item)
+				// but it must be a FULL collision check, also checking pairable state and masks.
+				// This is because AABB intersecting objects may have changed pairable state / mask
+				// such that they should no longer be paired. E.g. lights.
+				_check_for_collisions(true);
+			} // only if active
+		}
+	}
+
+	// cull tests
+	int cull_aabb(const Bounds &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = p_subindex_array;
+		params.mask = p_mask;
+		params.pairable_type = 0;
+		params.test_pairable_only = false;
+		params.abb.from(p_aabb);
+
+		tree.cull_aabb(params);
+
+		return params.result_count_overall;
+	}
+
+	int cull_segment(const Point &p_from, const Point &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = p_subindex_array;
+		params.mask = p_mask;
+		params.pairable_type = 0;
+
+		params.segment.from = p_from;
+		params.segment.to = p_to;
+
+		tree.cull_segment(params);
+
+		return params.result_count_overall;
+	}
+
+	int cull_point(const Point &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = p_subindex_array;
+		params.mask = p_mask;
+		params.pairable_type = 0;
+
+		params.point = p_point;
+
+		tree.cull_point(params);
+		return params.result_count_overall;
+	}
+
+	int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF) {
+		if (!p_convex.size()) {
+			return 0;
+		}
+
+		Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size());
+		if (convex_points.size() == 0) {
+			return 0;
+		}
+
+		typename BVHTREE_CLASS::CullParams params;
+		params.result_count_overall = 0;
+		params.result_max = p_result_max;
+		params.result_array = p_result_array;
+		params.subindex_array = nullptr;
+		params.mask = p_mask;
+		params.pairable_type = 0;
+
+		params.hull.planes = &p_convex[0];
+		params.hull.num_planes = p_convex.size();
+		params.hull.points = &convex_points[0];
+		params.hull.num_points = convex_points.size();
+
+		tree.cull_convex(params);
+
+		return params.result_count_overall;
+	}
+
+private:
+	// do this after moving etc.
+	void _check_for_collisions(bool p_full_check = false) {
+		if (!changed_items.size()) {
+			// noop
+			return;
+		}
+
+		Bounds bb;
+
+		typename BVHTREE_CLASS::CullParams params;
+
+		params.result_count_overall = 0;
+		params.result_max = INT_MAX;
+		params.result_array = nullptr;
+		params.subindex_array = nullptr;
+		params.mask = 0xFFFFFFFF;
+		params.pairable_type = 0;
+
+		for (unsigned int n = 0; n < changed_items.size(); n++) {
+			const BVHHandle &h = changed_items[n];
+
+			// use the expanded aabb for pairing
+			const Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
+			BVHABB_CLASS abb;
+			abb.from(expanded_aabb);
+
+			// find all the existing paired aabbs that are no longer
+			// paired, and send callbacks
+			_find_leavers(h, abb, p_full_check);
+
+			uint32_t changed_item_ref_id = h.id();
+
+			// set up the test from this item.
+			// this includes whether to test the non pairable tree,
+			// and the item mask.
+			tree.item_fill_cullparams(h, params);
+
+			params.abb = abb;
+
+			params.result_count_overall = 0; // might not be needed
+			tree.cull_aabb(params, false);
+
+			for (unsigned int i = 0; i < tree._cull_hits.size(); i++) {
+				uint32_t ref_id = tree._cull_hits[i];
+
+				// don't collide against ourself
+				if (ref_id == changed_item_ref_id) {
+					continue;
+				}
+
+#ifdef BVH_CHECKS
+				// if neither are pairable, they should ignore each other
+				// THIS SHOULD NEVER HAPPEN .. now we only test the pairable tree
+				// if the changed item is not pairable
+				CRASH_COND(params.test_pairable_only && !tree._extra[ref_id].pairable);
+#endif
+
+				// checkmasks is already done in the cull routine.
+				BVHHandle h_collidee;
+				h_collidee.set_id(ref_id);
+
+				// find NEW enterers, and send callbacks for them only
+				_collide(h, h_collidee);
+			}
+		}
+		_reset();
+	}
+
+public:
+	void item_get_AABB(BVHHandle p_handle, Bounds &r_aabb) {
+		BVHABB_CLASS abb;
+		tree.item_get_ABB(p_handle, abb);
+		abb.to(r_aabb);
+	}
+
+private:
+	// supplemental funcs
+	bool item_is_pairable(BVHHandle p_handle) const { return _get_extra(p_handle).pairable; }
+	T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; }
+	int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; }
+
+	void _unpair(BVHHandle p_from, BVHHandle p_to) {
+		tree._handle_sort(p_from, p_to);
+
+		typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
+		typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
+
+		// if the userdata is the same, no collisions should occur
+		if ((exa.userdata == exb.userdata) && exa.userdata) {
+			return;
+		}
+
+		typename BVHTREE_CLASS::ItemPairs &pairs_from = tree._pairs[p_from.id()];
+		typename BVHTREE_CLASS::ItemPairs &pairs_to = tree._pairs[p_to.id()];
+
+		void *ud_from = pairs_from.remove_pair_to(p_to);
+		pairs_to.remove_pair_to(p_from);
+
+		// callback
+		if (unpair_callback) {
+			unpair_callback(pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, ud_from);
+		}
+	}
+
+	// returns true if unpair
+	bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVHABB_CLASS &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) {
+		BVHABB_CLASS abb_to;
+		tree.item_get_ABB(p_to, abb_to);
+
+		// do they overlap?
+		if (p_abb_from.intersects(abb_to)) {
+			// the full check for pairable / non pairable and mask changes is extra expense
+			// this need not be done in most cases (for speed) except in the case where set_pairable is called
+			// where the masks etc of the objects in question may have changed
+			if (!p_full_check) {
+				return false;
+			}
+			const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_from);
+			const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_to);
+
+			// one of the two must be pairable to still pair
+			// if neither are pairable, we always unpair
+			if (exa.pairable || exb.pairable) {
+				// the masks must still be compatible to pair
+				// i.e. if there is a hit between the two, then they should stay paired
+				if (tree._cull_pairing_mask_test_hit(exa.pairable_mask, exa.pairable_type, exb.pairable_mask, exb.pairable_type)) {
+					return false;
+				}
+			}
+		}
+
+		_unpair(p_from, p_to);
+		return true;
+	}
+
+	// find all the existing paired aabbs that are no longer
+	// paired, and send callbacks
+	void _find_leavers(BVHHandle p_handle, const BVHABB_CLASS &expanded_abb_from, bool p_full_check) {
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
+
+		BVHABB_CLASS abb_from = expanded_abb_from;
+
+		// remove from pairing list for every partner
+		for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
+			BVHHandle h_to = p_from.extended_pairs[n].handle;
+			if (_find_leavers_process_pair(p_from, abb_from, p_handle, h_to, p_full_check)) {
+				// we need to keep the counter n up to date if we deleted a pair
+				// as the number of items in p_from.extended_pairs will have decreased by 1
+				// and we don't want to miss an item
+				n--;
+			}
+		}
+	}
+
+	// find NEW enterers, and send callbacks for them only
+	// handle a and b
+	void _collide(BVHHandle p_ha, BVHHandle p_hb) {
+		// only have to do this oneway, lower ID then higher ID
+		tree._handle_sort(p_ha, p_hb);
+
+		const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha);
+		const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb);
+
+		// if the userdata is the same, no collisions should occur
+		if ((exa.userdata == exb.userdata) && exa.userdata) {
+			return;
+		}
+
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_ha.id()];
+		typename BVHTREE_CLASS::ItemPairs &p_to = tree._pairs[p_hb.id()];
+
+		// does this pair exist already?
+		// or only check the one with lower number of pairs for greater speed
+		if (p_from.num_pairs <= p_to.num_pairs) {
+			if (p_from.contains_pair_to(p_hb)) {
+				return;
+			}
+		} else {
+			if (p_to.contains_pair_to(p_ha)) {
+				return;
+			}
+		}
+
+		// callback
+		void *callback_userdata = nullptr;
+
+		if (pair_callback) {
+			callback_userdata = pair_callback(pair_callback_userdata, p_ha, exa.userdata, exa.subindex, p_hb, exb.userdata, exb.subindex);
+		}
+
+		// new pair! .. only really need to store the userdata on the lower handle, but both have storage so...
+		p_from.add_pair_to(p_hb, callback_userdata);
+		p_to.add_pair_to(p_ha, callback_userdata);
+	}
+
+	// if we remove an item, we need to immediately remove the pairs, to prevent reading the pair after deletion
+	void _remove_pairs_containing(BVHHandle p_handle) {
+		typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
+
+		// remove from pairing list for every partner.
+		// can't easily use a for loop here, because removing changes the size of the list
+		while (p_from.extended_pairs.size()) {
+			BVHHandle h_to = p_from.extended_pairs[0].handle;
+			_unpair(p_handle, h_to);
+		}
+	}
+
+private:
+	const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const {
+		return tree._extra[p_handle.id()];
+	}
+	const typename BVHTREE_CLASS::ItemRef &_get_ref(BVHHandle p_handle) const {
+		return tree._refs[p_handle.id()];
+	}
+
+	void _reset() {
+		changed_items.clear();
+		_tick++;
+	}
+
+	void _add_changed_item(BVHHandle p_handle, const Bounds &aabb, bool p_check_aabb = true) {
+		// Note that non pairable items can pair with pairable,
+		// so all types must be added to the list
+
+		// aabb check with expanded aabb. This greatly decreases processing
+		// at the cost of slightly less accurate pairing checks
+		// Note this pairing AABB is separate from the AABB in the actual tree
+		Bounds &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
+
+		// passing p_check_aabb false disables the optimization which prevents collision checks if
+		// the aabb hasn't changed. This is needed where set_pairable has been called, but the position
+		// has not changed.
+		if (p_check_aabb && expanded_aabb.encloses(aabb)) {
+			return;
+		}
+
+		// ALWAYS update the new expanded aabb, even if already changed once
+		// this tick, because it is vital that the AABB is kept up to date
+		expanded_aabb = aabb;
+		expanded_aabb.grow_by(tree._pairing_expansion);
+
+		// this code is to ensure that changed items only appear once on the updated list
+		// collision checking them multiple times is not needed, and repeats the same thing
+		uint32_t &last_updated_tick = tree._extra[p_handle.id()].last_updated_tick;
+
+		if (last_updated_tick == _tick) {
+			return; // already on changed list
+		}
+
+		// mark as on list
+		last_updated_tick = _tick;
+
+		// add to the list
+		changed_items.push_back(p_handle);
+	}
+
+	void _remove_changed_item(BVHHandle p_handle) {
+		// Care has to be taken here for items that are deleted. The ref ID
+		// could be reused on the same tick for new items. This is probably
+		// rare but should be taken into consideration
+
+		// callbacks
+		_remove_pairs_containing(p_handle);
+
+		// remove from changed items (not very efficient yet)
+		for (int n = 0; n < (int)changed_items.size(); n++) {
+			if (changed_items[n] == p_handle) {
+				changed_items.remove_unordered(n);
+
+				// because we are using an unordered remove,
+				// the last changed item will now be at spot 'n',
+				// and we need to redo it, so we prevent moving on to
+				// the next n at the next for iteration.
+				n--;
+			}
+		}
+
+		// reset the last updated tick (may not be necessary but just in case)
+		tree._extra[p_handle.id()].last_updated_tick = 0;
+	}
+
+	PairCallback pair_callback;
+	UnpairCallback unpair_callback;
+	void *pair_callback_userdata;
+	void *unpair_callback_userdata;
+
+	BVHTREE_CLASS tree;
+
+	// for collision pairing,
+	// maintain a list of all items moved etc on each frame / tick
+	LocalVector<BVHHandle, uint32_t, true> changed_items;
+	uint32_t _tick;
+
+public:
+	BVH_Manager() {
+		_tick = 1; // start from 1 so items with 0 indicate never updated
+		pair_callback = nullptr;
+		unpair_callback = nullptr;
+		pair_callback_userdata = nullptr;
+		unpair_callback_userdata = nullptr;
+	}
+};
+
+#undef BVHTREE_CLASS
+
+#endif // BVH_H

core/math/bvh_abb.h

@@ -0,0 +1,276 @@
+/*************************************************************************/
+/*  bvh_abb.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 BVH_ABB_H
+#define BVH_ABB_H
+
+// special optimized version of axis aligned bounding box
+template <class Bounds = AABB, class Point = Vector3>
+struct BVH_ABB {
+	struct ConvexHull {
+		// convex hulls (optional)
+		const Plane *planes;
+		int num_planes;
+		const Vector3 *points;
+		int num_points;
+	};
+
+	struct Segment {
+		Point from;
+		Point to;
+	};
+
+	enum IntersectResult {
+		IR_MISS = 0,
+		IR_PARTIAL,
+		IR_FULL,
+	};
+
+	// we store mins with a negative value in order to test them with SIMD
+	Point min;
+	Point neg_max;
+
+	bool operator==(const BVH_ABB &o) const { return (min == o.min) && (neg_max == o.neg_max); }
+	bool operator!=(const BVH_ABB &o) const { return (*this == o) == false; }
+
+	void set(const Point &_min, const Point &_max) {
+		min = _min;
+		neg_max = -_max;
+	}
+
+	// to and from standard AABB
+	void from(const Bounds &p_aabb) {
+		min = p_aabb.position;
+		neg_max = -(p_aabb.position + p_aabb.size);
+	}
+
+	void to(Bounds &r_aabb) const {
+		r_aabb.position = min;
+		r_aabb.size = calculate_size();
+	}
+
+	void merge(const BVH_ABB &p_o) {
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			neg_max[axis] = MIN(neg_max[axis], p_o.neg_max[axis]);
+			min[axis] = MIN(min[axis], p_o.min[axis]);
+		}
+	}
+
+	Point calculate_size() const {
+		return -neg_max - min;
+	}
+
+	Point calculate_centre() const {
+		return Point((calculate_size() * 0.5) + min);
+	}
+
+	real_t get_proximity_to(const BVH_ABB &p_b) const {
+		const Point d = (min - neg_max) - (p_b.min - p_b.neg_max);
+		real_t proximity = 0.0;
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			proximity += Math::abs(d[axis]);
+		}
+		return proximity;
+	}
+
+	int select_by_proximity(const BVH_ABB &p_a, const BVH_ABB &p_b) const {
+		return (get_proximity_to(p_a) < get_proximity_to(p_b) ? 0 : 1);
+	}
+
+	uint32_t find_cutting_planes(const BVH_ABB::ConvexHull &p_hull, uint32_t *p_plane_ids) const {
+		uint32_t count = 0;
+
+		for (int n = 0; n < p_hull.num_planes; n++) {
+			const Plane &p = p_hull.planes[n];
+			if (intersects_plane(p)) {
+				p_plane_ids[count++] = n;
+			}
+		}
+
+		return count;
+	}
+
+	bool intersects_plane(const Plane &p_p) const {
+		Vector3 size = calculate_size();
+		Vector3 half_extents = size * 0.5;
+		Vector3 ofs = min + half_extents;
+
+		// forward side of plane?
+		Vector3 point_offset(
+				(p_p.normal.x < 0) ? -half_extents.x : half_extents.x,
+				(p_p.normal.y < 0) ? -half_extents.y : half_extents.y,
+				(p_p.normal.z < 0) ? -half_extents.z : half_extents.z);
+		Vector3 point = point_offset + ofs;
+
+		if (!p_p.is_point_over(point)) {
+			return false;
+		}
+
+		point = -point_offset + ofs;
+		if (p_p.is_point_over(point)) {
+			return false;
+		}
+
+		return true;
+	}
+
+	bool intersects_convex_optimized(const ConvexHull &p_hull, const uint32_t *p_plane_ids, uint32_t p_num_planes) const {
+		Vector3 size = calculate_size();
+		Vector3 half_extents = size * 0.5;
+		Vector3 ofs = min + half_extents;
+
+		for (unsigned int i = 0; i < p_num_planes; i++) {
+			const Plane &p = p_hull.planes[p_plane_ids[i]];
+			Vector3 point(
+					(p.normal.x > 0) ? -half_extents.x : half_extents.x,
+					(p.normal.y > 0) ? -half_extents.y : half_extents.y,
+					(p.normal.z > 0) ? -half_extents.z : half_extents.z);
+			point += ofs;
+			if (p.is_point_over(point)) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	bool intersects_convex_partial(const ConvexHull &p_hull) const {
+		Bounds bb;
+		to(bb);
+		return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points);
+	}
+
+	IntersectResult intersects_convex(const ConvexHull &p_hull) const {
+		if (intersects_convex_partial(p_hull)) {
+			// fully within? very important for tree checks
+			if (is_within_convex(p_hull)) {
+				return IR_FULL;
+			}
+
+			return IR_PARTIAL;
+		}
+
+		return IR_MISS;
+	}
+
+	bool is_within_convex(const ConvexHull &p_hull) const {
+		// use half extents routine
+		Bounds bb;
+		to(bb);
+		return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes);
+	}
+
+	bool is_point_within_hull(const ConvexHull &p_hull, const Vector3 &p_pt) const {
+		for (int n = 0; n < p_hull.num_planes; n++) {
+			if (p_hull.planes[n].distance_to(p_pt) > 0.0f) {
+				return false;
+			}
+		}
+		return true;
+	}
+
+	bool intersects_segment(const Segment &p_s) const {
+		Bounds bb;
+		to(bb);
+		return bb.intersects_segment(p_s.from, p_s.to);
+	}
+
+	bool intersects_point(const Point &p_pt) const {
+		if (_any_lessthan(-p_pt, neg_max)) {
+			return false;
+		}
+		if (_any_lessthan(p_pt, min)) {
+			return false;
+		}
+		return true;
+	}
+
+	bool intersects(const BVH_ABB &p_o) const {
+		if (_any_morethan(p_o.min, -neg_max)) {
+			return false;
+		}
+		if (_any_morethan(min, -p_o.neg_max)) {
+			return false;
+		}
+		return true;
+	}
+
+	bool is_other_within(const BVH_ABB &p_o) const {
+		if (_any_lessthan(p_o.neg_max, neg_max)) {
+			return false;
+		}
+		if (_any_lessthan(p_o.min, min)) {
+			return false;
+		}
+		return true;
+	}
+
+	void grow(const Point &p_change) {
+		neg_max -= p_change;
+		min -= p_change;
+	}
+
+	void expand(real_t p_change) {
+		Point change;
+		change.set_all(p_change);
+		grow(change);
+	}
+
+	// Actually surface area metric.
+	float get_area() const {
+		Point d = calculate_size();
+		return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
+	}
+
+	void set_to_max_opposite_extents() {
+		neg_max.set_all(FLT_MAX);
+		min = neg_max;
+	}
+
+	bool _any_morethan(const Point &p_a, const Point &p_b) const {
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			if (p_a[axis] > p_b[axis]) {
+				return true;
+			}
+		}
+		return false;
+	}
+
+	bool _any_lessthan(const Point &p_a, const Point &p_b) const {
+		for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
+			if (p_a[axis] < p_b[axis]) {
+				return true;
+			}
+		}
+		return false;
+	}
+};
+
+#endif // BVH_ABB_H

core/math/bvh_cull.inc

@@ -0,0 +1,534 @@
+public:
+// cull parameters is a convenient way of passing a bunch
+// of arguments through the culling functions without
+// writing loads of code. Not all members are used for some cull checks
+struct CullParams {
+	int result_count_overall; // both trees
+	int result_count; // this tree only
+	int result_max;
+	T **result_array;
+	int *subindex_array;
+
+	// nobody truly understands how masks are intended to work.
+	uint32_t mask;
+	uint32_t pairable_type;
+
+	// optional components for different tests
+	Vector3 point;
+	BVHABB_CLASS abb;
+	typename BVHABB_CLASS::ConvexHull hull;
+	typename BVHABB_CLASS::Segment segment;
+
+	// when collision testing, non pairable moving items
+	// only need to be tested against the pairable tree.
+	// collisions with other non pairable items are irrelevant.
+	bool test_pairable_only;
+};
+
+private:
+void _cull_translate_hits(CullParams &p) {
+	int num_hits = _cull_hits.size();
+	int left = p.result_max - p.result_count_overall;
+
+	if (num_hits > left) {
+		num_hits = left;
+	}
+
+	int out_n = p.result_count_overall;
+
+	for (int n = 0; n < num_hits; n++) {
+		uint32_t ref_id = _cull_hits[n];
+
+		const ItemExtra &ex = _extra[ref_id];
+		p.result_array[out_n] = ex.userdata;
+
+		if (p.subindex_array) {
+			p.subindex_array[out_n] = ex.subindex;
+		}
+
+		out_n++;
+	}
+
+	p.result_count = num_hits;
+	p.result_count_overall += num_hits;
+}
+
+public:
+int cull_convex(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		_cull_convex_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+int cull_segment(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		_cull_segment_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+int cull_point(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		_cull_point_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+int cull_aabb(CullParams &r_params, bool p_translate_hits = true) {
+	_cull_hits.clear();
+	r_params.result_count = 0;
+
+	for (int n = 0; n < NUM_TREES; n++) {
+		if (_root_node_id[n] == BVHCommon::INVALID) {
+			continue;
+		}
+
+		if ((n == 0) && r_params.test_pairable_only) {
+			continue;
+		}
+
+		_cull_aabb_iterative(_root_node_id[n], r_params);
+	}
+
+	if (p_translate_hits) {
+		_cull_translate_hits(r_params);
+	}
+
+	return r_params.result_count;
+}
+
+bool _cull_hits_full(const CullParams &p) {
+	// instead of checking every hit, we can do a lazy check for this condition.
+	// it isn't a problem if we write too much _cull_hits because they only the
+	// result_max amount will be translated and outputted. But we might as
+	// well stop our cull checks after the maximum has been reached.
+	return (int)_cull_hits.size() >= p.result_max;
+}
+
+// write this logic once for use in all routines
+// double check this as a possible source of bugs in future.
+bool _cull_pairing_mask_test_hit(uint32_t p_maskA, uint32_t p_typeA, uint32_t p_maskB, uint32_t p_typeB) const {
+	// double check this as a possible source of bugs in future.
+	bool A_match_B = p_maskA & p_typeB;
+
+	if (!A_match_B) {
+		bool B_match_A = p_maskB & p_typeA;
+		if (!B_match_A) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+void _cull_hit(uint32_t p_ref_id, CullParams &p) {
+	// take into account masks etc
+	// this would be more efficient to do before plane checks,
+	// but done here for ease to get started
+	if (USE_PAIRS) {
+		const ItemExtra &ex = _extra[p_ref_id];
+
+		if (!_cull_pairing_mask_test_hit(p.mask, p.pairable_type, ex.pairable_mask, ex.pairable_type)) {
+			return;
+		}
+	}
+
+	_cull_hits.push_back(p_ref_id);
+}
+
+bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) {
+	// our function parameters to keep on a stack
+	struct CullSegParams {
+		uint32_t node_id;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullSegParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullSegParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+
+	CullSegParams csp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(csp)) {
+		TNode &tnode = _nodes[csp.node_id];
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			TLeaf &leaf = _node_get_leaf(tnode);
+
+			// test children individually
+			for (int n = 0; n < leaf.num_items; n++) {
+				const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+				if (aabb.intersects_segment(r_params.segment)) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+			}
+		} else {
+			// test children individually
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+				const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
+
+				if (child_abb.intersects_segment(r_params.segment)) {
+					// add to the stack
+					CullSegParams *child = ii.request();
+					child->node_id = child_id;
+				}
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}
+
+bool _cull_point_iterative(uint32_t p_node_id, CullParams &r_params) {
+	// our function parameters to keep on a stack
+	struct CullPointParams {
+		uint32_t node_id;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullPointParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullPointParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+
+	CullPointParams cpp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(cpp)) {
+		TNode &tnode = _nodes[cpp.node_id];
+		// no hit with this node?
+		if (!tnode.aabb.intersects_point(r_params.point)) {
+			continue;
+		}
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			TLeaf &leaf = _node_get_leaf(tnode);
+
+			// test children individually
+			for (int n = 0; n < leaf.num_items; n++) {
+				if (leaf.get_aabb(n).intersects_point(r_params.point)) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+			}
+		} else {
+			// test children individually
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+
+				// add to the stack
+				CullPointParams *child = ii.request();
+				child->node_id = child_id;
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}
+
+bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
+	// our function parameters to keep on a stack
+	struct CullAABBParams {
+		uint32_t node_id;
+		bool fully_within;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullAABBParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullAABBParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+	ii.get_first()->fully_within = p_fully_within;
+
+	CullAABBParams cap;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(cap)) {
+		TNode &tnode = _nodes[cap.node_id];
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			TLeaf &leaf = _node_get_leaf(tnode);
+
+			// if fully within we can just add all items
+			// as long as they pass mask checks
+			if (cap.fully_within) {
+				for (int n = 0; n < leaf.num_items; n++) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+			} else {
+				for (int n = 0; n < leaf.num_items; n++) {
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects(r_params.abb)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+						// register hit
+						_cull_hit(child_id, r_params);
+					}
+				}
+			} // not fully within
+		} else {
+			if (!cap.fully_within) {
+				// test children individually
+				for (int n = 0; n < tnode.num_children; n++) {
+					uint32_t child_id = tnode.children[n];
+					const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
+
+					if (child_abb.intersects(r_params.abb)) {
+						// is the node totally within the aabb?
+						bool fully_within = r_params.abb.is_other_within(child_abb);
+
+						// add to the stack
+						CullAABBParams *child = ii.request();
+
+						// should always return valid child
+						child->node_id = child_id;
+						child->fully_within = fully_within;
+					}
+				}
+			} else {
+				for (int n = 0; n < tnode.num_children; n++) {
+					uint32_t child_id = tnode.children[n];
+
+					// add to the stack
+					CullAABBParams *child = ii.request();
+
+					// should always return valid child
+					child->node_id = child_id;
+					child->fully_within = true;
+				}
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}
+
+// returns full up with results
+bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
+	// our function parameters to keep on a stack
+	struct CullConvexParams {
+		uint32_t node_id;
+		bool fully_within;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<CullConvexParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (CullConvexParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+	ii.get_first()->fully_within = p_fully_within;
+
+	// preallocate these as a once off to be reused
+	uint32_t max_planes = r_params.hull.num_planes;
+	uint32_t *plane_ids = (uint32_t *)alloca(sizeof(uint32_t) * max_planes);
+
+	CullConvexParams ccp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(ccp)) {
+		const TNode &tnode = _nodes[ccp.node_id];
+
+		if (!ccp.fully_within) {
+			typename BVHABB_CLASS::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull);
+
+			switch (res) {
+				default: {
+					continue; // miss, just move on to the next node in the stack
+				} break;
+				case BVHABB_CLASS::IR_PARTIAL: {
+				} break;
+				case BVHABB_CLASS::IR_FULL: {
+					ccp.fully_within = true;
+				} break;
+			}
+
+		} // if not fully within already
+
+		if (tnode.is_leaf()) {
+			// lazy check for hits full up condition
+			if (_cull_hits_full(r_params)) {
+				return false;
+			}
+
+			const TLeaf &leaf = _node_get_leaf(tnode);
+
+			// if fully within, simply add all items to the result
+			// (taking into account masks)
+			if (ccp.fully_within) {
+				for (int n = 0; n < leaf.num_items; n++) {
+					uint32_t child_id = leaf.get_item_ref_id(n);
+
+					// register hit
+					_cull_hit(child_id, r_params);
+				}
+
+			} else {
+				// we can either use a naive check of all the planes against the AABB,
+				// or an optimized check, which finds in advance which of the planes can possibly
+				// cut the AABB, and only tests those. This can be much faster.
+#define BVH_CONVEX_CULL_OPTIMIZED
+#ifdef BVH_CONVEX_CULL_OPTIMIZED
+				// first find which planes cut the aabb
+				uint32_t num_planes = tnode.aabb.find_cutting_planes(r_params.hull, plane_ids);
+				BVH_ASSERT(num_planes <= max_planes);
+
+//#define BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+				// rigorous check
+				uint32_t results[MAX_ITEMS];
+				uint32_t num_results = 0;
+#endif
+
+				// test children individually
+				for (int n = 0; n < leaf.num_items; n++) {
+					//const Item &item = leaf.get_item(n);
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects_convex_optimized(r_params.hull, plane_ids, num_planes)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+						results[num_results++] = child_id;
+#endif
+
+						// register hit
+						_cull_hit(child_id, r_params);
+					}
+				}
+
+#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
+				uint32_t test_count = 0;
+
+				for (int n = 0; n < leaf.num_items; n++) {
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects_convex_partial(r_params.hull)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+						CRASH_COND(child_id != results[test_count++]);
+						CRASH_COND(test_count > num_results);
+					}
+				}
+#endif
+
+#else
+				// not BVH_CONVEX_CULL_OPTIMIZED
+				// test children individually
+				for (int n = 0; n < leaf.num_items; n++) {
+					const BVHABB_CLASS &aabb = leaf.get_aabb(n);
+
+					if (aabb.intersects_convex_partial(r_params.hull)) {
+						uint32_t child_id = leaf.get_item_ref_id(n);
+
+						// full up with results? exit early, no point in further testing
+						if (!_cull_hit(child_id, r_params))
+							return false;
+					}
+				}
+#endif // BVH_CONVEX_CULL_OPTIMIZED
+			} // if not fully within
+		} else {
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+
+				// add to the stack
+				CullConvexParams *child = ii.request();
+
+				// should always return valid child
+				child->node_id = child_id;
+				child->fully_within = ccp.fully_within;
+			}
+		}
+
+	} // while more nodes to pop
+
+	// true indicates results are not full
+	return true;
+}

core/math/bvh_debug.inc

@@ -0,0 +1,68 @@
+public:
+#ifdef BVH_VERBOSE
+void _debug_recursive_print_tree(int p_tree_id) const {
+	if (_root_node_id[p_tree_id] != BVHCommon::INVALID)
+		_debug_recursive_print_tree_node(_root_node_id[p_tree_id]);
+}
+
+String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const {
+	String sz = "(";
+	sz += itos(aabb.min.x);
+	sz += " ~ ";
+	sz += itos(-aabb.neg_max.x);
+	sz += ") (";
+
+	sz += itos(aabb.min.y);
+	sz += " ~ ";
+	sz += itos(-aabb.neg_max.y);
+	sz += ") (";
+
+	sz += itos(aabb.min.z);
+	sz += " ~ ";
+	sz += itos(-aabb.neg_max.z);
+	sz += ") ";
+
+	Vector3 size = aabb.calculate_size();
+	float vol = size.x * size.y * size.z;
+	sz += "vol " + itos(vol);
+
+	return sz;
+}
+
+void _debug_recursive_print_tree_node(uint32_t p_node_id, int depth = 0) const {
+	const TNode &tnode = _nodes[p_node_id];
+
+	String sz = "";
+	for (int n = 0; n < depth; n++) {
+		sz += "\t";
+	}
+	sz += itos(p_node_id);
+
+	if (tnode.is_leaf()) {
+		sz += " L";
+		sz += itos(tnode.height) + " ";
+		const TLeaf &leaf = _node_get_leaf(tnode);
+
+		sz += "[";
+		for (int n = 0; n < leaf.num_items; n++) {
+			if (n)
+				sz += ", ";
+			sz += "r";
+			sz += itos(leaf.get_item_ref_id(n));
+		}
+		sz += "]  ";
+	} else {
+		sz += " N";
+		sz += itos(tnode.height) + " ";
+	}
+
+	sz += _debug_aabb_to_string(tnode.aabb);
+	print_line(sz);
+
+	if (!tnode.is_leaf()) {
+		for (int n = 0; n < tnode.num_children; n++) {
+			_debug_recursive_print_tree_node(tnode.children[n], depth + 1);
+		}
+	}
+}
+#endif

core/math/bvh_integrity.inc

@@ -0,0 +1,42 @@
+void _integrity_check_all() {
+#ifdef BVH_INTEGRITY_CHECKS
+	for (int n = 0; n < NUM_TREES; n++) {
+		uint32_t root = _root_node_id[n];
+		if (root != BVHCommon::INVALID) {
+			_integrity_check_down(root);
+		}
+	}
+#endif
+}
+
+void _integrity_check_up(uint32_t p_node_id) {
+	TNode &node = _nodes[p_node_id];
+
+	BVHABB_CLASS abb = node.aabb;
+	node_update_aabb(node);
+
+	BVHABB_CLASS abb2 = node.aabb;
+	abb2.expand(-_node_expansion);
+
+	CRASH_COND(!abb.is_other_within(abb2));
+}
+
+void _integrity_check_down(uint32_t p_node_id) {
+	const TNode &node = _nodes[p_node_id];
+
+	if (node.is_leaf()) {
+		_integrity_check_up(p_node_id);
+	} else {
+		CRASH_COND(node.num_children != 2);
+
+		for (int n = 0; n < node.num_children; n++) {
+			uint32_t child_id = node.children[n];
+
+			// check the children parent pointers are correct
+			TNode &child = _nodes[child_id];
+			CRASH_COND(child.parent_id != p_node_id);
+
+			_integrity_check_down(child_id);
+		}
+	}
+}

core/math/bvh_logic.inc

@@ -0,0 +1,230 @@
+
+// for slow incremental optimization, we will periodically remove each
+// item from the tree and reinsert, to give it a chance to find a better position
+void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
+	// get the reference
+	ItemRef &ref = _refs[p_ref_id];
+
+	// no need to optimize inactive items
+	if (!ref.is_active()) {
+		return;
+	}
+
+	// special case of debug draw
+	if (ref.item_id == BVHCommon::INVALID) {
+		return;
+	}
+
+	BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
+
+	// some overlay elaborate way to find out which tree the node is in!
+	BVHHandle temp_handle;
+	temp_handle.set_id(p_ref_id);
+	_current_tree = _handle_get_tree_id(temp_handle);
+
+	// remove and reinsert
+	BVHABB_CLASS abb;
+	node_remove_item(p_ref_id, &abb);
+
+	// we must choose where to add to tree
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+	_node_add_item(ref.tnode_id, p_ref_id, abb);
+
+	refit_upward_and_balance(ref.tnode_id);
+}
+
+// from randy gaul balance function
+BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) {
+	BVHABB_CLASS c = a;
+	c.merge(b);
+	return c;
+}
+
+//--------------------------------------------------------------------------------------------------
+/**
+@file	q3DynamicAABBTree.h
+@author	Randy Gaul
+@date	10/10/2014
+	Copyright (c) 2014 Randy Gaul http://www.randygaul.net
+	This software is provided 'as-is', without any express or implied
+	warranty. In no event will the authors be held liable for any damages
+	arising from the use of this software.
+	Permission is granted to anyone to use this software for any purpose,
+	including commercial applications, and to alter it and redistribute it
+	freely, subject to the following restrictions:
+	  1. The origin of this software must not be misrepresented; you must not
+	     claim that you wrote the original software. If you use this software
+	     in a product, an acknowledgment in the product documentation would be
+	     appreciated but is not required.
+	  2. Altered source versions must be plainly marked as such, and must not
+	     be misrepresented as being the original software.
+	  3. This notice may not be removed or altered from any source distribution.
+*/
+//--------------------------------------------------------------------------------------------------
+
+// This function is based on the 'Balance' function from Randy Gaul's qu3e
+// https://github.com/RandyGaul/qu3e
+// It is MODIFIED from qu3e version.
+// This is the only function used (and _logic_abb_merge helper function).
+int32_t _logic_balance(int32_t iA) {
+	//	return iA; // uncomment this to bypass balance
+
+	TNode *A = &_nodes[iA];
+
+	if (A->is_leaf() || A->height == 1) {
+		return iA;
+	}
+
+	/*      A
+	      /   \
+	     B     C
+	    / \   / \
+	   D   E F   G
+	*/
+
+	CRASH_COND(A->num_children != 2);
+	int32_t iB = A->children[0];
+	int32_t iC = A->children[1];
+	TNode *B = &_nodes[iB];
+	TNode *C = &_nodes[iC];
+
+	int32_t balance = C->height - B->height;
+
+	// C is higher, promote C
+	if (balance > 1) {
+		int32_t iF = C->children[0];
+		int32_t iG = C->children[1];
+		TNode *F = &_nodes[iF];
+		TNode *G = &_nodes[iG];
+
+		// grandParent point to C
+		if (A->parent_id != BVHCommon::INVALID) {
+			if (_nodes[A->parent_id].children[0] == iA) {
+				_nodes[A->parent_id].children[0] = iC;
+
+			} else {
+				_nodes[A->parent_id].children[1] = iC;
+			}
+		} else {
+			// check this .. seems dodgy
+			change_root_node(iC);
+		}
+
+		// Swap A and C
+		C->children[0] = iA;
+		C->parent_id = A->parent_id;
+		A->parent_id = iC;
+
+		// Finish rotation
+		if (F->height > G->height) {
+			C->children[1] = iF;
+			A->children[1] = iG;
+			G->parent_id = iA;
+			A->aabb = _logic_abb_merge(B->aabb, G->aabb);
+			C->aabb = _logic_abb_merge(A->aabb, F->aabb);
+
+			A->height = 1 + MAX(B->height, G->height);
+			C->height = 1 + MAX(A->height, F->height);
+		}
+
+		else {
+			C->children[1] = iG;
+			A->children[1] = iF;
+			F->parent_id = iA;
+			A->aabb = _logic_abb_merge(B->aabb, F->aabb);
+			C->aabb = _logic_abb_merge(A->aabb, G->aabb);
+
+			A->height = 1 + MAX(B->height, F->height);
+			C->height = 1 + MAX(A->height, G->height);
+		}
+
+		return iC;
+	}
+
+	// B is higher, promote B
+	else if (balance < -1) {
+		int32_t iD = B->children[0];
+		int32_t iE = B->children[1];
+		TNode *D = &_nodes[iD];
+		TNode *E = &_nodes[iE];
+
+		// grandParent point to B
+		if (A->parent_id != BVHCommon::INVALID) {
+			if (_nodes[A->parent_id].children[0] == iA) {
+				_nodes[A->parent_id].children[0] = iB;
+			} else {
+				_nodes[A->parent_id].children[1] = iB;
+			}
+		}
+
+		else {
+			// check this .. seems dodgy
+			change_root_node(iB);
+		}
+
+		// Swap A and B
+		B->children[1] = iA;
+		B->parent_id = A->parent_id;
+		A->parent_id = iB;
+
+		// Finish rotation
+		if (D->height > E->height) {
+			B->children[0] = iD;
+			A->children[0] = iE;
+			E->parent_id = iA;
+			A->aabb = _logic_abb_merge(C->aabb, E->aabb);
+			B->aabb = _logic_abb_merge(A->aabb, D->aabb);
+
+			A->height = 1 + MAX(C->height, E->height);
+			B->height = 1 + MAX(A->height, D->height);
+		}
+
+		else {
+			B->children[0] = iE;
+			A->children[0] = iD;
+			D->parent_id = iA;
+			A->aabb = _logic_abb_merge(C->aabb, D->aabb);
+			B->aabb = _logic_abb_merge(A->aabb, E->aabb);
+
+			A->height = 1 + MAX(C->height, D->height);
+			B->height = 1 + MAX(A->height, E->height);
+		}
+
+		return iB;
+	}
+
+	return iA;
+}
+
+// either choose an existing node to add item to, or create a new node and return this
+uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
+	while (true) {
+		BVH_ASSERT(p_node_id != BVHCommon::INVALID);
+		TNode &tnode = _nodes[p_node_id];
+
+		if (tnode.is_leaf()) {
+			// if a leaf, and non full, use this to add to
+			if (!node_is_leaf_full(tnode)) {
+				return p_node_id;
+			}
+
+			// else split the leaf, and use one of the children to add to
+			return split_leaf(p_node_id, p_aabb);
+		}
+
+		// this should not happen???
+		// is still happening, need to debug and find circumstances. Is not that serious
+		// but would be nice to prevent. I think it only happens with the root node.
+		if (tnode.num_children == 1) {
+			WARN_PRINT_ONCE("BVH::recursive_choose_item_add_node, node with 1 child, recovering");
+			p_node_id = tnode.children[0];
+		} else {
+			BVH_ASSERT(tnode.num_children == 2);
+			TNode &childA = _nodes[tnode.children[0]];
+			TNode &childB = _nodes[tnode.children[1]];
+			int which = p_aabb.select_by_proximity(childA.aabb, childB.aabb);
+
+			p_node_id = tnode.children[which];
+		}
+	}
+}

core/math/bvh_misc.inc

@@ -0,0 +1,55 @@
+
+int _handle_get_tree_id(BVHHandle p_handle) const {
+	if (USE_PAIRS) {
+		int tree = 0;
+		if (_extra[p_handle.id()].pairable) {
+			tree = 1;
+		}
+		return tree;
+	}
+	return 0;
+}
+
+public:
+void _handle_sort(BVHHandle &p_ha, BVHHandle &p_hb) const {
+	if (p_ha.id() > p_hb.id()) {
+		BVHHandle temp = p_hb;
+		p_hb = p_ha;
+		p_ha = temp;
+	}
+}
+
+private:
+void create_root_node(int p_tree) {
+	// if there is no root node, create one
+	if (_root_node_id[p_tree] == BVHCommon::INVALID) {
+		uint32_t root_node_id;
+		TNode *node = _nodes.request(root_node_id);
+		node->clear();
+		_root_node_id[p_tree] = root_node_id;
+
+		// make the root node a leaf
+		uint32_t leaf_id;
+		TLeaf *leaf = _leaves.request(leaf_id);
+		leaf->clear();
+		node->neg_leaf_id = -(int)leaf_id;
+	}
+}
+
+bool node_is_leaf_full(TNode &tnode) const {
+	const TLeaf &leaf = _node_get_leaf(tnode);
+	return leaf.is_full();
+}
+
+public:
+TLeaf &_node_get_leaf(TNode &tnode) {
+	BVH_ASSERT(tnode.is_leaf());
+	return _leaves[tnode.get_leaf_id()];
+}
+
+const TLeaf &_node_get_leaf(const TNode &tnode) const {
+	BVH_ASSERT(tnode.is_leaf());
+	return _leaves[tnode.get_leaf_id()];
+}
+
+private:

core/math/bvh_pair.inc

@@ -0,0 +1,62 @@
+public:
+// note .. maybe this can be attached to another node structure?
+// depends which works best for cache.
+struct ItemPairs {
+	struct Link {
+		void set(BVHHandle h, void *ud) {
+			handle = h;
+			userdata = ud;
+		}
+		BVHHandle handle;
+		void *userdata;
+	};
+
+	void clear() {
+		num_pairs = 0;
+		extended_pairs.reset();
+		expanded_aabb = Bounds();
+	}
+
+	Bounds expanded_aabb;
+
+	// maybe we can just use the number in the vector TODO
+	int32_t num_pairs;
+	LocalVector<Link> extended_pairs;
+
+	void add_pair_to(BVHHandle h, void *p_userdata) {
+		Link temp;
+		temp.set(h, p_userdata);
+
+		extended_pairs.push_back(temp);
+		num_pairs++;
+	}
+
+	uint32_t find_pair_to(BVHHandle h) const {
+		for (int n = 0; n < num_pairs; n++) {
+			if (extended_pairs[n].handle == h) {
+				return n;
+			}
+		}
+		return -1;
+	}
+
+	bool contains_pair_to(BVHHandle h) const {
+		return find_pair_to(h) != BVHCommon::INVALID;
+	}
+
+	// return success
+	void *remove_pair_to(BVHHandle h) {
+		void *userdata = nullptr;
+
+		for (int n = 0; n < num_pairs; n++) {
+			if (extended_pairs[n].handle == h) {
+				userdata = extended_pairs[n].userdata;
+				extended_pairs.remove_unordered(n);
+				num_pairs--;
+				break;
+			}
+		}
+
+		return userdata;
+	}
+};

core/math/bvh_public.inc

@@ -0,0 +1,421 @@
+public:
+BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) {
+#ifdef BVH_VERBOSE_TREE
+	VERBOSE_PRINT("\nitem_add BEFORE");
+	_debug_recursive_print_tree(0);
+	VERBOSE_PRINT("\n");
+#endif
+
+	BVHABB_CLASS abb;
+	abb.from(p_aabb);
+
+	// handle to be filled with the new item ref
+	BVHHandle handle;
+
+	// ref id easier to pass around than handle
+	uint32_t ref_id;
+
+	// this should never fail
+	ItemRef *ref = _refs.request(ref_id);
+
+	// the extra data should be parallel list to the references
+	uint32_t extra_id;
+	ItemExtra *extra = _extra.request(extra_id);
+	BVH_ASSERT(extra_id == ref_id);
+
+	// pairs info
+	if (USE_PAIRS) {
+		uint32_t pairs_id;
+		ItemPairs *pairs = _pairs.request(pairs_id);
+		pairs->clear();
+		BVH_ASSERT(pairs_id == ref_id);
+	}
+
+	extra->subindex = p_subindex;
+	extra->userdata = p_userdata;
+	extra->last_updated_tick = 0;
+
+	// add an active reference to the list for slow incremental optimize
+	// this list must be kept in sync with the references as they are added or removed.
+	extra->active_ref_id = _active_refs.size();
+	_active_refs.push_back(ref_id);
+
+	if (USE_PAIRS) {
+		extra->pairable_mask = p_pairable_mask;
+		extra->pairable_type = p_pairable_type;
+		extra->pairable = p_pairable;
+	} else {
+		// just for safety, in case this gets queried etc
+		extra->pairable = 0;
+		p_pairable = false;
+	}
+
+	// assign to handle to return
+	handle.set_id(ref_id);
+
+	_current_tree = 0;
+	if (p_pairable) {
+		_current_tree = 1;
+	}
+
+	create_root_node(_current_tree);
+
+	// we must choose where to add to tree
+	if (p_active) {
+		ref->tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+
+		bool refit = _node_add_item(ref->tnode_id, ref_id, abb);
+
+		if (refit) {
+			// only need to refit from the parent
+			const TNode &add_node = _nodes[ref->tnode_id];
+			if (add_node.parent_id != BVHCommon::INVALID) {
+				refit_upward_and_balance(add_node.parent_id);
+			}
+		}
+	} else {
+		ref->set_inactive();
+	}
+
+#ifdef BVH_VERBOSE
+	// memory use
+	int mem = _refs.estimate_memory_use();
+	mem += _nodes.estimate_memory_use();
+
+	String sz = _debug_aabb_to_string(abb);
+	VERBOSE_PRINT("\titem_add [" + itos(ref_id) + "] " + itos(_refs.size()) + " refs,\t" + itos(_nodes.size()) + " nodes " + sz);
+	VERBOSE_PRINT("mem use : " + itos(mem) + ", num nodes : " + itos(_nodes.size()));
+
+#endif
+
+	return handle;
+}
+
+void _debug_print_refs() {
+#ifdef BVH_VERBOSE_TREE
+	print_line("refs.....");
+	for (int n = 0; n < _refs.size(); n++) {
+		const ItemRef &ref = _refs[n];
+		print_line("tnode_id " + itos(ref.tnode_id) + ", item_id " + itos(ref.item_id));
+	}
+
+#endif
+}
+
+// returns false if noop
+bool item_move(BVHHandle p_handle, const Bounds &p_aabb) {
+	uint32_t ref_id = p_handle.id();
+
+	// get the reference
+	ItemRef &ref = _refs[ref_id];
+	if (!ref.is_active()) {
+		return false;
+	}
+
+	BVHABB_CLASS abb;
+	abb.from(p_aabb);
+
+	BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
+	TNode &tnode = _nodes[ref.tnode_id];
+
+	// does it fit within the current aabb?
+	if (tnode.aabb.is_other_within(abb)) {
+		// do nothing .. fast path .. not moved enough to need refit
+
+		// however we WILL update the exact aabb in the leaf, as this will be needed
+		// for accurate collision detection
+		TLeaf &leaf = _node_get_leaf(tnode);
+
+		BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id);
+
+		// no change?
+		if (leaf_abb == abb) {
+			return false;
+		}
+
+		leaf_abb = abb;
+		_integrity_check_all();
+
+		return true;
+	}
+
+	_current_tree = _handle_get_tree_id(p_handle);
+
+	// remove and reinsert
+	node_remove_item(ref_id);
+
+	// we must choose where to add to tree
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+
+	// add to the tree
+	bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
+
+	// only need to refit from the PARENT
+	if (needs_refit) {
+		// only need to refit from the parent
+		const TNode &add_node = _nodes[ref.tnode_id];
+		if (add_node.parent_id != BVHCommon::INVALID) {
+			// not sure we need to rebalance all the time, this can be done less often
+			refit_upward(add_node.parent_id);
+		}
+		//refit_upward_and_balance(add_node.parent_id);
+	}
+
+	return true;
+}
+
+void item_remove(BVHHandle p_handle) {
+	uint32_t ref_id = p_handle.id();
+
+	_current_tree = _handle_get_tree_id(p_handle);
+
+	VERBOSE_PRINT("item_remove [" + itos(ref_id) + "] ");
+
+	////////////////////////////////////////
+	// remove the active reference from the list for slow incremental optimize
+	// this list must be kept in sync with the references as they are added or removed.
+	uint32_t active_ref_id = _extra[ref_id].active_ref_id;
+	uint32_t ref_id_moved_back = _active_refs[_active_refs.size() - 1];
+
+	// swap back and decrement for fast unordered remove
+	_active_refs[active_ref_id] = ref_id_moved_back;
+	_active_refs.resize(_active_refs.size() - 1);
+
+	// keep the moved active reference up to date
+	_extra[ref_id_moved_back].active_ref_id = active_ref_id;
+	////////////////////////////////////////
+
+	// remove the item from the node (only if active)
+	if (_refs[ref_id].is_active()) {
+		node_remove_item(ref_id);
+	}
+
+	// remove the item reference
+	_refs.free(ref_id);
+	_extra.free(ref_id);
+	if (USE_PAIRS) {
+		_pairs.free(ref_id);
+	}
+
+	// don't think refit_all is necessary?
+	//refit_all(_current_tree);
+
+#ifdef BVH_VERBOSE_TREE
+	_debug_recursive_print_tree(_current_tree);
+#endif
+}
+
+// returns success
+bool item_activate(BVHHandle p_handle, const Bounds &p_aabb) {
+	uint32_t ref_id = p_handle.id();
+	ItemRef &ref = _refs[ref_id];
+	if (ref.is_active()) {
+		// noop
+		return false;
+	}
+
+	// add to tree
+	BVHABB_CLASS abb;
+	abb.from(p_aabb);
+
+	_current_tree = _handle_get_tree_id(p_handle);
+
+	// we must choose where to add to tree
+	ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+	_node_add_item(ref.tnode_id, ref_id, abb);
+
+	refit_upward_and_balance(ref.tnode_id);
+
+	return true;
+}
+
+// returns success
+bool item_deactivate(BVHHandle p_handle) {
+	uint32_t ref_id = p_handle.id();
+	ItemRef &ref = _refs[ref_id];
+	if (!ref.is_active()) {
+		// noop
+		return false;
+	}
+
+	// remove from tree
+	BVHABB_CLASS abb;
+	node_remove_item(ref_id, &abb);
+
+	// mark as inactive
+	ref.set_inactive();
+	return true;
+}
+
+bool item_get_active(BVHHandle p_handle) const {
+	uint32_t ref_id = p_handle.id();
+	const ItemRef &ref = _refs[ref_id];
+	return ref.is_active();
+}
+
+// during collision testing, we want to set the mask and whether pairable for the item testing from
+void item_fill_cullparams(BVHHandle p_handle, CullParams &r_params) const {
+	uint32_t ref_id = p_handle.id();
+	const ItemExtra &extra = _extra[ref_id];
+
+	// testing from a non pairable item, we only want to test pairable items
+	r_params.test_pairable_only = extra.pairable == 0;
+
+	// we take into account the mask of the item testing from
+	r_params.mask = extra.pairable_mask;
+	r_params.pairable_type = extra.pairable_type;
+}
+
+bool item_is_pairable(const BVHHandle &p_handle) {
+	uint32_t ref_id = p_handle.id();
+	const ItemExtra &extra = _extra[ref_id];
+	return extra.pairable != 0;
+}
+
+void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) {
+	// change tree?
+	uint32_t ref_id = p_handle.id();
+	const ItemRef &ref = _refs[ref_id];
+
+	TNode &tnode = _nodes[ref.tnode_id];
+	TLeaf &leaf = _node_get_leaf(tnode);
+
+	r_abb = leaf.get_aabb(ref.item_id);
+}
+
+bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
+	// change tree?
+	uint32_t ref_id = p_handle.id();
+
+	ItemExtra &ex = _extra[ref_id];
+	ItemRef &ref = _refs[ref_id];
+
+	bool active = ref.is_active();
+	bool pairable_changed = (ex.pairable != 0) != p_pairable;
+	bool state_changed = pairable_changed || (ex.pairable_type != p_pairable_type) || (ex.pairable_mask != p_pairable_mask);
+
+	ex.pairable_type = p_pairable_type;
+	ex.pairable_mask = p_pairable_mask;
+
+	if (active && pairable_changed) {
+		// record abb
+		TNode &tnode = _nodes[ref.tnode_id];
+		TLeaf &leaf = _node_get_leaf(tnode);
+		BVHABB_CLASS abb = leaf.get_aabb(ref.item_id);
+
+		// make sure current tree is correct prior to changing
+		_current_tree = _handle_get_tree_id(p_handle);
+
+		// remove from old tree
+		node_remove_item(ref_id);
+
+		// we must set the pairable AFTER getting the current tree
+		// because the pairable status determines which tree
+		ex.pairable = p_pairable;
+
+		// add to new tree
+		_current_tree = _handle_get_tree_id(p_handle);
+		create_root_node(_current_tree);
+
+		// we must choose where to add to tree
+		ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
+		bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
+
+		// only need to refit from the PARENT
+		if (needs_refit) {
+			// only need to refit from the parent
+			const TNode &add_node = _nodes[ref.tnode_id];
+			if (add_node.parent_id != BVHCommon::INVALID) {
+				refit_upward_and_balance(add_node.parent_id);
+			}
+		}
+	} else {
+		// always keep this up to date
+		ex.pairable = p_pairable;
+	}
+
+	return state_changed;
+}
+
+void incremental_optimize() {
+	// first update all aabbs as one off step..
+	// this is cheaper than doing it on each move as each leaf may get touched multiple times
+	// in a frame.
+	for (int n = 0; n < NUM_TREES; n++) {
+		if (_root_node_id[n] != BVHCommon::INVALID) {
+			refit_branch(_root_node_id[n]);
+		}
+	}
+
+	// now do small section reinserting to get things moving
+	// gradually, and keep items in the right leaf
+	if (_current_active_ref >= _active_refs.size()) {
+		_current_active_ref = 0;
+	}
+
+	// special case
+	if (!_active_refs.size()) {
+		return;
+	}
+
+	uint32_t ref_id = _active_refs[_current_active_ref++];
+
+	_logic_item_remove_and_reinsert(ref_id);
+
+#ifdef BVH_VERBOSE
+	/*
+	// memory use
+	int mem_refs = _refs.estimate_memory_use();
+	int mem_nodes = _nodes.estimate_memory_use();
+	int mem_leaves = _leaves.estimate_memory_use();
+
+	String sz;
+	sz += "mem_refs : " + itos(mem_refs) + " ";
+	sz += "mem_nodes : " + itos(mem_nodes) + " ";
+	sz += "mem_leaves : " + itos(mem_leaves) + " ";
+	sz += ", num nodes : " + itos(_nodes.size());
+	print_line(sz);
+	*/
+#endif
+}
+
+void update() {
+	incremental_optimize();
+
+	// keep the expansion values up to date with the world bound
+//#define BVH_ALLOW_AUTO_EXPANSION
+#ifdef BVH_ALLOW_AUTO_EXPANSION
+	if (_auto_node_expansion || _auto_pairing_expansion) {
+		BVHABB_CLASS world_bound;
+		world_bound.set_to_max_opposite_extents();
+
+		bool bound_valid = false;
+
+		for (int n = 0; n < NUM_TREES; n++) {
+			uint32_t node_id = _root_node_id[n];
+			if (node_id != BVHCommon::INVALID) {
+				world_bound.merge(_nodes[node_id].aabb);
+				bound_valid = true;
+			}
+		}
+
+		// if there are no nodes, do nothing, but if there are...
+		if (bound_valid) {
+			Bounds bb;
+			world_bound.to(bb);
+			real_t size = bb.get_longest_axis_size();
+
+			// automatic AI decision for best parameters.
+			// These can be overridden in project settings.
+
+			// these magic numbers are determined by experiment
+			if (_auto_node_expansion) {
+				_node_expansion = size * 0.025;
+			}
+			if (_auto_pairing_expansion) {
+				_pairing_expansion = size * 0.009;
+			}
+		}
+	}
+#endif
+}

core/math/bvh_refit.inc

@@ -0,0 +1,141 @@
+void _debug_node_verify_bound(uint32_t p_node_id) {
+	TNode &node = _nodes[p_node_id];
+	BVHABB_CLASS abb_before = node.aabb;
+
+	node_update_aabb(node);
+
+	BVHABB_CLASS abb_after = node.aabb;
+	CRASH_COND(abb_before != abb_after);
+}
+
+void node_update_aabb(TNode &tnode) {
+	tnode.aabb.set_to_max_opposite_extents();
+	tnode.height = 0;
+
+	if (!tnode.is_leaf()) {
+		for (int n = 0; n < tnode.num_children; n++) {
+			uint32_t child_node_id = tnode.children[n];
+
+			// merge with child aabb
+			const TNode &tchild = _nodes[child_node_id];
+			tnode.aabb.merge(tchild.aabb);
+
+			// do heights at the same time
+			if (tchild.height > tnode.height) {
+				tnode.height = tchild.height;
+			}
+		}
+
+		// the height of a non leaf is always 1 bigger than the biggest child
+		tnode.height++;
+
+#ifdef BVH_CHECKS
+		if (!tnode.num_children) {
+			// the 'blank' aabb will screw up parent aabbs
+			WARN_PRINT("BVH_Tree::TNode no children, AABB is undefined");
+		}
+#endif
+	} else {
+		// leaf
+		const TLeaf &leaf = _node_get_leaf(tnode);
+
+		for (int n = 0; n < leaf.num_items; n++) {
+			tnode.aabb.merge(leaf.get_aabb(n));
+		}
+
+		// now the leaf items are unexpanded, we expand only in the node AABB
+		tnode.aabb.expand(_node_expansion);
+#ifdef BVH_CHECKS
+		if (!leaf.num_items) {
+			// the 'blank' aabb will screw up parent aabbs
+			WARN_PRINT("BVH_Tree::TLeaf no items, AABB is undefined");
+		}
+#endif
+	}
+}
+
+void refit_all(int p_tree_id) {
+	refit_downward(_root_node_id[p_tree_id]);
+}
+
+void refit_upward(uint32_t p_node_id) {
+	while (p_node_id != BVHCommon::INVALID) {
+		TNode &tnode = _nodes[p_node_id];
+		node_update_aabb(tnode);
+		p_node_id = tnode.parent_id;
+	}
+}
+
+void refit_upward_and_balance(uint32_t p_node_id) {
+	while (p_node_id != BVHCommon::INVALID) {
+		uint32_t before = p_node_id;
+		p_node_id = _logic_balance(p_node_id);
+
+		if (before != p_node_id) {
+			VERBOSE_PRINT("REBALANCED!");
+		}
+
+		TNode &tnode = _nodes[p_node_id];
+
+		// update overall aabb from the children
+		node_update_aabb(tnode);
+
+		p_node_id = tnode.parent_id;
+	}
+}
+
+void refit_downward(uint32_t p_node_id) {
+	TNode &tnode = _nodes[p_node_id];
+
+	// do children first
+	if (!tnode.is_leaf()) {
+		for (int n = 0; n < tnode.num_children; n++) {
+			refit_downward(tnode.children[n]);
+		}
+	}
+
+	node_update_aabb(tnode);
+}
+
+// go down to the leaves, then refit upward
+void refit_branch(uint32_t p_node_id) {
+	// our function parameters to keep on a stack
+	struct RefitParams {
+		uint32_t node_id;
+	};
+
+	// most of the iterative functionality is contained in this helper class
+	BVH_IterativeInfo<RefitParams> ii;
+
+	// alloca must allocate the stack from this function, it cannot be allocated in the
+	// helper class
+	ii.stack = (RefitParams *)alloca(ii.get_alloca_stacksize());
+
+	// seed the stack
+	ii.get_first()->node_id = p_node_id;
+
+	RefitParams rp;
+
+	// while there are still more nodes on the stack
+	while (ii.pop(rp)) {
+		TNode &tnode = _nodes[rp.node_id];
+
+		// do children first
+		if (!tnode.is_leaf()) {
+			for (int n = 0; n < tnode.num_children; n++) {
+				uint32_t child_id = tnode.children[n];
+
+				// add to the stack
+				RefitParams *child = ii.request();
+				child->node_id = child_id;
+			}
+		} else {
+			// leaf .. only refit upward if dirty
+			TLeaf &leaf = _node_get_leaf(tnode);
+			if (leaf.is_dirty()) {
+				leaf.set_dirty(false);
+				refit_upward(p_node_id);
+			}
+		}
+	} // while more nodes to pop
+}

core/math/bvh_split.inc

@@ -0,0 +1,294 @@
+void _split_inform_references(uint32_t p_node_id) {
+	TNode &node = _nodes[p_node_id];
+	TLeaf &leaf = _node_get_leaf(node);
+
+	for (int n = 0; n < leaf.num_items; n++) {
+		uint32_t ref_id = leaf.get_item_ref_id(n);
+
+		ItemRef &ref = _refs[ref_id];
+		ref.tnode_id = p_node_id;
+		ref.item_id = n;
+	}
+}
+
+void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds, const BVHABB_CLASS full_bound) {
+	// special case for low leaf sizes .. should static compile out
+	if (MAX_ITEMS < 4) {
+		uint32_t ind = group_a[0];
+
+		// add to b
+		group_b[num_b++] = ind;
+
+		// remove from a
+		group_a[0] = group_a[num_a - 1];
+		num_a--;
+		return;
+	}
+
+	Point centre = full_bound.calculate_centre();
+	Point size = full_bound.calculate_size();
+
+	int order[3];
+
+	order[0] = size.min_axis();
+	order[2] = size.max_axis();
+	order[1] = 3 - (order[0] + order[2]);
+
+	// simplest case, split on the longest axis
+	int split_axis = order[0];
+	for (int a = 0; a < num_a; a++) {
+		uint32_t ind = group_a[a];
+
+		if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) {
+			// add to b
+			group_b[num_b++] = ind;
+
+			// remove from a
+			group_a[a] = group_a[num_a - 1];
+			num_a--;
+
+			// do this one again, as it has been replaced
+			a--;
+		}
+	}
+
+	// detect when split on longest axis failed
+	int min_threshold = MAX_ITEMS / 4;
+	int min_group_size[3];
+	min_group_size[0] = MIN(num_a, num_b);
+	if (min_group_size[0] < min_threshold) {
+		// slow but sure .. first move everything back into a
+		for (int b = 0; b < num_b; b++) {
+			group_a[num_a++] = group_b[b];
+		}
+		num_b = 0;
+
+		// now calculate the best split
+		for (int axis = 1; axis < 3; axis++) {
+			split_axis = order[axis];
+			int count = 0;
+
+			for (int a = 0; a < num_a; a++) {
+				uint32_t ind = group_a[a];
+
+				if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) {
+					count++;
+				}
+			}
+
+			min_group_size[axis] = MIN(count, num_a - count);
+		} // for axis
+
+		// best axis
+		int best_axis = 0;
+		int best_min = min_group_size[0];
+		for (int axis = 1; axis < 3; axis++) {
+			if (min_group_size[axis] > best_min) {
+				best_min = min_group_size[axis];
+				best_axis = axis;
+			}
+		}
+
+		// now finally do the split
+		if (best_min > 0) {
+			split_axis = order[best_axis];
+
+			for (int a = 0; a < num_a; a++) {
+				uint32_t ind = group_a[a];
+
+				if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) {
+					// add to b
+					group_b[num_b++] = ind;
+
+					// remove from a
+					group_a[a] = group_a[num_a - 1];
+					num_a--;
+
+					// do this one again, as it has been replaced
+					a--;
+				}
+			}
+		} // if there was a split!
+	} // if the longest axis wasn't a good split
+
+	// special case, none crossed threshold
+	if (!num_b) {
+		uint32_t ind = group_a[0];
+
+		// add to b
+		group_b[num_b++] = ind;
+
+		// remove from a
+		group_a[0] = group_a[num_a - 1];
+		num_a--;
+	}
+	// opposite problem! :)
+	if (!num_a) {
+		uint32_t ind = group_b[0];
+
+		// add to a
+		group_a[num_a++] = ind;
+
+		// remove from b
+		group_b[0] = group_b[num_b - 1];
+		num_b--;
+	}
+}
+
+void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds) {
+	BVHABB_CLASS groupb_aabb;
+	groupb_aabb.set_to_max_opposite_extents();
+	for (int n = 0; n < num_b; n++) {
+		int which = group_b[n];
+		groupb_aabb.merge(temp_bounds[which]);
+	}
+	BVHABB_CLASS groupb_aabb_new;
+
+	BVHABB_CLASS rest_aabb;
+
+	float best_size = FLT_MAX;
+	int best_candidate = -1;
+
+	// find most likely from a to move into b
+	for (int check = 0; check < num_a; check++) {
+		rest_aabb.set_to_max_opposite_extents();
+		groupb_aabb_new = groupb_aabb;
+
+		// find aabb of all the rest
+		for (int rest = 0; rest < num_a; rest++) {
+			if (rest == check) {
+				continue;
+			}
+
+			int which = group_a[rest];
+			rest_aabb.merge(temp_bounds[which]);
+		}
+
+		groupb_aabb_new.merge(temp_bounds[group_a[check]]);
+
+		// now compare the sizes
+		float size = groupb_aabb_new.get_area() + rest_aabb.get_area();
+		if (size < best_size) {
+			best_size = size;
+			best_candidate = check;
+		}
+	}
+
+	// we should now have the best, move it from group a to group b
+	group_b[num_b++] = group_a[best_candidate];
+
+	// remove best candidate from group a
+	num_a--;
+	group_a[best_candidate] = group_a[num_a];
+}
+
+uint32_t split_leaf(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
+	return split_leaf_complex(p_node_id, p_added_item_aabb);
+}
+
+// aabb is the new inserted node
+uint32_t split_leaf_complex(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
+	VERBOSE_PRINT("split_leaf");
+
+	// note the tnode before and AFTER splitting may be a different address
+	// in memory because the vector could get relocated. So we need to reget
+	// the tnode after the split
+	BVH_ASSERT(_nodes[p_node_id].is_leaf());
+
+	// first create child leaf nodes
+	uint32_t *child_ids = (uint32_t *)alloca(sizeof(uint32_t) * MAX_CHILDREN);
+
+	for (int n = 0; n < MAX_CHILDREN; n++) {
+		// create node children
+		TNode *child_node = _nodes.request(child_ids[n]);
+
+		child_node->clear();
+
+		// back link to parent
+		child_node->parent_id = p_node_id;
+
+		// make each child a leaf node
+		node_make_leaf(child_ids[n]);
+	}
+
+	// don't get any leaves or nodes till AFTER the split
+	TNode &tnode = _nodes[p_node_id];
+	uint32_t orig_leaf_id = tnode.get_leaf_id();
+	const TLeaf &orig_leaf = _node_get_leaf(tnode);
+
+	// store the final child ids
+	for (int n = 0; n < MAX_CHILDREN; n++) {
+		tnode.children[n] = child_ids[n];
+	}
+
+	// mark as no longer a leaf node
+	tnode.num_children = MAX_CHILDREN;
+
+	// 2 groups, A and B, and assign children to each to split equally
+	int max_children = orig_leaf.num_items + 1; // plus 1 for the wildcard .. the item being added
+	//CRASH_COND(max_children > MAX_CHILDREN);
+
+	uint16_t *group_a = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
+	uint16_t *group_b = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
+
+	// we are copying the ABBs. This is ugly, but we need one extra for the inserted item...
+	BVHABB_CLASS *temp_bounds = (BVHABB_CLASS *)alloca(sizeof(BVHABB_CLASS) * max_children);
+
+	int num_a = max_children;
+	int num_b = 0;
+
+	// setup - start with all in group a
+	for (int n = 0; n < orig_leaf.num_items; n++) {
+		group_a[n] = n;
+		temp_bounds[n] = orig_leaf.get_aabb(n);
+	}
+	// wildcard
+	int wildcard = orig_leaf.num_items;
+
+	group_a[wildcard] = wildcard;
+	temp_bounds[wildcard] = p_added_item_aabb;
+
+	// we can choose here either an equal split, or just 1 in the new leaf
+	_split_leaf_sort_groups_simple(num_a, num_b, group_a, group_b, temp_bounds, tnode.aabb);
+
+	uint32_t wildcard_node = BVHCommon::INVALID;
+
+	// now there should be equal numbers in both groups
+	for (int n = 0; n < num_a; n++) {
+		int which = group_a[n];
+
+		if (which != wildcard) {
+			const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
+			uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
+			//const Item &source_item = orig_leaf.get_item(which);
+			_node_add_item(tnode.children[0], source_item_ref_id, source_item_aabb);
+		} else {
+			wildcard_node = tnode.children[0];
+		}
+	}
+	for (int n = 0; n < num_b; n++) {
+		int which = group_b[n];
+
+		if (which != wildcard) {
+			const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
+			uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
+			//const Item &source_item = orig_leaf.get_item(which);
+			_node_add_item(tnode.children[1], source_item_ref_id, source_item_aabb);
+		} else {
+			wildcard_node = tnode.children[1];
+		}
+	}
+
+	// now remove all items from the parent and replace with the child nodes
+	_leaves.free(orig_leaf_id);
+
+	// we should keep the references up to date!
+	for (int n = 0; n < MAX_CHILDREN; n++) {
+		_split_inform_references(tnode.children[n]);
+	}
+
+	refit_upward(p_node_id);
+
+	BVH_ASSERT(wildcard_node != BVHCommon::INVALID);
+	return wildcard_node;
+}

core/math/bvh_structs.inc

@@ -0,0 +1,181 @@
+
+public:
+struct ItemRef {
+	uint32_t tnode_id; // -1 is invalid
+	uint32_t item_id; // in the leaf
+
+	bool is_active() const { return tnode_id != BVHCommon::INACTIVE; }
+	void set_inactive() {
+		tnode_id = BVHCommon::INACTIVE;
+		item_id = BVHCommon::INACTIVE;
+	}
+};
+
+// extra info kept in separate parallel list to the references,
+// as this is less used as keeps cache better
+struct ItemExtra {
+	uint32_t last_updated_tick;
+	uint32_t pairable;
+	uint32_t pairable_mask;
+	uint32_t pairable_type;
+
+	int32_t subindex;
+
+	// the active reference is a separate list of which references
+	// are active so that we can slowly iterate through it over many frames for
+	// slow optimize.
+	uint32_t active_ref_id;
+
+	T *userdata;
+};
+
+// this is an item OR a child node depending on whether a leaf node
+struct Item {
+	BVHABB_CLASS aabb;
+	uint32_t item_ref_id;
+};
+
+// tree leaf
+struct TLeaf {
+	uint16_t num_items;
+
+private:
+	uint16_t dirty;
+	// separate data orientated lists for faster SIMD traversal
+	uint32_t item_ref_ids[MAX_ITEMS];
+	BVHABB_CLASS aabbs[MAX_ITEMS];
+
+public:
+	// accessors
+	BVHABB_CLASS &get_aabb(uint32_t p_id) { return aabbs[p_id]; }
+	const BVHABB_CLASS &get_aabb(uint32_t p_id) const { return aabbs[p_id]; }
+
+	uint32_t &get_item_ref_id(uint32_t p_id) { return item_ref_ids[p_id]; }
+	const uint32_t &get_item_ref_id(uint32_t p_id) const { return item_ref_ids[p_id]; }
+
+	bool is_dirty() const { return dirty; }
+	void set_dirty(bool p) { dirty = p; }
+
+	void clear() {
+		num_items = 0;
+		set_dirty(true);
+	}
+	bool is_full() const { return num_items >= MAX_ITEMS; }
+
+	void remove_item_unordered(uint32_t p_id) {
+		BVH_ASSERT(p_id < num_items);
+		num_items--;
+		aabbs[p_id] = aabbs[num_items];
+		item_ref_ids[p_id] = item_ref_ids[num_items];
+	}
+
+	uint32_t request_item() {
+		if (num_items < MAX_ITEMS) {
+			uint32_t id = num_items;
+			num_items++;
+			return id;
+		}
+		return -1;
+	}
+};
+
+// tree node
+struct TNode {
+	BVHABB_CLASS aabb;
+	// either number of children if positive
+	// or leaf id if negative (leaf id 0 is disallowed)
+	union {
+		int32_t num_children;
+		int32_t neg_leaf_id;
+	};
+	uint32_t parent_id; // or -1
+	uint16_t children[MAX_CHILDREN];
+
+	// height in the tree, where leaves are 0, and all above are 1+
+	// (or the highest where there is a tie off)
+	int32_t height;
+
+	bool is_leaf() const { return num_children < 0; }
+	void set_leaf_id(int id) { neg_leaf_id = -id; }
+	int get_leaf_id() const { return -neg_leaf_id; }
+
+	void clear() {
+		num_children = 0;
+		parent_id = BVHCommon::INVALID;
+		height = 0; // or -1 for testing
+
+		// for safety set to improbable value
+		aabb.set_to_max_opposite_extents();
+
+		// other members are not blanked for speed .. they may be uninitialized
+	}
+
+	bool is_full_of_children() const { return num_children >= MAX_CHILDREN; }
+
+	void remove_child_internal(uint32_t child_num) {
+		children[child_num] = children[num_children - 1];
+		num_children--;
+	}
+
+	int find_child(uint32_t p_child_node_id) {
+		BVH_ASSERT(!is_leaf());
+
+		for (int n = 0; n < num_children; n++) {
+			if (children[n] == p_child_node_id) {
+				return n;
+			}
+		}
+
+		// not found
+		return -1;
+	}
+};
+
+// instead of using linked list we maintain
+// item references (for quick lookup)
+PooledList<ItemRef, true> _refs;
+PooledList<ItemExtra, true> _extra;
+PooledList<ItemPairs> _pairs;
+
+// these 2 are not in sync .. nodes != leaves!
+PooledList<TNode, true> _nodes;
+PooledList<TLeaf, true> _leaves;
+
+// we can maintain an un-ordered list of which references are active,
+// in order to do a slow incremental optimize of the tree over each frame.
+// This will work best if dynamic objects and static objects are in a different tree.
+LocalVector<uint32_t, uint32_t, true> _active_refs;
+uint32_t _current_active_ref = 0;
+
+// instead of translating directly to the userdata output,
+// we keep an intermediate list of hits as reference IDs, which can be used
+// for pairing collision detection
+LocalVector<uint32_t, uint32_t, true> _cull_hits;
+
+// we now have multiple root nodes, allowing us to store
+// more than 1 tree. This can be more efficient, while sharing the same
+// common lists
+enum { NUM_TREES = 2,
+};
+
+// Tree 0 - Non pairable
+// Tree 1 - Pairable
+// This is more efficient because in physics we only need check non pairable against the pairable tree.
+uint32_t _root_node_id[NUM_TREES];
+int _current_tree = 0;
+
+// these values may need tweaking according to the project
+// the bound of the world, and the average velocities of the objects
+
+// node expansion is important in the rendering tree
+// larger values give less re-insertion as items move...
+// but on the other hand over estimates the bounding box of nodes.
+// we can either use auto mode, where the expansion is based on the root node size, or specify manually
+real_t _node_expansion = 0.5;
+bool _auto_node_expansion = true;
+
+// pairing expansion important for physics pairing
+// larger values gives more 'sticky' pairing, and is less likely to exhibit tunneling
+// we can either use auto mode, where the expansion is based on the root node size, or specify manually
+real_t _pairing_expansion = 0.1;
+bool _auto_pairing_expansion = true;

core/math/bvh_tree.h

@@ -0,0 +1,422 @@
+/*************************************************************************/
+/*  bvh_tree.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 BVH_TREE_H
+#define BVH_TREE_H
+
+// BVH Tree
+// This is an implementation of a dynamic BVH with templated leaf size.
+// This differs from most dynamic BVH in that it can handle more than 1 object
+// in leaf nodes. This can make it far more efficient in certain circumstances.
+// It also means that the splitting logic etc have to be completely different
+// to a simpler tree.
+// Note that MAX_CHILDREN should be fixed at 2 for now.
+
+#include "core/math/aabb.h"
+#include "core/math/bvh_abb.h"
+#include "core/math/geometry_3d.h"
+#include "core/math/vector3.h"
+#include "core/string/print_string.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/pooled_list.h"
+#include <limits.h>
+
+#define BVHABB_CLASS BVH_ABB<Bounds, Point>
+
+// never do these checks in release
+#if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED)
+//#define BVH_VERBOSE
+//#define BVH_VERBOSE_TREE
+
+//#define BVH_VERBOSE_FRAME
+//#define BVH_CHECKS
+//#define BVH_INTEGRITY_CHECKS
+#endif
+
+// debug only assert
+#ifdef BVH_CHECKS
+#define BVH_ASSERT(a) CRASH_COND((a) == false)
+#else
+#define BVH_ASSERT(a)
+#endif
+
+#ifdef BVH_VERBOSE
+#define VERBOSE_PRINT print_line
+#else
+#define VERBOSE_PRINT(a)
+#endif
+
+// really just a namespace
+struct BVHCommon {
+	// these could possibly also be the same constant,
+	// although this may be useful for debugging.
+	// or use zero for invalid and +1 based indices.
+	static const uint32_t INVALID = (0xffffffff);
+	static const uint32_t INACTIVE = (0xfffffffe);
+};
+
+// really a handle, can be anything
+// note that zero is a valid reference for the BVH .. this may involve using
+// a plus one based ID for clients that expect 0 to be invalid.
+struct BVHHandle {
+	// conversion operator
+	operator uint32_t() const { return _data; }
+	void set(uint32_t p_value) { _data = p_value; }
+
+	uint32_t _data;
+
+	void set_invalid() { _data = BVHCommon::INVALID; }
+	bool is_invalid() const { return _data == BVHCommon::INVALID; }
+	uint32_t id() const { return _data; }
+	void set_id(uint32_t p_id) { _data = p_id; }
+
+	bool operator==(const BVHHandle &p_h) const { return _data == p_h._data; }
+	bool operator!=(const BVHHandle &p_h) const { return (*this == p_h) == false; }
+};
+
+// helper class to make iterative versions of recursive functions
+template <class T>
+class BVH_IterativeInfo {
+public:
+	enum {
+		ALLOCA_STACK_SIZE = 128
+	};
+
+	int32_t depth = 1;
+	int32_t threshold = ALLOCA_STACK_SIZE - 2;
+	T *stack;
+	//only used in rare occasions when you run out of alloca memory
+	// because tree is too unbalanced.
+	LocalVector<T> aux_stack;
+	int32_t get_alloca_stacksize() const { return ALLOCA_STACK_SIZE * sizeof(T); }
+
+	T *get_first() const {
+		return &stack[0];
+	}
+
+	// pop the last member of the stack, or return false
+	bool pop(T &r_value) {
+		if (!depth) {
+			return false;
+		}
+
+		depth--;
+		r_value = stack[depth];
+		return true;
+	}
+
+	// request new addition to stack
+	T *request() {
+		if (depth > threshold) {
+			if (aux_stack.is_empty()) {
+				aux_stack.resize(ALLOCA_STACK_SIZE * 2);
+				memcpy(aux_stack.ptr(), stack, get_alloca_stacksize());
+			} else {
+				aux_stack.resize(aux_stack.size() * 2);
+			}
+			stack = aux_stack.ptr();
+			threshold = aux_stack.size() - 2;
+		}
+		return &stack[depth++];
+	}
+};
+
+template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false, class Bounds = AABB, class Point = Vector3>
+class BVH_Tree {
+	friend class BVH;
+
+#include "bvh_pair.inc"
+#include "bvh_structs.inc"
+
+public:
+	BVH_Tree() {
+		for (int n = 0; n < NUM_TREES; n++) {
+			_root_node_id[n] = BVHCommon::INVALID;
+		}
+
+		// disallow zero leaf ids
+		// (as these ids are stored as negative numbers in the node)
+		uint32_t dummy_leaf_id;
+		_leaves.request(dummy_leaf_id);
+	}
+
+private:
+	bool node_add_child(uint32_t p_node_id, uint32_t p_child_node_id) {
+		TNode &tnode = _nodes[p_node_id];
+		if (tnode.is_full_of_children()) {
+			return false;
+		}
+
+		tnode.children[tnode.num_children] = p_child_node_id;
+		tnode.num_children += 1;
+
+		// back link in the child to the parent
+		TNode &tnode_child = _nodes[p_child_node_id];
+		tnode_child.parent_id = p_node_id;
+
+		return true;
+	}
+
+	void node_replace_child(uint32_t p_parent_id, uint32_t p_old_child_id, uint32_t p_new_child_id) {
+		TNode &parent = _nodes[p_parent_id];
+		BVH_ASSERT(!parent.is_leaf());
+
+		int child_num = parent.find_child(p_old_child_id);
+		BVH_ASSERT(child_num != BVHCommon::INVALID);
+		parent.children[child_num] = p_new_child_id;
+
+		TNode &new_child = _nodes[p_new_child_id];
+		new_child.parent_id = p_parent_id;
+	}
+
+	void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, bool p_prevent_sibling = false) {
+		TNode &parent = _nodes[p_parent_id];
+		BVH_ASSERT(!parent.is_leaf());
+
+		int child_num = parent.find_child(p_child_id);
+		BVH_ASSERT(child_num != BVHCommon::INVALID);
+
+		parent.remove_child_internal(child_num);
+
+		// no need to keep back references for children at the moment
+
+		uint32_t sibling_id; // always a node id, as tnode is never a leaf
+		bool sibling_present = false;
+
+		// if there are more children, or this is the root node, don't try and delete
+		if (parent.num_children > 1) {
+			return;
+		}
+
+		// if there is 1 sibling, it can be moved to be a child of the
+		if (parent.num_children == 1) {
+			// else there is now a redundant node with one child, which can be removed
+			sibling_id = parent.children[0];
+			sibling_present = true;
+		}
+
+		// now there may be no children in this node .. in which case it can be deleted
+		// remove node if empty
+		// remove link from parent
+		uint32_t grandparent_id = parent.parent_id;
+
+		// special case for root node
+		if (grandparent_id == BVHCommon::INVALID) {
+			if (sibling_present) {
+				// change the root node
+				change_root_node(sibling_id);
+
+				// delete the old root node as no longer needed
+				_nodes.free(p_parent_id);
+			}
+
+			return;
+		}
+
+		if (sibling_present) {
+			node_replace_child(grandparent_id, p_parent_id, sibling_id);
+		} else {
+			node_remove_child(grandparent_id, p_parent_id, true);
+		}
+
+		// put the node on the free list to recycle
+		_nodes.free(p_parent_id);
+	}
+
+	// this relies on _current_tree being accurate
+	void change_root_node(uint32_t p_new_root_id) {
+		_root_node_id[_current_tree] = p_new_root_id;
+		TNode &root = _nodes[p_new_root_id];
+
+		// mark no parent
+		root.parent_id = BVHCommon::INVALID;
+	}
+
+	void node_make_leaf(uint32_t p_node_id) {
+		uint32_t child_leaf_id;
+		TLeaf *child_leaf = _leaves.request(child_leaf_id);
+		child_leaf->clear();
+
+		// zero is reserved at startup, to prevent this id being used
+		// (as they are stored as negative values in the node, and zero is already taken)
+		BVH_ASSERT(child_leaf_id != 0);
+
+		TNode &node = _nodes[p_node_id];
+		node.neg_leaf_id = -(int)child_leaf_id;
+	}
+
+	void node_remove_item(uint32_t p_ref_id, BVHABB_CLASS *r_old_aabb = nullptr) {
+		// get the reference
+		ItemRef &ref = _refs[p_ref_id];
+		uint32_t owner_node_id = ref.tnode_id;
+
+		// debug draw special
+		// This may not be needed
+		if (owner_node_id == BVHCommon::INVALID) {
+			return;
+		}
+
+		TNode &tnode = _nodes[owner_node_id];
+		CRASH_COND(!tnode.is_leaf());
+
+		TLeaf &leaf = _node_get_leaf(tnode);
+
+		// if the aabb is not determining the corner size, then there is no need to refit!
+		// (optimization, as merging AABBs takes a lot of time)
+		const BVHABB_CLASS &old_aabb = leaf.get_aabb(ref.item_id);
+
+		// shrink a little to prevent using corner aabbs
+		// in order to miss the corners first we shrink by node_expansion
+		// (which is added to the overall bound of the leaf), then we also
+		// shrink by an epsilon, in order to miss out the very corner aabbs
+		// which are important in determining the bound. Any other aabb
+		// within this can be removed and not affect the overall bound.
+		BVHABB_CLASS node_bound = tnode.aabb;
+		node_bound.expand(-_node_expansion - 0.001f);
+		bool refit = true;
+
+		if (node_bound.is_other_within(old_aabb)) {
+			refit = false;
+		}
+
+		// record the old aabb if required (for incremental remove_and_reinsert)
+		if (r_old_aabb) {
+			*r_old_aabb = old_aabb;
+		}
+
+		leaf.remove_item_unordered(ref.item_id);
+
+		if (leaf.num_items) {
+			// the swapped item has to have its reference changed to, to point to the new item id
+			uint32_t swapped_ref_id = leaf.get_item_ref_id(ref.item_id);
+
+			ItemRef &swapped_ref = _refs[swapped_ref_id];
+
+			swapped_ref.item_id = ref.item_id;
+
+			// only have to refit if it is an edge item
+			// This is a VERY EXPENSIVE STEP
+			// we defer the refit updates until the update function is called once per frame
+			if (refit) {
+				leaf.set_dirty(true);
+			}
+		} else {
+			// remove node if empty
+			// remove link from parent
+			if (tnode.parent_id != BVHCommon::INVALID) {
+				// DANGER .. this can potentially end up with root node with 1 child ...
+				// we don't want this and must check for it
+
+				uint32_t parent_id = tnode.parent_id;
+
+				node_remove_child(parent_id, owner_node_id);
+				refit_upward(parent_id);
+
+				// put the node on the free list to recycle
+				_nodes.free(owner_node_id);
+			}
+
+			// else if no parent, it is the root node. Do not delete
+		}
+
+		ref.tnode_id = BVHCommon::INVALID;
+		ref.item_id = BVHCommon::INVALID; // unset
+	}
+
+	// returns true if needs refit of PARENT tree only, the node itself AABB is calculated
+	// within this routine
+	bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVHABB_CLASS &p_aabb) {
+		ItemRef &ref = _refs[p_ref_id];
+		ref.tnode_id = p_node_id;
+
+		TNode &node = _nodes[p_node_id];
+		BVH_ASSERT(node.is_leaf());
+		TLeaf &leaf = _node_get_leaf(node);
+
+		// optimization - we only need to do a refit
+		// if the added item is changing the AABB of the node.
+		// in most cases it won't.
+		bool needs_refit = true;
+
+		// expand bound now
+		BVHABB_CLASS expanded = p_aabb;
+		expanded.expand(_node_expansion);
+
+		// the bound will only be valid if there is an item in there already
+		if (leaf.num_items) {
+			if (node.aabb.is_other_within(expanded)) {
+				// no change to node AABBs
+				needs_refit = false;
+			} else {
+				node.aabb.merge(expanded);
+			}
+		} else {
+			// bound of the node = the new aabb
+			node.aabb = expanded;
+		}
+
+		ref.item_id = leaf.request_item();
+		BVH_ASSERT(ref.item_id != BVHCommon::INVALID);
+
+		// set the aabb of the new item
+		leaf.get_aabb(ref.item_id) = p_aabb;
+
+		// back reference on the item back to the item reference
+		leaf.get_item_ref_id(ref.item_id) = p_ref_id;
+
+		return needs_refit;
+	}
+
+	uint32_t _node_create_another_child(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
+		uint32_t child_node_id;
+		TNode *child_node = _nodes.request(child_node_id);
+		child_node->clear();
+
+		// may not be necessary
+		child_node->aabb = p_aabb;
+
+		node_add_child(p_node_id, child_node_id);
+
+		return child_node_id;
+	}
+
+#include "bvh_cull.inc"
+#include "bvh_debug.inc"
+#include "bvh_integrity.inc"
+#include "bvh_logic.inc"
+#include "bvh_misc.inc"
+#include "bvh_public.inc"
+#include "bvh_refit.inc"
+#include "bvh_split.inc"
+};
+
+#undef VERBOSE_PRINT
+
+#endif // BVH_TREE_H

core/math/rect2.h

@@ -182,13 +182,17 @@ struct Rect2 {
 
 	inline Rect2 grow(real_t p_amount) const {
 		Rect2 g = *this;
-		g.position.x -= p_amount;
-		g.position.y -= p_amount;
-		g.size.width += p_amount * 2;
-		g.size.height += p_amount * 2;
+		g.grow_by(p_amount);
 		return g;
 	}
 
+	inline void grow_by(real_t p_amount) {
+		position.x -= p_amount;
+		position.y -= p_amount;
+		size.width += p_amount * 2;
+		size.height += p_amount * 2;
+	}
+
 	inline Rect2 grow_side(Side p_side, real_t p_amount) const {
 		Rect2 g = *this;
 		g = g.grow_individual((SIDE_LEFT == p_side) ? p_amount : 0,

core/math/vector2.h

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

core/math/vector3.cpp

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

core/math/vector3.h

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

core/templates/pooled_list.h

@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_3d_basic.h                                               */
+/*  pooled_list.h                                                        */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,78 +28,68 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BROAD_PHASE_BASIC_H
-#define BROAD_PHASE_BASIC_H
+#pragma once
 
-#include "broad_phase_3d_sw.h"
-#include "core/templates/map.h"
+// Simple template to provide a pool with O(1) allocate and free.
+// The freelist could alternatively be a linked list placed within the unused elements
+// to use less memory, however a separate freelist is probably more cache friendly.
 
-class BroadPhase3DBasic : public BroadPhase3DSW {
-	struct Element {
-		CollisionObject3DSW *owner;
-		bool _static;
-		AABB aabb;
-		int subindex;
-	};
+// NOTE : Take great care when using this with non POD types. The construction and destruction
+// is done in the LocalVector, NOT as part of the pool. So requesting a new item does not guarantee
+// a constructor is run, and free does not guarantee a destructor.
+// You should generally handle clearing
+// an item explicitly after a request, as it may contain 'leftovers'.
+// This is by design for fastest use in the BVH. If you want a more general pool
+// that does call constructors / destructors on request / free, this should probably be
+// a separate template.
 
-	Map<ID, Element> element_map;
+#include "core/templates/local_vector.h"
 
-	ID current;
+template <class T, bool force_trivial = false>
+class PooledList {
+	LocalVector<T, uint32_t, force_trivial> list;
+	LocalVector<uint32_t, uint32_t, true> freelist;
 
-	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;
-
-	PairCallback pair_callback;
-	void *pair_userdata;
-	UnpairCallback unpair_callback;
-	void *unpair_userdata;
+	// not all list members are necessarily used
+	int _used_size;
 
 public:
-	// 0 is an invalid ID
-	virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0);
-	virtual void move(ID p_id, const AABB &p_aabb);
-	virtual void set_static(ID p_id, bool p_static);
-	virtual void remove(ID p_id);
+	PooledList() {
+		_used_size = 0;
+	}
 
-	virtual CollisionObject3DSW *get_object(ID p_id) const;
-	virtual bool is_static(ID p_id) const;
-	virtual int get_subindex(ID p_id) const;
+	int estimate_memory_use() const {
+		return (list.size() * sizeof(T)) + (freelist.size() * sizeof(uint32_t));
+	}
 
-	virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
-	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
-	virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
+	const T &operator[](uint32_t p_index) const {
+		return list[p_index];
+	}
+	T &operator[](uint32_t p_index) {
+		return list[p_index];
+	}
 
-	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
+	int size() const { return _used_size; }
 
-	virtual void update();
+	T *request(uint32_t &r_id) {
+		_used_size++;
 
-	static BroadPhase3DSW *_create();
-	BroadPhase3DBasic();
+		if (freelist.size()) {
+			// pop from freelist
+			int new_size = freelist.size() - 1;
+			r_id = freelist[new_size];
+			freelist.resize(new_size);
+			return &list[r_id];
+		}
+
+		r_id = list.size();
+		list.resize(r_id + 1);
+		return &list[r_id];
+	}
+	void free(const uint32_t &p_id) {
+		// should not be on free list already
+		CRASH_COND(p_id >= list.size());
+		freelist.push_back(p_id);
+		_used_size--;
+	}
 };
-
-#endif // BROAD_PHASE_BASIC_H

doc/classes/ProjectSettings.xml

@@ -1197,12 +1197,6 @@
 			The CA certificates bundle to use for SSL connections. If this is set to a non-empty value, this will [i]override[/i] Godot's default [url=https://github.com/godotengine/godot/blob/master/thirdparty/certs/ca-certificates.crt]Mozilla certificate bundle[/url]. If left empty, the default certificate bundle will be used.
 			If in doubt, leave this setting empty.
 		</member>
-		<member name="physics/2d/bp_hash_table_size" type="int" setter="" getter="" default="4096">
-			Size of the hash table used for the broad-phase 2D hash grid algorithm.
-		</member>
-		<member name="physics/2d/cell_size" type="int" setter="" getter="" default="128">
-			Cell size used for the broad-phase 2D hash grid algorithm (in pixels).
-		</member>
 		<member name="physics/2d/default_angular_damp" type="float" setter="" getter="" default="1.0">
 			The default angular damp in 2D.
 			[b]Note:[/b] Good values are in the range [code]0[/code] to [code]1[/code]. At value [code]0[/code] objects will keep moving with the same velocity. Values greater than [code]1[/code] will aim to reduce the velocity to [code]0[/code] in less than a second e.g. a value of [code]2[/code] will aim to reduce the velocity to [code]0[/code] in half a second. A value equal to or greater than the physics frame rate ([member ProjectSettings.physics/common/physics_fps], [code]60[/code] by default) will bring the object to a stop in one iteration.
@@ -1239,9 +1233,6 @@
 			The default linear damp in 2D.
 			[b]Note:[/b] Good values are in the range [code]0[/code] to [code]1[/code]. At value [code]0[/code] objects will keep moving with the same velocity. Values greater than [code]1[/code] will aim to reduce the velocity to [code]0[/code] in less than a second e.g. a value of [code]2[/code] will aim to reduce the velocity to [code]0[/code] in half a second. A value equal to or greater than the physics frame rate ([member ProjectSettings.physics/common/physics_fps], [code]60[/code] by default) will bring the object to a stop in one iteration.
 		</member>
-		<member name="physics/2d/large_object_surface_threshold_in_cells" type="int" setter="" getter="" default="512">
-			Threshold defining the surface size that constitutes a large object with regard to cells in the broad-phase 2D hash grid algorithm.
-		</member>
 		<member name="physics/2d/physics_engine" type="String" setter="" getter="" default="&quot;DEFAULT&quot;">
 			Sets which physics engine to use for 2D physics.
 			"DEFAULT" and "GodotPhysics2D" are the same, as there is currently no alternative 2D physics server implemented.

servers/physics_2d/broad_phase_2d_basic.cpp

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

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

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

servers/physics_2d/broad_phase_2d_hash_grid.cpp

@@ -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;
-	}
-    }
-
-    */

servers/physics_2d/broad_phase_2d_hash_grid.h

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

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;

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

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;

servers/physics_3d/broad_phase_3d_basic.cpp

@@ -1,212 +0,0 @@
-/*************************************************************************/
-/*  broad_phase_3d_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_3d_basic.h"
-#include "core/string/print_string.h"
-#include "core/templates/list.h"
-
-BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) {
-	ERR_FAIL_COND_V(p_object == nullptr, 0);
-
-	current++;
-
-	Element e;
-	e.owner = p_object;
-	e._static = false;
-	e.subindex = p_subindex;
-
-	element_map[current] = e;
-	return current;
-}
-
-void BroadPhase3DBasic::move(ID p_id, const AABB &p_aabb) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-	E->get().aabb = p_aabb;
-}
-
-void BroadPhase3DBasic::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 BroadPhase3DBasic::remove(ID p_id) {
-	Map<ID, Element>::Element *E = element_map.find(p_id);
-	ERR_FAIL_COND(!E);
-	List<PairKey> to_erase;
-	//unpair must be done immediately on removal to avoid potential invalid pointers
-	for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) {
-		if (F->key().a == p_id || F->key().b == p_id) {
-			if (unpair_callback) {
-				Element *elem_A = &element_map[F->key().a];
-				Element *elem_B = &element_map[F->key().b];
-				unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata);
-			}
-			to_erase.push_back(F->key());
-		}
-	}
-	while (to_erase.size()) {
-		pair_map.erase(to_erase.front()->get());
-		to_erase.pop_front();
-	}
-	element_map.erase(E);
-}
-
-CollisionObject3DSW *BroadPhase3DBasic::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 BroadPhase3DBasic::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 BroadPhase3DBasic::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 BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW **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 AABB aabb = E->get().aabb;
-		if (aabb.has_point(p_point)) {
-			p_results[rc] = E->get().owner;
-			p_result_indices[rc] = E->get().subindex;
-			rc++;
-			if (rc >= p_max_results) {
-				break;
-			}
-		}
-	}
-
-	return rc;
-}
-
-int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **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 AABB 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 BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **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 AABB 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 BroadPhase3DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
-	pair_userdata = p_userdata;
-	pair_callback = p_pair_callback;
-}
-
-void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
-	unpair_userdata = p_userdata;
-	unpair_callback = p_unpair_callback;
-}
-
-void BroadPhase3DBasic::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);
-					}
-				}
-			}
-		}
-	}
-}
-
-BroadPhase3DSW *BroadPhase3DBasic::_create() {
-	return memnew(BroadPhase3DBasic);
-}
-
-BroadPhase3DBasic::BroadPhase3DBasic() {
-	current = 1;
-	unpair_callback = nullptr;
-	unpair_userdata = nullptr;
-	pair_callback = nullptr;
-	pair_userdata = nullptr;
-}

servers/physics_3d/broad_phase_3d_bvh.cpp

@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_octree.cpp                                               */
+/*  broad_phase_3d_bvh.cpp                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,55 +28,55 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "broad_phase_octree.h"
+#include "broad_phase_3d_bvh.h"
 #include "collision_object_3d_sw.h"
 
-BroadPhase3DSW::ID BroadPhaseOctree::create(CollisionObject3DSW *p_object, int p_subindex) {
-	ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
-	return oid;
+BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
+	ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
+	return oid + 1;
 }
 
-void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) {
-	octree.move(p_id, p_aabb);
+void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
+	bvh.move(p_id - 1, p_aabb);
 }
 
-void BroadPhaseOctree::set_static(ID p_id, bool p_static) {
-	CollisionObject3DSW *it = octree.get(p_id);
-	octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
+void BroadPhase3DBVH::set_static(ID p_id, bool p_static) {
+	CollisionObject3DSW *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 BroadPhaseOctree::remove(ID p_id) {
-	octree.erase(p_id);
+void BroadPhase3DBVH::remove(ID p_id) {
+	bvh.erase(p_id - 1);
 }
 
-CollisionObject3DSW *BroadPhaseOctree::get_object(ID p_id) const {
-	CollisionObject3DSW *it = octree.get(p_id);
+CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const {
+	CollisionObject3DSW *it = bvh.get(p_id - 1);
 	ERR_FAIL_COND_V(!it, nullptr);
 	return it;
 }
 
-bool BroadPhaseOctree::is_static(ID p_id) const {
-	return !octree.is_pairable(p_id);
+bool BroadPhase3DBVH::is_static(ID p_id) const {
+	return !bvh.is_pairable(p_id - 1);
 }
 
-int BroadPhaseOctree::get_subindex(ID p_id) const {
-	return octree.get_subindex(p_id);
+int BroadPhase3DBVH::get_subindex(ID p_id) const {
+	return bvh.get_subindex(p_id - 1);
 }
 
-int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
-	return octree.cull_point(p_point, p_results, p_max_results, p_result_indices);
+int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
+	return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices);
 }
 
-int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
-	return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
+int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **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 BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
-	return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
+int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **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 *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
-	BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
+void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
+	BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
 	if (!bpo->pair_callback) {
 		return nullptr;
 	}
@@ -84,8 +84,8 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio
 	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
 }
 
-void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
-	BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
+void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
+	BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
 	if (!bpo->unpair_callback) {
 		return;
 	}
@@ -93,27 +93,27 @@ void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, Collisi
 	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
 }
 
-void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
+void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 	pair_callback = p_pair_callback;
 	pair_userdata = p_userdata;
 }
 
-void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
+void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
 	unpair_callback = p_unpair_callback;
 	unpair_userdata = p_userdata;
 }
 
-void BroadPhaseOctree::update() {
-	// does.. not?
+void BroadPhase3DBVH::update() {
+	bvh.update();
 }
 
-BroadPhase3DSW *BroadPhaseOctree::_create() {
-	return memnew(BroadPhaseOctree);
+BroadPhase3DSW *BroadPhase3DBVH::_create() {
+	return memnew(BroadPhase3DBVH);
 }
 
-BroadPhaseOctree::BroadPhaseOctree() {
-	octree.set_pair_callback(_pair_callback, this);
-	octree.set_unpair_callback(_unpair_callback, this);
+BroadPhase3DBVH::BroadPhase3DBVH() {
+	bvh.set_pair_callback(_pair_callback, this);
+	bvh.set_unpair_callback(_unpair_callback, this);
 	pair_callback = nullptr;
 	pair_userdata = nullptr;
 	unpair_userdata = nullptr;

servers/physics_3d/broad_phase_3d_bvh.h

@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_octree.h                                                 */
+/*  broad_phase_3d_bvh.h                                                 */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,17 +28,17 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BROAD_PHASE_OCTREE_H
-#define BROAD_PHASE_OCTREE_H
+#ifndef BROAD_PHASE_3D_BVH_H
+#define BROAD_PHASE_3D_BVH_H
 
 #include "broad_phase_3d_sw.h"
-#include "core/math/octree.h"
+#include "core/math/bvh.h"
 
-class BroadPhaseOctree : public BroadPhase3DSW {
-	Octree<CollisionObject3DSW, true> octree;
+class BroadPhase3DBVH : public BroadPhase3DSW {
+	BVH_Manager<CollisionObject3DSW, true, 128> bvh;
 
-	static void *_pair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int);
-	static void _unpair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int, void *);
+	static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int);
+	static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *);
 
 	PairCallback pair_callback;
 	void *pair_userdata;
@@ -47,7 +47,7 @@ class BroadPhaseOctree : public BroadPhase3DSW {
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0);
+	virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
@@ -66,7 +66,7 @@ public:
 	virtual void update();
 
 	static BroadPhase3DSW *_create();
-	BroadPhaseOctree();
+	BroadPhase3DBVH();
 };
 
-#endif // BROAD_PHASE_OCTREE_H
+#endif // BROAD_PHASE_3D_BVH_H

servers/physics_3d/broad_phase_3d_sw.h

@@ -48,7 +48,7 @@ public:
 	typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0) = 0;
+	virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
 	virtual void move(ID p_id, const AABB &p_aabb) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
 	virtual void remove(ID p_id) = 0;

servers/physics_3d/collision_object_3d_sw.cpp

@@ -146,22 +146,23 @@ void CollisionObject3DSW::_update_shapes() {
 
 	for (int i = 0; i < shapes.size(); i++) {
 		Shape &s = shapes.write[i];
-		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..
 		AABB shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
 		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
 		s.aabb_cache = shape_aabb;
-		s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
 
 		Vector3 scale = xform.get_basis().get_scale();
 		s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
 
-		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);
 	}
 }
 
@@ -172,18 +173,19 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 
 	for (int i = 0; i < shapes.size(); i++) {
 		Shape &s = shapes.write[i];
-		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..
 		AABB shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
 		shape_aabb = xform.xform(shape_aabb);
-		shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
+		shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
 		s.aabb_cache = shape_aabb;
 
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
+			space->get_broadphase()->set_static(s.bpid, _static);
+		}
+
 		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
 }

servers/physics_3d/physics_server_3d_sw.cpp

@@ -30,8 +30,7 @@
 
 #include "physics_server_3d_sw.h"
 
-#include "broad_phase_3d_basic.h"
-#include "broad_phase_octree.h"
+#include "broad_phase_3d_bvh.h"
 #include "core/debugger/engine_debugger.h"
 #include "core/os/os.h"
 #include "joints/cone_twist_joint_3d_sw.h"
@@ -1755,7 +1754,8 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A,
 PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr;
 PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) {
 	singletonsw = this;
-	BroadPhase3DSW::create_func = BroadPhaseOctree::_create;
+	BroadPhase3DSW::create_func = BroadPhase3DBVH::_create;
+
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;