summaryrefslogtreecommitdiff
path: root/core
diff options
context:
space:
mode:
Diffstat (limited to 'core')
-rw-r--r--core/math/bvh.h695
-rw-r--r--core/math/bvh_abb.h276
-rw-r--r--core/math/bvh_cull.inc534
-rw-r--r--core/math/bvh_debug.inc68
-rw-r--r--core/math/bvh_integrity.inc42
-rw-r--r--core/math/bvh_logic.inc230
-rw-r--r--core/math/bvh_misc.inc55
-rw-r--r--core/math/bvh_pair.inc62
-rw-r--r--core/math/bvh_public.inc421
-rw-r--r--core/math/bvh_refit.inc141
-rw-r--r--core/math/bvh_split.inc294
-rw-r--r--core/math/bvh_structs.inc181
-rw-r--r--core/math/bvh_tree.h422
-rw-r--r--core/math/rect2.h12
-rw-r--r--core/math/vector2.h32
-rw-r--r--core/math/vector3.cpp8
-rw-r--r--core/math/vector3.h15
-rw-r--r--core/templates/pooled_list.h95
18 files changed, 3563 insertions, 20 deletions
diff --git a/core/math/bvh.h b/core/math/bvh.h
new file mode 100644
index 0000000000..cefbc9b0db
--- /dev/null
+++ b/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
diff --git a/core/math/bvh_abb.h b/core/math/bvh_abb.h
new file mode 100644
index 0000000000..bd9a01a87e
--- /dev/null
+++ b/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
diff --git a/core/math/bvh_cull.inc b/core/math/bvh_cull.inc
new file mode 100644
index 0000000000..cba8ea6cb3
--- /dev/null
+++ b/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;
+}
diff --git a/core/math/bvh_debug.inc b/core/math/bvh_debug.inc
new file mode 100644
index 0000000000..a97304334c
--- /dev/null
+++ b/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
diff --git a/core/math/bvh_integrity.inc b/core/math/bvh_integrity.inc
new file mode 100644
index 0000000000..02e9d30097
--- /dev/null
+++ b/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);
+ }
+ }
+}
diff --git a/core/math/bvh_logic.inc b/core/math/bvh_logic.inc
new file mode 100644
index 0000000000..d84c3f7830
--- /dev/null
+++ b/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];
+ }
+ }
+}
diff --git a/core/math/bvh_misc.inc b/core/math/bvh_misc.inc
new file mode 100644
index 0000000000..71aa0e4fe0
--- /dev/null
+++ b/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:
diff --git a/core/math/bvh_pair.inc b/core/math/bvh_pair.inc
new file mode 100644
index 0000000000..839db59a3a
--- /dev/null
+++ b/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;
+ }
+};
diff --git a/core/math/bvh_public.inc b/core/math/bvh_public.inc
new file mode 100644
index 0000000000..f1b6d6b1bf
--- /dev/null
+++ b/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
+}
diff --git a/core/math/bvh_refit.inc b/core/math/bvh_refit.inc
new file mode 100644
index 0000000000..514c853ac5
--- /dev/null
+++ b/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
+}
diff --git a/core/math/bvh_split.inc b/core/math/bvh_split.inc
new file mode 100644
index 0000000000..3fcc4c7b10
--- /dev/null
+++ b/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;
+}
diff --git a/core/math/bvh_structs.inc b/core/math/bvh_structs.inc
new file mode 100644
index 0000000000..4133ba6c10
--- /dev/null
+++ b/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;
diff --git a/core/math/bvh_tree.h b/core/math/bvh_tree.h
new file mode 100644
index 0000000000..64c5f6e254
--- /dev/null
+++ b/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
diff --git a/core/math/rect2.h b/core/math/rect2.h
index 512499bdb2..1dc027cf72 100644
--- a/core/math/rect2.h
+++ b/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,
diff --git a/core/math/vector2.h b/core/math/vector2.h
index edc6e3a3ef..6abe0f5ea9 100644
--- a/core/math/vector2.h
+++ b/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;
diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
index f0629d3db8..d4317d506c 100644
--- a/core/math/vector3.cpp
+++ b/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);
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 377581bb45..b47c3cc916 100644
--- a/core/math/vector3.h
+++ b/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;
diff --git a/core/templates/pooled_list.h b/core/templates/pooled_list.h
new file mode 100644
index 0000000000..b4a6d2d1dd
--- /dev/null
+++ b/core/templates/pooled_list.h
@@ -0,0 +1,95 @@
+/*************************************************************************/
+/* pooled_list.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. */
+/*************************************************************************/
+
+#pragma once
+
+// 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.
+
+// 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.
+
+#include "core/templates/local_vector.h"
+
+template <class T, bool force_trivial = false>
+class PooledList {
+ LocalVector<T, uint32_t, force_trivial> list;
+ LocalVector<uint32_t, uint32_t, true> freelist;
+
+ // not all list members are necessarily used
+ int _used_size;
+
+public:
+ PooledList() {
+ _used_size = 0;
+ }
+
+ int estimate_memory_use() const {
+ return (list.size() * sizeof(T)) + (freelist.size() * sizeof(uint32_t));
+ }
+
+ const T &operator[](uint32_t p_index) const {
+ return list[p_index];
+ }
+ T &operator[](uint32_t p_index) {
+ return list[p_index];
+ }
+
+ int size() const { return _used_size; }
+
+ T *request(uint32_t &r_id) {
+ _used_size++;
+
+ 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--;
+ }
+};
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-26 03:02:06 +0000