diff --git a/core/math/octree.h b/core/math/octree.h index a2565d31418..7d6e8c7d9e5 100644 --- a/core/math/octree.h +++ b/core/math/octree.h @@ -31,1356 +31,31 @@ #ifndef OCTREE_H #define OCTREE_H -#include "core/list.h" -#include "core/map.h" -#include "core/math/aabb.h" -#include "core/math/geometry.h" -#include "core/math/vector3.h" -#include "core/print_string.h" -#include "core/variant.h" - -typedef uint32_t OctreeElementID; - #define OCTREE_ELEMENT_INVALID_ID 0 #define OCTREE_SIZE_LIMIT 1e15 - -template -class Octree { -public: - typedef void *(*PairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int); - typedef void (*UnpairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int, void *); - -private: - enum { - - NEG = 0, - POS = 1, - }; - - enum { - OCTANT_NX_NY_NZ, - OCTANT_PX_NY_NZ, - OCTANT_NX_PY_NZ, - OCTANT_PX_PY_NZ, - OCTANT_NX_NY_PZ, - OCTANT_PX_NY_PZ, - OCTANT_NX_PY_PZ, - OCTANT_PX_PY_PZ - }; - - struct PairKey { - - union { - struct { - OctreeElementID A; - OctreeElementID B; - }; - uint64_t key; - }; - - _FORCE_INLINE_ bool operator<(const PairKey &p_pair) const { - - return key < p_pair.key; - } - - _FORCE_INLINE_ PairKey(OctreeElementID p_A, OctreeElementID p_B) { - - if (p_A < p_B) { - - A = p_A; - B = p_B; - } else { - - B = p_A; - A = p_B; - } - } - - _FORCE_INLINE_ PairKey() {} - }; - - struct Element; - - struct Octant { - - // cached for FAST plane check - AABB aabb; - - uint64_t last_pass; - Octant *parent; - Octant *children[8]; - - int children_count; // cache for amount of childrens (fast check for removal) - int parent_index; // cache for parent index (fast check for removal) - - List pairable_elements; - List elements; - - Octant() { - children_count = 0; - parent_index = -1; - last_pass = 0; - parent = NULL; - for (int i = 0; i < 8; i++) - children[i] = NULL; - } - - ~Octant() { - - /* - for (int i=0;i<8;i++) - memdelete_notnull(children[i]); - */ - } - }; - - struct PairData; - - struct Element { - - Octree *octree; - - T *userdata; - int subindex; - bool pairable; - uint32_t pairable_mask; - uint32_t pairable_type; - - uint64_t last_pass; - OctreeElementID _id; - Octant *common_parent; - - AABB aabb; - AABB container_aabb; - - List pair_list; - - struct OctantOwner { - - Octant *octant; - typename List::Element *E; - }; // an element can be in max 8 octants - - List octant_owners; - - Element() { - last_pass = 0; - _id = 0; - pairable = false; - subindex = 0; - userdata = 0; - octree = 0; - pairable_mask = 0; - pairable_type = 0; - common_parent = NULL; - } - }; - - struct PairData { - - int refcount; - bool intersect; - Element *A, *B; - void *ud; - typename List::Element *eA, *eB; - }; - - typedef Map, AL> ElementMap; - typedef Map, AL> PairMap; - ElementMap element_map; - PairMap pair_map; - - PairCallback pair_callback; - UnpairCallback unpair_callback; - void *pair_callback_userdata; - void *unpair_callback_userdata; - - OctreeElementID last_element_id; - uint64_t pass; - - real_t unit_size; - Octant *root; - int octant_count; - int pair_count; - - _FORCE_INLINE_ void _pair_check(PairData *p_pair) { - - bool intersect = p_pair->A->aabb.intersects_inclusive(p_pair->B->aabb); - - if (intersect != p_pair->intersect) { - - if (intersect) { - - if (pair_callback) { - p_pair->ud = pair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex); - } - pair_count++; - } else { - - if (unpair_callback) { - unpair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex, p_pair->ud); - } - pair_count--; - } - - p_pair->intersect = intersect; - } - } - - _FORCE_INLINE_ void _pair_reference(Element *p_A, Element *p_B) { - - if (p_A == p_B || (p_A->userdata == p_B->userdata && p_A->userdata)) - return; - - if (!(p_A->pairable_type & p_B->pairable_mask) && - !(p_B->pairable_type & p_A->pairable_mask)) - return; // none can pair with none - - PairKey key(p_A->_id, p_B->_id); - typename PairMap::Element *E = pair_map.find(key); - - if (!E) { - - PairData pdata; - pdata.refcount = 1; - pdata.A = p_A; - pdata.B = p_B; - pdata.intersect = false; - E = pair_map.insert(key, pdata); - E->get().eA = p_A->pair_list.push_back(&E->get()); - E->get().eB = p_B->pair_list.push_back(&E->get()); - - /* - if (pair_callback) - pair_callback(pair_callback_userdata,p_A->userdata,p_B->userdata); - */ - } else { - - E->get().refcount++; - } - } - - _FORCE_INLINE_ void _pair_unreference(Element *p_A, Element *p_B) { - - if (p_A == p_B) - return; - - PairKey key(p_A->_id, p_B->_id); - typename PairMap::Element *E = pair_map.find(key); - if (!E) { - return; // no pair - } - - E->get().refcount--; - - if (E->get().refcount == 0) { - // bye pair - - if (E->get().intersect) { - if (unpair_callback) { - unpair_callback(pair_callback_userdata, p_A->_id, p_A->userdata, p_A->subindex, p_B->_id, p_B->userdata, p_B->subindex, E->get().ud); - } - - pair_count--; - } - - if (p_A == E->get().B) { - //may be reaching inverted - SWAP(p_A, p_B); - } - - p_A->pair_list.erase(E->get().eA); - p_B->pair_list.erase(E->get().eB); - pair_map.erase(E); - } - } - - _FORCE_INLINE_ void _element_check_pairs(Element *p_element) { - - typename List::Element *E = p_element->pair_list.front(); - while (E) { - - _pair_check(E->get()); - E = E->next(); - } - } - - _FORCE_INLINE_ void _optimize() { - - while (root && root->children_count < 2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) { - - Octant *new_root = NULL; - if (root->children_count == 1) { - - for (int i = 0; i < 8; i++) { - - if (root->children[i]) { - new_root = root->children[i]; - root->children[i] = NULL; - break; - } - } - ERR_FAIL_COND(!new_root); - new_root->parent = NULL; - new_root->parent_index = -1; - } - - memdelete_allocator(root); - octant_count--; - root = new_root; - } - } - - void _insert_element(Element *p_element, Octant *p_octant); - void _ensure_valid_root(const AABB &p_aabb); - bool _remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit = NULL); - void _remove_element(Element *p_element); - void _pair_element(Element *p_element, Octant *p_octant); - void _unpair_element(Element *p_element, Octant *p_octant); - - struct _CullConvexData { - - const Plane *planes; - int plane_count; - const Vector3 *points; - int point_count; - T **result_array; - int *result_idx; - int result_max; - uint32_t mask; - }; - - void _cull_convex(Octant *p_octant, _CullConvexData *p_cull); - void _cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); - void _cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); - void _cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); - - void _remove_tree(Octant *p_octant) { - - if (!p_octant) - return; - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) - _remove_tree(p_octant->children[i]); - } - - memdelete_allocator(p_octant); - } - -public: - OctreeElementID create(T *p_userdata, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1); - void move(OctreeElementID p_id, const AABB &p_aabb); - void set_pairable(OctreeElementID p_id, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1); - void erase(OctreeElementID p_id); - - bool is_pairable(OctreeElementID p_id) const; - T *get(OctreeElementID p_id) const; - int get_subindex(OctreeElementID p_id) const; - - int cull_convex(const Vector &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF); - int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); - int cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); - - int cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); - - void set_pair_callback(PairCallback p_callback, void *p_userdata); - void set_unpair_callback(UnpairCallback p_callback, void *p_userdata); - - int get_octant_count() const { return octant_count; } - int get_pair_count() const { return pair_count; } - Octree(real_t p_unit_size = 1.0); - ~Octree() { _remove_tree(root); } -}; - -/* PRIVATE FUNCTIONS */ - -template -T *Octree::get(OctreeElementID p_id) const { - const typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, NULL); - return E->get().userdata; -} - -template -bool Octree::is_pairable(OctreeElementID p_id) const { - - const typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, false); - return E->get().pairable; -} - -template -int Octree::get_subindex(OctreeElementID p_id) const { - - const typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND_V(!E, -1); - return E->get().subindex; -} - -#define OCTREE_DIVISOR 4 - -template -void Octree::_insert_element(Element *p_element, Octant *p_octant) { - - real_t element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues - - if (p_octant->aabb.size.x / OCTREE_DIVISOR < element_size) { - //if (p_octant->aabb.size.x*0.5 < element_size) { - - /* at smallest possible size for the element */ - typename Element::OctantOwner owner; - owner.octant = p_octant; - - if (use_pairs && p_element->pairable) { - - p_octant->pairable_elements.push_back(p_element); - owner.E = p_octant->pairable_elements.back(); - } else { - - p_octant->elements.push_back(p_element); - owner.E = p_octant->elements.back(); - } - - p_element->octant_owners.push_back(owner); - - if (p_element->common_parent == NULL) { - p_element->common_parent = p_octant; - p_element->container_aabb = p_octant->aabb; - } else { - p_element->container_aabb.merge_with(p_octant->aabb); - } - - if (use_pairs && p_octant->children_count > 0) { - - pass++; //elements below this only get ONE reference added - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) { - _pair_element(p_element, p_octant->children[i]); - } - } - } - } else { - /* not big enough, send it to subitems */ - int splits = 0; - bool candidate = p_element->common_parent == NULL; - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) { - /* element exists, go straight to it */ - if (p_octant->children[i]->aabb.intersects_inclusive(p_element->aabb)) { - _insert_element(p_element, p_octant->children[i]); - splits++; - } - } else { - /* check against AABB where child should be */ - - AABB aabb = p_octant->aabb; - aabb.size *= 0.5; - - if (i & 1) - aabb.position.x += aabb.size.x; - if (i & 2) - aabb.position.y += aabb.size.y; - if (i & 4) - aabb.position.z += aabb.size.z; - - if (aabb.intersects_inclusive(p_element->aabb)) { - /* if actually intersects, create the child */ - - Octant *child = memnew_allocator(Octant, AL); - p_octant->children[i] = child; - child->parent = p_octant; - child->parent_index = i; - - child->aabb = aabb; - - p_octant->children_count++; - - _insert_element(p_element, child); - octant_count++; - splits++; - } - } - } - - if (candidate && splits > 1) { - - p_element->common_parent = p_octant; - } - } - - if (use_pairs) { - - typename List::Element *E = p_octant->pairable_elements.front(); - - while (E) { - _pair_reference(p_element, E->get()); - E = E->next(); - } - - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - _pair_reference(p_element, E->get()); - E = E->next(); - } - } - } -} - -template -void Octree::_ensure_valid_root(const AABB &p_aabb) { - - if (!root) { - // octre is empty - - AABB base(Vector3(), Vector3(1.0, 1.0, 1.0) * unit_size); - - while (!base.encloses(p_aabb)) { - - if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) { - /* grow towards positive */ - base.size *= 2.0; - } else { - base.position -= base.size; - base.size *= 2.0; - } - } - - root = memnew_allocator(Octant, AL); - - root->parent = NULL; - root->parent_index = -1; - root->aabb = base; - - octant_count++; - - } else { - - AABB base = root->aabb; - - while (!base.encloses(p_aabb)) { - - ERR_FAIL_COND_MSG(base.size.x > OCTREE_SIZE_LIMIT, "Octree upper size limit reached, does the AABB supplied contain NAN?"); - - Octant *gp = memnew_allocator(Octant, AL); - octant_count++; - root->parent = gp; - - if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) { - /* grow towards positive */ - base.size *= 2.0; - gp->aabb = base; - gp->children[0] = root; - root->parent_index = 0; - } else { - base.position -= base.size; - base.size *= 2.0; - gp->aabb = base; - gp->children[(1 << 0) | (1 << 1) | (1 << 2)] = root; // add at all-positive - root->parent_index = (1 << 0) | (1 << 1) | (1 << 2); - } - - gp->children_count = 1; - root = gp; - } - } -} - -template -bool Octree::_remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit) { - - bool octant_removed = false; - - while (true) { - - // check all exit conditions - - if (p_octant == p_limit) // reached limit, nothing to erase, exit - return octant_removed; - - bool unpaired = false; - - if (use_pairs && p_octant->last_pass != pass) { - // check whether we should unpair stuff - // always test pairable - typename List::Element *E = p_octant->pairable_elements.front(); - while (E) { - _pair_unreference(p_element, E->get()); - E = E->next(); - } - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - _pair_unreference(p_element, E->get()); - E = E->next(); - } - } - p_octant->last_pass = pass; - unpaired = true; - } - - bool removed = false; - - Octant *parent = p_octant->parent; - - if (p_octant->children_count == 0 && p_octant->elements.empty() && p_octant->pairable_elements.empty()) { - - // erase octant - - if (p_octant == root) { // won't have a parent, just erase - - root = NULL; - } else { - ERR_FAIL_INDEX_V(p_octant->parent_index, 8, octant_removed); - - parent->children[p_octant->parent_index] = NULL; - parent->children_count--; - } - - memdelete_allocator(p_octant); - octant_count--; - removed = true; - octant_removed = true; - } - - if (!removed && !unpaired) - return octant_removed; // no reason to keep going up anymore! was already visited and was not removed - - p_octant = parent; - } - - return octant_removed; -} - -template -void Octree::_unpair_element(Element *p_element, Octant *p_octant) { - - // always test pairable - typename List::Element *E = p_octant->pairable_elements.front(); - while (E) { - if (E->get()->last_pass != pass) { // only remove ONE reference - _pair_unreference(p_element, E->get()); - E->get()->last_pass = pass; - } - E = E->next(); - } - - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - if (E->get()->last_pass != pass) { // only remove ONE reference - _pair_unreference(p_element, E->get()); - E->get()->last_pass = pass; - } - E = E->next(); - } - } - - p_octant->last_pass = pass; - - if (p_octant->children_count == 0) - return; // small optimization for leafs - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) - _unpair_element(p_element, p_octant->children[i]); - } -} - -template -void Octree::_pair_element(Element *p_element, Octant *p_octant) { - - // always test pairable - - typename List::Element *E = p_octant->pairable_elements.front(); - - while (E) { - - if (E->get()->last_pass != pass) { // only get ONE reference - _pair_reference(p_element, E->get()); - E->get()->last_pass = pass; - } - E = E->next(); - } - - if (p_element->pairable) { - // and always test non-pairable if element is pairable - E = p_octant->elements.front(); - while (E) { - if (E->get()->last_pass != pass) { // only get ONE reference - _pair_reference(p_element, E->get()); - E->get()->last_pass = pass; - } - E = E->next(); - } - } - p_octant->last_pass = pass; - - if (p_octant->children_count == 0) - return; // small optimization for leafs - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i]) - _pair_element(p_element, p_octant->children[i]); - } -} - -template -void Octree::_remove_element(Element *p_element) { - - pass++; // will do a new pass for this - - typename List::Element *I = p_element->octant_owners.front(); - - /* FIRST remove going up normally */ - for (; I; I = I->next()) { - - Octant *o = I->get().octant; - - if (!use_pairs) // small speedup - o->elements.erase(I->get().E); - - _remove_element_from_octant(p_element, o); - } - - /* THEN remove going down */ - - I = p_element->octant_owners.front(); - - if (use_pairs) { - - for (; I; I = I->next()) { - - Octant *o = I->get().octant; - - // erase children pairs, they are erased ONCE even if repeated - pass++; - for (int i = 0; i < 8; i++) { - - if (o->children[i]) - _unpair_element(p_element, o->children[i]); - } - - if (p_element->pairable) - o->pairable_elements.erase(I->get().E); - else - o->elements.erase(I->get().E); - } - } - - p_element->octant_owners.clear(); - - if (use_pairs) { - - int remaining = p_element->pair_list.size(); - //p_element->pair_list.clear(); - ERR_FAIL_COND(remaining); - } -} - -template -OctreeElementID Octree::create(T *p_userdata, const AABB &p_aabb, int p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { - -// check for AABB validity -#ifdef DEBUG_ENABLED - ERR_FAIL_COND_V(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15, 0); - ERR_FAIL_COND_V(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15, 0); - ERR_FAIL_COND_V(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15, 0); - ERR_FAIL_COND_V(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0, 0); - ERR_FAIL_COND_V(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0, 0); - ERR_FAIL_COND_V(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0, 0); - ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.x), 0); - ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.y), 0); - ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.z), 0); - -#endif - typename ElementMap::Element *E = element_map.insert(last_element_id++, - Element()); - Element &e = E->get(); - - e.aabb = p_aabb; - e.userdata = p_userdata; - e.subindex = p_subindex; - e.last_pass = 0; - e.octree = this; - e.pairable = p_pairable; - e.pairable_type = p_pairable_type; - e.pairable_mask = p_pairable_mask; - e._id = last_element_id - 1; - - if (!e.aabb.has_no_surface()) { - _ensure_valid_root(p_aabb); - _insert_element(&e, root); - if (use_pairs) - _element_check_pairs(&e); - } - - return last_element_id - 1; -} - -template -void Octree::move(OctreeElementID p_id, const AABB &p_aabb) { - -#ifdef DEBUG_ENABLED - // check for AABB validity - ERR_FAIL_COND(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15); - ERR_FAIL_COND(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15); - ERR_FAIL_COND(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15); - ERR_FAIL_COND(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0); - ERR_FAIL_COND(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0); - ERR_FAIL_COND(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0); - ERR_FAIL_COND(Math::is_nan(p_aabb.size.x)); - ERR_FAIL_COND(Math::is_nan(p_aabb.size.y)); - ERR_FAIL_COND(Math::is_nan(p_aabb.size.z)); -#endif - typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - Element &e = E->get(); - - bool old_has_surf = !e.aabb.has_no_surface(); - bool new_has_surf = !p_aabb.has_no_surface(); - - if (old_has_surf != new_has_surf) { - - if (old_has_surf) { - _remove_element(&e); // removing - e.common_parent = NULL; - e.aabb = AABB(); - _optimize(); - } else { - _ensure_valid_root(p_aabb); // inserting - e.common_parent = NULL; - e.aabb = p_aabb; - _insert_element(&e, root); - if (use_pairs) - _element_check_pairs(&e); - } - - return; - } - - if (!old_has_surf) // doing nothing - return; - - // it still is enclosed in the same AABB it was assigned to - if (e.container_aabb.encloses(p_aabb)) { - - e.aabb = p_aabb; - if (use_pairs) - _element_check_pairs(&e); // must check pairs anyway - - return; - } - - AABB combined = e.aabb; - combined.merge_with(p_aabb); - _ensure_valid_root(combined); - - ERR_FAIL_COND(e.octant_owners.front() == NULL); - - /* FIND COMMON PARENT */ - - List owners = e.octant_owners; // save the octant owners - Octant *common_parent = e.common_parent; - ERR_FAIL_COND(!common_parent); - - //src is now the place towards where insertion is going to happen - pass++; - - while (common_parent && !common_parent->aabb.encloses(p_aabb)) - common_parent = common_parent->parent; - - ERR_FAIL_COND(!common_parent); - - //prepare for reinsert - e.octant_owners.clear(); - e.common_parent = NULL; - e.aabb = p_aabb; - - _insert_element(&e, common_parent); // reinsert from this point - - pass++; - - for (typename List::Element *F = owners.front(); F;) { - - Octant *o = F->get().octant; - typename List::Element *N = F->next(); - - /* - if (!use_pairs) - o->elements.erase( F->get().E ); - */ - - if (use_pairs && e.pairable) - o->pairable_elements.erase(F->get().E); - else - o->elements.erase(F->get().E); - - if (_remove_element_from_octant(&e, o, common_parent->parent)) { - - owners.erase(F); - } - - F = N; - } - - if (use_pairs) { - //unpair child elements in anything that survived - for (typename List::Element *F = owners.front(); F; F = F->next()) { - - Octant *o = F->get().octant; - - // erase children pairs, unref ONCE - pass++; - for (int i = 0; i < 8; i++) { - - if (o->children[i]) - _unpair_element(&e, o->children[i]); - } - } - - _element_check_pairs(&e); - } - - _optimize(); -} - -template -void Octree::set_pairable(OctreeElementID p_id, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { - - typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (p_pairable == e.pairable && e.pairable_type == p_pairable_type && e.pairable_mask == p_pairable_mask) - return; // no changes, return - - if (!e.aabb.has_no_surface()) { - _remove_element(&e); - } - - e.pairable = p_pairable; - e.pairable_type = p_pairable_type; - e.pairable_mask = p_pairable_mask; - e.common_parent = NULL; - - if (!e.aabb.has_no_surface()) { - _ensure_valid_root(e.aabb); - _insert_element(&e, root); - if (use_pairs) - _element_check_pairs(&e); - } -} - -template -void Octree::erase(OctreeElementID p_id) { - - typename ElementMap::Element *E = element_map.find(p_id); - ERR_FAIL_COND(!E); - - Element &e = E->get(); - - if (!e.aabb.has_no_surface()) { - - _remove_element(&e); - } - - element_map.erase(p_id); - _optimize(); -} - -template -void Octree::_cull_convex(Octant *p_octant, _CullConvexData *p_cull) { - - if (*p_cull->result_idx == p_cull->result_max) - return; //pointless - - if (!p_octant->elements.empty()) { - - typename List::Element *I; - I = p_octant->elements.front(); - - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) - continue; - e->last_pass = pass; - - if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - if (*p_cull->result_idx < p_cull->result_max) { - p_cull->result_array[*p_cull->result_idx] = e->userdata; - (*p_cull->result_idx)++; - } else { - - return; // pointless to continue - } - } - } - } - - if (use_pairs && !p_octant->pairable_elements.empty()) { - - typename List::Element *I; - I = p_octant->pairable_elements.front(); - - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) - continue; - e->last_pass = pass; - - if (e->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - - if (*p_cull->result_idx < p_cull->result_max) { - - p_cull->result_array[*p_cull->result_idx] = e->userdata; - (*p_cull->result_idx)++; - } else { - - return; // pointless to continue - } - } - } - } - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { - _cull_convex(p_octant->children[i], p_cull); - } - } -} - -template -void Octree::_cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (*p_result_idx == p_result_max) - return; //pointless - - if (!p_octant->elements.empty()) { - - typename List::Element *I; - I = p_octant->elements.front(); - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) - continue; - e->last_pass = pass; - - if (p_aabb.intersects_inclusive(e->aabb)) { - - if (*p_result_idx < p_result_max) { - - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) - p_subindex_array[*p_result_idx] = e->subindex; - - (*p_result_idx)++; - } else { - - return; // pointless to continue - } - } - } - } - - if (use_pairs && !p_octant->pairable_elements.empty()) { - - typename List::Element *I; - I = p_octant->pairable_elements.front(); - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) - continue; - e->last_pass = pass; - - if (p_aabb.intersects_inclusive(e->aabb)) { - - if (*p_result_idx < p_result_max) { - - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) - p_subindex_array[*p_result_idx] = e->subindex; - (*p_result_idx)++; - } else { - - return; // pointless to continue - } - } - } - } - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_inclusive(p_aabb)) { - _cull_aabb(p_octant->children[i], p_aabb, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); - } - } -} - -template -void Octree::_cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (*p_result_idx == p_result_max) - return; //pointless - - if (!p_octant->elements.empty()) { - - typename List::Element *I; - I = p_octant->elements.front(); - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) - continue; - e->last_pass = pass; - - if (e->aabb.intersects_segment(p_from, p_to)) { - - if (*p_result_idx < p_result_max) { - - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) - p_subindex_array[*p_result_idx] = e->subindex; - (*p_result_idx)++; - - } else { - - return; // pointless to continue - } - } - } - } - - if (use_pairs && !p_octant->pairable_elements.empty()) { - - typename List::Element *I; - I = p_octant->pairable_elements.front(); - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) - continue; - - e->last_pass = pass; - - if (e->aabb.intersects_segment(p_from, p_to)) { - - if (*p_result_idx < p_result_max) { - - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) - p_subindex_array[*p_result_idx] = e->subindex; - - (*p_result_idx)++; - - } else { - - return; // pointless to continue - } - } - } - } - - for (int i = 0; i < 8; i++) { - - if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_segment(p_from, p_to)) { - _cull_segment(p_octant->children[i], p_from, p_to, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); - } - } -} - -template -void Octree::_cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (*p_result_idx == p_result_max) - return; //pointless - - if (!p_octant->elements.empty()) { - - typename List::Element *I; - I = p_octant->elements.front(); - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) - continue; - e->last_pass = pass; - - if (e->aabb.has_point(p_point)) { - - if (*p_result_idx < p_result_max) { - - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) - p_subindex_array[*p_result_idx] = e->subindex; - (*p_result_idx)++; - - } else { - - return; // pointless to continue - } - } - } - } - - if (use_pairs && !p_octant->pairable_elements.empty()) { - - typename List::Element *I; - I = p_octant->pairable_elements.front(); - for (; I; I = I->next()) { - - Element *e = I->get(); - - if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) - continue; - - e->last_pass = pass; - - if (e->aabb.has_point(p_point)) { - - if (*p_result_idx < p_result_max) { - - p_result_array[*p_result_idx] = e->userdata; - if (p_subindex_array) - p_subindex_array[*p_result_idx] = e->subindex; - - (*p_result_idx)++; - - } else { - - return; // pointless to continue - } - } - } - } - - for (int i = 0; i < 8; i++) { - - //could be optimized.. - if (p_octant->children[i] && p_octant->children[i]->aabb.has_point(p_point)) { - _cull_point(p_octant->children[i], p_point, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); - } - } -} - -template -int Octree::cull_convex(const Vector &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask) { - - if (!root || p_convex.size() == 0) - return 0; - - Vector convex_points = Geometry::compute_convex_mesh_points(&p_convex[0], p_convex.size()); - if (convex_points.size() == 0) - return 0; - - int result_count = 0; - pass++; - _CullConvexData cdata; - cdata.planes = &p_convex[0]; - cdata.plane_count = p_convex.size(); - cdata.points = &convex_points[0]; - cdata.point_count = convex_points.size(); - cdata.result_array = p_result_array; - cdata.result_max = p_result_max; - cdata.result_idx = &result_count; - cdata.mask = p_mask; - - _cull_convex(root, &cdata); - - return result_count; -} - -template -int Octree::cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (!root) - return 0; - - int result_count = 0; - pass++; - _cull_aabb(root, p_aabb, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); - - return result_count; -} - -template -int Octree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (!root) - return 0; - - int result_count = 0; - pass++; - _cull_segment(root, p_from, p_to, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); - - return result_count; -} - -template -int Octree::cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { - - if (!root) - return 0; - - int result_count = 0; - pass++; - _cull_point(root, p_point, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); - - return result_count; -} - -template -void Octree::set_pair_callback(PairCallback p_callback, void *p_userdata) { - - pair_callback = p_callback; - pair_callback_userdata = p_userdata; -} -template -void Octree::set_unpair_callback(UnpairCallback p_callback, void *p_userdata) { - - unpair_callback = p_callback; - unpair_callback_userdata = p_userdata; -} - -template -Octree::Octree(real_t p_unit_size) { - - last_element_id = 1; - pass = 1; - unit_size = p_unit_size; - root = NULL; - - octant_count = 0; - pair_count = 0; - - pair_callback = NULL; - unpair_callback = NULL; - pair_callback_userdata = NULL; - unpair_callback_userdata = NULL; -} - -#endif +#define OCTREE_DEFAULT_OCTANT_LIMIT 0 + +// We want 2 versions of the octree, Octree +// and Octree_CL which uses cached lists (optimized). +// we don't want to use the extra memory of cached lists on +// the non cached list version, so we use macros +// to avoid duplicating the code which is in octree_definition. +// The name of the class is overridden and the changes with the define +// OCTREE_USE_CACHED_LISTS. + +// The two classes can be used identically but one contains the cached +// list optimization. + +// standard octree +#define OCTREE_CLASS_NAME Octree +#undef OCTREE_USE_CACHED_LISTS +#include "octree_definition.inc" +#undef OCTREE_CLASS_NAME + +// cached lists octree +#define OCTREE_CLASS_NAME Octree_CL +#define OCTREE_USE_CACHED_LISTS +#include "octree_definition.inc" +#undef OCTREE_CLASS_NAME + +#endif // OCTREE_H diff --git a/core/math/octree_definition.inc b/core/math/octree_definition.inc new file mode 100644 index 00000000000..27982666894 --- /dev/null +++ b/core/math/octree_definition.inc @@ -0,0 +1,1763 @@ +// DO NOT ADD INCLUDE GUARDS OR PRAGMA ONCE. +// This file will be included more than once. + +/*************************************************************************/ +/* octree_definition.inc */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "core/list.h" +#include "core/local_vector.h" +#include "core/map.h" +#include "core/math/aabb.h" +#include "core/math/geometry.h" +#include "core/math/vector3.h" +#include "core/print_string.h" +#include "core/variant.h" + +typedef uint32_t OctreeElementID; + +// macro to reduce boiler plate code when providing function implementations +#define OCTREE_FUNC(RET_VALUE) template \ +RET_VALUE OCTREE_CLASS_NAME + +#define OCTREE_FUNC_CONSTRUCTOR template \ +OCTREE_CLASS_NAME + +template +class OCTREE_CLASS_NAME { +public: + typedef void *(*PairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int); + typedef void (*UnpairCallback)(void *, OctreeElementID, T *, int, OctreeElementID, T *, int, void *); + +private: + enum { + + NEG = 0, + POS = 1, + }; + + enum { + OCTANT_NX_NY_NZ, + OCTANT_PX_NY_NZ, + OCTANT_NX_PY_NZ, + OCTANT_PX_PY_NZ, + OCTANT_NX_NY_PZ, + OCTANT_PX_NY_PZ, + OCTANT_NX_PY_PZ, + OCTANT_PX_PY_PZ + }; + + struct PairKey { + + union { + struct { + OctreeElementID A; + OctreeElementID B; + }; + uint64_t key; + }; + + _FORCE_INLINE_ bool operator<(const PairKey &p_pair) const { + + return key < p_pair.key; + } + + _FORCE_INLINE_ PairKey(OctreeElementID p_A, OctreeElementID p_B) { + + if (p_A < p_B) { + + A = p_A; + B = p_B; + } else { + + B = p_A; + A = p_B; + } + } + + _FORCE_INLINE_ PairKey() {} + }; + + struct Element; + +#ifdef OCTREE_USE_CACHED_LISTS + // instead of iterating the linked list every time within octants, + // we can cache a linear list of prepared elements containing essential data + // for fast traversal, and rebuild it only when an octant changes. + struct CachedList { + LocalVector aabbs; + LocalVector elements; + + void update(List &eles) { + // make sure local vector doesn't delete the memory + // no need to be thrashing allocations + aabbs.clear(); + elements.clear(); + + typename List::Element *E = eles.front(); + while (E) { + Element *e = E->get(); + aabbs.push_back(e->aabb); + elements.push_back(e); + E = E->next(); + } + } + }; +#endif + + struct Octant { + + // cached for FAST plane check + AABB aabb; + + uint64_t last_pass; + Octant *parent; + Octant *children[8]; + + int children_count; // cache for amount of childrens (fast check for removal) + int parent_index; // cache for parent index (fast check for removal) + + List pairable_elements; + List elements; + +#ifdef OCTREE_USE_CACHED_LISTS + // cached lists are linear in memory so are faster than using linked list + CachedList clist_pairable; + CachedList clist; + + // use dirty flag to indicate when cached lists need updating + // this avoids having to update the cached list on lots of octants + // if nothing is moving in them. + bool dirty; + + void update_cached_lists() { + if (!dirty) { +#ifdef TOOLS_ENABLED +//#define OCTREE_CACHED_LIST_ERROR_CHECKS +#endif +#ifdef OCTREE_CACHED_LIST_ERROR_CHECKS + // debug - this will slow down performance a lot, + // only enable these error checks for testing that the cached + // lists are up to date. + int hash_before_P = clist_pairable.aabbs.size(); + int hash_before_N = clist.aabbs.size(); + clist_pairable.update(pairable_elements); + clist.update(elements); + int hash_after_P = clist_pairable.aabbs.size(); + int hash_after_N = clist.aabbs.size(); + + ERR_FAIL_COND(hash_before_P != hash_after_P); + ERR_FAIL_COND(hash_before_N != hash_after_N); +#endif + return; + } + clist_pairable.update(pairable_elements); + clist.update(elements); + dirty = false; + } +#endif + + Octant() { + children_count = 0; + parent_index = -1; + last_pass = 0; + parent = NULL; +#ifdef OCTREE_USE_CACHED_LISTS + dirty = true; +#endif + for (int i = 0; i < 8; i++) + children[i] = NULL; + } + + ~Octant() { + + /* + for (int i=0;i<8;i++) + memdelete_notnull(children[i]); + */ + } + }; + + struct PairData; + + struct Element { + + OCTREE_CLASS_NAME *octree; + + T *userdata; + int subindex; + bool pairable; + uint32_t pairable_mask; + uint32_t pairable_type; + + uint64_t last_pass; + OctreeElementID _id; + Octant *common_parent; + + AABB aabb; + AABB container_aabb; + + List pair_list; + + struct OctantOwner { + + Octant *octant; + typename List::Element *E; + }; // an element can be in max 8 octants + + List octant_owners; + +#ifdef OCTREE_USE_CACHED_LISTS + // when moving we need make all owner octants dirty, because the AABB can change. + void moving() { + for (typename List::Element *F = octant_owners.front(); F;) { + Octant *o = F->get().octant; + o->dirty = true; + F = F->next(); + } + } +#endif + + Element() { + last_pass = 0; + _id = 0; + pairable = false; + subindex = 0; + userdata = 0; + octree = 0; + pairable_mask = 0; + pairable_type = 0; + common_parent = NULL; + } + }; + + struct PairData { + + int refcount; + bool intersect; + Element *A, *B; + void *ud; + typename List::Element *eA, *eB; + }; + + typedef Map, AL> ElementMap; + typedef Map, AL> PairMap; + ElementMap element_map; + PairMap pair_map; + + PairCallback pair_callback; + UnpairCallback unpair_callback; + void *pair_callback_userdata; + void *unpair_callback_userdata; + + OctreeElementID last_element_id; + uint64_t pass; + + real_t unit_size; + Octant *root; + int octant_count; + int pair_count; + int octant_elements_limit; + + _FORCE_INLINE_ void _pair_check(PairData *p_pair) { + + bool intersect = p_pair->A->aabb.intersects_inclusive(p_pair->B->aabb); + + if (intersect != p_pair->intersect) { + + if (intersect) { + + if (pair_callback) { + p_pair->ud = pair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex); + } + pair_count++; + } else { + + if (unpair_callback) { + unpair_callback(pair_callback_userdata, p_pair->A->_id, p_pair->A->userdata, p_pair->A->subindex, p_pair->B->_id, p_pair->B->userdata, p_pair->B->subindex, p_pair->ud); + } + pair_count--; + } + + p_pair->intersect = intersect; + } + } + + _FORCE_INLINE_ void _pair_reference(Element *p_A, Element *p_B) { + + if (p_A == p_B || (p_A->userdata == p_B->userdata && p_A->userdata)) + return; + + if (!(p_A->pairable_type & p_B->pairable_mask) && + !(p_B->pairable_type & p_A->pairable_mask)) + return; // none can pair with none + + PairKey key(p_A->_id, p_B->_id); + typename PairMap::Element *E = pair_map.find(key); + + if (!E) { + + PairData pdata; + pdata.refcount = 1; + pdata.A = p_A; + pdata.B = p_B; + pdata.intersect = false; + E = pair_map.insert(key, pdata); + E->get().eA = p_A->pair_list.push_back(&E->get()); + E->get().eB = p_B->pair_list.push_back(&E->get()); + + /* + if (pair_callback) + pair_callback(pair_callback_userdata,p_A->userdata,p_B->userdata); + */ + } else { + + E->get().refcount++; + } + } + + _FORCE_INLINE_ void _pair_unreference(Element *p_A, Element *p_B) { + + if (p_A == p_B) + return; + + PairKey key(p_A->_id, p_B->_id); + typename PairMap::Element *E = pair_map.find(key); + if (!E) { + return; // no pair + } + + E->get().refcount--; + + if (E->get().refcount == 0) { + // bye pair + + if (E->get().intersect) { + if (unpair_callback) { + unpair_callback(pair_callback_userdata, p_A->_id, p_A->userdata, p_A->subindex, p_B->_id, p_B->userdata, p_B->subindex, E->get().ud); + } + + pair_count--; + } + + if (p_A == E->get().B) { + //may be reaching inverted + SWAP(p_A, p_B); + } + + p_A->pair_list.erase(E->get().eA); + p_B->pair_list.erase(E->get().eB); + pair_map.erase(E); + } + } + + _FORCE_INLINE_ void _element_check_pairs(Element *p_element) { + + typename List::Element *E = p_element->pair_list.front(); + while (E) { + + _pair_check(E->get()); + E = E->next(); + } + } + + _FORCE_INLINE_ void _optimize() { + + while (root && root->children_count < 2 && !root->elements.size() && !(use_pairs && root->pairable_elements.size())) { + + Octant *new_root = NULL; + if (root->children_count == 1) { + + for (int i = 0; i < 8; i++) { + + if (root->children[i]) { + new_root = root->children[i]; + root->children[i] = NULL; + break; + } + } + ERR_FAIL_COND(!new_root); + new_root->parent = NULL; + new_root->parent_index = -1; + } + + memdelete_allocator(root); + octant_count--; + root = new_root; + } + } + + void _insert_element(Element *p_element, Octant *p_octant); + void _ensure_valid_root(const AABB &p_aabb); + bool _remove_element_pair_and_remove_empty_octants(Element *p_element, Octant *p_octant, Octant *p_limit = NULL); + void _remove_element(Element *p_element); + void _pair_element(Element *p_element, Octant *p_octant); + void _unpair_element(Element *p_element, Octant *p_octant); + + struct _CullConvexData { + + const Plane *planes; + int plane_count; + const Vector3 *points; + int point_count; + T **result_array; + int *result_idx; + int result_max; + uint32_t mask; + }; + + void _cull_convex(Octant *p_octant, _CullConvexData *p_cull); + void _cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); + void _cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); + void _cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask); + + void _remove_tree(Octant *p_octant) { + + if (!p_octant) + return; + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i]) + _remove_tree(p_octant->children[i]); + } + + memdelete_allocator(p_octant); + } + +#ifdef TOOLS_ENABLED + String debug_aabb_to_string(const AABB &aabb) const; + void debug_octant(const Octant &oct, int depth = 0); +#endif + +public: + OctreeElementID create(T *p_userdata, const AABB &p_aabb = AABB(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1); + void move(OctreeElementID p_id, const AABB &p_aabb); + void set_pairable(OctreeElementID p_id, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t pairable_mask = 1); + void erase(OctreeElementID p_id); + + bool is_pairable(OctreeElementID p_id) const; + T *get(OctreeElementID p_id) const; + int get_subindex(OctreeElementID p_id) const; + + int cull_convex(const Vector &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF); + int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); + int cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); + + int cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = NULL, uint32_t p_mask = 0xFFFFFFFF); + + void set_pair_callback(PairCallback p_callback, void *p_userdata); + void set_unpair_callback(UnpairCallback p_callback, void *p_userdata); + + int get_octant_count() const { return octant_count; } + int get_pair_count() const { return pair_count; } + void set_octant_elements_limit(int p_limit) { octant_elements_limit = p_limit; } + + // just convenience for project settings, as users don't need to know exact numbers + void set_balance(float p_bal) // 0.0 is optimized for multiple tests, 1.0 is for multiple edits (moves etc) + { + float v = CLAMP(p_bal, 0.0f, 1.0f); + v *= v; + v *= v; + v *= 8096.0f; // these values have been found empirically + int l = 0 + v; + set_octant_elements_limit(l); + } + +#ifdef TOOLS_ENABLED + void debug_octants(); +#endif + + OCTREE_CLASS_NAME(real_t p_unit_size = 1.0); + ~OCTREE_CLASS_NAME() { _remove_tree(root); } +}; + +/* PRIVATE FUNCTIONS */ + +OCTREE_FUNC(T *)::get(OctreeElementID p_id) const { + const typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, NULL); + return E->get().userdata; +} + +OCTREE_FUNC(bool)::is_pairable(OctreeElementID p_id) const { + + const typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, false); + return E->get().pairable; +} + +OCTREE_FUNC(int)::get_subindex(OctreeElementID p_id) const { + + const typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND_V(!E, -1); + return E->get().subindex; +} + +#define OCTREE_DIVISOR 4 + +OCTREE_FUNC(void)::_insert_element(Element *p_element, Octant *p_octant) { + + real_t element_size = p_element->aabb.get_longest_axis_size() * 1.01; // avoid precision issues + + // don't create new child octants unless there is more than a certain number in + // this octant. This prevents runaway creation of too many octants, and is more efficient + // because brute force is faster up to a certain point. + bool can_split = true; + + if (p_element->pairable) { + if (p_octant->pairable_elements.size() < octant_elements_limit) + can_split = false; + } else { + if (p_octant->elements.size() < octant_elements_limit) + can_split = false; + } + + if (!can_split || (element_size > (p_octant->aabb.size.x / OCTREE_DIVISOR))) { + + /* at smallest possible size for the element */ + typename Element::OctantOwner owner; + owner.octant = p_octant; + + if (use_pairs && p_element->pairable) { + + p_octant->pairable_elements.push_back(p_element); + owner.E = p_octant->pairable_elements.back(); + } else { + + p_octant->elements.push_back(p_element); + owner.E = p_octant->elements.back(); + } +#ifdef OCTREE_USE_CACHED_LISTS + p_octant->dirty = true; +#endif + p_element->octant_owners.push_back(owner); + + if (p_element->common_parent == NULL) { + p_element->common_parent = p_octant; + p_element->container_aabb = p_octant->aabb; + } else { + p_element->container_aabb.merge_with(p_octant->aabb); + } + + if (use_pairs && p_octant->children_count > 0) { + + pass++; //elements below this only get ONE reference added + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i]) { + _pair_element(p_element, p_octant->children[i]); + } + } + } + } else { + /* not big enough, send it to subitems */ + int splits = 0; + bool candidate = p_element->common_parent == NULL; + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i]) { + /* element exists, go straight to it */ + if (p_octant->children[i]->aabb.intersects_inclusive(p_element->aabb)) { + _insert_element(p_element, p_octant->children[i]); + splits++; + } + } else { + /* check against AABB where child should be */ + + AABB aabb = p_octant->aabb; + aabb.size *= 0.5; + + if (i & 1) + aabb.position.x += aabb.size.x; + if (i & 2) + aabb.position.y += aabb.size.y; + if (i & 4) + aabb.position.z += aabb.size.z; + + if (aabb.intersects_inclusive(p_element->aabb)) { + /* if actually intersects, create the child */ + + Octant *child = memnew_allocator(Octant, AL); + p_octant->children[i] = child; + child->parent = p_octant; + child->parent_index = i; + + child->aabb = aabb; + + p_octant->children_count++; + + _insert_element(p_element, child); + octant_count++; + splits++; + } + } + } + + if (candidate && splits > 1) { + + p_element->common_parent = p_octant; + } + } + + if (use_pairs) { + + typename List::Element *E = p_octant->pairable_elements.front(); + + while (E) { + _pair_reference(p_element, E->get()); + E = E->next(); + } + + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E = p_octant->elements.front(); + while (E) { + _pair_reference(p_element, E->get()); + E = E->next(); + } + } + } +} + +OCTREE_FUNC(void)::_ensure_valid_root(const AABB &p_aabb) { + + if (!root) { + // octre is empty + + AABB base(Vector3(), Vector3(1.0, 1.0, 1.0) * unit_size); + + while (!base.encloses(p_aabb)) { + + if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) { + /* grow towards positive */ + base.size *= 2.0; + } else { + base.position -= base.size; + base.size *= 2.0; + } + } + + root = memnew_allocator(Octant, AL); + + root->parent = NULL; + root->parent_index = -1; + root->aabb = base; + + octant_count++; + + } else { + + AABB base = root->aabb; + + while (!base.encloses(p_aabb)) { + + ERR_FAIL_COND_MSG(base.size.x > OCTREE_SIZE_LIMIT, "Octree upper size limit reached, does the AABB supplied contain NAN?"); + + Octant *gp = memnew_allocator(Octant, AL); + octant_count++; + root->parent = gp; + + if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) { + /* grow towards positive */ + base.size *= 2.0; + gp->aabb = base; + gp->children[0] = root; + root->parent_index = 0; + } else { + base.position -= base.size; + base.size *= 2.0; + gp->aabb = base; + gp->children[(1 << 0) | (1 << 1) | (1 << 2)] = root; // add at all-positive + root->parent_index = (1 << 0) | (1 << 1) | (1 << 2); + } + + gp->children_count = 1; + root = gp; + } + } +} + +OCTREE_FUNC(bool)::_remove_element_pair_and_remove_empty_octants(Element *p_element, Octant *p_octant, Octant *p_limit) { + + bool octant_removed = false; + + while (true) { + + // check all exit conditions + + if (p_octant == p_limit) // reached limit, nothing to erase, exit + return octant_removed; + + bool unpaired = false; + + if (use_pairs && p_octant->last_pass != pass) { + // check whether we should unpair stuff + // always test pairable + typename List::Element *E = p_octant->pairable_elements.front(); + while (E) { + _pair_unreference(p_element, E->get()); + E = E->next(); + } + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E = p_octant->elements.front(); + while (E) { + _pair_unreference(p_element, E->get()); + E = E->next(); + } + } + p_octant->last_pass = pass; + unpaired = true; + } + + bool removed = false; + + Octant *parent = p_octant->parent; + + if (p_octant->children_count == 0 && p_octant->elements.empty() && p_octant->pairable_elements.empty()) { + + // erase octant + + if (p_octant == root) { // won't have a parent, just erase + + root = NULL; + } else { + ERR_FAIL_INDEX_V(p_octant->parent_index, 8, octant_removed); + + parent->children[p_octant->parent_index] = NULL; + parent->children_count--; + } + + memdelete_allocator(p_octant); + octant_count--; + removed = true; + octant_removed = true; + } + + if (!removed && !unpaired) + return octant_removed; // no reason to keep going up anymore! was already visited and was not removed + + p_octant = parent; + } + + return octant_removed; +} + +OCTREE_FUNC(void)::_unpair_element(Element *p_element, Octant *p_octant) { + + // always test pairable + typename List::Element *E = p_octant->pairable_elements.front(); + while (E) { + if (E->get()->last_pass != pass) { // only remove ONE reference + _pair_unreference(p_element, E->get()); + E->get()->last_pass = pass; + } + E = E->next(); + } + + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E = p_octant->elements.front(); + while (E) { + if (E->get()->last_pass != pass) { // only remove ONE reference + _pair_unreference(p_element, E->get()); + E->get()->last_pass = pass; + } + E = E->next(); + } + } + + p_octant->last_pass = pass; + + if (p_octant->children_count == 0) + return; // small optimization for leafs + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i]) + _unpair_element(p_element, p_octant->children[i]); + } +} + +OCTREE_FUNC(void)::_pair_element(Element *p_element, Octant *p_octant) { + + // always test pairable + + typename List::Element *E = p_octant->pairable_elements.front(); + + while (E) { + + if (E->get()->last_pass != pass) { // only get ONE reference + _pair_reference(p_element, E->get()); + E->get()->last_pass = pass; + } + E = E->next(); + } + + if (p_element->pairable) { + // and always test non-pairable if element is pairable + E = p_octant->elements.front(); + while (E) { + if (E->get()->last_pass != pass) { // only get ONE reference + _pair_reference(p_element, E->get()); + E->get()->last_pass = pass; + } + E = E->next(); + } + } + p_octant->last_pass = pass; + + if (p_octant->children_count == 0) + return; // small optimization for leafs + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i]) + _pair_element(p_element, p_octant->children[i]); + } +} + +OCTREE_FUNC(void)::_remove_element(Element *p_element) { + + pass++; // will do a new pass for this + + typename List::Element *I = p_element->octant_owners.front(); + + for (; I; I = I->next()) { + + Octant *o = I->get().octant; + + if (!use_pairs) { + o->elements.erase(I->get().E); + } else { + // erase children pairs, they are erased ONCE even if repeated + pass++; + for (int i = 0; i < 8; i++) { + if (o->children[i]) { + _unpair_element(p_element, o->children[i]); + } + } + + if (p_element->pairable) { + o->pairable_elements.erase(I->get().E); + } else { + o->elements.erase(I->get().E); + } + } + +#ifdef OCTREE_USE_CACHED_LISTS + o->dirty = true; +#endif + _remove_element_pair_and_remove_empty_octants(p_element, o); + } + + p_element->octant_owners.clear(); + + if (use_pairs) { + + int remaining = p_element->pair_list.size(); + //p_element->pair_list.clear(); + ERR_FAIL_COND(remaining); + } +} + +OCTREE_FUNC(OctreeElementID)::create(T *p_userdata, const AABB &p_aabb, int p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { + +// check for AABB validity +#ifdef DEBUG_ENABLED + ERR_FAIL_COND_V(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15, 0); + ERR_FAIL_COND_V(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15, 0); + ERR_FAIL_COND_V(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15, 0); + ERR_FAIL_COND_V(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0, 0); + ERR_FAIL_COND_V(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0, 0); + ERR_FAIL_COND_V(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0, 0); + ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.x), 0); + ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.y), 0); + ERR_FAIL_COND_V(Math::is_nan(p_aabb.size.z), 0); + +#endif + typename ElementMap::Element *E = element_map.insert(last_element_id++, + Element()); + Element &e = E->get(); + + e.aabb = p_aabb; + e.userdata = p_userdata; + e.subindex = p_subindex; + e.last_pass = 0; + e.octree = this; + e.pairable = p_pairable; + e.pairable_type = p_pairable_type; + e.pairable_mask = p_pairable_mask; + e._id = last_element_id - 1; + + if (!e.aabb.has_no_surface()) { + _ensure_valid_root(p_aabb); + _insert_element(&e, root); + if (use_pairs) + _element_check_pairs(&e); + } + + return last_element_id - 1; +} + +OCTREE_FUNC(void)::move(OctreeElementID p_id, const AABB &p_aabb) { + +#ifdef DEBUG_ENABLED + // check for AABB validity + ERR_FAIL_COND(p_aabb.position.x > 1e15 || p_aabb.position.x < -1e15); + ERR_FAIL_COND(p_aabb.position.y > 1e15 || p_aabb.position.y < -1e15); + ERR_FAIL_COND(p_aabb.position.z > 1e15 || p_aabb.position.z < -1e15); + ERR_FAIL_COND(p_aabb.size.x > 1e15 || p_aabb.size.x < 0.0); + ERR_FAIL_COND(p_aabb.size.y > 1e15 || p_aabb.size.y < 0.0); + ERR_FAIL_COND(p_aabb.size.z > 1e15 || p_aabb.size.z < 0.0); + ERR_FAIL_COND(Math::is_nan(p_aabb.size.x)); + ERR_FAIL_COND(Math::is_nan(p_aabb.size.y)); + ERR_FAIL_COND(Math::is_nan(p_aabb.size.z)); +#endif + typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND(!E); + Element &e = E->get(); + + bool old_has_surf = !e.aabb.has_no_surface(); + bool new_has_surf = !p_aabb.has_no_surface(); + + if (old_has_surf != new_has_surf) { + + if (old_has_surf) { + _remove_element(&e); // removing + e.common_parent = NULL; + e.aabb = AABB(); + _optimize(); + } else { + _ensure_valid_root(p_aabb); // inserting + e.common_parent = NULL; + e.aabb = p_aabb; + _insert_element(&e, root); + if (use_pairs) + _element_check_pairs(&e); + } + + return; + } + + if (!old_has_surf) // doing nothing + return; + + // it still is enclosed in the same AABB it was assigned to + if (e.container_aabb.encloses(p_aabb)) { + + e.aabb = p_aabb; + if (use_pairs) + _element_check_pairs(&e); // must check pairs anyway + +#ifdef OCTREE_USE_CACHED_LISTS + e.moving(); +#endif + return; + } + + AABB combined = e.aabb; + combined.merge_with(p_aabb); + _ensure_valid_root(combined); + + ERR_FAIL_COND(e.octant_owners.front() == NULL); + + /* FIND COMMON PARENT */ + + List owners = e.octant_owners; // save the octant owners + Octant *common_parent = e.common_parent; + ERR_FAIL_COND(!common_parent); + + //src is now the place towards where insertion is going to happen + pass++; + + while (common_parent && !common_parent->aabb.encloses(p_aabb)) + common_parent = common_parent->parent; + + ERR_FAIL_COND(!common_parent); + + //prepare for reinsert + e.octant_owners.clear(); + e.common_parent = NULL; + e.aabb = p_aabb; + + _insert_element(&e, common_parent); // reinsert from this point + + pass++; + + for (typename List::Element *F = owners.front(); F;) { + + Octant *o = F->get().octant; + typename List::Element *N = F->next(); + + /* + if (!use_pairs) + o->elements.erase( F->get().E ); + */ + + if (use_pairs && e.pairable) + o->pairable_elements.erase(F->get().E); + else + o->elements.erase(F->get().E); + +#ifdef OCTREE_USE_CACHED_LISTS + o->dirty = true; +#endif + + if (_remove_element_pair_and_remove_empty_octants(&e, o, common_parent->parent)) { + + owners.erase(F); + } + + F = N; + } + + if (use_pairs) { + //unpair child elements in anything that survived + for (typename List::Element *F = owners.front(); F; F = F->next()) { + + Octant *o = F->get().octant; + + // erase children pairs, unref ONCE + pass++; + for (int i = 0; i < 8; i++) { + + if (o->children[i]) + _unpair_element(&e, o->children[i]); + } + } + + _element_check_pairs(&e); + } + + _optimize(); +} + +OCTREE_FUNC(void)::set_pairable(OctreeElementID p_id, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) { + + typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e = E->get(); + + if (p_pairable == e.pairable && e.pairable_type == p_pairable_type && e.pairable_mask == p_pairable_mask) + return; // no changes, return + + if (!e.aabb.has_no_surface()) { + _remove_element(&e); + } + + e.pairable = p_pairable; + e.pairable_type = p_pairable_type; + e.pairable_mask = p_pairable_mask; + e.common_parent = NULL; + + if (!e.aabb.has_no_surface()) { + _ensure_valid_root(e.aabb); + _insert_element(&e, root); + if (use_pairs) + _element_check_pairs(&e); + } +} + +OCTREE_FUNC(void)::erase(OctreeElementID p_id) { + + typename ElementMap::Element *E = element_map.find(p_id); + ERR_FAIL_COND(!E); + + Element &e = E->get(); + + if (!e.aabb.has_no_surface()) { + + _remove_element(&e); + } + + element_map.erase(p_id); + _optimize(); +} + +OCTREE_FUNC(void)::_cull_convex(Octant *p_octant, _CullConvexData *p_cull) { + + if (*p_cull->result_idx == p_cull->result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist.aabbs[n]; + Element *e = p_octant->clist.elements[n]; + + // in most cases with the cached linear list tests we will do the AABB checks BEFORE last pass and cull mask. + // The reason is that the later checks are more expensive because they are not in cache, and many of the AABB + // tests will fail so we can avoid these cache misses. + if (aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) + continue; + e->last_pass = pass; + + if (*p_cull->result_idx < p_cull->result_max) { + p_cull->result_array[*p_cull->result_idx] = e->userdata; + (*p_cull->result_idx)++; + } else { + return; // pointless to continue + } + } + } // for n +#else + typename List::Element *I; + I = p_octant->elements.front(); + + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) + continue; + e->last_pass = pass; + + if (aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { + if (*p_cull->result_idx < p_cull->result_max) { + p_cull->result_array[*p_cull->result_idx] = e->userdata; + (*p_cull->result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#endif + } // if elements not empty + + if (use_pairs && !p_octant->pairable_elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist_pairable.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist_pairable.aabbs[n]; + Element *e = p_octant->clist_pairable.elements[n]; + + if (aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) + continue; + e->last_pass = pass; + + if (*p_cull->result_idx < p_cull->result_max) { + p_cull->result_array[*p_cull->result_idx] = e->userdata; + (*p_cull->result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#else + + typename List::Element *I; + I = p_octant->pairable_elements.front(); + + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_cull->mask))) + continue; + e->last_pass = pass; + + if (aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { + + if (*p_cull->result_idx < p_cull->result_max) { + + p_cull->result_array[*p_cull->result_idx] = e->userdata; + (*p_cull->result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#endif + } + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_convex_shape(p_cull->planes, p_cull->plane_count, p_cull->points, p_cull->point_count)) { + _cull_convex(p_octant->children[i], p_cull); + } + } +} + +OCTREE_FUNC(void)::_cull_aabb(Octant *p_octant, const AABB &p_aabb, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { + + if (*p_result_idx == p_result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist.aabbs[n]; + Element *e = p_octant->clist.elements[n]; + + if (p_aabb.intersects_inclusive(aabb)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#else + typename List::Element *I; + I = p_octant->elements.front(); + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (p_aabb.intersects_inclusive(aabb)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#endif + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist_pairable.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist_pairable.aabbs[n]; + Element *e = p_octant->clist_pairable.elements[n]; + + if (p_aabb.intersects_inclusive(aabb)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#else + + typename List::Element *I; + I = p_octant->pairable_elements.front(); + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (p_aabb.intersects_inclusive(aabb)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + } else { + + return; // pointless to continue + } + } + } +#endif + } + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_inclusive(p_aabb)) { + _cull_aabb(p_octant->children[i], p_aabb, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); + } + } +} + +OCTREE_FUNC(void)::_cull_segment(Octant *p_octant, const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { + + if (*p_result_idx == p_result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist.aabbs[n]; + Element *e = p_octant->clist.elements[n]; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (aabb.intersects_segment(p_from, p_to)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#else + + typename List::Element *I; + I = p_octant->elements.front(); + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (aabb.intersects_segment(p_from, p_to)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#endif + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist_pairable.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist_pairable.aabbs[n]; + Element *e = p_octant->clist_pairable.elements[n]; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + + e->last_pass = pass; + + if (aabb.intersects_segment(p_from, p_to)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#else + typename List::Element *I; + I = p_octant->pairable_elements.front(); + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + + e->last_pass = pass; + + if (aabb.intersects_segment(p_from, p_to)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#endif + } + + for (int i = 0; i < 8; i++) { + + if (p_octant->children[i] && p_octant->children[i]->aabb.intersects_segment(p_from, p_to)) { + _cull_segment(p_octant->children[i], p_from, p_to, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); + } + } +} + +OCTREE_FUNC(void)::_cull_point(Octant *p_octant, const Vector3 &p_point, T **p_result_array, int *p_result_idx, int p_result_max, int *p_subindex_array, uint32_t p_mask) { + + if (*p_result_idx == p_result_max) + return; //pointless + + if (!p_octant->elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist.aabbs[n]; + Element *e = p_octant->clist.elements[n]; + + if (aabb.has_point(p_point)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#else + typename List::Element *I; + I = p_octant->elements.front(); + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + e->last_pass = pass; + + if (aabb.has_point(p_point)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#endif + } + + if (use_pairs && !p_octant->pairable_elements.empty()) { + +#ifdef OCTREE_USE_CACHED_LISTS + // make sure cached list of element pointers and aabbs is up to date if this octant is dirty + p_octant->update_cached_lists(); + + int num_elements = p_octant->clist_pairable.elements.size(); + for (int n = 0; n < num_elements; n++) { + const AABB &aabb = p_octant->clist_pairable.aabbs[n]; + Element *e = p_octant->clist_pairable.elements[n]; + + if (aabb.has_point(p_point)) { + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + + e->last_pass = pass; + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#else + typename List::Element *I; + I = p_octant->pairable_elements.front(); + for (; I; I = I->next()) { + + Element *e = I->get(); + const AABB &aabb = e->aabb; + + if (e->last_pass == pass || (use_pairs && !(e->pairable_type & p_mask))) + continue; + + e->last_pass = pass; + + if (aabb.has_point(p_point)) { + + if (*p_result_idx < p_result_max) { + + p_result_array[*p_result_idx] = e->userdata; + if (p_subindex_array) + p_subindex_array[*p_result_idx] = e->subindex; + + (*p_result_idx)++; + + } else { + + return; // pointless to continue + } + } + } +#endif + } + + for (int i = 0; i < 8; i++) { + + //could be optimized.. + if (p_octant->children[i] && p_octant->children[i]->aabb.has_point(p_point)) { + _cull_point(p_octant->children[i], p_point, p_result_array, p_result_idx, p_result_max, p_subindex_array, p_mask); + } + } +} + +OCTREE_FUNC(int)::cull_convex(const Vector &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask) { + + if (!root || p_convex.size() == 0) + return 0; + + Vector convex_points = Geometry::compute_convex_mesh_points(&p_convex[0], p_convex.size()); + if (convex_points.size() == 0) + return 0; + + int result_count = 0; + pass++; + _CullConvexData cdata; + cdata.planes = &p_convex[0]; + cdata.plane_count = p_convex.size(); + cdata.points = &convex_points[0]; + cdata.point_count = convex_points.size(); + cdata.result_array = p_result_array; + cdata.result_max = p_result_max; + cdata.result_idx = &result_count; + cdata.mask = p_mask; + + _cull_convex(root, &cdata); + + return result_count; +} + +OCTREE_FUNC(int)::cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { + + if (!root) + return 0; + + int result_count = 0; + pass++; + _cull_aabb(root, p_aabb, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); + + return result_count; +} + +OCTREE_FUNC(int)::cull_segment(const Vector3 &p_from, const Vector3 &p_to, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { + + if (!root) + return 0; + + int result_count = 0; + pass++; + _cull_segment(root, p_from, p_to, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); + + return result_count; +} + +OCTREE_FUNC(int)::cull_point(const Vector3 &p_point, T **p_result_array, int p_result_max, int *p_subindex_array, uint32_t p_mask) { + + if (!root) + return 0; + + int result_count = 0; + pass++; + _cull_point(root, p_point, p_result_array, &result_count, p_result_max, p_subindex_array, p_mask); + + return result_count; +} + +OCTREE_FUNC(void)::set_pair_callback(PairCallback p_callback, void *p_userdata) { + + pair_callback = p_callback; + pair_callback_userdata = p_userdata; +} + +OCTREE_FUNC(void)::set_unpair_callback(UnpairCallback p_callback, void *p_userdata) { + + unpair_callback = p_callback; + unpair_callback_userdata = p_userdata; +} + +OCTREE_FUNC_CONSTRUCTOR::OCTREE_CLASS_NAME(real_t p_unit_size) { + + last_element_id = 1; + pass = 1; + unit_size = p_unit_size; + root = NULL; + + octant_count = 0; + pair_count = 0; + octant_elements_limit = OCTREE_DEFAULT_OCTANT_LIMIT; + + pair_callback = NULL; + unpair_callback = NULL; + pair_callback_userdata = NULL; + unpair_callback_userdata = NULL; +} + +#ifdef TOOLS_ENABLED +OCTREE_FUNC(String)::debug_aabb_to_string(const AABB &aabb) const { + String sz; + sz = "( " + String(aabb.position); + sz += " ) - ( "; + Vector3 max = aabb.position + aabb.size; + sz += String(max) + " )"; + return sz; +} + +OCTREE_FUNC(void)::debug_octants() { + if (root) + debug_octant(*root); +} + +OCTREE_FUNC(void)::debug_octant(const Octant &oct, int depth) { + String sz = ""; + for (int d = 0; d < depth; d++) + sz += "\t"; + + sz += "Octant " + debug_aabb_to_string(oct.aabb); + sz += "\tnum_children " + itos(oct.children_count); + sz += ", num_eles " + itos(oct.elements.size()); + sz += ", num_paired_eles" + itos(oct.pairable_elements.size()); + print_line(sz); + + for (int n = 0; n < 8; n++) { + const Octant *pChild = oct.children[n]; + if (pChild) { + debug_octant(*pChild, depth + 1); + } + } +} +#endif // TOOLS_ENABLED + +#undef OCTREE_FUNC diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml index fd88d83cc60..c845a157b89 100644 --- a/doc/classes/ProjectSettings.xml +++ b/doc/classes/ProjectSettings.xml @@ -1174,6 +1174,10 @@ Lower-end override for [member rendering/quality/shadows/filter_mode] on mobile devices, due to performance concerns or driver support. + + The rendering octree balance can be changed to favor smaller ([code]0[/code]), or larger ([code]1[/code]) branches. + Larger branches can increase performance significantly in some projects. + Improves quality of subsurface scattering, but cost significantly increases. diff --git a/servers/visual/visual_server_scene.cpp b/servers/visual/visual_server_scene.cpp index f7420b76639..59689f3cab2 100644 --- a/servers/visual/visual_server_scene.cpp +++ b/servers/visual/visual_server_scene.cpp @@ -260,6 +260,7 @@ RID VisualServerScene::scenario_create() { RID scenario_rid = scenario_owner.make_rid(scenario); scenario->self = scenario_rid; + scenario->octree.set_balance(GLOBAL_GET("rendering/quality/spatial_partitioning/render_tree_balance")); scenario->octree.set_pair_callback(_instance_pair, this); scenario->octree.set_unpair_callback(_instance_unpair, this); scenario->reflection_probe_shadow_atlas = VSG::scene_render->shadow_atlas_create(); diff --git a/servers/visual/visual_server_scene.h b/servers/visual/visual_server_scene.h index a174d9e616e..ee1fd408f4b 100644 --- a/servers/visual/visual_server_scene.h +++ b/servers/visual/visual_server_scene.h @@ -108,7 +108,7 @@ public: VS::ScenarioDebugMode debug; RID self; - Octree octree; + Octree_CL octree; List directional_lights; RID environment; diff --git a/servers/visual_server.cpp b/servers/visual_server.cpp index d88ef711da7..1c8b87bdc2b 100644 --- a/servers/visual_server.cpp +++ b/servers/visual_server.cpp @@ -2432,6 +2432,10 @@ VisualServer::VisualServer() { GLOBAL_DEF("rendering/quality/filters/use_nearest_mipmap_filter", false); + const char *sz_balance_render_tree = "rendering/quality/spatial_partitioning/render_tree_balance"; + GLOBAL_DEF(sz_balance_render_tree, 0.17f); + ProjectSettings::get_singleton()->set_custom_property_info(sz_balance_render_tree, PropertyInfo(Variant::REAL, sz_balance_render_tree, PROPERTY_HINT_RANGE, "0.0,1.0,0.01")); + GLOBAL_DEF("rendering/batching/options/use_batching", true); GLOBAL_DEF_RST("rendering/batching/options/use_batching_in_editor", true); GLOBAL_DEF("rendering/batching/options/single_rect_fallback", false);