667c970b77
Prevents adding new octants until a limiting number of elements have been added to the current octant. This enables balancing the benefits of brute force against the benefits of spatial partitioning. The limit can be set per octree. Project settings are added for rendering octree to set the best balance per project depending on number of tests per frame / tick, and the amount of editing of the octree. Fixes octants being leaked when removing elements. Optimize octree with cached linear lists Storing elements in octants using linked lists is efficient for housekeeping but very slow for testing. This optimization stores additional local_vectors with Element pointers and AABBs which are cached and only updated when a dirty flag is set on the octant. This is selectable with 2 versions of Octree : Octree and Octree_CL, Octree being the old behaviour. At present the cached list version is only used for the visual server octree (rendering) as it has only been demonstrated to be faster there so far. This uses slightly more memory (probably a few kb in most cases) but can be significantly faster during testing (culling etc). Co-authored-by: Sergey Minakov <naithar@icloud.com>
1763 lines
45 KiB
C++
1763 lines
45 KiB
C++
// 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 <class T, bool use_pairs, class AL> \
|
|
RET_VALUE OCTREE_CLASS_NAME<T, use_pairs, AL>
|
|
|
|
#define OCTREE_FUNC_CONSTRUCTOR template <class T, bool use_pairs, class AL> \
|
|
OCTREE_CLASS_NAME<T, use_pairs, AL>
|
|
|
|
template <class T, bool use_pairs = false, class AL = DefaultAllocator>
|
|
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<AABB> aabbs;
|
|
LocalVector<Element *> elements;
|
|
|
|
void update(List<Element *, AL> &eles) {
|
|
// make sure local vector doesn't delete the memory
|
|
// no need to be thrashing allocations
|
|
aabbs.clear();
|
|
elements.clear();
|
|
|
|
typename List<Element *, AL>::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<Element *, AL> pairable_elements;
|
|
List<Element *, AL> 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<PairData *, AL> pair_list;
|
|
|
|
struct OctantOwner {
|
|
|
|
Octant *octant;
|
|
typename List<Element *, AL>::Element *E;
|
|
}; // an element can be in max 8 octants
|
|
|
|
List<OctantOwner, AL> 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<typename Element::OctantOwner, AL>::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<PairData *, AL>::Element *eA, *eB;
|
|
};
|
|
|
|
typedef Map<OctreeElementID, Element, Comparator<OctreeElementID>, AL> ElementMap;
|
|
typedef Map<PairKey, PairData, Comparator<PairKey>, 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<PairData *, AL>::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<Octant, AL>(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<Octant, AL>(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<Plane> &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 *, AL>::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 *, AL>::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<Octant, AL>(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 *, AL>::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 *, AL>::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<typename Element::OctantOwner, AL>::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<typename Element::OctantOwner, AL> 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<typename Element::OctantOwner, AL>::Element *F = owners.front(); F;) {
|
|
|
|
Octant *o = F->get().octant;
|
|
typename List<typename Element::OctantOwner, AL>::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<typename Element::OctantOwner, AL>::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 *, AL>::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 *, AL>::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 *, AL>::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 *, AL>::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 *, AL>::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 *, AL>::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 *, AL>::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 *, AL>::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<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask) {
|
|
|
|
if (!root || p_convex.size() == 0)
|
|
return 0;
|
|
|
|
Vector<Vector3> 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
|