Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 #include <stdlib.h> 00030 #include <math.h> 00031 #include <algorithm> 00032 00033 ///////////// 00034 // OctNode // 00035 ///////////// 00036 00037 namespace pcl 00038 { 00039 namespace poisson 00040 { 00041 00042 template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthShift=5; 00043 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift=19; 00044 template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthMask=(1<<DepthShift)-1; 00045 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetMask=(1<<OffsetShift)-1; 00046 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift1=DepthShift; 00047 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift2=OffsetShift1+OffsetShift; 00048 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift3=OffsetShift2+OffsetShift; 00049 00050 template<class NodeData,class Real> int OctNode<NodeData,Real>::UseAlloc=0; 00051 template<class NodeData,class Real> Allocator<OctNode<NodeData,Real> > OctNode<NodeData,Real>::internalAllocator; 00052 00053 template<class NodeData,class Real> 00054 void OctNode<NodeData,Real>::SetAllocator(int blockSize) 00055 { 00056 if(blockSize>0) 00057 { 00058 UseAlloc=1; 00059 internalAllocator.set(blockSize); 00060 } 00061 else{UseAlloc=0;} 00062 } 00063 template<class NodeData,class Real> 00064 int OctNode<NodeData,Real>::UseAllocator(void){return UseAlloc;} 00065 00066 template <class NodeData,class Real> 00067 OctNode<NodeData,Real>::OctNode(void){ 00068 parent=children=NULL; 00069 d=off[0]=off[1]=off[2]=0; 00070 } 00071 00072 template <class NodeData,class Real> 00073 OctNode<NodeData,Real>::~OctNode(void){ 00074 if(!UseAlloc){if(children){delete[] children;}} 00075 parent=children=NULL; 00076 } 00077 template <class NodeData,class Real> 00078 void OctNode<NodeData,Real>::setFullDepth(int maxDepth){ 00079 if( maxDepth ) 00080 { 00081 if( !children ) initChildren(); 00082 for( int i=0 ; i<8 ; i++ ) children[i].setFullDepth( maxDepth-1 ); 00083 } 00084 } 00085 00086 template <class NodeData,class Real> 00087 int OctNode<NodeData,Real>::initChildren(void){ 00088 int i,j,k; 00089 00090 if(UseAlloc){children=internalAllocator.newElements(8);} 00091 else{ 00092 if(children){delete[] children;} 00093 children=NULL; 00094 children=new OctNode[Cube::CORNERS]; 00095 } 00096 if(!children){ 00097 fprintf(stderr,"Failed to initialize children in OctNode::initChildren\n"); 00098 exit(0); 00099 return 0; 00100 } 00101 int d,off[3]; 00102 depthAndOffset(d,off); 00103 for(i=0;i<2;i++){ 00104 for(j=0;j<2;j++){ 00105 for(k=0;k<2;k++){ 00106 int idx=Cube::CornerIndex(i,j,k); 00107 children[idx].parent=this; 00108 children[idx].children=NULL; 00109 int off2[3]; 00110 off2[0]=(off[0]<<1)+i; 00111 off2[1]=(off[1]<<1)+j; 00112 off2[2]=(off[2]<<1)+k; 00113 Index(d+1,off2,children[idx].d,children[idx].off); 00114 } 00115 } 00116 } 00117 return 1; 00118 } 00119 template <class NodeData,class Real> 00120 inline void OctNode<NodeData,Real>::Index(int depth,const int offset[3],short& d,short off[3]){ 00121 d=short(depth); 00122 off[0]=short((1<<depth)+offset[0]-1); 00123 off[1]=short((1<<depth)+offset[1]-1); 00124 off[2]=short((1<<depth)+offset[2]-1); 00125 } 00126 00127 template<class NodeData,class Real> 00128 inline void OctNode<NodeData,Real>::depthAndOffset(int& depth,int offset[3]) const { 00129 depth=int(d); 00130 offset[0]=(int(off[0])+1)&(~(1<<depth)); 00131 offset[1]=(int(off[1])+1)&(~(1<<depth)); 00132 offset[2]=(int(off[2])+1)&(~(1<<depth)); 00133 } 00134 template<class NodeData,class Real> 00135 inline int OctNode<NodeData,Real>::depth(void) const {return int(d);} 00136 template<class NodeData,class Real> 00137 inline void OctNode<NodeData,Real>::DepthAndOffset(const long long& index,int& depth,int offset[3]){ 00138 depth=int(index&DepthMask); 00139 offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<<depth)); 00140 offset[1]=(int((index>>OffsetShift2)&OffsetMask)+1)&(~(1<<depth)); 00141 offset[2]=(int((index>>OffsetShift3)&OffsetMask)+1)&(~(1<<depth)); 00142 } 00143 template<class NodeData,class Real> 00144 inline int OctNode<NodeData,Real>::Depth(const long long& index){return int(index&DepthMask);} 00145 template <class NodeData,class Real> 00146 void OctNode<NodeData,Real>::centerAndWidth(Point3D<Real>& center,Real& width) const{ 00147 int depth,offset[3]; 00148 depth=int(d); 00149 offset[0]=(int(off[0])+1)&(~(1<<depth)); 00150 offset[1]=(int(off[1])+1)&(~(1<<depth)); 00151 offset[2]=(int(off[2])+1)&(~(1<<depth)); 00152 width=Real(1.0/(1<<depth)); 00153 for(int dim=0;dim<DIMENSION;dim++){center.coords[dim]=Real(0.5+offset[dim])*width;} 00154 } 00155 template< class NodeData , class Real > 00156 bool OctNode< NodeData , Real >::isInside( Point3D< Real > p ) const 00157 { 00158 Point3D< Real > c; 00159 Real w; 00160 centerAndWidth( c , w ); 00161 w /= 2; 00162 return (c[0]-w)<p[0] && p[0]<=(c[0]+w) && (c[1]-w)<p[1] && p[1]<=(c[1]+w) && (c[2]-w)<p[2] && p[2]<=(c[2]+w); 00163 } 00164 template <class NodeData,class Real> 00165 inline void OctNode<NodeData,Real>::CenterAndWidth(const long long& index,Point3D<Real>& center,Real& width){ 00166 int depth,offset[3]; 00167 depth=index&DepthMask; 00168 offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<<depth)); 00169 offset[1]=(int((index>>OffsetShift2)&OffsetMask)+1)&(~(1<<depth)); 00170 offset[2]=(int((index>>OffsetShift3)&OffsetMask)+1)&(~(1<<depth)); 00171 width=Real(1.0/(1<<depth)); 00172 for(int dim=0;dim<DIMENSION;dim++){center.coords[dim]=Real(0.5+offset[dim])*width;} 00173 } 00174 00175 template <class NodeData,class Real> 00176 int OctNode<NodeData,Real>::maxDepth(void) const{ 00177 if(!children){return 0;} 00178 else{ 00179 int c,d; 00180 for(int i=0;i<Cube::CORNERS;i++){ 00181 d=children[i].maxDepth(); 00182 if(!i || d>c){c=d;} 00183 } 00184 return c+1; 00185 } 00186 } 00187 template <class NodeData,class Real> 00188 int OctNode<NodeData,Real>::nodes(void) const{ 00189 if(!children){return 1;} 00190 else{ 00191 int c=0; 00192 for(int i=0;i<Cube::CORNERS;i++){c+=children[i].nodes();} 00193 return c+1; 00194 } 00195 } 00196 template <class NodeData,class Real> 00197 int OctNode<NodeData,Real>::leaves(void) const{ 00198 if(!children){return 1;} 00199 else{ 00200 int c=0; 00201 for(int i=0;i<Cube::CORNERS;i++){c+=children[i].leaves();} 00202 return c; 00203 } 00204 } 00205 template<class NodeData,class Real> 00206 int OctNode<NodeData,Real>::maxDepthLeaves(int maxDepth) const{ 00207 if(depth()>maxDepth){return 0;} 00208 if(!children){return 1;} 00209 else{ 00210 int c=0; 00211 for(int i=0;i<Cube::CORNERS;i++){c+=children[i].maxDepthLeaves(maxDepth);} 00212 return c; 00213 } 00214 } 00215 template <class NodeData,class Real> 00216 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::root(void) const{ 00217 const OctNode* temp=this; 00218 while(temp->parent){temp=temp->parent;} 00219 return temp; 00220 } 00221 00222 00223 template <class NodeData,class Real> 00224 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextBranch( const OctNode* current ) const 00225 { 00226 if( !current->parent || current==this ) return NULL; 00227 if(current-current->parent->children==Cube::CORNERS-1) return nextBranch( current->parent ); 00228 else return current+1; 00229 } 00230 template <class NodeData,class Real> 00231 OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextBranch(OctNode* current){ 00232 if(!current->parent || current==this){return NULL;} 00233 if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);} 00234 else{return current+1;} 00235 } 00236 template< class NodeData , class Real > 00237 const OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( const OctNode* current ) const 00238 { 00239 if( !current->parent || current==this ) return NULL; 00240 if( current-current->parent->children==0 ) return prevBranch( current->parent ); 00241 else return current-1; 00242 } 00243 template< class NodeData , class Real > 00244 OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( OctNode* current ) 00245 { 00246 if( !current->parent || current==this ) return NULL; 00247 if( current-current->parent->children==0 ) return prevBranch( current->parent ); 00248 else return current-1; 00249 } 00250 template <class NodeData,class Real> 00251 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextLeaf(const OctNode* current) const{ 00252 if(!current){ 00253 const OctNode<NodeData,Real>* temp=this; 00254 while(temp->children){temp=&temp->children[0];} 00255 return temp; 00256 } 00257 if(current->children){return current->nextLeaf();} 00258 const OctNode* temp=nextBranch(current); 00259 if(!temp){return NULL;} 00260 else{return temp->nextLeaf();} 00261 } 00262 template <class NodeData,class Real> 00263 OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextLeaf(OctNode* current){ 00264 if(!current){ 00265 OctNode<NodeData,Real>* temp=this; 00266 while(temp->children){temp=&temp->children[0];} 00267 return temp; 00268 } 00269 if(current->children){return current->nextLeaf();} 00270 OctNode* temp=nextBranch(current); 00271 if(!temp){return NULL;} 00272 else{return temp->nextLeaf();} 00273 } 00274 00275 template <class NodeData,class Real> 00276 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextNode( const OctNode* current ) const 00277 { 00278 if( !current ) return this; 00279 else if( current->children ) return ¤t->children[0]; 00280 else return nextBranch(current); 00281 } 00282 template <class NodeData,class Real> 00283 OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextNode( OctNode* current ) 00284 { 00285 if( !current ) return this; 00286 else if( current->children ) return ¤t->children[0]; 00287 else return nextBranch( current ); 00288 } 00289 00290 template <class NodeData,class Real> 00291 void OctNode<NodeData,Real>::printRange(void) const{ 00292 Point3D<Real> center; 00293 Real width; 00294 centerAndWidth(center,width); 00295 for(int dim=0;dim<DIMENSION;dim++){ 00296 printf("%[%f,%f]",center.coords[dim]-width/2,center.coords[dim]+width/2); 00297 if(dim<DIMENSION-1){printf("x");} 00298 else printf("\n"); 00299 } 00300 } 00301 00302 template <class NodeData,class Real> 00303 void OctNode<NodeData,Real>::AdjacencyCountFunction::Function(const OctNode<NodeData,Real>* node1,const OctNode<NodeData,Real>* node2){count++;} 00304 00305 template <class NodeData,class Real> 00306 template<class NodeAdjacencyFunction> 00307 void OctNode<NodeData,Real>::processNodeNodes(OctNode* node,NodeAdjacencyFunction* F,int processCurrent){ 00308 if(processCurrent){F->Function(this,node);} 00309 if(children){__processNodeNodes(node,F);} 00310 } 00311 template <class NodeData,class Real> 00312 template<class NodeAdjacencyFunction> 00313 void OctNode<NodeData,Real>::processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent){ 00314 if(processCurrent){F->Function(this,node);} 00315 if(children){ 00316 int c1,c2,c3,c4; 00317 Cube::FaceCorners(fIndex,c1,c2,c3,c4); 00318 __processNodeFaces(node,F,c1,c2,c3,c4); 00319 } 00320 } 00321 template <class NodeData,class Real> 00322 template<class NodeAdjacencyFunction> 00323 void OctNode<NodeData,Real>::processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent){ 00324 if(processCurrent){F->Function(this,node);} 00325 if(children){ 00326 int c1,c2; 00327 Cube::EdgeCorners(eIndex,c1,c2); 00328 __processNodeEdges(node,F,c1,c2); 00329 } 00330 } 00331 template <class NodeData,class Real> 00332 template<class NodeAdjacencyFunction> 00333 void OctNode<NodeData,Real>::processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent){ 00334 if(processCurrent){F->Function(this,node);} 00335 OctNode<NodeData,Real>* temp=this; 00336 while(temp->children){ 00337 temp=&temp->children[cIndex]; 00338 F->Function(temp,node); 00339 } 00340 } 00341 template <class NodeData,class Real> 00342 template<class NodeAdjacencyFunction> 00343 void OctNode<NodeData,Real>::__processNodeNodes(OctNode* node,NodeAdjacencyFunction* F){ 00344 F->Function(&children[0],node); 00345 F->Function(&children[1],node); 00346 F->Function(&children[2],node); 00347 F->Function(&children[3],node); 00348 F->Function(&children[4],node); 00349 F->Function(&children[5],node); 00350 F->Function(&children[6],node); 00351 F->Function(&children[7],node); 00352 if(children[0].children){children[0].__processNodeNodes(node,F);} 00353 if(children[1].children){children[1].__processNodeNodes(node,F);} 00354 if(children[2].children){children[2].__processNodeNodes(node,F);} 00355 if(children[3].children){children[3].__processNodeNodes(node,F);} 00356 if(children[4].children){children[4].__processNodeNodes(node,F);} 00357 if(children[5].children){children[5].__processNodeNodes(node,F);} 00358 if(children[6].children){children[6].__processNodeNodes(node,F);} 00359 if(children[7].children){children[7].__processNodeNodes(node,F);} 00360 } 00361 template <class NodeData,class Real> 00362 template<class NodeAdjacencyFunction> 00363 void OctNode<NodeData,Real>::__processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2){ 00364 F->Function(&children[cIndex1],node); 00365 F->Function(&children[cIndex2],node); 00366 if(children[cIndex1].children){children[cIndex1].__processNodeEdges(node,F,cIndex1,cIndex2);} 00367 if(children[cIndex2].children){children[cIndex2].__processNodeEdges(node,F,cIndex1,cIndex2);} 00368 } 00369 template <class NodeData,class Real> 00370 template<class NodeAdjacencyFunction> 00371 void OctNode<NodeData,Real>::__processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4){ 00372 F->Function(&children[cIndex1],node); 00373 F->Function(&children[cIndex2],node); 00374 F->Function(&children[cIndex3],node); 00375 F->Function(&children[cIndex4],node); 00376 if(children[cIndex1].children){children[cIndex1].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} 00377 if(children[cIndex2].children){children[cIndex2].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} 00378 if(children[cIndex3].children){children[cIndex3].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} 00379 if(children[cIndex4].children){children[cIndex4].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} 00380 } 00381 template<class NodeData,class Real> 00382 template<class NodeAdjacencyFunction> 00383 void OctNode<NodeData,Real>::ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent){ 00384 int c1[3],c2[3],w1,w2; 00385 node1->centerIndex(maxDepth+1,c1); 00386 node2->centerIndex(maxDepth+1,c2); 00387 w1=node1->width(maxDepth+1); 00388 w2=node2->width(maxDepth+1); 00389 00390 ProcessNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); 00391 } 00392 template<class NodeData,class Real> 00393 template<class NodeAdjacencyFunction> 00394 void OctNode<NodeData,Real>::ProcessNodeAdjacentNodes(int dx,int dy,int dz, 00395 OctNode<NodeData,Real>* node1,int radius1, 00396 OctNode<NodeData,Real>* node2,int radius2,int width2, 00397 NodeAdjacencyFunction* F,int processCurrent){ 00398 if(!Overlap(dx,dy,dz,radius1+radius2)){return;} 00399 if(processCurrent){F->Function(node2,node1);} 00400 if(!node2->children){return;} 00401 __ProcessNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); 00402 } 00403 template<class NodeData,class Real> 00404 template<class TerminatingNodeAdjacencyFunction> 00405 void OctNode<NodeData,Real>::ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent){ 00406 int c1[3],c2[3],w1,w2; 00407 node1->centerIndex(maxDepth+1,c1); 00408 node2->centerIndex(maxDepth+1,c2); 00409 w1=node1->width(maxDepth+1); 00410 w2=node2->width(maxDepth+1); 00411 00412 ProcessTerminatingNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); 00413 } 00414 template<class NodeData,class Real> 00415 template<class TerminatingNodeAdjacencyFunction> 00416 void OctNode<NodeData,Real>::ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz, 00417 OctNode<NodeData,Real>* node1,int radius1, 00418 OctNode<NodeData,Real>* node2,int radius2,int width2, 00419 TerminatingNodeAdjacencyFunction* F,int processCurrent) 00420 { 00421 if(!Overlap(dx,dy,dz,radius1+radius2)){return;} 00422 if(processCurrent){F->Function(node2,node1);} 00423 if(!node2->children){return;} 00424 __ProcessTerminatingNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); 00425 } 00426 template<class NodeData,class Real> 00427 template<class PointAdjacencyFunction> 00428 void OctNode<NodeData,Real>::ProcessPointAdjacentNodes( int maxDepth , const int c1[3] , OctNode* node2 , int width2 , PointAdjacencyFunction* F , int processCurrent ) 00429 { 00430 int c2[3] , w2; 00431 node2->centerIndex( maxDepth+1 , c2 ); 00432 w2 = node2->width( maxDepth+1 ); 00433 ProcessPointAdjacentNodes( c1[0]-c2[0] , c1[1]-c2[1] , c1[2]-c2[2] , node2 , (width2*w2)>>1 , w2 , F , processCurrent ); 00434 } 00435 template<class NodeData,class Real> 00436 template<class PointAdjacencyFunction> 00437 void OctNode<NodeData,Real>::ProcessPointAdjacentNodes(int dx,int dy,int dz, 00438 OctNode<NodeData,Real>* node2,int radius2,int width2, 00439 PointAdjacencyFunction* F,int processCurrent) 00440 { 00441 if( !Overlap(dx,dy,dz,radius2) ) return; 00442 if( processCurrent ) F->Function(node2); 00443 if( !node2->children ) return; 00444 __ProcessPointAdjacentNodes( -dx , -dy , -dz , node2 , radius2 , width2>>1 , F ); 00445 } 00446 template<class NodeData,class Real> 00447 template<class NodeAdjacencyFunction> 00448 void OctNode<NodeData,Real>::ProcessFixedDepthNodeAdjacentNodes(int maxDepth, 00449 OctNode<NodeData,Real>* node1,int width1, 00450 OctNode<NodeData,Real>* node2,int width2, 00451 int depth,NodeAdjacencyFunction* F,int processCurrent) 00452 { 00453 int c1[3],c2[3],w1,w2; 00454 node1->centerIndex(maxDepth+1,c1); 00455 node2->centerIndex(maxDepth+1,c2); 00456 w1=node1->width(maxDepth+1); 00457 w2=node2->width(maxDepth+1); 00458 00459 ProcessFixedDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); 00460 } 00461 template<class NodeData,class Real> 00462 template<class NodeAdjacencyFunction> 00463 void OctNode<NodeData,Real>::ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz, 00464 OctNode<NodeData,Real>* node1,int radius1, 00465 OctNode<NodeData,Real>* node2,int radius2,int width2, 00466 int depth,NodeAdjacencyFunction* F,int processCurrent) 00467 { 00468 int d=node2->depth(); 00469 if(d>depth){return;} 00470 if(!Overlap(dx,dy,dz,radius1+radius2)){return;} 00471 if(d==depth){if(processCurrent){F->Function(node2,node1);}} 00472 else{ 00473 if(!node2->children){return;} 00474 __ProcessFixedDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,depth-1,F); 00475 } 00476 } 00477 template<class NodeData,class Real> 00478 template<class NodeAdjacencyFunction> 00479 void OctNode<NodeData,Real>::ProcessMaxDepthNodeAdjacentNodes(int maxDepth, 00480 OctNode<NodeData,Real>* node1,int width1, 00481 OctNode<NodeData,Real>* node2,int width2, 00482 int depth,NodeAdjacencyFunction* F,int processCurrent) 00483 { 00484 int c1[3],c2[3],w1,w2; 00485 node1->centerIndex(maxDepth+1,c1); 00486 node2->centerIndex(maxDepth+1,c2); 00487 w1=node1->width(maxDepth+1); 00488 w2=node2->width(maxDepth+1); 00489 ProcessMaxDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); 00490 } 00491 template<class NodeData,class Real> 00492 template<class NodeAdjacencyFunction> 00493 void OctNode<NodeData,Real>::ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz, 00494 OctNode<NodeData,Real>* node1,int radius1, 00495 OctNode<NodeData,Real>* node2,int radius2,int width2, 00496 int depth,NodeAdjacencyFunction* F,int processCurrent) 00497 { 00498 int d=node2->depth(); 00499 if(d>depth){return;} 00500 if(!Overlap(dx,dy,dz,radius1+radius2)){return;} 00501 if(processCurrent){F->Function(node2,node1);} 00502 if(d<depth && node2->children){__ProcessMaxDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2>>1,depth-1,F);} 00503 } 00504 template <class NodeData,class Real> 00505 template<class NodeAdjacencyFunction> 00506 void OctNode<NodeData,Real>::__ProcessNodeAdjacentNodes(int dx,int dy,int dz, 00507 OctNode* node1,int radius1, 00508 OctNode* node2,int radius2,int cWidth2, 00509 NodeAdjacencyFunction* F) 00510 { 00511 int cWidth=cWidth2>>1; 00512 int radius=radius2>>1; 00513 int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); 00514 if(o){ 00515 int dx1=dx-cWidth; 00516 int dx2=dx+cWidth; 00517 int dy1=dy-cWidth; 00518 int dy2=dy+cWidth; 00519 int dz1=dz-cWidth; 00520 int dz2=dz+cWidth; 00521 if(o& 1){F->Function(&node2->children[0],node1);if(node2->children[0].children){__ProcessNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,F);}} 00522 if(o& 2){F->Function(&node2->children[1],node1);if(node2->children[1].children){__ProcessNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,F);}} 00523 if(o& 4){F->Function(&node2->children[2],node1);if(node2->children[2].children){__ProcessNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,F);}} 00524 if(o& 8){F->Function(&node2->children[3],node1);if(node2->children[3].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,F);}} 00525 if(o& 16){F->Function(&node2->children[4],node1);if(node2->children[4].children){__ProcessNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,F);}} 00526 if(o& 32){F->Function(&node2->children[5],node1);if(node2->children[5].children){__ProcessNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,F);}} 00527 if(o& 64){F->Function(&node2->children[6],node1);if(node2->children[6].children){__ProcessNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,F);}} 00528 if(o&128){F->Function(&node2->children[7],node1);if(node2->children[7].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} 00529 } 00530 } 00531 template <class NodeData,class Real> 00532 template<class TerminatingNodeAdjacencyFunction> 00533 void OctNode<NodeData,Real>::__ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz, 00534 OctNode* node1,int radius1, 00535 OctNode* node2,int radius2,int cWidth2, 00536 TerminatingNodeAdjacencyFunction* F) 00537 { 00538 int cWidth=cWidth2>>1; 00539 int radius=radius2>>1; 00540 int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); 00541 if(o){ 00542 int dx1=dx-cWidth; 00543 int dx2=dx+cWidth; 00544 int dy1=dy-cWidth; 00545 int dy2=dy+cWidth; 00546 int dz1=dz-cWidth; 00547 int dz2=dz+cWidth; 00548 if(o& 1){if(F->Function(&node2->children[0],node1) && node2->children[0].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,F);}} 00549 if(o& 2){if(F->Function(&node2->children[1],node1) && node2->children[1].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,F);}} 00550 if(o& 4){if(F->Function(&node2->children[2],node1) && node2->children[2].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,F);}} 00551 if(o& 8){if(F->Function(&node2->children[3],node1) && node2->children[3].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,F);}} 00552 if(o& 16){if(F->Function(&node2->children[4],node1) && node2->children[4].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,F);}} 00553 if(o& 32){if(F->Function(&node2->children[5],node1) && node2->children[5].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,F);}} 00554 if(o& 64){if(F->Function(&node2->children[6],node1) && node2->children[6].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,F);}} 00555 if(o&128){if(F->Function(&node2->children[7],node1) && node2->children[7].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} 00556 } 00557 } 00558 template <class NodeData,class Real> 00559 template<class PointAdjacencyFunction> 00560 void OctNode<NodeData,Real>::__ProcessPointAdjacentNodes(int dx,int dy,int dz, 00561 OctNode* node2,int radius2,int cWidth2, 00562 PointAdjacencyFunction* F) 00563 { 00564 int cWidth=cWidth2>>1; 00565 int radius=radius2>>1; 00566 int o=ChildOverlap(dx,dy,dz,radius,cWidth); 00567 if( o ) 00568 { 00569 int dx1=dx-cWidth; 00570 int dx2=dx+cWidth; 00571 int dy1=dy-cWidth; 00572 int dy2=dy+cWidth; 00573 int dz1=dz-cWidth; 00574 int dz2=dz+cWidth; 00575 if(o& 1){F->Function(&node2->children[0]);if(node2->children[0].children){__ProcessPointAdjacentNodes(dx1,dy1,dz1,&node2->children[0],radius,cWidth,F);}} 00576 if(o& 2){F->Function(&node2->children[1]);if(node2->children[1].children){__ProcessPointAdjacentNodes(dx2,dy1,dz1,&node2->children[1],radius,cWidth,F);}} 00577 if(o& 4){F->Function(&node2->children[2]);if(node2->children[2].children){__ProcessPointAdjacentNodes(dx1,dy2,dz1,&node2->children[2],radius,cWidth,F);}} 00578 if(o& 8){F->Function(&node2->children[3]);if(node2->children[3].children){__ProcessPointAdjacentNodes(dx2,dy2,dz1,&node2->children[3],radius,cWidth,F);}} 00579 if(o& 16){F->Function(&node2->children[4]);if(node2->children[4].children){__ProcessPointAdjacentNodes(dx1,dy1,dz2,&node2->children[4],radius,cWidth,F);}} 00580 if(o& 32){F->Function(&node2->children[5]);if(node2->children[5].children){__ProcessPointAdjacentNodes(dx2,dy1,dz2,&node2->children[5],radius,cWidth,F);}} 00581 if(o& 64){F->Function(&node2->children[6]);if(node2->children[6].children){__ProcessPointAdjacentNodes(dx1,dy2,dz2,&node2->children[6],radius,cWidth,F);}} 00582 if(o&128){F->Function(&node2->children[7]);if(node2->children[7].children){__ProcessPointAdjacentNodes(dx2,dy2,dz2,&node2->children[7],radius,cWidth,F);}} 00583 } 00584 } 00585 template <class NodeData,class Real> 00586 template<class NodeAdjacencyFunction> 00587 void OctNode<NodeData,Real>::__ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz, 00588 OctNode* node1,int radius1, 00589 OctNode* node2,int radius2,int cWidth2, 00590 int depth,NodeAdjacencyFunction* F) 00591 { 00592 int cWidth=cWidth2>>1; 00593 int radius=radius2>>1; 00594 int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); 00595 if(o){ 00596 int dx1=dx-cWidth; 00597 int dx2=dx+cWidth; 00598 int dy1=dy-cWidth; 00599 int dy2=dy+cWidth; 00600 int dz1=dz-cWidth; 00601 int dz2=dz+cWidth; 00602 if(node2->depth()==depth){ 00603 if(o& 1){F->Function(&node2->children[0],node1);} 00604 if(o& 2){F->Function(&node2->children[1],node1);} 00605 if(o& 4){F->Function(&node2->children[2],node1);} 00606 if(o& 8){F->Function(&node2->children[3],node1);} 00607 if(o& 16){F->Function(&node2->children[4],node1);} 00608 if(o& 32){F->Function(&node2->children[5],node1);} 00609 if(o& 64){F->Function(&node2->children[6],node1);} 00610 if(o&128){F->Function(&node2->children[7],node1);} 00611 } 00612 else{ 00613 if(o& 1){if(node2->children[0].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}} 00614 if(o& 2){if(node2->children[1].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}} 00615 if(o& 4){if(node2->children[2].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}} 00616 if(o& 8){if(node2->children[3].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}} 00617 if(o& 16){if(node2->children[4].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}} 00618 if(o& 32){if(node2->children[5].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}} 00619 if(o& 64){if(node2->children[6].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}} 00620 if(o&128){if(node2->children[7].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}} 00621 } 00622 } 00623 } 00624 template <class NodeData,class Real> 00625 template<class NodeAdjacencyFunction> 00626 void OctNode<NodeData,Real>::__ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz, 00627 OctNode* node1,int radius1, 00628 OctNode* node2,int radius2,int cWidth2, 00629 int depth,NodeAdjacencyFunction* F) 00630 { 00631 int cWidth=cWidth2>>1; 00632 int radius=radius2>>1; 00633 int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); 00634 if(o){ 00635 int dx1=dx-cWidth; 00636 int dx2=dx+cWidth; 00637 int dy1=dy-cWidth; 00638 int dy2=dy+cWidth; 00639 int dz1=dz-cWidth; 00640 int dz2=dz+cWidth; 00641 if(node2->depth()<=depth){ 00642 if(o& 1){F->Function(&node2->children[0],node1);} 00643 if(o& 2){F->Function(&node2->children[1],node1);} 00644 if(o& 4){F->Function(&node2->children[2],node1);} 00645 if(o& 8){F->Function(&node2->children[3],node1);} 00646 if(o& 16){F->Function(&node2->children[4],node1);} 00647 if(o& 32){F->Function(&node2->children[5],node1);} 00648 if(o& 64){F->Function(&node2->children[6],node1);} 00649 if(o&128){F->Function(&node2->children[7],node1);} 00650 } 00651 if(node2->depth()<depth){ 00652 if(o& 1){if(node2->children[0].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}} 00653 if(o& 2){if(node2->children[1].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}} 00654 if(o& 4){if(node2->children[2].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}} 00655 if(o& 8){if(node2->children[3].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}} 00656 if(o& 16){if(node2->children[4].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}} 00657 if(o& 32){if(node2->children[5].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}} 00658 if(o& 64){if(node2->children[6].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}} 00659 if(o&128){if(node2->children[7].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}} 00660 } 00661 } 00662 } 00663 template <class NodeData,class Real> 00664 inline int OctNode<NodeData,Real>::ChildOverlap(int dx,int dy,int dz,int d,int cRadius2) 00665 { 00666 int w1=d-cRadius2; 00667 int w2=d+cRadius2; 00668 int overlap=0; 00669 00670 int test=0,test1=0; 00671 if(dx<w2 && dx>-w1){test =1;} 00672 if(dx<w1 && dx>-w2){test|=2;} 00673 00674 if(!test){return 0;} 00675 if(dz<w2 && dz>-w1){test1 =test;} 00676 if(dz<w1 && dz>-w2){test1|=test<<4;} 00677 00678 if(!test1){return 0;} 00679 if(dy<w2 && dy>-w1){overlap =test1;} 00680 if(dy<w1 && dy>-w2){overlap|=test1<<2;} 00681 return overlap; 00682 } 00683 00684 template <class NodeData,class Real> 00685 OctNode<NodeData,Real>* OctNode<NodeData,Real>::getNearestLeaf(const Point3D<Real>& p){ 00686 Point3D<Real> center; 00687 Real width; 00688 OctNode<NodeData,Real>* temp; 00689 int cIndex; 00690 if(!children){return this;} 00691 centerAndWidth(center,width); 00692 temp=this; 00693 while(temp->children){ 00694 cIndex=CornerIndex(center,p); 00695 temp=&temp->children[cIndex]; 00696 width/=2; 00697 if(cIndex&1){center.coords[0]+=width/2;} 00698 else {center.coords[0]-=width/2;} 00699 if(cIndex&2){center.coords[1]+=width/2;} 00700 else {center.coords[1]-=width/2;} 00701 if(cIndex&4){center.coords[2]+=width/2;} 00702 else {center.coords[2]-=width/2;} 00703 } 00704 return temp; 00705 } 00706 template <class NodeData,class Real> 00707 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::getNearestLeaf(const Point3D<Real>& p) const{ 00708 int nearest; 00709 Real temp,dist2; 00710 if(!children){return this;} 00711 for(int i=0;i<Cube::CORNERS;i++){ 00712 temp=SquareDistance(children[i].center,p); 00713 if(!i || temp<dist2){ 00714 dist2=temp; 00715 nearest=i; 00716 } 00717 } 00718 return children[nearest].getNearestLeaf(p); 00719 } 00720 00721 template <class NodeData,class Real> 00722 int OctNode<NodeData,Real>::CommonEdge(const OctNode<NodeData,Real>* node1,int eIndex1,const OctNode<NodeData,Real>* node2,int eIndex2){ 00723 int o1,o2,i1,i2,j1,j2; 00724 00725 Cube::FactorEdgeIndex(eIndex1,o1,i1,j1); 00726 Cube::FactorEdgeIndex(eIndex2,o2,i2,j2); 00727 if(o1!=o2){return 0;} 00728 00729 int dir[2]; 00730 int idx1[2]; 00731 int idx2[2]; 00732 switch(o1){ 00733 case 0: dir[0]=1; dir[1]=2; break; 00734 case 1: dir[0]=0; dir[1]=2; break; 00735 case 2: dir[0]=0; dir[1]=1; break; 00736 }; 00737 int d1,d2,off1[3],off2[3]; 00738 node1->depthAndOffset(d1,off1); 00739 node2->depthAndOffset(d2,off2); 00740 idx1[0]=off1[dir[0]]+(1<<d1)+i1; 00741 idx1[1]=off1[dir[1]]+(1<<d1)+j1; 00742 idx2[0]=off2[dir[0]]+(1<<d2)+i2; 00743 idx2[1]=off2[dir[1]]+(1<<d2)+j2; 00744 if(d1>d2){ 00745 idx2[0]<<=(d1-d2); 00746 idx2[1]<<=(d1-d2); 00747 } 00748 else{ 00749 idx1[0]<<=(d2-d1); 00750 idx1[1]<<=(d2-d1); 00751 } 00752 if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;} 00753 else {return 0;} 00754 } 00755 template<class NodeData,class Real> 00756 int OctNode<NodeData,Real>::CornerIndex(const Point3D<Real>& center,const Point3D<Real>& p){ 00757 int cIndex=0; 00758 if(p.coords[0]>center.coords[0]){cIndex|=1;} 00759 if(p.coords[1]>center.coords[1]){cIndex|=2;} 00760 if(p.coords[2]>center.coords[2]){cIndex|=4;} 00761 return cIndex; 00762 } 00763 template <class NodeData,class Real> 00764 template<class NodeData2> 00765 OctNode<NodeData,Real>& OctNode<NodeData,Real>::operator = (const OctNode<NodeData2,Real>& node){ 00766 int i; 00767 if(children){delete[] children;} 00768 children=NULL; 00769 00770 d=node.depth (); 00771 for(i=0;i<DIMENSION;i++){this->offset[i] = node.offset[i];} 00772 if(node.children){ 00773 initChildren(); 00774 for(i=0;i<Cube::CORNERS;i++){children[i] = node.children[i];} 00775 } 00776 return *this; 00777 } 00778 template <class NodeData,class Real> 00779 int OctNode<NodeData,Real>::CompareForwardDepths(const void* v1,const void* v2){ 00780 return ((const OctNode<NodeData,Real>*)v1)->depth-((const OctNode<NodeData,Real>*)v2)->depth; 00781 } 00782 template< class NodeData , class Real > 00783 int OctNode< NodeData , Real >::CompareByDepthAndXYZ( const void* v1 , const void* v2 ) 00784 { 00785 const OctNode<NodeData,Real> *n1 = (*(const OctNode< NodeData , Real >**)v1); 00786 const OctNode<NodeData,Real> *n2 = (*(const OctNode< NodeData , Real >**)v2); 00787 if( n1->d!=n2->d ) return int(n1->d)-int(n2->d); 00788 else if( n1->off[0]!=n2->off[0] ) return int(n1->off[0]) - int(n2->off[0]); 00789 else if( n1->off[1]!=n2->off[1] ) return int(n1->off[1]) - int(n2->off[1]); 00790 else if( n1->off[2]!=n2->off[2] ) return int(n1->off[2]) - int(n2->off[2]); 00791 return 0; 00792 } 00793 00794 long long _InterleaveBits( int p[3] ) 00795 { 00796 long long key = 0; 00797 for( int i=0 ; i<32 ; i++ ) key |= ( ( p[0] & (1<<i) )<<(2*i) ) | ( ( p[1] & (1<<i) )<<(2*i+1) ) | ( ( p[2] & (1<<i) )<<(2*i+2) ); 00798 return key; 00799 } 00800 template <class NodeData,class Real> 00801 int OctNode<NodeData,Real>::CompareByDepthAndZIndex( const void* v1 , const void* v2 ) 00802 { 00803 const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1); 00804 const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2); 00805 int d1 , off1[3] , d2 , off2[3]; 00806 n1->depthAndOffset( d1 , off1 ) , n2->depthAndOffset( d2 , off2 ); 00807 if ( d1>d2 ) return 1; 00808 else if( d1<d2 ) return -1; 00809 long long k1 = _InterleaveBits( off1 ) , k2 = _InterleaveBits( off2 ); 00810 if ( k1>k2 ) return 1; 00811 else if( k1<k2 ) return -1; 00812 else return 0; 00813 } 00814 00815 template <class NodeData,class Real> 00816 int OctNode<NodeData,Real>::CompareForwardPointerDepths( const void* v1 , const void* v2 ) 00817 { 00818 const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1); 00819 const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2); 00820 if(n1->d!=n2->d){return int(n1->d)-int(n2->d);} 00821 while( n1->parent!=n2->parent ) 00822 { 00823 n1=n1->parent; 00824 n2=n2->parent; 00825 } 00826 if(n1->off[0]!=n2->off[0]){return int(n1->off[0])-int(n2->off[0]);} 00827 if(n1->off[1]!=n2->off[1]){return int(n1->off[1])-int(n2->off[1]);} 00828 return int(n1->off[2])-int(n2->off[2]); 00829 return 0; 00830 } 00831 template <class NodeData,class Real> 00832 int OctNode<NodeData,Real>::CompareBackwardDepths(const void* v1,const void* v2){ 00833 return ((const OctNode<NodeData,Real>*)v2)->depth-((const OctNode<NodeData,Real>*)v1)->depth; 00834 } 00835 template <class NodeData,class Real> 00836 int OctNode<NodeData,Real>::CompareBackwardPointerDepths(const void* v1,const void* v2){ 00837 return (*(const OctNode<NodeData,Real>**)v2)->depth()-(*(const OctNode<NodeData,Real>**)v1)->depth(); 00838 } 00839 template <class NodeData,class Real> 00840 inline int OctNode<NodeData,Real>::Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2){ 00841 int d=depth2-depth1; 00842 Real w=multiplier2+multiplier1*(1<<d); 00843 Real w2=Real((1<<(d-1))-0.5); 00844 if( 00845 fabs(Real(offSet2[0]-(offSet1[0]<<d))-w2)>=w || 00846 fabs(Real(offSet2[1]-(offSet1[1]<<d))-w2)>=w || 00847 fabs(Real(offSet2[2]-(offSet1[2]<<d))-w2)>=w 00848 ){return 0;} 00849 return 1; 00850 } 00851 template <class NodeData,class Real> 00852 inline int OctNode<NodeData,Real>::Overlap(int c1,int c2,int c3,int dWidth){ 00853 if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;} 00854 else{return 1;} 00855 } 00856 template <class NodeData,class Real> 00857 OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);} 00858 template <class NodeData,class Real> 00859 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);} 00860 template <class NodeData,class Real> 00861 OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off,int forceChildren){ 00862 if(!parent){return NULL;} 00863 int pIndex=int(this-parent->children); 00864 pIndex^=(1<<dir); 00865 if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];} 00866 else{ 00867 OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren); 00868 if(!temp){return NULL;} 00869 if(!temp->children){ 00870 if(forceChildren){temp->initChildren();} 00871 else{return temp;} 00872 } 00873 return &temp->children[pIndex]; 00874 } 00875 } 00876 template <class NodeData,class Real> 00877 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off) const { 00878 if(!parent){return NULL;} 00879 int pIndex=int(this-parent->children); 00880 pIndex^=(1<<dir); 00881 if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];} 00882 else{ 00883 const OctNode* temp=parent->__faceNeighbor(dir,off); 00884 if(!temp || !temp->children){return temp;} 00885 else{return &temp->children[pIndex];} 00886 } 00887 } 00888 00889 template <class NodeData,class Real> 00890 OctNode<NodeData,Real>* OctNode<NodeData,Real>::edgeNeighbor(int edgeIndex,int forceChildren){ 00891 int idx[2],o,i[2]; 00892 Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); 00893 switch(o){ 00894 case 0: idx[0]=1; idx[1]=2; break; 00895 case 1: idx[0]=0; idx[1]=2; break; 00896 case 2: idx[0]=0; idx[1]=1; break; 00897 }; 00898 return __edgeNeighbor(o,i,idx,forceChildren); 00899 } 00900 template <class NodeData,class Real> 00901 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::edgeNeighbor(int edgeIndex) const { 00902 int idx[2],o,i[2]; 00903 Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); 00904 switch(o){ 00905 case 0: idx[0]=1; idx[1]=2; break; 00906 case 1: idx[0]=0; idx[1]=2; break; 00907 case 2: idx[0]=0; idx[1]=1; break; 00908 }; 00909 return __edgeNeighbor(o,i,idx); 00910 } 00911 template <class NodeData,class Real> 00912 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{ 00913 if(!parent){return NULL;} 00914 int pIndex=int(this-parent->children); 00915 int aIndex,x[DIMENSION]; 00916 00917 Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); 00918 aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; 00919 pIndex^=(7 ^ (1<<o)); 00920 if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor 00921 const OctNode* temp=parent->__faceNeighbor(idx[0],i[0]); 00922 if(!temp || !temp->children){return NULL;} 00923 else{return &temp->children[pIndex];} 00924 } 00925 else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor 00926 const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]); 00927 if(!temp || !temp->children){return NULL;} 00928 else{return &temp->children[pIndex];} 00929 } 00930 else if(aIndex==0) { // I can get the neighbor from the parent 00931 return &parent->children[pIndex]; 00932 } 00933 else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor 00934 const OctNode* temp=parent->__edgeNeighbor(o,i,idx); 00935 if(!temp || !temp->children){return temp;} 00936 else{return &temp->children[pIndex];} 00937 } 00938 else{return NULL;} 00939 } 00940 template <class NodeData,class Real> 00941 OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){ 00942 if(!parent){return NULL;} 00943 int pIndex=int(this-parent->children); 00944 int aIndex,x[DIMENSION]; 00945 00946 Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); 00947 aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; 00948 pIndex^=(7 ^ (1<<o)); 00949 if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor 00950 OctNode* temp=parent->__faceNeighbor(idx[0],i[0],0); 00951 if(!temp || !temp->children){return NULL;} 00952 else{return &temp->children[pIndex];} 00953 } 00954 else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor 00955 OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0); 00956 if(!temp || !temp->children){return NULL;} 00957 else{return &temp->children[pIndex];} 00958 } 00959 else if(aIndex==0) { // I can get the neighbor from the parent 00960 return &parent->children[pIndex]; 00961 } 00962 else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor 00963 OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren); 00964 if(!temp){return NULL;} 00965 if(!temp->children){ 00966 if(forceChildren){temp->initChildren();} 00967 else{return temp;} 00968 } 00969 return &temp->children[pIndex]; 00970 } 00971 else{return NULL;} 00972 } 00973 00974 template <class NodeData,class Real> 00975 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::cornerNeighbor(int cornerIndex) const { 00976 int pIndex,aIndex=0; 00977 if(!parent){return NULL;} 00978 00979 pIndex=int(this-parent->children); 00980 aIndex=(cornerIndex ^ pIndex); // The disagreement bits 00981 pIndex=(~pIndex)&7; // The antipodal point 00982 if(aIndex==7){ // Agree on no bits 00983 return &parent->children[pIndex]; 00984 } 00985 else if(aIndex==0){ // Agree on all bits 00986 const OctNode* temp=((const OctNode*)parent)->cornerNeighbor(cornerIndex); 00987 if(!temp || !temp->children){return temp;} 00988 else{return &temp->children[pIndex];} 00989 } 00990 else if(aIndex==6){ // Agree on face 0 00991 const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1); 00992 if(!temp || !temp->children){return NULL;} 00993 else{return & temp->children[pIndex];} 00994 } 00995 else if(aIndex==5){ // Agree on face 1 00996 const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1); 00997 if(!temp || !temp->children){return NULL;} 00998 else{return & temp->children[pIndex];} 00999 } 01000 else if(aIndex==3){ // Agree on face 2 01001 const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2); 01002 if(!temp || !temp->children){return NULL;} 01003 else{return & temp->children[pIndex];} 01004 } 01005 else if(aIndex==4){ // Agree on edge 2 01006 const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); 01007 if(!temp || !temp->children){return NULL;} 01008 else{return & temp->children[pIndex];} 01009 } 01010 else if(aIndex==2){ // Agree on edge 1 01011 const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); 01012 if(!temp || !temp->children){return NULL;} 01013 else{return & temp->children[pIndex];} 01014 } 01015 else if(aIndex==1){ // Agree on edge 0 01016 const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); 01017 if(!temp || !temp->children){return NULL;} 01018 else{return & temp->children[pIndex];} 01019 } 01020 else{return NULL;} 01021 } 01022 template <class NodeData,class Real> 01023 OctNode<NodeData,Real>* OctNode<NodeData,Real>::cornerNeighbor(int cornerIndex,int forceChildren){ 01024 int pIndex,aIndex=0; 01025 if(!parent){return NULL;} 01026 01027 pIndex=int(this-parent->children); 01028 aIndex=(cornerIndex ^ pIndex); // The disagreement bits 01029 pIndex=(~pIndex)&7; // The antipodal point 01030 if(aIndex==7){ // Agree on no bits 01031 return &parent->children[pIndex]; 01032 } 01033 else if(aIndex==0){ // Agree on all bits 01034 OctNode* temp=((OctNode*)parent)->cornerNeighbor(cornerIndex,forceChildren); 01035 if(!temp){return NULL;} 01036 if(!temp->children){ 01037 if(forceChildren){temp->initChildren();} 01038 else{return temp;} 01039 } 01040 return &temp->children[pIndex]; 01041 } 01042 else if(aIndex==6){ // Agree on face 0 01043 OctNode* temp=((OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1,0); 01044 if(!temp || !temp->children){return NULL;} 01045 else{return & temp->children[pIndex];} 01046 } 01047 else if(aIndex==5){ // Agree on face 1 01048 OctNode* temp=((OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0); 01049 if(!temp || !temp->children){return NULL;} 01050 else{return & temp->children[pIndex];} 01051 } 01052 else if(aIndex==3){ // Agree on face 2 01053 OctNode* temp=((OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0); 01054 if(!temp || !temp->children){return NULL;} 01055 else{return & temp->children[pIndex];} 01056 } 01057 else if(aIndex==4){ // Agree on edge 2 01058 OctNode* temp=((OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); 01059 if(!temp || !temp->children){return NULL;} 01060 else{return & temp->children[pIndex];} 01061 } 01062 else if(aIndex==2){ // Agree on edge 1 01063 OctNode* temp=((OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); 01064 if(!temp || !temp->children){return NULL;} 01065 else{return & temp->children[pIndex];} 01066 } 01067 else if(aIndex==1){ // Agree on edge 0 01068 OctNode* temp=((OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); 01069 if(!temp || !temp->children){return NULL;} 01070 else{return & temp->children[pIndex];} 01071 } 01072 else{return NULL;} 01073 } 01074 //////////////////////// 01075 // OctNodeNeighborKey // 01076 //////////////////////// 01077 template<class NodeData,class Real> 01078 OctNode<NodeData,Real>::Neighbors3::Neighbors3(void){clear();} 01079 template<class NodeData,class Real> 01080 void OctNode<NodeData,Real>::Neighbors3::clear(void){ 01081 for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} 01082 } 01083 template<class NodeData,class Real> 01084 OctNode<NodeData,Real>::NeighborKey3::NeighborKey3(void){ neighbors=NULL; } 01085 template<class NodeData,class Real> 01086 OctNode<NodeData,Real>::NeighborKey3::~NeighborKey3(void) 01087 { 01088 if( neighbors ) delete[] neighbors; 01089 neighbors = NULL; 01090 } 01091 01092 template<class NodeData,class Real> 01093 void OctNode<NodeData,Real>::NeighborKey3::set( int d ) 01094 { 01095 if( neighbors ) delete[] neighbors; 01096 neighbors = NULL; 01097 if( d<0 ) return; 01098 neighbors = new Neighbors3[d+1]; 01099 } 01100 template< class NodeData , class Real > 01101 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d ) 01102 { 01103 if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) ) 01104 { 01105 neighbors[d].clear(); 01106 01107 if( !d ) neighbors[d].neighbors[1][1][1] = root; 01108 else 01109 { 01110 Neighbors3& temp = setNeighbors( root , p , d-1 ); 01111 01112 int i , j , k , x1 , y1 , z1 , x2 , y2 , z2; 01113 Point3D< Real > c; 01114 Real w; 01115 temp.neighbors[1][1][1]->centerAndWidth( c , w ); 01116 int idx = CornerIndex( c , p ); 01117 Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); 01118 Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 ); 01119 01120 if( !temp.neighbors[1][1][1]->children ) temp.neighbors[1][1][1]->initChildren(); 01121 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01122 neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)]; 01123 01124 01125 // Set the neighbors from across the faces 01126 i=x1<<1; 01127 if( temp.neighbors[i][1][1] ) 01128 { 01129 if( !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren(); 01130 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; 01131 } 01132 j=y1<<1; 01133 if( temp.neighbors[1][j][1] ) 01134 { 01135 if( !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren(); 01136 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; 01137 } 01138 k=z1<<1; 01139 if( temp.neighbors[1][1][k] ) 01140 { 01141 if( !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren(); 01142 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; 01143 } 01144 01145 // Set the neighbors from across the edges 01146 i=x1<<1 , j=y1<<1; 01147 if( temp.neighbors[i][j][1] ) 01148 { 01149 if( !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren(); 01150 for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; 01151 } 01152 i=x1<<1 , k=z1<<1; 01153 if( temp.neighbors[i][1][k] ) 01154 { 01155 if( !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren(); 01156 for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; 01157 } 01158 j=y1<<1 , k=z1<<1; 01159 if( temp.neighbors[1][j][k] ) 01160 { 01161 if( !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren(); 01162 for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; 01163 } 01164 01165 // Set the neighbor from across the corner 01166 i=x1<<1 , j=y1<<1 , k=z1<<1; 01167 if( temp.neighbors[i][j][k] ) 01168 { 01169 if( !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren(); 01170 neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01171 } 01172 } 01173 } 01174 return neighbors[d]; 01175 } 01176 template< class NodeData , class Real > 01177 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d ) 01178 { 01179 if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) ) 01180 { 01181 neighbors[d].clear(); 01182 01183 if( !d ) neighbors[d].neighbors[1][1][1] = root; 01184 else 01185 { 01186 Neighbors3& temp = getNeighbors( root , p , d-1 ); 01187 01188 int i , j , k , x1 , y1 , z1 , x2 , y2 , z2; 01189 Point3D< Real > c; 01190 Real w; 01191 temp.neighbors[1][1][1]->centerAndWidth( c , w ); 01192 int idx = CornerIndex( c , p ); 01193 Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); 01194 Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 ); 01195 01196 if( !temp.neighbors[1][1][1] || !temp.neighbors[1][1][1]->children ) 01197 { 01198 fprintf( stderr , "[ERROR] Couldn't find node at appropriate depth\n" ); 01199 exit( 0 ); 01200 } 01201 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01202 neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)]; 01203 01204 01205 // Set the neighbors from across the faces 01206 i=x1<<1; 01207 if( temp.neighbors[i][1][1] ) 01208 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; 01209 j=y1<<1; 01210 if( temp.neighbors[1][j][1] ) 01211 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; 01212 k=z1<<1; 01213 if( temp.neighbors[1][1][k] ) 01214 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; 01215 01216 // Set the neighbors from across the edges 01217 i=x1<<1 , j=y1<<1; 01218 if( temp.neighbors[i][j][1] ) 01219 for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; 01220 i=x1<<1 , k=z1<<1; 01221 if( temp.neighbors[i][1][k] ) 01222 for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; 01223 j=y1<<1 , k=z1<<1; 01224 if( temp.neighbors[1][j][k] ) 01225 for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; 01226 01227 // Set the neighbor from across the corner 01228 i=x1<<1 , j=y1<<1 , k=z1<<1; 01229 if( temp.neighbors[i][j][k] ) 01230 neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01231 } 01232 } 01233 return neighbors[d]; 01234 } 01235 01236 template< class NodeData , class Real > 01237 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* node ) 01238 { 01239 int d = node->depth(); 01240 if( node==neighbors[d].neighbors[1][1][1] ) 01241 { 01242 bool reset = false; 01243 for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) if( !neighbors[d].neighbors[i][j][k] ) reset = true; 01244 if( reset ) neighbors[d].neighbors[1][1][1] = NULL; 01245 } 01246 if( node!=neighbors[d].neighbors[1][1][1] ) 01247 { 01248 neighbors[d].clear(); 01249 01250 if( !node->parent ) neighbors[d].neighbors[1][1][1] = node; 01251 else 01252 { 01253 int i,j,k,x1,y1,z1,x2,y2,z2; 01254 int idx=int(node-node->parent->children); 01255 Cube::FactorCornerIndex( idx ,x1,y1,z1); 01256 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); 01257 for(i=0;i<2;i++){ 01258 for(j=0;j<2;j++){ 01259 for(k=0;k<2;k++){ 01260 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; 01261 } 01262 } 01263 } 01264 Neighbors3& temp=setNeighbors(node->parent); 01265 01266 // Set the neighbors from across the faces 01267 i=x1<<1; 01268 if(temp.neighbors[i][1][1]){ 01269 if(!temp.neighbors[i][1][1]->children){temp.neighbors[i][1][1]->initChildren();} 01270 for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} 01271 } 01272 j=y1<<1; 01273 if(temp.neighbors[1][j][1]){ 01274 if(!temp.neighbors[1][j][1]->children){temp.neighbors[1][j][1]->initChildren();} 01275 for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} 01276 } 01277 k=z1<<1; 01278 if(temp.neighbors[1][1][k]){ 01279 if(!temp.neighbors[1][1][k]->children){temp.neighbors[1][1][k]->initChildren();} 01280 for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} 01281 } 01282 01283 // Set the neighbors from across the edges 01284 i=x1<<1; j=y1<<1; 01285 if(temp.neighbors[i][j][1]){ 01286 if(!temp.neighbors[i][j][1]->children){temp.neighbors[i][j][1]->initChildren();} 01287 for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} 01288 } 01289 i=x1<<1; k=z1<<1; 01290 if(temp.neighbors[i][1][k]){ 01291 if(!temp.neighbors[i][1][k]->children){temp.neighbors[i][1][k]->initChildren();} 01292 for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} 01293 } 01294 j=y1<<1; k=z1<<1; 01295 if(temp.neighbors[1][j][k]){ 01296 if(!temp.neighbors[1][j][k]->children){temp.neighbors[1][j][k]->initChildren();} 01297 for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} 01298 } 01299 01300 // Set the neighbor from across the corner 01301 i=x1<<1; j=y1<<1; k=z1<<1; 01302 if(temp.neighbors[i][j][k]){ 01303 if(!temp.neighbors[i][j][k]->children){temp.neighbors[i][j][k]->initChildren();} 01304 neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01305 } 01306 } 01307 } 01308 return neighbors[d]; 01309 } 01310 // Note the assumption is that if you enable an edge, you also enable adjacent faces. 01311 // And, if you enable a corner, you enable adjacent edges and faces. 01312 template< class NodeData , class Real > 01313 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* node , bool flags[3][3][3] ) 01314 { 01315 int d = node->depth(); 01316 if( node==neighbors[d].neighbors[1][1][1] ) 01317 { 01318 bool reset = false; 01319 for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) if( flags[i][j][k] && !neighbors[d].neighbors[i][j][k] ) reset = true; 01320 if( reset ) neighbors[d].neighbors[1][1][1] = NULL; 01321 } 01322 if( node!=neighbors[d].neighbors[1][1][1] ) 01323 { 01324 neighbors[d].clear(); 01325 01326 if( !node->parent ) neighbors[d].neighbors[1][1][1] = node; 01327 else 01328 { 01329 int x1,y1,z1,x2,y2,z2; 01330 int idx=int(node-node->parent->children); 01331 Cube::FactorCornerIndex( idx ,x1,y1,z1); 01332 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); 01333 for( int i=0 ; i<2 ; i++ ) 01334 for( int j=0 ; j<2 ; j++ ) 01335 for( int k=0 ; k<2 ; k++ ) 01336 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; 01337 01338 Neighbors3& temp=setNeighbors( node->parent , flags ); 01339 01340 // Set the neighbors from across the faces 01341 { 01342 int i=x1<<1; 01343 if( temp.neighbors[i][1][1] ) 01344 { 01345 if( flags[i][1][1] && !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren(); 01346 if( temp.neighbors[i][1][1]->children ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; 01347 } 01348 } 01349 { 01350 int j = y1<<1; 01351 if( temp.neighbors[1][j][1] ) 01352 { 01353 if( flags[1][j][1] && !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren(); 01354 if( temp.neighbors[1][j][1]->children ) for( int i=0 ; i<2 ; i++ ) for( int k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; 01355 } 01356 } 01357 { 01358 int k = z1<<1; 01359 if( temp.neighbors[1][1][k] ) 01360 { 01361 if( flags[1][1][k] && !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren(); 01362 if( temp.neighbors[1][1][k]->children ) for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; 01363 } 01364 } 01365 01366 // Set the neighbors from across the edges 01367 { 01368 int i=x1<<1 , j=y1<<1; 01369 if( temp.neighbors[i][j][1] ) 01370 { 01371 if( flags[i][j][1] && !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren(); 01372 if( temp.neighbors[i][j][1]->children ) for( int k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; 01373 } 01374 } 01375 { 01376 int i=x1<<1 , k=z1<<1; 01377 if( temp.neighbors[i][1][k] ) 01378 { 01379 if( flags[i][1][k] && !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren(); 01380 if( temp.neighbors[i][1][k]->children ) for( int j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; 01381 } 01382 } 01383 { 01384 int j=y1<<1 , k=z1<<1; 01385 if( temp.neighbors[1][j][k] ) 01386 { 01387 if( flags[1][j][k] && !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren(); 01388 if( temp.neighbors[1][j][k]->children ) for( int i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; 01389 } 01390 } 01391 01392 // Set the neighbor from across the corner 01393 { 01394 int i=x1<<1 , j=y1<<1 , k=z1<<1; 01395 if( temp.neighbors[i][j][k] ) 01396 { 01397 if( flags[i][j][k] && !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren(); 01398 if( temp.neighbors[i][j][k]->children ) neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01399 } 01400 } 01401 } 01402 } 01403 return neighbors[d]; 01404 } 01405 01406 template<class NodeData,class Real> 01407 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors(OctNode<NodeData,Real>* node){ 01408 int d=node->depth(); 01409 if(node!=neighbors[d].neighbors[1][1][1]){ 01410 neighbors[d].clear(); 01411 01412 if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} 01413 else{ 01414 int i,j,k,x1,y1,z1,x2,y2,z2; 01415 int idx=int(node-node->parent->children); 01416 Cube::FactorCornerIndex( idx ,x1,y1,z1); 01417 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); 01418 for(i=0;i<2;i++){ 01419 for(j=0;j<2;j++){ 01420 for(k=0;k<2;k++){ 01421 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; 01422 } 01423 } 01424 } 01425 Neighbors3& temp=getNeighbors(node->parent); 01426 01427 // Set the neighbors from across the faces 01428 i=x1<<1; 01429 if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){ 01430 for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} 01431 } 01432 j=y1<<1; 01433 if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){ 01434 for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} 01435 } 01436 k=z1<<1; 01437 if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){ 01438 for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} 01439 } 01440 01441 // Set the neighbors from across the edges 01442 i=x1<<1; j=y1<<1; 01443 if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){ 01444 for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} 01445 } 01446 i=x1<<1; k=z1<<1; 01447 if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){ 01448 for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} 01449 } 01450 j=y1<<1; k=z1<<1; 01451 if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){ 01452 for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} 01453 } 01454 01455 // Set the neighbor from across the corner 01456 i=x1<<1; j=y1<<1; k=z1<<1; 01457 if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){ 01458 neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01459 } 01460 } 01461 } 01462 return neighbors[node->depth()]; 01463 } 01464 01465 /////////////////////// 01466 // ConstNeighborKey3 // 01467 /////////////////////// 01468 template<class NodeData,class Real> 01469 OctNode<NodeData,Real>::ConstNeighbors3::ConstNeighbors3(void){clear();} 01470 template<class NodeData,class Real> 01471 void OctNode<NodeData,Real>::ConstNeighbors3::clear(void){ 01472 for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} 01473 } 01474 template<class NodeData,class Real> 01475 OctNode<NodeData,Real>::ConstNeighborKey3::ConstNeighborKey3(void){neighbors=NULL;} 01476 template<class NodeData,class Real> 01477 OctNode<NodeData,Real>::ConstNeighborKey3::~ConstNeighborKey3(void){ 01478 if(neighbors){delete[] neighbors;} 01479 neighbors=NULL; 01480 } 01481 01482 template<class NodeData,class Real> 01483 void OctNode<NodeData,Real>::ConstNeighborKey3::set(int d){ 01484 if(neighbors){delete[] neighbors;} 01485 neighbors=NULL; 01486 if(d<0){return;} 01487 neighbors=new ConstNeighbors3[d+1]; 01488 } 01489 template<class NodeData,class Real> 01490 typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors(const OctNode<NodeData,Real>* node){ 01491 int d=node->depth(); 01492 if(node!=neighbors[d].neighbors[1][1][1]){ 01493 neighbors[d].clear(); 01494 01495 if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} 01496 else{ 01497 int i,j,k,x1,y1,z1,x2,y2,z2; 01498 int idx=int(node-node->parent->children); 01499 Cube::FactorCornerIndex( idx ,x1,y1,z1); 01500 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); 01501 for(i=0;i<2;i++){ 01502 for(j=0;j<2;j++){ 01503 for(k=0;k<2;k++){ 01504 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; 01505 } 01506 } 01507 } 01508 ConstNeighbors3& temp=getNeighbors(node->parent); 01509 01510 // Set the neighbors from across the faces 01511 i=x1<<1; 01512 if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){ 01513 for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} 01514 } 01515 j=y1<<1; 01516 if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){ 01517 for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} 01518 } 01519 k=z1<<1; 01520 if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){ 01521 for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} 01522 } 01523 01524 // Set the neighbors from across the edges 01525 i=x1<<1; j=y1<<1; 01526 if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){ 01527 for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} 01528 } 01529 i=x1<<1; k=z1<<1; 01530 if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){ 01531 for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} 01532 } 01533 j=y1<<1; k=z1<<1; 01534 if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){ 01535 for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} 01536 } 01537 01538 // Set the neighbor from across the corner 01539 i=x1<<1; j=y1<<1; k=z1<<1; 01540 if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){ 01541 neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01542 } 01543 } 01544 } 01545 return neighbors[node->depth()]; 01546 } 01547 template<class NodeData,class Real> 01548 typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors( const OctNode<NodeData,Real>* node , int minDepth ) 01549 { 01550 int d=node->depth(); 01551 if( d<minDepth ) fprintf( stderr , "[ERROR] Node depth lower than min-depth: %d < %d\n" , d , minDepth ) , exit( 0 ); 01552 if( node!=neighbors[d].neighbors[1][1][1] ) 01553 { 01554 neighbors[d].clear(); 01555 01556 if( d==minDepth ) neighbors[d].neighbors[1][1][1]=node; 01557 else 01558 { 01559 int i,j,k,x1,y1,z1,x2,y2,z2; 01560 int idx = int(node-node->parent->children); 01561 Cube::FactorCornerIndex( idx ,x1,y1,z1); 01562 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); 01563 01564 ConstNeighbors3& temp=getNeighbors( node->parent , minDepth ); 01565 01566 // Set the syblings 01567 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01568 neighbors[d].neighbors[x2+i][y2+j][z2+k] = &node->parent->children[ Cube::CornerIndex(i,j,k) ]; 01569 01570 // Set the neighbors from across the faces 01571 i=x1<<1; 01572 if( temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children ) 01573 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; 01574 01575 j=y1<<1; 01576 if( temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children ) 01577 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; 01578 01579 k=z1<<1; 01580 if( temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children ) 01581 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; 01582 01583 // Set the neighbors from across the edges 01584 i=x1<<1 , j=y1<<1; 01585 if( temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children ) 01586 for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; 01587 01588 i=x1<<1 , k=z1<<1; 01589 if( temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children ) 01590 for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; 01591 01592 j=y1<<1 , k=z1<<1; 01593 if( temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children ) 01594 for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; 01595 01596 // Set the neighbor from across the corner 01597 i=x1<<1 , j=y1<<1 , k=z1<<1; 01598 if( temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children ) 01599 neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; 01600 } 01601 } 01602 return neighbors[node->depth()]; 01603 } 01604 01605 template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); } 01606 template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); } 01607 template< class NodeData , class Real > 01608 void OctNode< NodeData , Real >::Neighbors5::clear( void ) 01609 { 01610 for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL; 01611 } 01612 template< class NodeData , class Real > 01613 void OctNode< NodeData , Real >::ConstNeighbors5::clear( void ) 01614 { 01615 for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL; 01616 } 01617 template< class NodeData , class Real > 01618 OctNode< NodeData , Real >::NeighborKey5::NeighborKey5( void ) 01619 { 01620 _depth = -1; 01621 neighbors = NULL; 01622 } 01623 template< class NodeData , class Real > 01624 OctNode< NodeData , Real >::ConstNeighborKey5::ConstNeighborKey5( void ) 01625 { 01626 _depth = -1; 01627 neighbors = NULL; 01628 } 01629 template< class NodeData , class Real > 01630 OctNode< NodeData , Real >::NeighborKey5::~NeighborKey5( void ) 01631 { 01632 if( neighbors ) delete[] neighbors; 01633 neighbors = NULL; 01634 } 01635 template< class NodeData , class Real > 01636 OctNode< NodeData , Real >::ConstNeighborKey5::~ConstNeighborKey5( void ) 01637 { 01638 if( neighbors ) delete[] neighbors; 01639 neighbors = NULL; 01640 } 01641 template< class NodeData , class Real > 01642 void OctNode< NodeData , Real >::NeighborKey5::set( int d ) 01643 { 01644 if( neighbors ) delete[] neighbors; 01645 neighbors = NULL; 01646 if(d<0) return; 01647 _depth = d; 01648 neighbors=new Neighbors5[d+1]; 01649 } 01650 template< class NodeData , class Real > 01651 void OctNode< NodeData , Real >::ConstNeighborKey5::set( int d ) 01652 { 01653 if( neighbors ) delete[] neighbors; 01654 neighbors = NULL; 01655 if(d<0) return; 01656 _depth = d; 01657 neighbors=new ConstNeighbors5[d+1]; 01658 } 01659 template< class NodeData , class Real > 01660 typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::getNeighbors( OctNode* node ) 01661 { 01662 int d=node->depth(); 01663 if( node!=neighbors[d].neighbors[2][2][2] ) 01664 { 01665 neighbors[d].clear(); 01666 01667 if( !node->parent ) neighbors[d].neighbors[2][2][2]=node; 01668 else 01669 { 01670 getNeighbors( node->parent ); 01671 Neighbors5& temp = neighbors[d-1]; 01672 int x1 , y1 , z1 , x2 , y2 , z2; 01673 int idx = int( node - node->parent->children ); 01674 Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); 01675 01676 Neighbors5& n = neighbors[d]; 01677 Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 ); 01678 int i , j , k; 01679 int fx0 = x2+1 , fy0 = y2+1 , fz0 = z2+1; // Indices of the bottom left corner of the parent within the 5x5x5 01680 int cx1 = x1*2+1 , cy1 = y1*2+1 , cz1 = z1*2+1; 01681 int cx2 = x2*2+1 , cy2 = y2*2+1 , cz2 = z2*2+1; 01682 int fx1 = x1*3 , fy1 = y1*3 , fz1 = z1*3; 01683 int fx2 = x2*4 , fy2 = y2*4 , fz2 = z2*4; 01684 01685 // Set the syblings 01686 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01687 n.neighbors[fx0+i][fy0+j][fz0+k] = node->parent->children + Cube::CornerIndex( i , j , k ); 01688 01689 // Set the neighbors from across the faces 01690 if( temp.neighbors[cx1][2][2] && temp.neighbors[cx1][2][2]->children ) 01691 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01692 n.neighbors[fx1+i][fy0+j][fz0+k] = temp.neighbors[cx1][2][2]->children + Cube::CornerIndex( i , j , k ); 01693 if( temp.neighbors[2][cy1][2] && temp.neighbors[2][cy1][2]->children ) 01694 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01695 n.neighbors[fx0+i][fy1+j][fz0+k] = temp.neighbors[2][cy1][2]->children + Cube::CornerIndex( i , j , k ); 01696 if( temp.neighbors[2][2][cz1] && temp.neighbors[2][2][cz1]->children ) 01697 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01698 n.neighbors[fx0+i][fy0+j][fz1+k] = temp.neighbors[2][2][cz1]->children + Cube::CornerIndex( i , j , k ); 01699 if( temp.neighbors[cx2][2][2] && temp.neighbors[cx2][2][2]->children ) 01700 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01701 n.neighbors[fx2 ][fy0+j][fz0+k] = temp.neighbors[cx2][2][2]->children + Cube::CornerIndex( x1 , j , k ); 01702 if( temp.neighbors[2][cy2][2] && temp.neighbors[2][cy2][2]->children ) 01703 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) 01704 n.neighbors[fx0+i][fy2 ][fz0+k] = temp.neighbors[2][cy2][2]->children + Cube::CornerIndex( i , y1 , k ); 01705 if( temp.neighbors[2][2][cz2] && temp.neighbors[2][2][cz2]->children ) 01706 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) 01707 n.neighbors[fx0+i][fy0+j][fz2 ] = temp.neighbors[2][2][cz2]->children + Cube::CornerIndex( i , j , z1 ); 01708 01709 // Set the neighbors from across the edges 01710 if( temp.neighbors[cx1][cy1][2] && temp.neighbors[cx1][cy1][2]->children ) 01711 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01712 n.neighbors[fx1+i][fy1+j][fz0+k] = temp.neighbors[cx1][cy1][2]->children + Cube::CornerIndex( i , j , k ); 01713 if( temp.neighbors[cx1][2][cz1] && temp.neighbors[cx1][2][cz1]->children ) 01714 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01715 n.neighbors[fx1+i][fy0+j][fz1+k] = temp.neighbors[cx1][2][cz1]->children + Cube::CornerIndex( i , j , k ); 01716 if( temp.neighbors[2][cy1][cz1] && temp.neighbors[2][cy1][cz1]->children ) 01717 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01718 n.neighbors[fx0+i][fy1+j][fz1+k] = temp.neighbors[2][cy1][cz1]->children + Cube::CornerIndex( i , j , k ); 01719 if( temp.neighbors[cx1][cy2][2] && temp.neighbors[cx1][cy2][2]->children ) 01720 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) 01721 n.neighbors[fx1+i][fy2 ][fz0+k] = temp.neighbors[cx1][cy2][2]->children + Cube::CornerIndex( i , y1 , k ); 01722 if( temp.neighbors[cx1][2][cz2] && temp.neighbors[cx1][2][cz2]->children ) 01723 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) 01724 n.neighbors[fx1+i][fy0+j][fz2 ] = temp.neighbors[cx1][2][cz2]->children + Cube::CornerIndex( i , j , z1 ); 01725 if( temp.neighbors[cx2][cy1][2] && temp.neighbors[cx2][cy1][2]->children ) 01726 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01727 n.neighbors[fx2 ][fy1+j][fz0+k] = temp.neighbors[cx2][cy1][2]->children + Cube::CornerIndex( x1 , j , k ); 01728 if( temp.neighbors[2][cy1][cz2] && temp.neighbors[2][cy1][cz2]->children ) 01729 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) 01730 n.neighbors[fx0+i][fy1+j][fz2 ] = temp.neighbors[2][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 ); 01731 if( temp.neighbors[cx2][2][cz1] && temp.neighbors[cx2][2][cz1]->children ) 01732 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01733 n.neighbors[fx2 ][fy0+j][fz1+k] = temp.neighbors[cx2][2][cz1]->children + Cube::CornerIndex( x1 , j , k ); 01734 if( temp.neighbors[2][cy2][cz1] && temp.neighbors[2][cy2][cz1]->children ) 01735 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) 01736 n.neighbors[fx0+i][fy2 ][fz1+k] = temp.neighbors[2][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k ); 01737 if( temp.neighbors[cx2][cy2][2] && temp.neighbors[cx2][cy2][2]->children ) 01738 for( k=0 ; k<2 ; k++ ) 01739 n.neighbors[fx2 ][fy2 ][fz0+k] = temp.neighbors[cx2][cy2][2]->children + Cube::CornerIndex( x1 , y1 , k ); 01740 if( temp.neighbors[cx2][2][cz2] && temp.neighbors[cx2][2][cz2]->children ) 01741 for( j=0 ; j<2 ; j++ ) 01742 n.neighbors[fx2 ][fy0+j][fz2 ] = temp.neighbors[cx2][2][cz2]->children + Cube::CornerIndex( x1 , j , z1 ); 01743 if( temp.neighbors[2][cy2][cz2] && temp.neighbors[2][cy2][cz2]->children ) 01744 for( i=0 ; i<2 ; i++ ) 01745 n.neighbors[fx0+i][fy2 ][fz2 ] = temp.neighbors[2][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 ); 01746 01747 // Set the neighbor from across the corners 01748 if( temp.neighbors[cx1][cy1][cz1] && temp.neighbors[cx1][cy1][cz1]->children ) 01749 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01750 n.neighbors[fx1+i][fy1+j][fz1+k] = temp.neighbors[cx1][cy1][cz1]->children + Cube::CornerIndex( i , j , k ); 01751 if( temp.neighbors[cx1][cy1][cz2] && temp.neighbors[cx1][cy1][cz2]->children ) 01752 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) 01753 n.neighbors[fx1+i][fy1+j][fz2 ] = temp.neighbors[cx1][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 ); 01754 if( temp.neighbors[cx1][cy2][cz1] && temp.neighbors[cx1][cy2][cz1]->children ) 01755 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) 01756 n.neighbors[fx1+i][fy2 ][fz1+k] = temp.neighbors[cx1][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k ); 01757 if( temp.neighbors[cx2][cy1][cz1] && temp.neighbors[cx2][cy1][cz1]->children ) 01758 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) 01759 n.neighbors[fx2 ][fy1+j][fz1+k] = temp.neighbors[cx2][cy1][cz1]->children + Cube::CornerIndex( x1 , j , k ); 01760 if( temp.neighbors[cx1][cy2][cz2] && temp.neighbors[cx1][cy2][cz2]->children ) 01761 for( i=0 ; i<2 ; i++ ) 01762 n.neighbors[fx1+i][fy2 ][fz2 ] = temp.neighbors[cx1][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 ); 01763 if( temp.neighbors[cx2][cy1][cz2] && temp.neighbors[cx2][cy1][cz2]->children ) 01764 for( j=0 ; j<2 ; j++ ) 01765 n.neighbors[fx2 ][fy1+j][fz2 ] = temp.neighbors[cx2][cy1][cz2]->children + Cube::CornerIndex( x1 , j , z1 ); 01766 if( temp.neighbors[cx2][cy2][cz1] && temp.neighbors[cx2][cy2][cz1]->children ) 01767 for( k=0 ; k<2 ; k++ ) 01768 n.neighbors[fx2 ][fy2 ][fz1+k] = temp.neighbors[cx2][cy2][cz1]->children + Cube::CornerIndex( x1 , y1 , k ); 01769 if( temp.neighbors[cx2][cy2][cz2] && temp.neighbors[cx2][cy2][cz2]->children ) 01770 n.neighbors[fx2 ][fy2 ][fz2 ] = temp.neighbors[cx2][cy2][cz2]->children + Cube::CornerIndex( x1 , y1 , z1 ); 01771 } 01772 } 01773 return neighbors[d]; 01774 } 01775 template< class NodeData , class Real > 01776 typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) 01777 { 01778 int d=node->depth(); 01779 if( node!=neighbors[d].neighbors[2][2][2] ) 01780 { 01781 neighbors[d].clear(); 01782 01783 if( !node->parent ) neighbors[d].neighbors[2][2][2]=node; 01784 else 01785 { 01786 setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd ); 01787 Neighbors5& temp = neighbors[d-1]; 01788 int x1 , y1 , z1 , x2 , y2 , z2 , ii , jj , kk; 01789 int idx = int( node-node->parent->children ); 01790 Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); 01791 01792 for( int i=xStart ; i<xEnd ; i++ ) 01793 { 01794 x2 = i+x1; 01795 ii = x2&1; 01796 x2 = 1+(x2>>1); 01797 for( int j=yStart ; j<yEnd ; j++ ) 01798 { 01799 y2 = j+y1; 01800 jj = y2&1; 01801 y2 = 1+(y2>>1); 01802 for( int k=zStart ; k<zEnd ; k++ ) 01803 { 01804 z2 = k+z1; 01805 kk = z2&1; 01806 z2 = 1+(z2>>1); 01807 if(temp.neighbors[x2][y2][z2] ) 01808 { 01809 if( !temp.neighbors[x2][y2][z2]->children ) temp.neighbors[x2][y2][z2]->initChildren(); 01810 neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk); 01811 } 01812 } 01813 } 01814 } 01815 } 01816 } 01817 return neighbors[d]; 01818 } 01819 template< class NodeData , class Real > 01820 typename OctNode< NodeData , Real >::ConstNeighbors5& OctNode< NodeData , Real >::ConstNeighborKey5::getNeighbors( const OctNode* node ) 01821 { 01822 int d=node->depth(); 01823 if( node!=neighbors[d].neighbors[2][2][2] ) 01824 { 01825 neighbors[d].clear(); 01826 01827 if(!node->parent) neighbors[d].neighbors[2][2][2]=node; 01828 else 01829 { 01830 getNeighbors( node->parent ); 01831 ConstNeighbors5& temp = neighbors[d-1]; 01832 int x1,y1,z1,x2,y2,z2,ii,jj,kk; 01833 int idx=int(node-node->parent->children); 01834 Cube::FactorCornerIndex(idx,x1,y1,z1); 01835 01836 for(int i=0;i<5;i++) 01837 { 01838 x2=i+x1; 01839 ii=x2&1; 01840 x2=1+(x2>>1); 01841 for(int j=0;j<5;j++) 01842 { 01843 y2=j+y1; 01844 jj=y2&1; 01845 y2=1+(y2>>1); 01846 for(int k=0;k<5;k++) 01847 { 01848 z2=k+z1; 01849 kk=z2&1; 01850 z2=1+(z2>>1); 01851 if(temp.neighbors[x2][y2][z2] && temp.neighbors[x2][y2][z2]->children) 01852 neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk); 01853 } 01854 } 01855 } 01856 } 01857 } 01858 return neighbors[d]; 01859 } 01860 01861 01862 template <class NodeData,class Real> 01863 int OctNode<NodeData,Real>::write(const char* fileName) const{ 01864 FILE* fp=fopen(fileName,"wb"); 01865 if(!fp){return 0;} 01866 int ret=write(fp); 01867 fclose(fp); 01868 return ret; 01869 } 01870 template <class NodeData,class Real> 01871 int OctNode<NodeData,Real>::write(FILE* fp) const{ 01872 fwrite(this,sizeof(OctNode<NodeData,Real>),1,fp); 01873 if(children){for(int i=0;i<Cube::CORNERS;i++){children[i].write(fp);}} 01874 return 1; 01875 } 01876 template <class NodeData,class Real> 01877 int OctNode<NodeData,Real>::read(const char* fileName){ 01878 FILE* fp=fopen(fileName,"rb"); 01879 if(!fp){return 0;} 01880 int ret=read(fp); 01881 fclose(fp); 01882 return ret; 01883 } 01884 template <class NodeData,class Real> 01885 int OctNode<NodeData,Real>::read(FILE* fp){ 01886 fread(this,sizeof(OctNode<NodeData,Real>),1,fp); 01887 parent=NULL; 01888 if(children){ 01889 children=NULL; 01890 initChildren(); 01891 for(int i=0;i<Cube::CORNERS;i++){ 01892 children[i].read(fp); 01893 children[i].parent=this; 01894 } 01895 } 01896 return 1; 01897 } 01898 template<class NodeData,class Real> 01899 int OctNode<NodeData,Real>::width(int maxDepth) const { 01900 int d=depth(); 01901 return 1<<(maxDepth-d); 01902 } 01903 template<class NodeData,class Real> 01904 void OctNode<NodeData,Real>::centerIndex(int maxDepth,int index[DIMENSION]) const { 01905 int d,o[3]; 01906 depthAndOffset(d,o); 01907 for(int i=0;i<DIMENSION;i++){index[i]=BinaryNode<Real>::CornerIndex(maxDepth,d+1,o[i]<<1,1);} 01908 } 01909 01910 } 01911 }