diff options
Diffstat (limited to 'core/math/octree.h')
| -rw-r--r-- | core/math/octree.h | 1461 |
1 files changed, 1461 insertions, 0 deletions
diff --git a/core/math/octree.h b/core/math/octree.h new file mode 100644 index 0000000000..b64ba381c0 --- /dev/null +++ b/core/math/octree.h @@ -0,0 +1,1461 @@ +/*************************************************************************/ +/* octree.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef OCTREE_H +#define OCTREE_H + +#include "vector3.h" +#include "aabb.h" +#include "list.h" +#include "variant.h" +#include "map.h" +#include "print_string.h" + +/** + @author Juan Linietsky <reduzio@gmail.com> +*/ + +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 { + + // 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; + + 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<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; + + + 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( 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_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); } +}; + + +/* PRIVATE FUNCTIONS */ + +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) { + + float 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( p_element->aabb ) ) { + _insert_element( p_element, p_octant->children[i] ); + splits++; + } + } else { + /* check againt AABB where child should be */ + + AABB aabb=p_octant->aabb; + aabb.size*=0.5; + + if (i&1) + aabb.pos.x+=aabb.size.x; + if (i&2) + aabb.pos.y+=aabb.size.y; + if (i&4) + aabb.pos.z+=aabb.size.z; + + if (aabb.intersects( 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(); + } + } + } + + +} + + +template<class T,bool use_pairs,class AL> +void Octree<T,use_pairs,AL>::_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.pos.x+base.size.x) <= ABS(base.pos.x) ) { + /* grow towards positive */ + base.size*=2.0; + } else { + base.pos-=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.pos.x+base.size.x) <= ABS(base.pos.x) ) { + /* grow towards positive */ + base.size*=2.0; + gp->aabb=base; + gp->children[0]=root; + root->parent_index=0; + } else { + base.pos-=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<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) { + + // 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 wether 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; +} + +template<class T,bool use_pairs,class AL> +void Octree<T,use_pairs,AL>::_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]); + } +} + +template<class T,bool use_pairs,class AL> +void Octree<T,use_pairs,AL>::_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]); + } +} + +template<class T,bool use_pairs,class AL> +void Octree<T,use_pairs,AL>::_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(); + + + /* 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<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) { + + // check for AABB validity +#ifdef DEBUG_ENABLED + ERR_FAIL_COND_V( p_aabb.pos.x > 1e15 || p_aabb.pos.x < -1e15, 0 ); + ERR_FAIL_COND_V( p_aabb.pos.y > 1e15 || p_aabb.pos.y < -1e15, 0 ); + ERR_FAIL_COND_V( p_aabb.pos.z > 1e15 || p_aabb.pos.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 + // check for AABB validity + ERR_FAIL_COND( p_aabb.pos.x > 1e15 || p_aabb.pos.x < -1e15 ); + ERR_FAIL_COND( p_aabb.pos.y > 1e15 || p_aabb.pos.y < -1e15 ); + ERR_FAIL_COND( p_aabb.pos.z > 1e15 || p_aabb.pos.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(); + +#if 0 + + pass++; + if (!e.aabb.has_no_surface()) { + _remove_element(&e); + } + + e.aabb=p_aabb; + + if (!e.aabb.has_no_surface()) { + _ensure_valid_root(p_aabb); + + _insert_element(&e,root); + if (use_pairs) + _element_check_pairs(&e); + + } + + _optimize(); + +#else + + 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<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 *E=owners.front();E;) { + + Octant *o=E->get().octant; + typename List<typename Element::OctantOwner,AL>::Element *N=E->next(); + +// if (!use_pairs) +// o->elements.erase( E->get().E ); + + 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) { + //unpair child elements in anything that survived + for(typename List<typename Element::OctantOwner,AL>::Element *E=owners.front();E;E=E->next()) { + + Octant *o=E->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(); +#endif + + +} + +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; // 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<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; //pointless + + 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; // pointless to continue + } + } + } + } + + 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; // 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)) { + _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; //pointless + + 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(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*,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(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(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; //pointless + + 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; // pointless to continue + } + } + } + } + + 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; // 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<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; //pointless + + 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; // pointless to continue + } + } + } + } + + 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; // 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<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 |