octree_template.h
Go to the documentation of this file.
00001 /****************************************************************************
00002 * VCGLib                                                            o o     *
00003 * Visual and Computer Graphics Library                            o     o   *
00004 *                                                                _   O  _   *
00005 * Copyright(C) 2004                                                \/)\/    *
00006 * Visual Computing Lab                                            /\/|      *
00007 * ISTI - Italian National Research Council                           |      *
00008 *                                                                    \      *
00009 * All rights reserved.                                                      *
00010 *                                                                           *
00011 * This program is free software; you can redistribute it and/or modify      *
00012 * it under the terms of the GNU General Public License as published by      *
00013 * the Free Software Foundation; either version 2 of the License, or         *
00014 * (at your option) any later version.                                       *
00015 *                                                                           *
00016 * This program is distributed in the hope that it will be useful,           *
00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020 * for more details.                                                         *
00021 *                                                                           *
00022 ****************************************************************************/
00023 
00024 
00025 #ifndef VCG_SPACE_INDEX_OCTREETEMPLATE_H
00026 #define VCG_SPACE_INDEX_OCTREETEMPLATE_H
00027 
00028 #include <vcg/space/point3.h>
00029 #include <vcg/space/box3.h>
00030 #include <vector>
00031 
00032 
00033 namespace vcg
00034 {
00035 
00036 /* Octree Template
00037 
00038 Tiene un dataset volumetrico come un octree
00039 Assunzione che la grandezza sia una potenza di due
00040 La prof max e' fissa.
00041 E' un octree in cui il dato e' nella cella dell'octree.
00042 Anche i nodi non foglia hanno il dato Voxel
00043 
00044 Assunzioni sul tipo voxel:
00045 che abbia definiti gli operatori per poterci fare sopra pushpull.
00046 
00047 Si tiene int invece di puntatori per garantirsi reallocazione dinamica.
00048 I dati veri e propri stanno in un vettore di nodi
00049 */
00050 
00051 template <typename VOXEL_TYPE, class SCALAR_TYPE>
00052 class OctreeTemplate
00053 {
00054 protected:
00055         struct Node;
00056 
00057 public:
00058         // Octree Type Definitions
00059         typedef unsigned long long                              ZOrderType;
00060         typedef SCALAR_TYPE                                                             ScalarType;
00061         typedef VOXEL_TYPE                                      VoxelType;
00062         typedef VoxelType                                                         * VoxelPointer;
00063         typedef vcg::Point3i                                                    CenterType;
00064         static const ScalarType                                         EXPANSION_FACTOR;
00065         typedef Node                                                                                    NodeType;
00066         typedef int                                                                                             NodeIndex;
00067         typedef NodeType                                                                * NodePointer;
00068         typedef vcg::Box3<ScalarType>                   BoundingBoxType;
00069         typedef vcg::Point3<ScalarType> CoordinateType;
00070 
00071 protected:
00072         /*
00073         * Inner structures:
00074         * Contains the information related to the octree node
00075         */
00076         struct Node
00077         {
00078                 // Default constructor: fill the data members with non-meaningful values
00079                 Node()
00080                 {
00081                         parent = NULL;
00082                         level  = -1;
00083                 }
00084 
00085                 // Constructor: create a new Node
00086                 Node(NodePointer parent, int level)
00087                 {
00088                         this->parent    = parent;
00089                         this->level             = (char) level;
00090                 }
00091 
00092     virtual NodePointer &Son(int sonIndex) = 0;
00093 
00094     virtual bool        IsLeaf() = 0;
00095 
00096                 // The position of the center of the node in integer coords in the 0..2^(2*sz) -1 range
00097                 // The root has position (lsz/2,lsz/2,lsz/2)
00098                 CenterType      center;
00099                 char                            level;
00100                 NodePointer parent;
00101                 VoxelType               voxel;
00102         };
00103 
00104         /*
00105         * Inner struct: Node
00106         */
00107         struct InnerNode : public Node
00108         {
00109                 InnerNode() : Node() {};
00110                 InnerNode(NodePointer parent, int level) : Node(parent, level)
00111                 {
00112                         memset(&sons[0], 0, 8*sizeof(Node*));
00113                 }
00114 
00115                 inline NodePointer &Son(int sonIndex)
00116                 {
00117                         assert(0<=sonIndex && sonIndex<=8);
00118                         return sons[sonIndex];
00119                 }
00120 
00121                 inline bool IsLeaf()
00122                 {
00123                         return false;
00124                 }
00125 
00126                 NodePointer sons[8];
00127         };
00128 
00129         /*
00130         * Inner struct: Leaf
00131         */
00132         struct Leaf : public Node
00133         {
00134                 Leaf() : Node() {};
00135                 Leaf(NodePointer parent, int level) : Node(parent, level) {}
00136 
00137                 inline NodePointer &Son(int /*sonIndex*/)
00138                 {
00139                         assert(false);
00140                         static NodePointer p = NULL;
00141                         return p;
00142                 }
00143 
00144                 inline bool IsLeaf()
00145                 {
00146                         return true;
00147                 }
00148         };
00149 
00150 public:
00151         // Inizializza l'octree
00152         void Initialize(int maximumDepth)
00153         {
00154                 this->maximumDepth      = maximumDepth;
00155                 size    = 1<< maximumDepth;                     // e.g. 1*2^maxDepth
00156                 lSize = 1<<(maximumDepth+1);    // e.g. 1*2^(maxDepth+1)
00157 
00158                 InnerNode *root = new InnerNode(NULL,0);
00159                 nodes.clear();
00160                 nodes.push_back( root );
00161                 root->center    = CenterType(size, size, size);
00162 
00163                 ScalarType szf = (ScalarType) size;
00164                 leafDimension  = boundingBox.Dim();
00165                 leafDimension /= szf;
00166                 leafDiagonal     = leafDimension.Norm();
00167         };
00168 
00169         // Return the octree bounding-box
00170         inline BoundingBoxType          BoundingBox()                                                           { return boundingBox;                           }
00171 
00172         // Return the Voxel of the n-th node
00173         inline VoxelPointer     Voxel(const NodePointer n)                              { return &(n->voxel);                           }
00174 
00175         // Return the octree node count
00176         inline int                                      NodeCount()                                                                     const { return int(nodes.size()); }
00177 
00178         // Return the root index
00179         inline                          NodePointer Root()                                      const   { return nodes[0];                                      }
00180 
00181         // Return the level of the n-th node
00182         inline char                                     Level(const NodePointer n)      const   { return n->level;                                      }
00183 
00184         // Return the referente to the i-th son of the n-th node
00185         inline NodePointer&     Son(NodePointer n, int i)               const   { return n->Son(i);                                     }
00186 
00187         // Return the parent index of the n-th node
00188         inline NodePointer      Parent(const NodePointer n) const       { return n->parent;                                     }
00189 
00190         // Return the index of the current node in its father
00191         int WhatSon(NodePointer n) const
00192         {
00193                 if(n==Root())
00194                         assert(false);
00195 
00196                 NodePointer parent = Parent(n);
00197                 for(int i=0;i<8;++i)
00198                         if(parent->Son(i)==n)
00199                                 return i;
00200 
00201                 return -1;
00202         }
00203 
00204         // Return the center of the n-th node
00205         inline CenterType CenterInOctreeCoordinates(const NodePointer n) const { return n->center;}
00206 
00211         inline void CenterInWorldCoordinates(const NodePointer n, CoordinateType &wc_Center) const
00212         {
00213                 assert(0<=n && n<NodeCount());
00214 
00215                 int shift = maximumDepth - Level(n) + 1;
00216                 CoordinateType ocCenter = CenterInOctreeCoordinates(n);
00217                 CoordinateType nodeSize = boundingBox.Dim()/float(1<<Level(n));
00218                 wc_Center.X() = boundingBox.min.X() + (nodeSize.X()*(0.5f+(ocCenter.X()>>shift)));
00219                 wc_Center.Y() = boundingBox.min.Y() + (nodeSize.Y()*(0.5f+(ocCenter.Y()>>shift)));
00220                 wc_Center.Z() = boundingBox.min.Z() + (nodeSize.Z()*(0.5f+(ocCenter.Z()>>shift)));
00221         };
00222 
00223         // Given a node (even not leaf) it returns the center of the box it represent.
00224         // the center is expressed not in world-coordinates.
00225         // e.g. the root is (sz/2,sz/2,sz/2);
00226         // and the finest element in the grid in lower left corner has center (.5, .5, .5)
00227         /*
00228 
00229         4----------------   4----------------   4----------------
00230         |   |   |   |   |   |       |       |   |               |
00231         3---+---+---+---|   3       |       |   3               |
00232         |   |   |   |   |   |       |       |   |               |
00233         2---+---+---+---|   2---+---+---+---|   2       c       |
00234         |   |   |   |   |   |       |       |   |               |
00235         1---+---+---+---|   1   b   +       |   1               |
00236         | a |   |   |   |   |       |       |   |               |
00237         0---1---2---3---4   0---1---2---3---4   0---1---2---3---4
00238 
00239         This is a tree with maxdepth==2, so sz is 2^2=4
00240 
00241         a) a leaf at the deepest level 2 has position (.5,.5)
00242         b) a mid node (lev 1) has position (1,1)
00243         c) root has level 0 and position (sz/2,sz/2) = (2,2)
00244 
00245         The center of a node has integer coords in the 2^(MaxDepth+1) range.
00246 
00247         The other approach is to use position as a bit string
00248         codifying the tree path, but in this case you have to
00249         supply also the level (e.g. the string length)
00250         you desire. The lower left corner node is always 0 ( (,) for the root (0,0) level 1, and (00,00) for level 2)
00251 
00252         |              ~~~              |
00253         |      0~~      |      1~~      |
00254         |  00~  |  01~  |  10~  |  11~  |
00255         |000|001|010|011|100|101|110|111|
00256 
00257         The interesting properties is that
00258         if your octree represent a space [minv,maxv] and you want
00259         to find the octree cell containing a point p in [minv,maxv]
00260         you just have to convert p in the range [0,sz) truncate it to an integer and use it as a path.
00261         For example, consider an octree of depth 3, representing a range [0..100)
00262         sz=8 (each cell contains form 0 to 12.5
00263         the point
00264         5               -> 0.4  -> path is 000
00265         45      -> 3.6  -> path is 011
00266         50  -> 4.0      -> path is 100
00267         100 -> 8                -> ERROR the interval is right open!!!
00268 
00269         Note how each cell is meant to contains a right open interval (e.g. the first cell contains [0,12.5) and the second [12.5,25) and so on)
00270 
00271         The center of each cell can simply be obtained by adding .5 to the path of the leaves.
00272         */
00273         CoordinateType Center(NodePointer n) const
00274         {
00275                 CoordinateType center;
00276                 center.Import(GetPath(n));
00277                 center+=Point3f(.5f,.5f,.5f);
00278 
00279                 //TODO verify the assert
00280                 assert(center==nodes[n]->center);
00281 
00282                 return center;
00283         }
00284 
00285         // Return the bounding-box of the n-th node expressed in world-coordinate
00286         BoundingBoxType BoundingBoxInWorldCoordinates(const NodePointer n)
00287         {
00288                 char level = Level(n);
00289                 int shift  = maximumDepth-level+1;
00290                 CoordinateType  nodeDim = boundingBox.Dim()/float(1<<level);
00291                 CenterType                      center   = CenterInOctreeCoordinates(n);
00292                 BoundingBoxType nodeBB;
00293                 nodeBB.min.X() = boundingBox.min.X() + (nodeDim.X()*(center.X()>>shift));
00294                 nodeBB.min.Y() = boundingBox.min.Y() + (nodeDim.Y()*(center.Y()>>shift));
00295                 nodeBB.min.Z() = boundingBox.min.Z() + (nodeDim.Z()*(center.Z()>>shift));
00296                 nodeBB.max = nodeBB.min+nodeDim;
00297                 return nodeBB;
00298         };
00299 
00305         inline void BoundingBoxInWorldCoordinates(const NodePointer n, BoundingBoxType &wc_bb) const
00306         {
00307                 char level = Level(n);
00308                 int      shift = maximumDepth - level + 1;
00309                 CoordinateType node_dimension = boundingBox.Dim()/ScalarType(1<<level);
00310                 wc_bb.min.X()   = boundingBox.min.X()+(node_dimension.X()*(n->center.X()>>shift));
00311                 wc_bb.min.Y()   = boundingBox.min.Y()+(node_dimension.Y()*(n->center.Y()>>shift));
00312                 wc_bb.min.Z()   = boundingBox.min.Z()+(node_dimension.Z()*(n->center.Z()>>shift));
00313                 wc_bb.max       = wc_bb.min+node_dimension;
00314         };
00315 
00316         // Return one of the 8 subb box of a given box.
00317         BoundingBoxType SubBox(BoundingBoxType &lbb, int i)
00318         {
00319                 BoundingBoxType bs;
00320                 if (i&1)        bs.min.X()=(lbb.min.X()+(bs.max.X()=lbb.max.X()))/2.0f;
00321                 else                    bs.max.X()=((bs.min.X()=lbb.min.X())+lbb.max.X())/2.0f;
00322                 if (i&2)        bs.min.Y()=(lbb.min.Y()+(bs.max.Y()=lbb.max.Y()))/2.0f;
00323                 else                    bs.max.Y()=((bs.min.Y()=lbb.min.Y())+lbb.max.Y())/2.0f;
00324                 if (i&4)        bs.min.Z()=(lbb.min.Z()+(bs.max.Z()=lbb.max.Z()))/2.0f;
00325                 else                    bs.max.Z()=((bs.min.Z()=lbb.min.Z())+lbb.max.Z())/2.0f;
00326 
00327                 return bs;
00328         }
00329 
00330         // Given the bounding-box and the center (both in world-coordinates)
00331         // of a node, return the bounding-box (in world-coordinats) of the i-th son
00332         BoundingBoxType SubBoxAndCenterInWorldCoordinates(BoundingBoxType &lbb, CoordinateType &center, int i)
00333         {
00334                 BoundingBoxType bs;
00335                 if (i&1)
00336                 {
00337                         bs.min[0]=center[0];
00338                         bs.max[0]=lbb.max[0];
00339                 }
00340                 else
00341                 {
00342                         bs.min[0]=lbb.min[0];
00343                         bs.max[0]=center[0];
00344                 }
00345                 if (i&2)
00346                 {
00347                         bs.min[1]=center[1];
00348                         bs.max[1]=lbb.max[1];
00349                 }
00350                 else
00351                 {
00352                         bs.max[1]=center[1];
00353                         bs.min[1]=lbb.min[1];
00354                 }
00355                 if (i&4)
00356                 {
00357                         bs.min[2]=center[2];
00358                         bs.max[2]=lbb.max[2];
00359                 }
00360                 else
00361                 {
00362                         bs.max[2]=center[2];
00363                         bs.min[2]=lbb.min[2];
00364                 }
00365                 return bs;
00366         };
00367 
00368         /*
00369         * Add a new Node to the octree.
00370         *       The created node is the i-th son of the node pointed to by parent.
00371         *       Return the pointer to the new node
00372         */
00373         NodePointer NewNode(NodePointer parent, int i)
00374         {
00375                 assert(0<=i && i<8);
00376                 assert(Son(parent, i)==NULL);
00377 
00378                 //int index  = NodeCount();
00379                 char level = Level(parent)+1;
00380 
00381                 Node *node = (level<maximumDepth)? (Node*) new InnerNode(parent, level) : (Node*) new Leaf(parent, level);
00382                 nodes.push_back( node );
00383                 Son(parent, i) = node;
00384 
00385                 CenterType *parentCenter = &(parent->center);
00386                 int displacement = 1<<(maximumDepth-level);
00387                 node->center.X() = parentCenter->X() + ((i&1)? displacement : -displacement);
00388                 node->center.Y() = parentCenter->Y() + ((i&2)? displacement : -displacement);
00389                 node->center.Z() = parentCenter->Z() + ((i&4)? displacement : -displacement);
00390 
00391                 return node;
00392         }
00393 
00394         // Aggiunge un nodo all'octree nella posizione specificata e al livello specificato.
00395         // Vengono inoltre inseriti tutti gli antenati mancanti per andare dalla radice
00396         // al nodo ed al livello specificato seguendo path.
00397         NodePointer AddNode(CenterType path)
00398         {
00399                 //the input coordinates must be in the range 0..2^maxdepth
00400                 assert(path[0]>=0 && path[0]<size);
00401                 assert(path[1]>=0 && path[1]<size);
00402                 assert(path[2]>=0 && path[2]<size);
00403 
00404                 NodePointer curNode     = Root();
00405                 int rootLevel                           = 0;
00406                 int shiftLevel                  = maximumDepth-1;
00407 
00408                 while(shiftLevel >= rootLevel)
00409                 {
00410                         int nextSon=0;
00411                         if((path[0]>>shiftLevel)%2) nextSon +=1;
00412                         if((path[1]>>shiftLevel)%2) nextSon +=2;
00413                         if((path[2]>>shiftLevel)%2) nextSon +=4;
00414                         NodePointer nextNode = Son(curNode, nextSon);
00415                         if(nextNode!=NULL) // nessun nodo pu aver Root() per figlio
00416                                 curNode = nextNode;
00417                         else
00418                         {
00419                                 NodePointer newNode = NewNode(curNode, nextSon);
00420                                 assert(Son(curNode, nextSon)==newNode); // TODO delete an assignment
00421                                 curNode=newNode;
00422                         }
00423                         --shiftLevel;
00424                 }
00425                 return curNode;
00426         }
00427 
00432         // Convert the point p coordinates to the integer based representation
00433         // in the range 0..size, where size is 2^maxdepth
00434         CenterType Interize(const CoordinateType &pf) const
00435         {
00436                 CenterType pi;
00437 
00438                 assert(pf.X()>=boundingBox.min.X() &&  pf.X()<=boundingBox.max.X());
00439                 assert(pf.Y()>=boundingBox.min.Y() &&  pf.Y()<=boundingBox.max.Y());
00440                 assert(pf.Z()>=boundingBox.min.Z() &&  pf.Z()<=boundingBox.max.Z());
00441 
00442                 pi.X() = int((pf.X() - boundingBox.min.X()) * size / (boundingBox.max.X() - boundingBox.min.X()));
00443                 pi.Y() = int((pf.Y() - boundingBox.min.Y()) * size / (boundingBox.max.Y() - boundingBox.min.Y()));
00444                 pi.Z() = int((pf.Z() - boundingBox.min.Z()) * size / (boundingBox.max.Z() - boundingBox.min.Z()));
00445 
00446                 return pi;
00447         }
00448 
00449         // Inverse function of Interize;
00450         // Return to the original coords space (not to the original values!!)
00451         CoordinateType DeInterize(const CenterType &pi ) const
00452         {
00453                 CoordinateType pf;
00454 
00455                 assert(pi.X()>=0 && pi.X()<size);
00456                 assert(pi.Y()>=0 && pi.Y()<size);
00457                 assert(pi.Z()>=0 && pi.Z()<size);
00458 
00459                 pf.X() = pi.X() * (boundingBox.max.X() - boundingBox.min.X()) / size + boundingBox.min.X();
00460                 pf.Y() = pi.Y() * (boundingBox.max.Y() - boundingBox.min.Y()) / size + boundingBox.min.Y();
00461                 pf.Z() = pi.Z() * (boundingBox.max.Z() - boundingBox.min.Z()) / size + boundingBox.min.Z();
00462 
00463                 return pf;
00464         }
00465 
00466         // Compute the z-ordering integer value for a given node;
00467         // this value can be used to compute a complete ordering of the nodes of a given level of the octree.
00468         // It assumes that the octree has a max depth of 10.
00469         ZOrderType ZOrder(NodePointer n)                                                                                        const   { return ZOrder(GetPath(n), Level(n)); }
00470         ZOrderType ComputeZOrder(const CoordinateType &query) const { return ZOrder(CenterType::Construct(Interize(query)), maximumDepth); };
00471 
00472         inline ZOrderType ZOrder(const CenterType &path, const char level) const
00473         {
00474                 ZOrderType finalPosition = 0;
00475                 ZOrderType currentPosition;
00476 
00477                 for(int i=0; i<level; ++i)
00478                 {
00479                         currentPosition = 0;
00480                         int mask=1<<i;
00481                         if(path[0]&mask) currentPosition|=1;
00482                         if(path[1]&mask) currentPosition|=2;
00483                         if(path[2]&mask) currentPosition|=4;
00484                         currentPosition = currentPosition<<(i*3);
00485                         finalPosition    |= currentPosition;
00486                 }
00487                 return finalPosition;
00488         };
00489 
00490         // Funzione principale di accesso secondo un path;
00491         // restituisce l'indice del voxel di profondita' massima
00492         // che contiene il punto espresso in range 0..2^maxk
00493         NodePointer DeepestNode(CenterType path, int MaxLev)
00494         {
00495                 assert(path[0]>=0 && path[0]<size);
00496                 assert(path[1]>=0 && path[1]<size);
00497                 assert(path[2]>=0 && path[2]<size);
00498 
00499                 NodePointer curNode     = Root();
00500                 int shift                                               = maximumDepth-1;
00501 
00502                 while(shift && Level(curNode) < MaxLev)
00503                 {
00504                         int son = 0;
00505                         if((path[0]>>shift)%2) son +=1;
00506                         if((path[1]>>shift)%2) son +=2;
00507                         if((path[2]>>shift)%2) son +=4;
00508                         NodePointer nextNode = Son(curNode, son);
00509                         if(nextNode!=NULL)
00510                                 curNode=nextNode;
00511                         else
00512                                 break;
00513 
00514                         --shift;
00515                 }
00516                 return curNode;
00517         }
00518 
00519 
00520         // Return the 'path' from root to the specified node;
00521         // the path is coded as a point3s; each bit of each component code the direction in one level
00522         // only the last 'level' bits of the returned value are meaningful
00523         // for example for the root no bit are meaningfull (path is 0)
00524         // for the first level only one bit of each one of the three components are maninguful;
00525         CenterType GetPath(NodePointer n) const
00526         {
00527                 if(n==Root())
00528                         return CenterType(0,0,0);
00529 
00530                 CenterType      path(0,0,0);
00531 
00532                 int shift, mask, son;
00533                 int startingLevel = int(Level(n));
00534                 while (n!=Root())
00535                 {
00536                         shift = startingLevel-Level(n); //nodes[n].level
00537                         mask = 1 << shift;                                                      // e.g. 1*2^shift
00538                         son = WhatSon(n);
00539                         if(son&1) path[0] |= mask;
00540                         if(son&2) path[1] |= mask;
00541                         if(son&4) path[2] |= mask;
00542                         n = Parent(n);                                                                  // nodes[n].parent
00543                 }
00544                 return path;
00545         }
00546 
00547         // Dato un punto 3D nello spazio restituisce un array contenente
00548         // i puntatori ai nodi che lo contengono, dalla radice fino alle foglie.
00549         // I nodi mancanti dalla radice fino a profondit maxDepth vengono aggiunti.
00550         // In posizione i ci sar il nodo di livello i.
00551         // Restituisce lo z-order del punto p
00552         ZOrderType BuildRoute(const CoordinateType &p, NodePointer *&route)
00553         {
00554                 assert( boundingBox.min.X()<=p.X() && p.X()<=boundingBox.max.X() );
00555                 assert( boundingBox.min.Y()<=p.Y() && p.Y()<=boundingBox.max.Y() );
00556                 assert( boundingBox.min.Z()<=p.Z() && p.Z()<=boundingBox.max.Z() );
00557 
00558                 route[0]                                                = Root();
00559                 NodePointer curNode = Root();
00560                 int shift                                               = maximumDepth-1;
00561                 CenterType path                 = CenterType::Construct(Interize(p));
00562                 while(shift >= 0)
00563                 {
00564                         int son = 0;
00565                         if((path[0]>>shift)%2) son +=1;
00566                         if((path[1]>>shift)%2) son +=2;
00567                         if((path[2]>>shift)%2) son +=4;
00568                         NodePointer nextNode = Son(curNode, son);
00569 
00570                         if(nextNode!=NULL)
00571                         {
00572                                 route[maximumDepth-shift] = nextNode;
00573                                 curNode                                                                         = nextNode;
00574                         }
00575                         else
00576                         {
00577                                 NodePointer newNode = NewNode(curNode, son);
00578                                 route[maximumDepth-shift] = newNode;
00579                                 curNode = newNode;
00580                         }
00581                         --shift;
00582                 }
00583                 return ZOrder(route[maximumDepth]);
00584         }; //end of BuildRoute
00585 
00586 
00587         // Restituisce il percorso dalla radice fino al nodo di profondit
00588         // massima presente nell'octree contenente il nodo p. Nessun nuovo nodo  aggiunto
00589         // all'octree. In route sono inseriti gli indici dei nodi contenti p, dalla radice
00590         // fino al nodo di profontid massima presente; nelle eventuali posizioni rimaste
00591         // libere  inserito il valore -1. Restituisce true se il punto p cade in una foglia
00592         // dell'otree, false altrimenti
00593         bool GetRoute(const CoordinateType &p, NodePointer *&route)
00594         {
00595                 assert( boundingBox.min.X()<=p.X() && p.X()<=boundingBox.max.X() );
00596                 assert( boundingBox.min.Y()<=p.Y() && p.Y()<=boundingBox.max.Y() );
00597                 assert( boundingBox.min.Z()<=p.Z() && p.Z()<=boundingBox.max.Z() );
00598 
00599                 memset(route, NULL, maximumDepth*sizeof(NodePointer));
00600 
00601                 CenterType path                          = CenterType::Construct(Interize(p));
00602                 int shift                                                        = maximumDepth-1;
00603                 NodePointer finalLevel = Root();
00604                 NodePointer curNode              = Root();
00605 
00606                 while(shift >= finalLevel)
00607                 {
00608                         int son=0;
00609                         if((path[0]>>shift)%2) son +=1;
00610                         if((path[1]>>shift)%2) son +=2;
00611                         if((path[2]>>shift)%2) son +=4;
00612                         NodePointer nextNode = Son(curNode, son);
00613                         if(nextNode!=NULL)
00614                         {
00615                                 route[maximumDepth-shift] = nextNode;
00616                                 curNode = nextNode;
00617                         }
00618                         else
00619                                 return false;
00620 
00621                         --shift;
00622                 }
00623                 return true;
00624         }; //end of GetReoute
00625 
00626         // Data una bounding-box bb_query, calcola l'insieme dei nodi di
00627         // profondit depth il cui bounding-box ha intersezione non nulla con
00628         // bb (la bounding-box dell'octree); i puntatori a tali nodi sono
00629         // inseriti progressivamente in contained_nodes.
00630         // The vector nodes must be cleared before calling this method.
00631         void ContainedNodes
00632         (
00633                 BoundingBoxType                                          &query,
00634                 std::vector< NodePointer > &nodes,
00635                 int                                                                                                     depth,
00636                 NodePointer                                                                     n,
00637                 BoundingBoxType                                          &nodeBB)
00638         {
00639                 if (!query.Collide(nodeBB))
00640                         return;
00641 
00642                 if (Level(n)==depth)
00643                         nodes.push_back(n);
00644                 else
00645                 {
00646                         NodePointer     son;
00647                         BoundingBoxType sonBB;
00648                         CoordinateType  nodeCenter = nodeBB.Center();
00649                         for (int s=0; s<8; s++)
00650                         {
00651                                 son = Son(n, s);
00652                                 if (son!=NULL)
00653                                 {
00654                                         sonBB = SubBoxAndCenterInWorldCoordinates(nodeBB, nodeCenter, s);
00655                                         ContainedNodes(query, nodes, depth, son, sonBB);
00656                                 }
00657                         }
00658                 }
00659         }; //end of ContainedNodes
00660 
00661 
00662         // Data una bounding-box bb, calcola l'insieme delle foglie il cui
00663         // bounding-box ha intersezione non nulla con bb; i loro indici
00664         // sono inseriti all'interno di leaves.
00665         void ContainedLeaves(
00666                 BoundingBoxType                                          &query,
00667                 std::vector< NodePointer > &leaves,
00668                 NodePointer                                                                     node,
00669                 BoundingBoxType                                          &nodeBB
00670                 )
00671         {
00672                 NodePointer     son;
00673                 BoundingBoxType sonBB;
00674                 CoordinateType nodeCenter = nodeBB.Center();
00675                 for (int s=0; s<8; s++)
00676                 {
00677                         son = Son(node, s); //nodes[nodeIndex].sonIndex[s]
00678                         if (son!=NULL)
00679                         {
00680                                 sonBB = SubBoxAndCenterInWorldCoordinates(nodeBB, nodeCenter, s);
00681                                 if ( query.Collide(sonBB) )
00682                                 {
00683                                         if ( son->IsLeaf() )
00684                                                 leaves.push_back(son);
00685                                         else
00686                                                 ContainedLeaves(query, leaves, son, sonBB);
00687                                 }
00688                         }
00689                 }
00690         }; //end of ContainedLeaves
00691 
00692 
00693         /*
00694         * Octree Data Members
00695         */
00696 public:
00697         // the size of the finest grid available (2^maxDepth)
00698         int                                             size;
00699 
00700         // double the size(2^maxDepth)
00701         int                                             lSize;
00702 
00703         // The allowed maximum depth
00704         int                                             maximumDepth;
00705 
00706         // The dimension of a leaf
00707         CoordinateType  leafDimension;
00708 
00709         // The diagonal of a leaf
00710         ScalarType                      leafDiagonal;
00711 
00712         // The Octree nodes
00713         std::vector< Node* > nodes;
00714 
00715         // The bounding box containing the octree (in world coordinate)
00716         BoundingBoxType                         boundingBox;
00717 }; //end of class OctreeTemplate
00718 
00719 template <typename VOXEL_TYPE, class SCALAR_TYPE>
00720 const SCALAR_TYPE OctreeTemplate<VOXEL_TYPE, SCALAR_TYPE>::EXPANSION_FACTOR  = SCALAR_TYPE(0.035);
00721 }
00722 
00723 #endif //VCG_SPACE_INDEX_OCTREETEMPLATE_H


shape_reconstruction
Author(s): Roberto Martín-Martín
autogenerated on Sat Jun 8 2019 18:33:33