Home

nevrax.com

Nevrax.org News Mailing-list Documentation CVS Bugs License

Documentation                       Search   quad_tree.h Go to the documentation of this file. /* Copyright, 2000 Nevrax Ltd. * * This file is part of NEVRAX NEL. * NEVRAX NEL is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) * any later version. * NEVRAX NEL is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * General Public License for more details. * You should have received a copy of the GNU General Public License * along with NEVRAX NEL; see the file COPYING. If not, write to the * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, * MA 02111-1307, USA. */ #ifndef NL_QUAD_TREE_H #define NL_QUAD_TREE_H #include "nel/misc/debug.h" #include "nel/misc/vector.h" #include "nel/misc/plane.h" #include "nel/misc/matrix.h" #include <list> #include <vector> namespace NL3D { template<class T> class CQuadTree { public: class CIterator; friend class CIterator; class CConstIterator; friend class CConstIterator; public: CQuadTree(); ~CQuadTree(); void changeBase(const NLMISC::CMatrix& base); void create(uint DepthMax, const NLMISC::CVector& center, float size); void clear(); void eraseAll(); void erase(CIterator it); CIterator insert(const NLMISC::CVector &bboxmin, const NLMISC::CVector &bboxmax, const T &val); void clearSelection(); void selectAll(); void select(const NLMISC::CVector &bboxmin, const NLMISC::CVector &bboxmax); void select(const std::vector<NLMISC::CPlane> &BVolume); void selectRay(const NLMISC::CVector& source, const NLMISC::CVector& dir); void selectSegment(const NLMISC::CVector& source, const NLMISC::CVector& dest); CIterator begin(); CIterator end(); // ================= // ================= // IMPLEMENTATION. // ================= // ================= private:// Classes. // ================= // ================= // CBaseNode. // ================= // ================= // Links fo an element node class. class CBaseNode { public: // for the Selection list. CBaseNode *Prev, *Next; // for the quadnode list. A node MAY be pointed by 4 quad (each having the same level). CBaseNode *QuadPrevs[4]; CBaseNode *QuadNexts[4]; public: CBaseNode() { Prev=Next=NULL; QuadPrevs[0]= QuadPrevs[1]= QuadPrevs[2]= QuadPrevs[3]= NULL; QuadNexts[0]= QuadNexts[1]= QuadNexts[2]= QuadNexts[3]= NULL; } virtual ~CBaseNode() {} // Empty destructor, but declare it as virtual... void clear() // update links. { // On le retire de la selection. if(Prev) Prev->Next= Next; if(Next) Next->Prev= Prev; Prev=Next=NULL; // On le retire des listes dans les quads. for(uint i=0;i<4;i++) { if(QuadPrevs[i]) {nlassert(QuadPrevs[i]->QuadNexts[i]==this); QuadPrevs[i]->QuadNexts[i]= QuadNexts[i];} if(QuadNexts[i]) {nlassert(QuadNexts[i]->QuadPrevs[i]==this); QuadNexts[i]->QuadPrevs[i]= QuadPrevs[i];} QuadPrevs[i]=NULL; QuadNexts[i]=NULL; } } bool isSelected() // return true if Prev is not NULL!!! { return Prev!=NULL; } }; // ================= // ================= // CNode. // ================= // ================= // An element node class. class CNode : public CBaseNode { public: // A base node, plus the Value. T Value; CNode(const T &val) : Value(val) {} }; // ================= // ================= // CQuadNode. // ================= // ================= class CQuadNode { public: uint Level; NLMISC::CVector BBoxMin, BBoxMax; bool BBoxNeverRescale; CQuadNode *Sons[4]; CBaseNode RootNode; // First element of the element list in this quad. uint ListIndex; // [0,3]. index of wich list to follow in "Node.QuadNexts[]". /* Topology of sons (top view: axe x/z): 0--1 | | 2--3 */ public: // ============================================================================================ CQuadNode() { Level=0; Sons[0]= Sons[1]= Sons[2]= Sons[3]= NULL; ListIndex= 0; BBoxNeverRescale=true; } // ============================================================================================ bool isLeaf() { return Sons[0]==NULL; } // ============================================================================================ void clear() { // Deletons les element dans ce quad node. CBaseNode *p; while( (p=RootNode.QuadNexts[ListIndex]) ) { p->clear(); // On clear les links. => RootNode.QuadNexts[ListIndex] modifié implicitement. delete p; // On delete cet element!! } // Deletons les quad fils. for(uint i=0;i<4;i++) { if(Sons[i]) { Sons[i]->clear(); delete Sons[i]; Sons[i]= NULL; } } } // ============================================================================================ void split() { nlassert(isLeaf()); sint i; for(i=0;i<4;i++) { Sons[i]= new CQuadNode; Sons[i]->Level= Level+1; Sons[i]->ListIndex= i; } // Middle compute. NLMISC::CVector MidLeft(0,0,0), MidRight(0,0,0), MidTop(0,0,0), MidBottom(0,0,0), Middle(0,0,0); MidLeft.x = BBoxMin.x; MidLeft.z = (BBoxMin.z + BBoxMax.z)/2; MidRight.x = BBoxMax.x; MidRight.z = (BBoxMin.z + BBoxMax.z)/2; MidTop.x = (BBoxMin.x + BBoxMax.x)/2; MidTop.z = BBoxMin.z; MidBottom.x= (BBoxMin.x + BBoxMax.x)/2; MidBottom.z= BBoxMax.z; Middle.x = MidTop.x; Middle.z = MidLeft.z; // Sons compute. // Don't care of Y. Sons[0]->BBoxMin = BBoxMin; Sons[0]->BBoxMax = Middle; Sons[1]->BBoxMin = MidTop ; Sons[1]->BBoxMax = MidRight; Sons[2]->BBoxMin = MidLeft; Sons[2]->BBoxMax = MidBottom; Sons[3]->BBoxMin = Middle; Sons[3]->BBoxMax = BBoxMax; } // ============================================================================================ // This is a quadtree, so those tests just test against x/z. (the base of the box). bool includeBoxQuad(const NLMISC::CVector &boxmin, const NLMISC::CVector &boxmax) { if( BBoxMin.x<= boxmin.x && BBoxMax.x>= boxmax.x && BBoxMin.z<= boxmin.z && BBoxMax.z>= boxmax.z) return true; else return false; } // ============================================================================================ bool intersectBoxQuad(const NLMISC::CVector &boxmin, const NLMISC::CVector &boxmax) { // inequality and equality is very important, to ensure that a element box will not fit in too many quad boxes. if(boxmin.x > BBoxMax.x) return false; if(boxmin.z > BBoxMax.z) return false; if(boxmax.x <= BBoxMin.x) return false; if(boxmax.z <= BBoxMin.z) return false; return true; } // ============================================================================================ bool intersectBox(const NLMISC::CVector &boxmin, const NLMISC::CVector &boxmax) { // inequality and equality is very important, to ensure that a element box will not fit in too many quad boxes. if(boxmin.x > BBoxMax.x) return false; if(boxmin.y > BBoxMax.y) return false; if(boxmin.z > BBoxMax.z) return false; if(boxmax.x <= BBoxMin.x) return false; if(boxmax.y <= BBoxMin.y) return false; if(boxmax.z <= BBoxMin.z) return false; return true; } // ============================================================================================ bool intersectBox(std::vector<NLMISC::CPlane> &BVolume) { const NLMISC::CVector &b1=BBoxMin; const NLMISC::CVector &b2=BBoxMax; for(sint i=0;i<(int)BVolume.size();i++) { const NLMISC::CPlane &plane=BVolume[i]; // If only one of the box vertex is IN the plane, then all the box is IN this plane. if(plane* NLMISC::CVector(b1.x, b1.y, b1.z)<=0) continue; if(plane* NLMISC::CVector(b1.x, b1.y, b2.z)<=0) continue; if(plane* NLMISC::CVector(b1.x, b2.y, b1.z)<=0) continue; if(plane* NLMISC::CVector(b1.x, b2.y, b2.z)<=0) continue; if(plane* NLMISC::CVector(b2.x, b1.y, b1.z)<=0) continue; if(plane* NLMISC::CVector(b2.x, b1.y, b2.z)<=0) continue; if(plane* NLMISC::CVector(b2.x, b2.y, b1.z)<=0) continue; if(plane* NLMISC::CVector(b2.x, b2.y, b2.z)<=0) continue; // If ALL box vertices are OUT of this plane, then the box is OUT of the entire volume. return false; } // TODO. This is a simple box detection. The box is not really clipped and sometimes, the box will said // it intersect but it is not the case... Here, We should test the real box volume, against BVolume. // But this is more expensive... return true; } // ============================================================================================ void addElement(CNode *newNode) { nlassert(newNode->QuadNexts[ListIndex]==NULL); newNode->QuadPrevs[ListIndex]= &RootNode; newNode->QuadNexts[ListIndex]= RootNode.QuadNexts[ListIndex]; if(RootNode.QuadNexts[ListIndex]) RootNode.QuadNexts[ListIndex]->QuadPrevs[ListIndex]= newNode; RootNode.QuadNexts[ListIndex]= newNode; } // ============================================================================================ // Insertion of a node in this quad node, or his sons... void insert(const NLMISC::CVector &boxmin, const NLMISC::CVector &boxmax, uint wantdepth, CNode *newNode) { if(Level==0) { // Tous les elements qui sortent du quadtree sont forcément dans le noeud root. if(!includeBoxQuad(boxmin, boxmax)) { // Il faut agrandir la BBox en Y du quadnode. if(BBoxNeverRescale) { BBoxMin.y= boxmin.y; BBoxMax.y= boxmax.y; BBoxNeverRescale= false; } else { BBoxMin.y= std::min(boxmin.y, BBoxMin.y); BBoxMax.y= std::max(boxmax.y, BBoxMax.y); } addElement(newNode); return; } } // Si au moins une partie de l'element n'est pas dans le noeud de ce quad, exit. if(!intersectBoxQuad(boxmin, boxmax)) return; // Que l'on insere ici ou dans les fils, il faut agrandir la BBox en Y du quadnode. if(BBoxNeverRescale) { BBoxMin.y= boxmin.y; BBoxMax.y= boxmax.y; BBoxNeverRescale= false; } else { BBoxMin.y= std::min(boxmin.y, BBoxMin.y); BBoxMax.y= std::max(boxmax.y, BBoxMax.y); } // Si on est au bon, niveau, on a plus qu'à l'insérer dans ce node. if(wantdepth==Level) { addElement(newNode); } else { // Si le quad est une feuille, il faut le splitter (car on est pas encore arrivé au bon niveau). if(isLeaf()) split(); // Et on cherche à mettre l'élément dans un de ces noeuds. Sons[0]->insert(boxmin, boxmax, wantdepth, newNode); Sons[1]->insert(boxmin, boxmax, wantdepth, newNode); Sons[2]->insert(boxmin, boxmax, wantdepth, newNode); Sons[3]->insert(boxmin, boxmax, wantdepth, newNode); } } // ============================================================================================ // Selection. void selectLocalNodes(CBaseNode &selroot) { CBaseNode *p= RootNode.QuadNexts[ListIndex]; while(p) { if(!p->isSelected()) { p->Prev= &selroot; p->Next= selroot.Next; if(selroot.Next) selroot.Next->Prev= p; selroot.Next= p; } p=p->QuadNexts[ListIndex]; } } // ============================================================================================ void selectAll(CBaseNode &selroot) { selectLocalNodes(selroot); if(!isLeaf()) { Sons[0]->selectAll(selroot); Sons[1]->selectAll(selroot); Sons[2]->selectAll(selroot); Sons[3]->selectAll(selroot); } } // ============================================================================================ void select(CBaseNode &selroot, const NLMISC::CVector &bboxmin, const NLMISC::CVector &bboxmax) { // TODO: // ya un bug avec le level0: en effet la bbox n'a pas été agrandie pour contenir les elements. if(!intersectBox(bboxmin, bboxmax)) return; selectLocalNodes(selroot); if(!isLeaf()) { Sons[0]->select(selroot, bboxmin, bboxmax); Sons[1]->select(selroot, bboxmin, bboxmax); Sons[2]->select(selroot, bboxmin, bboxmax); Sons[3]->select(selroot, bboxmin, bboxmax); } } // ============================================================================================ void select(CBaseNode &selroot, std::vector<NLMISC::CPlane> &BVolume) { // TODO: // ya un bug avec le level0: en effet la bbox n'a pas été agrandie pour contenir les elements. if(!intersectBox(BVolume)) return; selectLocalNodes(selroot); if(!isLeaf()) { Sons[0]->select(selroot, BVolume); Sons[1]->select(selroot, BVolume); Sons[2]->select(selroot, BVolume); Sons[3]->select(selroot, BVolume); } } }; // ================= // ================= // Attributes/Methods/iterators.. // ================= // ================= private:// Attributes. CQuadNode _QuadRoot; CBaseNode _Selection; uint _DepthMax; float _Size; NLMISC::CMatrix _ChangeBasis; private:// Methods. public: // CLASS const_iterator. class const_iterator { public: const_iterator() {_Ptr=NULL;} const_iterator(CNode *p) : _Ptr(p) {} const_iterator(const CIterator& x) : _Ptr(x._Ptr) {} const T& operator*() const {return _Ptr->Value; } // Doesn't work... /*const T* operator->() const {return (&**this); }*/ const_iterator& operator++() {_Ptr = (CNode*)(_Ptr->Next); return (*this); } const_iterator operator++(int) {const_iterator tmp = *this; ++*this; return (tmp); } const_iterator& operator--() {_Ptr = (CNode*)(_Ptr->Prev); return (*this); } const_iterator operator--(int) {const_iterator tmp = *this; --*this; return (tmp); } bool operator==(const const_iterator& x) const {return (_Ptr == x._Ptr); } bool operator!=(const const_iterator& x) const {return (!(*this == x)); } protected: CNode *_Ptr; friend class CQuadTree<T>; friend class CIterator; }; // CLASS CIterator class CIterator : public const_iterator { public: CIterator() {_Ptr=NULL;} CIterator(CNode *p) : const_iterator(p) {} T& operator*() const {return _Ptr->Value; } // Doesn't work... /*T* operator->() const {return (&**this); }*/ CIterator& operator++() {_Ptr = (CNode*)(_Ptr->Next); return (*this); } CIterator operator++(int) {CIterator tmp = *this; ++*this; return (tmp); } CIterator& operator--() {_Ptr = (CNode*)(_Ptr->Prev); return (*this); } CIterator operator--(int) {CIterator tmp = *this; --*this; return (tmp); } bool operator==(const const_iterator& x) const {return (_Ptr == x._Ptr); } bool operator!=(const const_iterator& x) const {return (!(*this == x)); } protected: friend class CQuadTree<T>; }; }; // ============================================================================================ // ============================================================================================ // Template CQuadTree implementation. Construction/Destruction. // ============================================================================================ // ============================================================================================ // ============================================================================================ template<class T> CQuadTree<T>::CQuadTree() { _Selection.Next= NULL; _DepthMax= 0; _QuadRoot.BBoxMin.set(-0.5, 0, -0.5); _QuadRoot.BBoxMax.set( 0.5, 0, 0.5); _Size=1; _ChangeBasis.identity(); } // ============================================================================================ template<class T> void CQuadTree<T>::changeBase(const NLMISC::CMatrix& base) { _ChangeBasis=base; } // ============================================================================================ template<class T> CQuadTree<T>::~CQuadTree<T>() { clear(); } // ============================================================================================ template<class T> void CQuadTree<T>::clear() { _QuadRoot.clear(); _Selection.Next= NULL; } // ============================================================================================ template<class T> void CQuadTree<T>::create(uint DepthMax, const NLMISC::CVector& center, float size) { NLMISC::CVector mycenter=_ChangeBasis*center; clear(); _DepthMax= DepthMax; _Size= size; _QuadRoot.BBoxMin= mycenter-NLMISC::CVector(size/2, 0 , size/2); _QuadRoot.BBoxMax= mycenter+NLMISC::CVector(size/2, 0 , size/2); } // ============================================================================================ // ============================================================================================ // Template CQuadTree implementation. Element Insertion/Deletion. // ============================================================================================ // ============================================================================================ // ============================================================================================ template<class T> void CQuadTree<T>::eraseAll() { CIterator it; std::vector<CIterator> its; // First, make a copy of all elements. selectAll(); for(it= begin();it!=end();it++) { its.push_back(it); } // Then erase them. Must do it OUTSIDE the select loop. for(sint i=0;i<(sint)its.size();i++) { erase(its[i]); } } // ============================================================================================ template<class T> void CQuadTree<T>::erase(CIterator it) { CNode *p=it._Ptr; if(p) { // Clear links. p->clear(); // delete it!! delete p; } } // ============================================================================================ template<class T> CQuadTree<T>::CIterator CQuadTree<T>::insert(const NLMISC::CVector &bboxmin, const NLMISC::CVector &bboxmax, const T &val) { NLMISC::CVector myboxmin2=_ChangeBasis*bboxmin; NLMISC::CVector myboxmax2=_ChangeBasis*bboxmax; NLMISC::CVector myboxmin (std::min (myboxmin2.x, myboxmax2.x), std::min (myboxmin2.y, myboxmax2.y), std::min (myboxmin2.z, myboxmax2.z)); NLMISC::CVector myboxmax (std::max (myboxmin2.x, myboxmax2.x), std::max (myboxmin2.y, myboxmax2.y), std::max (myboxmin2.z, myboxmax2.z)); CNode *newNode=new CNode(val); nlassert(myboxmax.x>=myboxmin.x); nlassert(myboxmax.y>=myboxmin.y); nlassert(myboxmax.z>=myboxmin.z); float boxsize= std::max(myboxmax.x-myboxmin.x, myboxmax.z-myboxmin.z ); // Prevent float precision problems. Increase bbox size a little. boxsize*=1.01f; // We must find the level quad which is just bigger. float wantsize=_Size; uint wantdepth=0; while(boxsize<wantsize/2 && wantdepth<_DepthMax) { wantsize/=2; wantdepth++; } _QuadRoot.insert(myboxmin, myboxmax, wantdepth, newNode); return CIterator(newNode); } // ============================================================================================ // ============================================================================================ // Template CQuadTree implementation. Quad Selection, element iteration. // ============================================================================================ // ============================================================================================ // ============================================================================================ template<class T> void CQuadTree<T>::clearSelection() { CBaseNode *p; while(p=_Selection.Next) { // On retire ce noeud de la selection. Ce qui va modifier implicitement _Selection.Next. if(p->Prev) p->Prev->Next= p->Next; if(p->Next) p->Next->Prev= p->Prev; p->Prev=p->Next=NULL; } } // ============================================================================================ template<class T> void CQuadTree<T>::selectAll() { clearSelection(); _QuadRoot.selectAll(_Selection); } // ============================================================================================ template<class T> void CQuadTree<T>::select(const NLMISC::CVector &bboxmin, const NLMISC::CVector &bboxmax) { NLMISC::CVector myboxmin2=_ChangeBasis*bboxmin; NLMISC::CVector myboxmax2=_ChangeBasis*bboxmax; NLMISC::CVector bboxminCopy (std::min (myboxmin2.x, myboxmax2.x), std::min (myboxmin2.y, myboxmax2.y), std::min (myboxmin2.z, myboxmax2.z)); NLMISC::CVector bboxmaxCopy (std::max (myboxmin2.x, myboxmax2.x), std::max (myboxmin2.y, myboxmax2.y), std::max (myboxmin2.z, myboxmax2.z)); clearSelection(); _QuadRoot.select(_Selection, bboxminCopy, bboxmaxCopy); } // ============================================================================================ template<class T> void CQuadTree<T>::select(const std::vector<NLMISC::CPlane> &BVolume) { std::vector<NLMISC::CPlane> BVolumeCopy; BVolumeCopy.resize (BVolume.size()); for (int i=0; i<(int)BVolumeCopy.size(); i++) { BVolumeCopy[i]=BVolume[i]*((_ChangeBasis).inverted()); } clearSelection(); _QuadRoot.select(_Selection, BVolumeCopy); } // ============================================================================================ template<class T> void CQuadTree<T>::selectRay(const NLMISC::CVector& source, const NLMISC::CVector& dir) { NLMISC::CMatrix mat; mat.identity (); // Set a wrong matrix NLMISC::CVector vTmp=dir^((fabs(vTmp*CVector(1,0,0))>0.f)?CVector(1,0,0):CVector(0,1,0)); mat.setRot (dir, vTmp, dir^vTmp); // Normalize it Yoyo! mat.normalize (NLMISC::CMatrix::XYZ); // Get the planes.. std::vector<NLMISC::CPlane> BVolume; BVolume.reserve (4); // Setup the planes NLMISC::CPlane plane; plane.make (mat.getJ(), source); BVolume.push_back (plane); plane.make (mat.getK(), source); BVolume.push_back (plane); plane.make (-mat.getJ(), source); BVolume.push_back (plane); plane.make (-mat.getK(), source); BVolume.push_back (plane); // Select the nodes select (BVolume); } // ============================================================================================ template<class T> void CQuadTree<T>::selectSegment(const NLMISC::CVector& source, const NLMISC::CVector& dest) { NLMISC::CMatrix mat; mat.identity (); // Set a wrong matrix CVector dir=dest-source; NLMISC::CVector vTmp=dir^((fabs(vTmp*CVector(1,0,0))>0.f)?CVector(1,0,0):CVector(0,1,0)); mat.setRot (dir, vTmp, dir^vTmp); // Normalize it Yoyo! mat.normalize (NLMISC::CMatrix::XYZ); // Get the planes.. std::vector<NLMISC::CPlane> BVolume; BVolume.reserve (4); // Setup the planes NLMISC::CPlane plane; plane.make (mat.getJ(), source); BVolume.push_back (plane); plane.make (mat.getK(), source); BVolume.push_back (plane); plane.make (-mat.getJ(), source); BVolume.push_back (plane); plane.make (-mat.getK(), source); BVolume.push_back (plane); plane.make (mat.getI(), dest); BVolume.push_back (plane); plane.make (-mat.getI(), source); BVolume.push_back (plane); // Select the nodes select (BVolume); } // ============================================================================================ template<class T> CQuadTree<T>::CIterator CQuadTree<T>::begin() { return (CNode*)(_Selection.Next); } // ============================================================================================ template<class T> CQuadTree<T>::CIterator CQuadTree<T>::end() { return CIterator(NULL); } } #endif