123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386 |
- #ifndef OCTREE_H
- #define OCTREE_H
- #include "core/list.h"
- #include "core/map.h"
- #include "core/math/aabb.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 T, bool use_pairs = false, class AL = DefaultAllocator>
- 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 {
-
- AABB aabb;
- uint64_t last_pass;
- Octant *parent;
- Octant *children[8];
- int children_count;
- int parent_index;
- List<Element *, AL> pairable_elements;
- List<Element *, AL> elements;
- Octant() {
- children_count = 0;
- parent_index = -1;
- last_pass = 0;
- parent = NULL;
- for (int i = 0; i < 8; i++)
- children[i] = NULL;
- }
- ~Octant() {
-
- }
- };
- 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<PairData *, AL> pair_list;
- struct OctantOwner {
- Octant *octant;
- typename List<Element *, AL>::Element *E;
- };
- List<OctantOwner, AL> 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<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;
- _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;
- 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());
-
- } 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;
- }
- E->get().refcount--;
- if (E->get().refcount == 0) {
-
- 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) {
-
- 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_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;
- 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);
- }
- 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; }
- Octree(real_t p_unit_size = 1.0);
- ~Octree() { _remove_tree(root); }
- };
- template <class T, bool use_pairs, class AL>
- T *Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- bool Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- int Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_insert_element(Element *p_element, Octant *p_octant) {
- real_t element_size = p_element->aabb.get_longest_axis_size() * 1.01;
- if (p_octant->aabb.size.x / OCTREE_DIVISOR < element_size) {
-
-
- 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++;
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
- _pair_element(p_element, p_octant->children[i]);
- }
- }
- }
- } else {
-
- int splits = 0;
- bool candidate = p_element->common_parent == NULL;
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i]) {
-
- if (p_octant->children[i]->aabb.intersects_inclusive(p_element->aabb)) {
- _insert_element(p_element, p_octant->children[i]);
- splits++;
- }
- } else {
-
- 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)) {
-
- 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) {
-
- E = p_octant->elements.front();
- while (E) {
- _pair_reference(p_element, E->get());
- E = E->next();
- }
- }
- }
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_ensure_valid_root(const AABB &p_aabb) {
- if (!root) {
-
- 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)) {
-
- 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)) {
- if (base.size.x > OCTREE_SIZE_LIMIT) {
- ERR_EXPLAIN("Octree upper size limit reeached, does the AABB supplied contain NAN?");
- ERR_FAIL();
- }
- Octant *gp = memnew_allocator(Octant, AL);
- octant_count++;
- root->parent = gp;
- if (ABS(base.position.x + base.size.x) <= ABS(base.position.x)) {
-
- 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;
- root->parent_index = (1 << 0) | (1 << 1) | (1 << 2);
- }
- gp->children_count = 1;
- root = gp;
- }
- }
- }
- template <class T, bool use_pairs, class AL>
- bool Octree<T, use_pairs, AL>::_remove_element_from_octant(Element *p_element, Octant *p_octant, Octant *p_limit) {
- bool octant_removed = false;
- while (true) {
-
- if (p_octant == p_limit)
- return octant_removed;
- bool unpaired = false;
- if (use_pairs && p_octant->last_pass != pass) {
-
-
- 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) {
-
- 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()) {
-
- if (p_octant == root) {
- 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;
- p_octant = parent;
- }
- return octant_removed;
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_unpair_element(Element *p_element, Octant *p_octant) {
-
- typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front();
- while (E) {
- if (E->get()->last_pass != pass) {
- _pair_unreference(p_element, E->get());
- E->get()->last_pass = pass;
- }
- E = E->next();
- }
- if (p_element->pairable) {
-
- E = p_octant->elements.front();
- while (E) {
- if (E->get()->last_pass != pass) {
- _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;
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i])
- _unpair_element(p_element, p_octant->children[i]);
- }
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_pair_element(Element *p_element, Octant *p_octant) {
-
- typename List<Element *, AL>::Element *E = p_octant->pairable_elements.front();
- while (E) {
- if (E->get()->last_pass != pass) {
- _pair_reference(p_element, E->get());
- E->get()->last_pass = pass;
- }
- E = E->next();
- }
- if (p_element->pairable) {
-
- E = p_octant->elements.front();
- while (E) {
- if (E->get()->last_pass != pass) {
- _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;
- for (int i = 0; i < 8; i++) {
- if (p_octant->children[i])
- _pair_element(p_element, p_octant->children[i]);
- }
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_remove_element(Element *p_element) {
- pass++;
- 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);
- _remove_element_from_octant(p_element, o);
- }
-
- I = p_element->octant_owners.front();
- if (use_pairs) {
- for (; I; I = I->next()) {
- Octant *o = I->get().octant;
-
- 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();
-
- ERR_FAIL_COND(remaining);
- }
- }
- template <class T, bool use_pairs, class AL>
- OctreeElementID Octree<T, use_pairs, AL>::create(T *p_userdata, const AABB &p_aabb, int p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
- #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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::move(OctreeElementID p_id, const AABB &p_aabb) {
- #ifdef DEBUG_ENABLED
-
- 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);
- e.common_parent = NULL;
- e.aabb = AABB();
- _optimize();
- } else {
- _ensure_valid_root(p_aabb);
- e.common_parent = NULL;
- e.aabb = p_aabb;
- _insert_element(&e, root);
- if (use_pairs)
- _element_check_pairs(&e);
- }
- return;
- }
- if (!old_has_surf)
- return;
-
- if (e.container_aabb.encloses(p_aabb)) {
- e.aabb = p_aabb;
- if (use_pairs)
- _element_check_pairs(&e);
- return;
- }
- AABB combined = e.aabb;
- combined.merge_with(p_aabb);
- _ensure_valid_root(combined);
- ERR_FAIL_COND(e.octant_owners.front() == NULL);
-
- List<typename Element::OctantOwner, AL> owners = e.octant_owners;
- Octant *common_parent = e.common_parent;
- ERR_FAIL_COND(!common_parent);
-
- pass++;
- while (common_parent && !common_parent->aabb.encloses(p_aabb))
- common_parent = common_parent->parent;
- ERR_FAIL_COND(!common_parent);
-
- e.octant_owners.clear();
- e.common_parent = NULL;
- e.aabb = p_aabb;
- _insert_element(&e, common_parent);
- pass++;
- for (typename List<typename Element::OctantOwner, AL>::Element *E = owners.front(); E;) {
- Octant *o = E->get().octant;
- typename List<typename Element::OctantOwner, AL>::Element *N = E->next();
-
- if (use_pairs && e.pairable)
- o->pairable_elements.erase(E->get().E);
- else
- o->elements.erase(E->get().E);
- if (_remove_element_from_octant(&e, o, common_parent->parent)) {
- owners.erase(E);
- }
- E = N;
- }
- if (use_pairs) {
-
- for (typename List<typename Element::OctantOwner, AL>::Element *E = owners.front(); E; E = E->next()) {
- Octant *o = E->get().octant;
-
- pass++;
- for (int i = 0; i < 8; i++) {
- if (o->children[i])
- _unpair_element(&e, o->children[i]);
- }
- }
- _element_check_pairs(&e);
- }
- _optimize();
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::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;
- 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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_cull_convex(Octant *p_octant, _CullConvexData *p_cull) {
- if (*p_cull->result_idx == p_cull->result_max)
- return;
- if (!p_octant->elements.empty()) {
- typename List<Element *, AL>::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)) {
- 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;
- }
- }
- }
- }
- if (use_pairs && !p_octant->pairable_elements.empty()) {
- typename List<Element *, AL>::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)) {
- 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;
- }
- }
- }
- }
- 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)) {
- _cull_convex(p_octant->children[i], p_cull);
- }
- }
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_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;
- if (!p_octant->elements.empty()) {
- typename List<Element *, AL>::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;
- }
- }
- }
- }
- if (use_pairs && !p_octant->pairable_elements.empty()) {
- typename List<Element *, AL>::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;
- }
- }
- }
- }
- 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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_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;
- if (!p_octant->elements.empty()) {
- typename List<Element *, AL>::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;
- }
- }
- }
- }
- if (use_pairs && !p_octant->pairable_elements.empty()) {
- typename List<Element *, AL>::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;
- }
- }
- }
- }
- 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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::_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;
- if (!p_octant->elements.empty()) {
- typename List<Element *, AL>::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;
- }
- }
- }
- }
- if (use_pairs && !p_octant->pairable_elements.empty()) {
- typename List<Element *, AL>::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;
- }
- }
- }
- }
- for (int i = 0; i < 8; i++) {
-
- 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 <class T, bool use_pairs, class AL>
- int Octree<T, use_pairs, AL>::cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask) {
- if (!root)
- return 0;
- int result_count = 0;
- pass++;
- _CullConvexData cdata;
- cdata.planes = &p_convex[0];
- cdata.plane_count = p_convex.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 <class T, bool use_pairs, class AL>
- int Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- int Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- int Octree<T, use_pairs, AL>::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 <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::set_pair_callback(PairCallback p_callback, void *p_userdata) {
- pair_callback = p_callback;
- pair_callback_userdata = p_userdata;
- }
- template <class T, bool use_pairs, class AL>
- void Octree<T, use_pairs, AL>::set_unpair_callback(UnpairCallback p_callback, void *p_userdata) {
- unpair_callback = p_callback;
- unpair_callback_userdata = p_userdata;
- }
- template <class T, bool use_pairs, class AL>
- Octree<T, use_pairs, AL>::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
|