Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
Octree.inl
1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12 
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
27 */
28 
29 #ifndef DOXY_IGNORE_THIS
30 
31 #include <cstdlib>
32 #include <cmath>
33 #include <cstddef>
34 #include <algorithm>
35 
37 // OctNode //
39 template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthShift=5;
40 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift=19;
41 template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthMask=(1<<DepthShift)-1;
42 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetMask=(1<<OffsetShift)-1;
43 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift1=DepthShift;
44 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift2=OffsetShift1+OffsetShift;
45 template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift3=OffsetShift2+OffsetShift;
46 
47 template<class NodeData,class Real> int OctNode<NodeData,Real>::UseAlloc=0;
48 template<class NodeData,class Real> AllocatorT<OctNode<NodeData,Real> > OctNode<NodeData,Real>::Allocator;
49 
50 template<class NodeData,class Real>
51 void OctNode<NodeData,Real>::SetAllocator(int blockSize)
52 {
53  if(blockSize>0)
54  {
55  UseAlloc=1;
56  Allocator.set(blockSize);
57  }
58  else{UseAlloc=0;}
59 }
60 template<class NodeData,class Real>
61 int OctNode<NodeData,Real>::UseAllocator(void){return UseAlloc;}
62 
63 template <class NodeData,class Real>
64 OctNode<NodeData,Real>::OctNode(void){
65  parent=children=NULL;
66  d=off[0]=off[1]=off[2]=0;
67 }
68 
69 template <class NodeData,class Real>
70 OctNode<NodeData,Real>::~OctNode(void){
71  if(!UseAlloc){if(children){delete[] children;}}
72  parent=children=NULL;
73 }
74 template <class NodeData,class Real>
75 void OctNode<NodeData,Real>::setFullDepth(int maxDepth){
76  if( maxDepth )
77  {
78  if( !children ) initChildren();
79  for( int i=0 ; i<8 ; i++ ) children[i].setFullDepth( maxDepth-1 );
80  }
81 }
82 
83 template <class NodeData,class Real>
84 int OctNode<NodeData,Real>::initChildren( void )
85 {
86  if( UseAlloc ) children=Allocator.newElements(8);
87  else
88  {
89  if( children ) delete[] children;
90  children = NULL;
91  children = new OctNode[Cube::CORNERS];
92  }
93  if( !children )
94  {
95  fprintf(stderr,"Failed to initialize children in OctNode::initChildren\n");
96  exit(0);
97  return 0;
98  }
99  int d , off[3];
100  depthAndOffset( d , off );
101  for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
102  {
103  int idx=Cube::CornerIndex(i,j,k);
104  children[idx].parent = this;
105  children[idx].children = NULL;
106  int off2[3];
107  off2[0] = (off[0]<<1)+i;
108  off2[1] = (off[1]<<1)+j;
109  off2[2] = (off[2]<<1)+k;
110  Index( d+1 , off2 , children[idx].d , children[idx].off );
111  }
112  return 1;
113 }
114 template <class NodeData,class Real>
115 inline void OctNode<NodeData,Real>::Index(int depth,const int offset[3],short& d,short off[3]){
116  d=short(depth);
117  off[0]=short((1<<depth)+offset[0]-1);
118  off[1]=short((1<<depth)+offset[1]-1);
119  off[2]=short((1<<depth)+offset[2]-1);
120 }
121 
122 template<class NodeData,class Real>
123 inline void OctNode<NodeData,Real>::depthAndOffset(int& depth,int offset[3]) const {
124  depth=int(d);
125  offset[0]=(int(off[0])+1)&(~(1<<depth));
126  offset[1]=(int(off[1])+1)&(~(1<<depth));
127  offset[2]=(int(off[2])+1)&(~(1<<depth));
128 }
129 template<class NodeData,class Real>
130 inline int OctNode<NodeData,Real>::depth(void) const {return int(d);}
131 template<class NodeData,class Real>
132 inline void OctNode<NodeData,Real>::DepthAndOffset(const long long& index,int& depth,int offset[3]){
133  depth=int(index&DepthMask);
134  offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<<depth));
135  offset[1]=(int((index>>OffsetShift2)&OffsetMask)+1)&(~(1<<depth));
136  offset[2]=(int((index>>OffsetShift3)&OffsetMask)+1)&(~(1<<depth));
137 }
138 template<class NodeData,class Real>
139 inline int OctNode<NodeData,Real>::Depth(const long long& index){return int(index&DepthMask);}
140 template <class NodeData,class Real>
141 void OctNode<NodeData,Real>::centerAndWidth(Point3D<Real>& center,Real& width) const{
142  int depth,offset[3];
143  depth=int(d);
144  offset[0]=(int(off[0])+1)&(~(1<<depth));
145  offset[1]=(int(off[1])+1)&(~(1<<depth));
146  offset[2]=(int(off[2])+1)&(~(1<<depth));
147  width=Real(1.0/(1<<depth));
148  for(int dim=0;dim<DIMENSION;dim++){center.coords[dim]=Real(0.5+offset[dim])*width;}
149 }
150 template< class NodeData , class Real >
151 bool OctNode< NodeData , Real >::isInside( Point3D< Real > p ) const
152 {
153  Point3D< Real > c;
154  Real w;
155  centerAndWidth( c , w );
156  w /= 2;
157  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);
158 }
159 template <class NodeData,class Real>
160 inline void OctNode<NodeData,Real>::CenterAndWidth(const long long& index,Point3D<Real>& center,Real& width){
161  int depth,offset[3];
162  depth=index&DepthMask;
163  offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<<depth));
164  offset[1]=(int((index>>OffsetShift2)&OffsetMask)+1)&(~(1<<depth));
165  offset[2]=(int((index>>OffsetShift3)&OffsetMask)+1)&(~(1<<depth));
166  width=Real(1.0/(1<<depth));
167  for(int dim=0;dim<DIMENSION;dim++){center.coords[dim]=Real(0.5+offset[dim])*width;}
168 }
169 
170 template <class NodeData,class Real>
171 int OctNode<NodeData,Real>::maxDepth(void) const{
172  if(!children){return 0;}
173  else{
174  int c=0;
175  int d=0;
176  for(unsigned int i=0;i<Cube::CORNERS;i++){
177  d=children[i].maxDepth();
178  if(!i || d>c){c=d;}
179  }
180  return c+1;
181  }
182 }
183 template <class NodeData,class Real>
184 int OctNode<NodeData,Real>::nodes(void) const{
185  if(!children){return 1;}
186  else{
187  int c=0;
188  for(unsigned int i=0;i<Cube::CORNERS;i++){c+=children[i].nodes();}
189  return c+1;
190  }
191 }
192 template <class NodeData,class Real>
193 int OctNode<NodeData,Real>::leaves(void) const{
194  if(!children){return 1;}
195  else{
196  int c=0;
197  for(unsigned int i=0;i<Cube::CORNERS;i++){c+=children[i].leaves();}
198  return c;
199  }
200 }
201 template<class NodeData,class Real>
202 int OctNode<NodeData,Real>::maxDepthLeaves(int maxDepth) const{
203  if(depth()>maxDepth){return 0;}
204  if(!children){return 1;}
205  else{
206  int c=0;
207  for(unsigned int i=0;i<Cube::CORNERS;i++){c+=children[i].maxDepthLeaves(maxDepth);}
208  return c;
209  }
210 }
211 template <class NodeData,class Real>
212 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::root(void) const{
213  const OctNode* temp=this;
214  while(temp->parent){temp=temp->parent;}
215  return temp;
216 }
217 
218 
219 template <class NodeData,class Real>
220 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextBranch( const OctNode* current ) const
221 {
222  if( !current->parent || current==this ) return NULL;
223  if(current-current->parent->children==Cube::CORNERS-1) return nextBranch( current->parent );
224  else return current+1;
225 }
226 template <class NodeData,class Real>
227 OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextBranch(OctNode* current){
228  if(!current->parent || current==this){return NULL;}
229  if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);}
230  else{return current+1;}
231 }
232 template< class NodeData , class Real >
233 const OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( const OctNode* current ) const
234 {
235  if( !current->parent || current==this ) return NULL;
236  if( current-current->parent->children==0 ) return prevBranch( current->parent );
237  else return current-1;
238 }
239 template< class NodeData , class Real >
240 OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( OctNode* current )
241 {
242  if( !current->parent || current==this ) return NULL;
243  if( current-current->parent->children==0 ) return prevBranch( current->parent );
244  else return current-1;
245 }
246 template <class NodeData,class Real>
247 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextLeaf(const OctNode* current) const{
248  if(!current){
249  const OctNode<NodeData,Real>* temp=this;
250  while(temp->children){temp=&temp->children[0];}
251  return temp;
252  }
253  if(current->children){return current->nextLeaf();}
254  const OctNode* temp=nextBranch(current);
255  if(!temp){return NULL;}
256  else{return temp->nextLeaf();}
257 }
258 template <class NodeData,class Real>
259 OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextLeaf(OctNode* current){
260  if(!current){
261  OctNode<NodeData,Real>* temp=this;
262  while(temp->children){temp=&temp->children[0];}
263  return temp;
264  }
265  if(current->children){return current->nextLeaf();}
266  OctNode* temp=nextBranch(current);
267  if(!temp){return NULL;}
268  else{return temp->nextLeaf();}
269 }
270 
271 template <class NodeData,class Real>
272 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextNode( const OctNode* current ) const
273 {
274  if( !current ) return this;
275  else if( current->children ) return &current->children[0];
276  else return nextBranch(current);
277 }
278 template <class NodeData,class Real>
279 OctNode<NodeData,Real>* OctNode<NodeData,Real>::nextNode( OctNode* current )
280 {
281  if( !current ) return this;
282  else if( current->children ) return &current->children[0];
283  else return nextBranch( current );
284 }
285 
286 template <class NodeData,class Real>
287 void OctNode<NodeData,Real>::printRange(void) const{
288  Point3D<Real> center;
289  Real width;
290  centerAndWidth(center,width);
291  for(int dim=0;dim<DIMENSION;dim++){
292  printf("[%f,%f]",center.coords[dim]-width/2,center.coords[dim]+width/2);
293  if(dim<DIMENSION-1){printf("x");}
294  else printf("\n");
295  }
296 }
297 
298 template <class NodeData,class Real>
299 void OctNode<NodeData,Real>::AdjacencyCountFunction::Function(const OctNode<NodeData,Real>* node1,const OctNode<NodeData,Real>* node2){count++;}
300 
301 template <class NodeData,class Real>
302 template<class NodeAdjacencyFunction>
303 void OctNode<NodeData,Real>::processNodeNodes(OctNode* node,NodeAdjacencyFunction* F,int processCurrent){
304  if(processCurrent){F->Function(this,node);}
305  if(children){__processNodeNodes(node,F);}
306 }
307 template <class NodeData,class Real>
308 template<class NodeAdjacencyFunction>
309 void OctNode<NodeData,Real>::processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent){
310  if(processCurrent){F->Function(this,node);}
311  if(children){
312  int c1,c2,c3,c4;
313  Cube::FaceCorners(fIndex,c1,c2,c3,c4);
314  __processNodeFaces(node,F,c1,c2,c3,c4);
315  }
316 }
317 template <class NodeData,class Real>
318 template<class NodeAdjacencyFunction>
319 void OctNode<NodeData,Real>::processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent){
320  if(processCurrent){F->Function(this,node);}
321  if(children){
322  int c1,c2;
323  Cube::EdgeCorners(eIndex,c1,c2);
324  __processNodeEdges(node,F,c1,c2);
325  }
326 }
327 template <class NodeData,class Real>
328 template<class NodeAdjacencyFunction>
329 void OctNode<NodeData,Real>::processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent){
330  if(processCurrent){F->Function(this,node);}
331  OctNode<NodeData,Real>* temp=this;
332  while(temp->children){
333  temp=&temp->children[cIndex];
334  F->Function(temp,node);
335  }
336 }
337 template <class NodeData,class Real>
338 template<class NodeAdjacencyFunction>
339 void OctNode<NodeData,Real>::__processNodeNodes(OctNode* node,NodeAdjacencyFunction* F){
340  F->Function(&children[0],node);
341  F->Function(&children[1],node);
342  F->Function(&children[2],node);
343  F->Function(&children[3],node);
344  F->Function(&children[4],node);
345  F->Function(&children[5],node);
346  F->Function(&children[6],node);
347  F->Function(&children[7],node);
348  if(children[0].children){children[0].__processNodeNodes(node,F);}
349  if(children[1].children){children[1].__processNodeNodes(node,F);}
350  if(children[2].children){children[2].__processNodeNodes(node,F);}
351  if(children[3].children){children[3].__processNodeNodes(node,F);}
352  if(children[4].children){children[4].__processNodeNodes(node,F);}
353  if(children[5].children){children[5].__processNodeNodes(node,F);}
354  if(children[6].children){children[6].__processNodeNodes(node,F);}
355  if(children[7].children){children[7].__processNodeNodes(node,F);}
356 }
357 template <class NodeData,class Real>
358 template<class NodeAdjacencyFunction>
359 void OctNode<NodeData,Real>::__processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2){
360  F->Function(&children[cIndex1],node);
361  F->Function(&children[cIndex2],node);
362  if(children[cIndex1].children){children[cIndex1].__processNodeEdges(node,F,cIndex1,cIndex2);}
363  if(children[cIndex2].children){children[cIndex2].__processNodeEdges(node,F,cIndex1,cIndex2);}
364 }
365 template <class NodeData,class Real>
366 template<class NodeAdjacencyFunction>
367 void OctNode<NodeData,Real>::__processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4){
368  F->Function(&children[cIndex1],node);
369  F->Function(&children[cIndex2],node);
370  F->Function(&children[cIndex3],node);
371  F->Function(&children[cIndex4],node);
372  if(children[cIndex1].children){children[cIndex1].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);}
373  if(children[cIndex2].children){children[cIndex2].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);}
374  if(children[cIndex3].children){children[cIndex3].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);}
375  if(children[cIndex4].children){children[cIndex4].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);}
376 }
377 template<class NodeData,class Real>
378 template<class NodeAdjacencyFunction>
379 void OctNode<NodeData,Real>::ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent){
380  int c1[3],c2[3],w1,w2;
381  node1->centerIndex(maxDepth+1,c1);
382  node2->centerIndex(maxDepth+1,c2);
383  w1=node1->width(maxDepth+1);
384  w2=node2->width(maxDepth+1);
385 
386  ProcessNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent);
387 }
388 template<class NodeData,class Real>
389 template<class NodeAdjacencyFunction>
390 void OctNode<NodeData,Real>::ProcessNodeAdjacentNodes(int dx,int dy,int dz,
391  OctNode<NodeData,Real>* node1,int radius1,
392  OctNode<NodeData,Real>* node2,int radius2,int width2,
393  NodeAdjacencyFunction* F,int processCurrent){
394  if(!Overlap(dx,dy,dz,radius1+radius2)){return;}
395  if(processCurrent){F->Function(node2,node1);}
396  if(!node2->children){return;}
397  __ProcessNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F);
398 }
399 template<class NodeData,class Real>
400 template<class TerminatingNodeAdjacencyFunction>
401 void OctNode<NodeData,Real>::ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent){
402  int c1[3],c2[3],w1,w2;
403  node1->centerIndex(maxDepth+1,c1);
404  node2->centerIndex(maxDepth+1,c2);
405  w1=node1->width(maxDepth+1);
406  w2=node2->width(maxDepth+1);
407 
408  ProcessTerminatingNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent);
409 }
410 template<class NodeData,class Real>
411 template<class TerminatingNodeAdjacencyFunction>
412 void OctNode<NodeData,Real>::ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,
413  OctNode<NodeData,Real>* node1,int radius1,
414  OctNode<NodeData,Real>* node2,int radius2,int width2,
415  TerminatingNodeAdjacencyFunction* F,int processCurrent)
416 {
417  if(!Overlap(dx,dy,dz,radius1+radius2)){return;}
418  if(processCurrent){F->Function(node2,node1);}
419  if(!node2->children){return;}
420  __ProcessTerminatingNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F);
421 }
422 template<class NodeData,class Real>
423 template<class PointAdjacencyFunction>
424 void OctNode<NodeData,Real>::ProcessPointAdjacentNodes( int maxDepth , const int c1[3] , OctNode* node2 , int width2 , PointAdjacencyFunction* F , int processCurrent )
425 {
426  int c2[3] , w2;
427  node2->centerIndex( maxDepth+1 , c2 );
428  w2 = node2->width( maxDepth+1 );
429  ProcessPointAdjacentNodes( c1[0]-c2[0] , c1[1]-c2[1] , c1[2]-c2[2] , node2 , (width2*w2)>>1 , w2 , F , processCurrent );
430 }
431 template<class NodeData,class Real>
432 template<class PointAdjacencyFunction>
433 void OctNode<NodeData,Real>::ProcessPointAdjacentNodes(int dx,int dy,int dz,
434  OctNode<NodeData,Real>* node2,int radius2,int width2,
435  PointAdjacencyFunction* F,int processCurrent)
436 {
437  if( !Overlap(dx,dy,dz,radius2) ) return;
438  if( processCurrent ) F->Function(node2);
439  if( !node2->children ) return;
440  __ProcessPointAdjacentNodes( -dx , -dy , -dz , node2 , radius2 , width2>>1 , F );
441 }
442 template<class NodeData,class Real>
443 template<class NodeAdjacencyFunction>
444 void OctNode<NodeData,Real>::ProcessFixedDepthNodeAdjacentNodes(int maxDepth,
445  OctNode<NodeData,Real>* node1,int width1,
446  OctNode<NodeData,Real>* node2,int width2,
447  int depth,NodeAdjacencyFunction* F,int processCurrent)
448 {
449  int c1[3],c2[3],w1,w2;
450  node1->centerIndex(maxDepth+1,c1);
451  node2->centerIndex(maxDepth+1,c2);
452  w1=node1->width(maxDepth+1);
453  w2=node2->width(maxDepth+1);
454 
455  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);
456 }
457 template<class NodeData,class Real>
458 template<class NodeAdjacencyFunction>
459 void OctNode<NodeData,Real>::ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,
460  OctNode<NodeData,Real>* node1,int radius1,
461  OctNode<NodeData,Real>* node2,int radius2,int width2,
462  int depth,NodeAdjacencyFunction* F,int processCurrent)
463 {
464  int d=node2->depth();
465  if(d>depth){return;}
466  if(!Overlap(dx,dy,dz,radius1+radius2)){return;}
467  if(d==depth){if(processCurrent){F->Function(node2,node1);}}
468  else{
469  if(!node2->children){return;}
470  __ProcessFixedDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,depth-1,F);
471  }
472 }
473 template<class NodeData,class Real>
474 template<class NodeAdjacencyFunction>
475 void OctNode<NodeData,Real>::ProcessMaxDepthNodeAdjacentNodes(int maxDepth,
476  OctNode<NodeData,Real>* node1,int width1,
477  OctNode<NodeData,Real>* node2,int width2,
478  int depth,NodeAdjacencyFunction* F,int processCurrent)
479 {
480  int c1[3],c2[3],w1,w2;
481  node1->centerIndex(maxDepth+1,c1);
482  node2->centerIndex(maxDepth+1,c2);
483  w1=node1->width(maxDepth+1);
484  w2=node2->width(maxDepth+1);
485  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);
486 }
487 template<class NodeData,class Real>
488 template<class NodeAdjacencyFunction>
489 void OctNode<NodeData,Real>::ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,
490  OctNode<NodeData,Real>* node1,int radius1,
491  OctNode<NodeData,Real>* node2,int radius2,int width2,
492  int depth,NodeAdjacencyFunction* F,int processCurrent)
493 {
494  int d=node2->depth();
495  if(d>depth){return;}
496  if(!Overlap(dx,dy,dz,radius1+radius2)){return;}
497  if(processCurrent){F->Function(node2,node1);}
498  if(d<depth && node2->children){__ProcessMaxDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2>>1,depth-1,F);}
499 }
500 template <class NodeData,class Real>
501 template<class NodeAdjacencyFunction>
502 void OctNode<NodeData,Real>::__ProcessNodeAdjacentNodes(int dx,int dy,int dz,
503  OctNode* node1,int radius1,
504  OctNode* node2,int radius2,int cWidth2,
505  NodeAdjacencyFunction* F)
506 {
507  int cWidth=cWidth2>>1;
508  int radius=radius2>>1;
509  int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth);
510  if(o){
511  int dx1=dx-cWidth;
512  int dx2=dx+cWidth;
513  int dy1=dy-cWidth;
514  int dy2=dy+cWidth;
515  int dz1=dz-cWidth;
516  int dz2=dz+cWidth;
517  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);}}
518  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);}}
519  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);}}
520  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);}}
521  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);}}
522  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);}}
523  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);}}
524  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);}}
525  }
526 }
527 template <class NodeData,class Real>
528 template<class TerminatingNodeAdjacencyFunction>
529 void OctNode<NodeData,Real>::__ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,
530  OctNode* node1,int radius1,
531  OctNode* node2,int radius2,int cWidth2,
532  TerminatingNodeAdjacencyFunction* F)
533 {
534  int cWidth=cWidth2>>1;
535  int radius=radius2>>1;
536  int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth);
537  if(o){
538  int dx1=dx-cWidth;
539  int dx2=dx+cWidth;
540  int dy1=dy-cWidth;
541  int dy2=dy+cWidth;
542  int dz1=dz-cWidth;
543  int dz2=dz+cWidth;
544  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);}}
545  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);}}
546  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);}}
547  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);}}
548  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);}}
549  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);}}
550  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);}}
551  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);}}
552  }
553 }
554 template <class NodeData,class Real>
555 template<class PointAdjacencyFunction>
556 void OctNode<NodeData,Real>::__ProcessPointAdjacentNodes(int dx,int dy,int dz,
557  OctNode* node2,int radius2,int cWidth2,
558  PointAdjacencyFunction* F)
559 {
560  int cWidth=cWidth2>>1;
561  int radius=radius2>>1;
562  int o=ChildOverlap(dx,dy,dz,radius,cWidth);
563  if( o )
564  {
565  int dx1=dx-cWidth;
566  int dx2=dx+cWidth;
567  int dy1=dy-cWidth;
568  int dy2=dy+cWidth;
569  int dz1=dz-cWidth;
570  int dz2=dz+cWidth;
571  if(o& 1){F->Function(&node2->children[0]);if(node2->children[0].children){__ProcessPointAdjacentNodes(dx1,dy1,dz1,&node2->children[0],radius,cWidth,F);}}
572  if(o& 2){F->Function(&node2->children[1]);if(node2->children[1].children){__ProcessPointAdjacentNodes(dx2,dy1,dz1,&node2->children[1],radius,cWidth,F);}}
573  if(o& 4){F->Function(&node2->children[2]);if(node2->children[2].children){__ProcessPointAdjacentNodes(dx1,dy2,dz1,&node2->children[2],radius,cWidth,F);}}
574  if(o& 8){F->Function(&node2->children[3]);if(node2->children[3].children){__ProcessPointAdjacentNodes(dx2,dy2,dz1,&node2->children[3],radius,cWidth,F);}}
575  if(o& 16){F->Function(&node2->children[4]);if(node2->children[4].children){__ProcessPointAdjacentNodes(dx1,dy1,dz2,&node2->children[4],radius,cWidth,F);}}
576  if(o& 32){F->Function(&node2->children[5]);if(node2->children[5].children){__ProcessPointAdjacentNodes(dx2,dy1,dz2,&node2->children[5],radius,cWidth,F);}}
577  if(o& 64){F->Function(&node2->children[6]);if(node2->children[6].children){__ProcessPointAdjacentNodes(dx1,dy2,dz2,&node2->children[6],radius,cWidth,F);}}
578  if(o&128){F->Function(&node2->children[7]);if(node2->children[7].children){__ProcessPointAdjacentNodes(dx2,dy2,dz2,&node2->children[7],radius,cWidth,F);}}
579  }
580 }
581 template <class NodeData,class Real>
582 template<class NodeAdjacencyFunction>
583 void OctNode<NodeData,Real>::__ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,
584  OctNode* node1,int radius1,
585  OctNode* node2,int radius2,int cWidth2,
586  int depth,NodeAdjacencyFunction* F)
587 {
588  int cWidth=cWidth2>>1;
589  int radius=radius2>>1;
590  int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth);
591  if(o){
592  int dx1=dx-cWidth;
593  int dx2=dx+cWidth;
594  int dy1=dy-cWidth;
595  int dy2=dy+cWidth;
596  int dz1=dz-cWidth;
597  int dz2=dz+cWidth;
598  if(node2->depth()==depth){
599  if(o& 1){F->Function(&node2->children[0],node1);}
600  if(o& 2){F->Function(&node2->children[1],node1);}
601  if(o& 4){F->Function(&node2->children[2],node1);}
602  if(o& 8){F->Function(&node2->children[3],node1);}
603  if(o& 16){F->Function(&node2->children[4],node1);}
604  if(o& 32){F->Function(&node2->children[5],node1);}
605  if(o& 64){F->Function(&node2->children[6],node1);}
606  if(o&128){F->Function(&node2->children[7],node1);}
607  }
608  else{
609  if(o& 1){if(node2->children[0].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}}
610  if(o& 2){if(node2->children[1].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}}
611  if(o& 4){if(node2->children[2].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}}
612  if(o& 8){if(node2->children[3].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}}
613  if(o& 16){if(node2->children[4].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}}
614  if(o& 32){if(node2->children[5].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}}
615  if(o& 64){if(node2->children[6].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}}
616  if(o&128){if(node2->children[7].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}}
617  }
618  }
619 }
620 template <class NodeData,class Real>
621 template<class NodeAdjacencyFunction>
622 void OctNode<NodeData,Real>::__ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,
623  OctNode* node1,int radius1,
624  OctNode* node2,int radius2,int cWidth2,
625  int depth,NodeAdjacencyFunction* F)
626 {
627  int cWidth=cWidth2>>1;
628  int radius=radius2>>1;
629  int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth);
630  if(o){
631  int dx1=dx-cWidth;
632  int dx2=dx+cWidth;
633  int dy1=dy-cWidth;
634  int dy2=dy+cWidth;
635  int dz1=dz-cWidth;
636  int dz2=dz+cWidth;
637  if(node2->depth()<=depth){
638  if(o& 1){F->Function(&node2->children[0],node1);}
639  if(o& 2){F->Function(&node2->children[1],node1);}
640  if(o& 4){F->Function(&node2->children[2],node1);}
641  if(o& 8){F->Function(&node2->children[3],node1);}
642  if(o& 16){F->Function(&node2->children[4],node1);}
643  if(o& 32){F->Function(&node2->children[5],node1);}
644  if(o& 64){F->Function(&node2->children[6],node1);}
645  if(o&128){F->Function(&node2->children[7],node1);}
646  }
647  if(node2->depth()<depth){
648  if(o& 1){if(node2->children[0].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}}
649  if(o& 2){if(node2->children[1].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}}
650  if(o& 4){if(node2->children[2].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}}
651  if(o& 8){if(node2->children[3].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}}
652  if(o& 16){if(node2->children[4].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}}
653  if(o& 32){if(node2->children[5].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}}
654  if(o& 64){if(node2->children[6].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}}
655  if(o&128){if(node2->children[7].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}}
656  }
657  }
658 }
659 template <class NodeData,class Real>
660 inline int OctNode<NodeData,Real>::ChildOverlap(int dx,int dy,int dz,int d,int cRadius2)
661 {
662  int w1=d-cRadius2;
663  int w2=d+cRadius2;
664  int overlap=0;
665 
666  int test=0,test1=0;
667  if(dx<w2 && dx>-w1){test =1;}
668  if(dx<w1 && dx>-w2){test|=2;}
669 
670  if(!test){return 0;}
671  if(dz<w2 && dz>-w1){test1 =test;}
672  if(dz<w1 && dz>-w2){test1|=test<<4;}
673 
674  if(!test1){return 0;}
675  if(dy<w2 && dy>-w1){overlap =test1;}
676  if(dy<w1 && dy>-w2){overlap|=test1<<2;}
677  return overlap;
678 }
679 
680 template <class NodeData,class Real>
681 OctNode<NodeData,Real>* OctNode<NodeData,Real>::getNearestLeaf(const Point3D<Real>& p){
682  Point3D<Real> center;
683  Real width;
684  OctNode<NodeData,Real>* temp;
685  int cIndex;
686  if(!children){return this;}
687  centerAndWidth(center,width);
688  temp=this;
689  while(temp->children){
690  cIndex=CornerIndex(center,p);
691  temp=&temp->children[cIndex];
692  width/=2;
693  if(cIndex&1){center.coords[0]+=width/2;}
694  else {center.coords[0]-=width/2;}
695  if(cIndex&2){center.coords[1]+=width/2;}
696  else {center.coords[1]-=width/2;}
697  if(cIndex&4){center.coords[2]+=width/2;}
698  else {center.coords[2]-=width/2;}
699  }
700  return temp;
701 }
702 template <class NodeData,class Real>
703 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::getNearestLeaf(const Point3D<Real>& p) const{
704  int nearest;
705  Real temp,dist2;
706  if(!children){return this;}
707  for(unsigned int i=0;i<Cube::CORNERS;i++){
708  temp=SquareDistance(children[i].center,p);
709  if(!i || temp<dist2){
710  dist2=temp;
711  nearest=i;
712  }
713  }
714  return children[nearest].getNearestLeaf(p);
715 }
716 
717 template <class NodeData,class Real>
718 int OctNode<NodeData,Real>::CommonEdge(const OctNode<NodeData,Real>* node1,int eIndex1,const OctNode<NodeData,Real>* node2,int eIndex2){
719  int o1,o2,i1,i2,j1,j2;
720 
721  Cube::FactorEdgeIndex(eIndex1,o1,i1,j1);
722  Cube::FactorEdgeIndex(eIndex2,o2,i2,j2);
723  if(o1!=o2){return 0;}
724 
725  int dir[2];
726  int idx1[2];
727  int idx2[2];
728  switch(o1){
729  case 0: dir[0]=1; dir[1]=2; break;
730  case 1: dir[0]=0; dir[1]=2; break;
731  case 2: dir[0]=0; dir[1]=1; break;
732  };
733  int d1,d2,off1[3],off2[3];
734  node1->depthAndOffset(d1,off1);
735  node2->depthAndOffset(d2,off2);
736  idx1[0]=off1[dir[0]]+(1<<d1)+i1;
737  idx1[1]=off1[dir[1]]+(1<<d1)+j1;
738  idx2[0]=off2[dir[0]]+(1<<d2)+i2;
739  idx2[1]=off2[dir[1]]+(1<<d2)+j2;
740  if(d1>d2){
741  idx2[0]<<=(d1-d2);
742  idx2[1]<<=(d1-d2);
743  }
744  else{
745  idx1[0]<<=(d2-d1);
746  idx1[1]<<=(d2-d1);
747  }
748  if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;}
749  else {return 0;}
750 }
751 template<class NodeData,class Real>
752 int OctNode<NodeData,Real>::CornerIndex(const Point3D<Real>& center,const Point3D<Real>& p){
753  int cIndex=0;
754  if(p.coords[0]>center.coords[0]){cIndex|=1;}
755  if(p.coords[1]>center.coords[1]){cIndex|=2;}
756  if(p.coords[2]>center.coords[2]){cIndex|=4;}
757  return cIndex;
758 }
759 template <class NodeData,class Real>
760 template<class NodeData2>
761 OctNode<NodeData,Real>& OctNode<NodeData,Real>::operator = (const OctNode<NodeData2,Real>& node){
762 
763  if(children){delete[] children;}
764  children=NULL;
765 
766  this->depth=node.depth;
767  for(unsigned int i=0;i<DIMENSION;i++){this->offset[i] = node.offset[i];}
768  if(node.children){
769  initChildren();
770  for(unsigned int i=0;i<Cube::CORNERS;i++){children[i] = node.children[i];}
771  }
772  return *this;
773 }
774 template <class NodeData,class Real>
775 int OctNode<NodeData,Real>::CompareForwardDepths(const void* v1,const void* v2){
776  return static_cast<const OctNode<NodeData,Real>*>(v1)->depth-static_cast<const OctNode<NodeData,Real>*>(v2)->depth;
777 }
778 template< class NodeData , class Real >
779 int OctNode< NodeData , Real >::CompareByDepthAndXYZ( const void* v1 , const void* v2 )
780 {
781  const OctNode<NodeData,Real> *n1 = (*(const OctNode< NodeData , Real >**)v1);
782  const OctNode<NodeData,Real> *n2 = (*(const OctNode< NodeData , Real >**)v2);
783  if( n1->d!=n2->d ) return int(n1->d)-int(n2->d);
784  else if( n1->off[0]!=n2->off[0] ) return int(n1->off[0]) - int(n2->off[0]);
785  else if( n1->off[1]!=n2->off[1] ) return int(n1->off[1]) - int(n2->off[1]);
786  else if( n1->off[2]!=n2->off[2] ) return int(n1->off[2]) - int(n2->off[2]);
787  return 0;
788 }
789 
790 long long _InterleaveBits( int p[3] )
791 {
792  long long key = 0;
793  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) );
794  return key;
795 }
796 template <class NodeData,class Real>
797 int OctNode<NodeData,Real>::CompareByDepthAndZIndex( const void* v1 , const void* v2 )
798 {
799  const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1);
800  const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2);
801  int d1 , off1[3] , d2 , off2[3];
802  n1->depthAndOffset( d1 , off1 ) , n2->depthAndOffset( d2 , off2 );
803  if ( d1>d2 ) return 1;
804  else if( d1<d2 ) return -1;
805  long long k1 = _InterleaveBits( off1 ) , k2 = _InterleaveBits( off2 );
806  if ( k1>k2 ) return 1;
807  else if( k1<k2 ) return -1;
808  else return 0;
809 }
810 
811 template <class NodeData,class Real>
812 int OctNode<NodeData,Real>::CompareForwardPointerDepths( const void* v1 , const void* v2 )
813 {
814  const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1);
815  const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2);
816  if(n1->d!=n2->d){return int(n1->d)-int(n2->d);}
817  while( n1->parent!=n2->parent )
818  {
819  n1=n1->parent;
820  n2=n2->parent;
821  }
822  if(n1->off[0]!=n2->off[0]){return int(n1->off[0])-int(n2->off[0]);}
823  if(n1->off[1]!=n2->off[1]){return int(n1->off[1])-int(n2->off[1]);}
824  return int(n1->off[2])-int(n2->off[2]);
825  return 0;
826 }
827 template <class NodeData,class Real>
828 int OctNode<NodeData,Real>::CompareBackwardDepths(const void* v1,const void* v2){
829  return static_cast<const OctNode<NodeData,Real>*>(v2)->depth-static_cast<const OctNode<NodeData,Real>*>(v1)->depth;
830 }
831 template <class NodeData,class Real>
832 int OctNode<NodeData,Real>::CompareBackwardPointerDepths(const void* v1,const void* v2){
833  return (*static_cast<const OctNode<NodeData,Real>**>(v2))->depth()-(*static_cast<const OctNode<NodeData,Real>**>(v1))->depth();
834 }
835 template <class NodeData,class Real>
836 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){
837  int d=depth2-depth1;
838  Real w=multiplier2+multiplier1*(1<<d);
839  Real w2=Real((1<<(d-1))-0.5);
840  if(
841  fabs(Real(offSet2[0]-(offSet1[0]<<d))-w2)>=w ||
842  fabs(Real(offSet2[1]-(offSet1[1]<<d))-w2)>=w ||
843  fabs(Real(offSet2[2]-(offSet1[2]<<d))-w2)>=w
844  ){return 0;}
845  return 1;
846 }
847 template <class NodeData,class Real>
848 inline int OctNode<NodeData,Real>::Overlap(int c1,int c2,int c3,int dWidth){
849  if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;}
850  else{return 1;}
851 }
852 template <class NodeData,class Real>
853 OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);}
854 template <class NodeData,class Real>
855 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);}
856 template <class NodeData,class Real>
857 OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off,int forceChildren){
858  if(!parent){return NULL;}
859  ptrdiff_t pIndex= ptrdiff_t(-(this->parent->children - this));
860  pIndex^=(1<<dir);
861  if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
862  else{
863  OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren);
864  if(!temp){return NULL;}
865  if(!temp->children){
866  if(forceChildren){temp->initChildren();}
867  else{return temp;}
868  }
869  return &temp->children[pIndex];
870  }
871 }
872 template <class NodeData,class Real>
873 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off) const {
874  if(!parent){return NULL;}
875  ptrdiff_t pIndex= ptrdiff_t(-(this->parent->children - this));
876  pIndex^=(1<<dir);
877  if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
878  else{
879  const OctNode* temp=parent->__faceNeighbor(dir,off);
880  if(!temp || !temp->children){return temp;}
881  else{return &temp->children[pIndex];}
882  }
883 }
884 
885 template <class NodeData,class Real>
886 OctNode<NodeData,Real>* OctNode<NodeData,Real>::edgeNeighbor(int edgeIndex,int forceChildren){
887  int idx[2],o,i[2];
888  Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]);
889  switch(o){
890  case 0: idx[0]=1; idx[1]=2; break;
891  case 1: idx[0]=0; idx[1]=2; break;
892  case 2: idx[0]=0; idx[1]=1; break;
893  };
894  return __edgeNeighbor(o,i,idx,forceChildren);
895 }
896 template <class NodeData,class Real>
897 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::edgeNeighbor(int edgeIndex) const {
898  int idx[2],o,i[2];
899  Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]);
900  switch(o){
901  case 0: idx[0]=1; idx[1]=2; break;
902  case 1: idx[0]=0; idx[1]=2; break;
903  case 2: idx[0]=0; idx[1]=1; break;
904  };
905  return __edgeNeighbor(o,i,idx);
906 }
907 template <class NodeData,class Real>
908 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{
909  if(!parent){return NULL;}
910  ptrdiff_t pIndex= ptrdiff_t(-(this->parent->children - this));
911  int aIndex,x[DIMENSION];
912 
913  Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
914  aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3;
915  pIndex^=(7 ^ (1<<o));
916  if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor
917  const OctNode* temp=parent->__faceNeighbor(idx[0],i[0]);
918  if(!temp || !temp->children){return NULL;}
919  else{return &temp->children[pIndex];}
920  }
921  else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor
922  const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]);
923  if(!temp || !temp->children){return NULL;}
924  else{return &temp->children[pIndex];}
925  }
926  else if(aIndex==0) { // I can get the neighbor from the parent
927  return &parent->children[pIndex];
928  }
929  else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor
930  const OctNode* temp=parent->__edgeNeighbor(o,i,idx);
931  if(!temp || !temp->children){return temp;}
932  else{return &temp->children[pIndex];}
933  }
934  else{return NULL;}
935 }
936 template <class NodeData,class Real>
937 OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){
938  if(!parent){return NULL;}
939  ptrdiff_t pIndex= ptrdiff_t(-(this->parent->children - this));
940  int aIndex,x[DIMENSION];
941 
942  Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
943  aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3;
944  pIndex^=(7 ^ (1<<o));
945  if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor
946  OctNode* temp=parent->__faceNeighbor(idx[0],i[0],0);
947  if(!temp || !temp->children){return NULL;}
948  else{return &temp->children[pIndex];}
949  }
950  else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor
951  OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0);
952  if(!temp || !temp->children){return NULL;}
953  else{return &temp->children[pIndex];}
954  }
955  else if(aIndex==0) { // I can get the neighbor from the parent
956  return &parent->children[pIndex];
957  }
958  else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor
959  OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren);
960  if(!temp){return NULL;}
961  if(!temp->children){
962  if(forceChildren){temp->initChildren();}
963  else{return temp;}
964  }
965  return &temp->children[pIndex];
966  }
967  else{return NULL;}
968 }
969 
970 template <class NodeData,class Real>
971 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::cornerNeighbor(int cornerIndex) const {
972  int aIndex=0;
973  if(!parent){return NULL;}
974 
975  ptrdiff_t pIndex= ptrdiff_t(-(this->parent->children - this));
976  aIndex=(cornerIndex ^ pIndex); // The disagreement bits
977  pIndex=(~pIndex)&7; // The antipodal point
978  if(aIndex==7){ // Agree on no bits
979  return &parent->children[pIndex];
980  }
981  else if(aIndex==0){ // Agree on all bits
982  const OctNode* temp=static_cast<const OctNode*>(parent)->cornerNeighbor(cornerIndex);
983  if(!temp || !temp->children){return temp;}
984  else{return &temp->children[pIndex];}
985  }
986  else if(aIndex==6){ // Agree on face 0
987  const OctNode* temp=static_cast<const OctNode*>(parent)->__faceNeighbor(0,cornerIndex & 1);
988  if(!temp || !temp->children){return NULL;}
989  else{return & temp->children[pIndex];}
990  }
991  else if(aIndex==5){ // Agree on face 1
992  const OctNode* temp=static_cast<const OctNode*>(parent)->__faceNeighbor(1,(cornerIndex & 2)>>1);
993  if(!temp || !temp->children){return NULL;}
994  else{return & temp->children[pIndex];}
995  }
996  else if(aIndex==3){ // Agree on face 2
997  const OctNode* temp=static_cast<const OctNode*>(parent)->__faceNeighbor(2,(cornerIndex & 4)>>2);
998  if(!temp || !temp->children){return NULL;}
999  else{return & temp->children[pIndex];}
1000  }
1001  else if(aIndex==4){ // Agree on edge 2
1002  const OctNode* temp=static_cast<const OctNode*>(parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) );
1003  if(!temp || !temp->children){return NULL;}
1004  else{return & temp->children[pIndex];}
1005  }
1006  else if(aIndex==2){ // Agree on edge 1
1007  const OctNode* temp=static_cast<const OctNode*>(parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) );
1008  if(!temp || !temp->children){return NULL;}
1009  else{return & temp->children[pIndex];}
1010  }
1011  else if(aIndex==1){ // Agree on edge 0
1012  const OctNode* temp=static_cast<const OctNode*>(parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 );
1013  if(!temp || !temp->children){return NULL;}
1014  else{return & temp->children[pIndex];}
1015  }
1016  else{return NULL;}
1017 }
1018 template <class NodeData,class Real>
1019 OctNode<NodeData,Real>* OctNode<NodeData,Real>::cornerNeighbor(int cornerIndex,int forceChildren){
1020  int aIndex=0;
1021  if(!parent){return NULL;}
1022 
1023  ptrdiff_t pIndex= ptrdiff_t(-(this->parent->children - this));
1024  aIndex=(cornerIndex ^ pIndex); // The disagreement bits
1025  pIndex=(~pIndex)&7; // The antipodal point
1026  if(aIndex==7){ // Agree on no bits
1027  return &parent->children[pIndex];
1028  }
1029  else if(aIndex==0){ // Agree on all bits
1030  OctNode* temp=static_cast<OctNode*>(parent)->cornerNeighbor(cornerIndex,forceChildren);
1031  if(!temp){return NULL;}
1032  if(!temp->children){
1033  if(forceChildren){temp->initChildren();}
1034  else{return temp;}
1035  }
1036  return &temp->children[pIndex];
1037  }
1038  else if(aIndex==6){ // Agree on face 0
1039  OctNode* temp=static_cast<OctNode*>(parent)->__faceNeighbor(0,cornerIndex & 1,0);
1040  if(!temp || !temp->children){return NULL;}
1041  else{return & temp->children[pIndex];}
1042  }
1043  else if(aIndex==5){ // Agree on face 1
1044  OctNode* temp=static_cast<OctNode*>(parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0);
1045  if(!temp || !temp->children){return NULL;}
1046  else{return & temp->children[pIndex];}
1047  }
1048  else if(aIndex==3){ // Agree on face 2
1049  OctNode* temp=static_cast<OctNode*>(parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0);
1050  if(!temp || !temp->children){return NULL;}
1051  else{return & temp->children[pIndex];}
1052  }
1053  else if(aIndex==4){ // Agree on edge 2
1054  OctNode* temp=static_cast<OctNode*>(parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) );
1055  if(!temp || !temp->children){return NULL;}
1056  else{return & temp->children[pIndex];}
1057  }
1058  else if(aIndex==2){ // Agree on edge 1
1059  OctNode* temp=static_cast<OctNode*>(parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) );
1060  if(!temp || !temp->children){return NULL;}
1061  else{return & temp->children[pIndex];}
1062  }
1063  else if(aIndex==1){ // Agree on edge 0
1064  OctNode* temp=static_cast<OctNode*>(parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 );
1065  if(!temp || !temp->children){return NULL;}
1066  else{return & temp->children[pIndex];}
1067  }
1068  else{return NULL;}
1069 }
1071 // OctNodeNeighborKey //
1073 template<class NodeData,class Real>
1074 OctNode<NodeData,Real>::Neighbors3::Neighbors3(void){clear();}
1075 template<class NodeData,class Real>
1076 void OctNode<NodeData,Real>::Neighbors3::clear(void){
1077  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;}}}
1078 }
1079 template<class NodeData,class Real>
1080 OctNode<NodeData,Real>::NeighborKey3::NeighborKey3(void){ neighbors=NULL; }
1081 template<class NodeData,class Real>
1082 OctNode<NodeData,Real>::NeighborKey3::~NeighborKey3(void)
1083 {
1084  if( neighbors ) delete[] neighbors;
1085  neighbors = NULL;
1086 }
1087 
1088 template<class NodeData,class Real>
1089 void OctNode<NodeData,Real>::NeighborKey3::set( int d )
1090 {
1091  if( neighbors ) delete[] neighbors;
1092  neighbors = NULL;
1093  if( d<0 ) return;
1094  neighbors = new Neighbors3[d+1];
1095 }
1096 template< class NodeData , class Real >
1097 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d )
1098 {
1099  if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) )
1100  {
1101  neighbors[d].clear();
1102 
1103  if( !d ) neighbors[d].neighbors[1][1][1] = root;
1104  else
1105  {
1106  Neighbors3& temp = setNeighbors( root , p , d-1 );
1107 
1108  int i , j , k , x1 , y1 , z1 , x2 , y2 , z2;
1109  Point3D< Real > c;
1110  Real w;
1111  temp.neighbors[1][1][1]->centerAndWidth( c , w );
1112  int idx = CornerIndex( c , p );
1113  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1114  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1115 
1116  if( !temp.neighbors[1][1][1]->children ) temp.neighbors[1][1][1]->initChildren();
1117  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1118  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)];
1119 
1120 
1121  // Set the neighbors from across the faces
1122  i=x1<<1;
1123  if( temp.neighbors[i][1][1] )
1124  {
1125  if( !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren();
1126  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)];
1127  }
1128  j=y1<<1;
1129  if( temp.neighbors[1][j][1] )
1130  {
1131  if( !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren();
1132  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)];
1133  }
1134  k=z1<<1;
1135  if( temp.neighbors[1][1][k] )
1136  {
1137  if( !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren();
1138  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)];
1139  }
1140 
1141  // Set the neighbors from across the edges
1142  i=x1<<1 , j=y1<<1;
1143  if( temp.neighbors[i][j][1] )
1144  {
1145  if( !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren();
1146  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1147  }
1148  i=x1<<1 , k=z1<<1;
1149  if( temp.neighbors[i][1][k] )
1150  {
1151  if( !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren();
1152  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1153  }
1154  j=y1<<1 , k=z1<<1;
1155  if( temp.neighbors[1][j][k] )
1156  {
1157  if( !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren();
1158  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1159  }
1160 
1161  // Set the neighbor from across the corner
1162  i=x1<<1 , j=y1<<1 , k=z1<<1;
1163  if( temp.neighbors[i][j][k] )
1164  {
1165  if( !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren();
1166  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1167  }
1168  }
1169  }
1170  return neighbors[d];
1171 }
1172 template< class NodeData , class Real >
1173 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d )
1174 {
1175  if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) )
1176  {
1177  neighbors[d].clear();
1178 
1179  if( !d ) neighbors[d].neighbors[1][1][1] = root;
1180  else
1181  {
1182  Neighbors3& temp = getNeighbors( root , p , d-1 );
1183 
1184  int i , j , k , x1 , y1 , z1 , x2 , y2 , z2;
1185  Point3D< Real > c;
1186  Real w;
1187  temp.neighbors[1][1][1]->centerAndWidth( c , w );
1188  int idx = CornerIndex( c , p );
1189  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1190  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1191 
1192  if( !temp.neighbors[1][1][1] || !temp.neighbors[1][1][1]->children )
1193  {
1194  fprintf( stderr , "[ERROR] Couldn't find node at appropriate depth\n" );
1195  exit( 0 );
1196  }
1197  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1198  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)];
1199 
1200 
1201  // Set the neighbors from across the faces
1202  i=x1<<1;
1203  if( temp.neighbors[i][1][1] )
1204  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)];
1205  j=y1<<1;
1206  if( temp.neighbors[1][j][1] )
1207  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)];
1208  k=z1<<1;
1209  if( temp.neighbors[1][1][k] )
1210  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)];
1211 
1212  // Set the neighbors from across the edges
1213  i=x1<<1 , j=y1<<1;
1214  if( temp.neighbors[i][j][1] )
1215  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1216  i=x1<<1 , k=z1<<1;
1217  if( temp.neighbors[i][1][k] )
1218  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1219  j=y1<<1 , k=z1<<1;
1220  if( temp.neighbors[1][j][k] )
1221  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1222 
1223  // Set the neighbor from across the corner
1224  i=x1<<1 , j=y1<<1 , k=z1<<1;
1225  if( temp.neighbors[i][j][k] )
1226  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1227  }
1228  }
1229  return neighbors[d];
1230 }
1231 
1232 template< class NodeData , class Real >
1233 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* node )
1234 {
1235  int d = node->depth();
1236  if( node==neighbors[d].neighbors[1][1][1] )
1237  {
1238  bool reset = false;
1239  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;
1240  if( reset ) neighbors[d].neighbors[1][1][1] = NULL;
1241  }
1242  if( node!=neighbors[d].neighbors[1][1][1] )
1243  {
1244  neighbors[d].clear();
1245 
1246  if( !node->parent ) neighbors[d].neighbors[1][1][1] = node;
1247  else
1248  {
1249  int i,j,k,x1,y1,z1,x2,y2,z2;
1250  int idx=int(node-node->parent->children);
1251  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1252  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1253  for(i=0;i<2;i++){
1254  for(j=0;j<2;j++){
1255  for(k=0;k<2;k++){
1256  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1257  }
1258  }
1259  }
1260  Neighbors3& temp=setNeighbors(node->parent);
1261 
1262  // Set the neighbors from across the faces
1263  i=x1<<1;
1264  if(temp.neighbors[i][1][1]){
1265  if(!temp.neighbors[i][1][1]->children) temp.neighbors[i][1][1]->initChildren();
1266  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)];
1267  }
1268  j=y1<<1;
1269  if(temp.neighbors[1][j][1]){
1270  if(!temp.neighbors[1][j][1]->children){temp.neighbors[1][j][1]->initChildren();}
1271  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)];}}
1272  }
1273  k=z1<<1;
1274  if(temp.neighbors[1][1][k]){
1275  if(!temp.neighbors[1][1][k]->children){temp.neighbors[1][1][k]->initChildren();}
1276  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)];}}
1277  }
1278 
1279  // Set the neighbors from across the edges
1280  i=x1<<1; j=y1<<1;
1281  if(temp.neighbors[i][j][1]){
1282  if(!temp.neighbors[i][j][1]->children){temp.neighbors[i][j][1]->initChildren();}
1283  for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1284  }
1285  i=x1<<1; k=z1<<1;
1286  if(temp.neighbors[i][1][k]){
1287  if(!temp.neighbors[i][1][k]->children){temp.neighbors[i][1][k]->initChildren();}
1288  for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1289  }
1290  j=y1<<1; k=z1<<1;
1291  if(temp.neighbors[1][j][k]){
1292  if(!temp.neighbors[1][j][k]->children){temp.neighbors[1][j][k]->initChildren();}
1293  for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1294  }
1295 
1296  // Set the neighbor from across the corner
1297  i=x1<<1; j=y1<<1; k=z1<<1;
1298  if(temp.neighbors[i][j][k]){
1299  if(!temp.neighbors[i][j][k]->children){temp.neighbors[i][j][k]->initChildren();}
1300  neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1301  }
1302  }
1303  }
1304  return neighbors[d];
1305 }
1306 // Note the assumption is that if you enable an edge, you also enable adjacent faces.
1307 // And, if you enable a corner, you enable adjacent edges and faces.
1308 template< class NodeData , class Real >
1309 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* node , bool flags[3][3][3] )
1310 {
1311  int d = node->depth();
1312  if( node==neighbors[d].neighbors[1][1][1] )
1313  {
1314  bool reset = false;
1315  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;
1316  if( reset ) neighbors[d].neighbors[1][1][1] = NULL;
1317  }
1318  if( node!=neighbors[d].neighbors[1][1][1] )
1319  {
1320  neighbors[d].clear();
1321 
1322  if( !node->parent ) neighbors[d].neighbors[1][1][1] = node;
1323  else
1324  {
1325  int x1,y1,z1,x2,y2,z2;
1326  int idx=int(node-node->parent->children);
1327  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1328  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1329  for( int i=0 ; i<2 ; i++ )
1330  for( int j=0 ; j<2 ; j++ )
1331  for( int k=0 ; k<2 ; k++ )
1332  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1333 
1334  Neighbors3& temp=setNeighbors( node->parent , flags );
1335 
1336  // Set the neighbors from across the faces
1337  {
1338  int i=x1<<1;
1339  if( temp.neighbors[i][1][1] )
1340  {
1341  if( flags[i][1][1] && !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren();
1342  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)];
1343  }
1344  }
1345  {
1346  int j = y1<<1;
1347  if( temp.neighbors[1][j][1] )
1348  {
1349  if( flags[1][j][1] && !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren();
1350  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)];
1351  }
1352  }
1353  {
1354  int k = z1<<1;
1355  if( temp.neighbors[1][1][k] )
1356  {
1357  if( flags[1][1][k] && !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren();
1358  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)];
1359  }
1360  }
1361 
1362  // Set the neighbors from across the edges
1363  {
1364  int i=x1<<1 , j=y1<<1;
1365  if( temp.neighbors[i][j][1] )
1366  {
1367  if( flags[i][j][1] && !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren();
1368  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)];
1369  }
1370  }
1371  {
1372  int i=x1<<1 , k=z1<<1;
1373  if( temp.neighbors[i][1][k] )
1374  {
1375  if( flags[i][1][k] && !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren();
1376  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)];
1377  }
1378  }
1379  {
1380  int j=y1<<1 , k=z1<<1;
1381  if( temp.neighbors[1][j][k] )
1382  {
1383  if( flags[1][j][k] && !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren();
1384  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)];
1385  }
1386  }
1387 
1388  // Set the neighbor from across the corner
1389  {
1390  int i=x1<<1 , j=y1<<1 , k=z1<<1;
1391  if( temp.neighbors[i][j][k] )
1392  {
1393  if( flags[i][j][k] && !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren();
1394  if( temp.neighbors[i][j][k]->children ) neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1395  }
1396  }
1397  }
1398  }
1399  return neighbors[d];
1400 }
1401 
1402 template<class NodeData,class Real>
1403 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors(OctNode<NodeData,Real>* node){
1404  int d=node->depth();
1405  if(node!=neighbors[d].neighbors[1][1][1]){
1406  neighbors[d].clear();
1407 
1408  if(!node->parent){neighbors[d].neighbors[1][1][1]=node;}
1409  else{
1410  int i,j,k,x1,y1,z1,x2,y2,z2;
1411  int idx=int(node-node->parent->children);
1412  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1413  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1414  for(i=0;i<2;i++){
1415  for(j=0;j<2;j++){
1416  for(k=0;k<2;k++){
1417  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1418  }
1419  }
1420  }
1421  Neighbors3& temp=getNeighbors(node->parent);
1422 
1423  // Set the neighbors from across the faces
1424  i=x1<<1;
1425  if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){
1426  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)];}}
1427  }
1428  j=y1<<1;
1429  if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){
1430  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)];}}
1431  }
1432  k=z1<<1;
1433  if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){
1434  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)];}}
1435  }
1436 
1437  // Set the neighbors from across the edges
1438  i=x1<<1; j=y1<<1;
1439  if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){
1440  for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1441  }
1442  i=x1<<1; k=z1<<1;
1443  if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){
1444  for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1445  }
1446  j=y1<<1; k=z1<<1;
1447  if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){
1448  for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1449  }
1450 
1451  // Set the neighbor from across the corner
1452  i=x1<<1; j=y1<<1; k=z1<<1;
1453  if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){
1454  neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1455  }
1456  }
1457  }
1458  return neighbors[node->depth()];
1459 }
1460 
1462 // ConstNeighborKey3 //
1464 template<class NodeData,class Real>
1465 OctNode<NodeData,Real>::ConstNeighbors3::ConstNeighbors3(void){clear();}
1466 template<class NodeData,class Real>
1467 void OctNode<NodeData,Real>::ConstNeighbors3::clear(void){
1468  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;}}}
1469 }
1470 template<class NodeData,class Real>
1471 OctNode<NodeData,Real>::ConstNeighborKey3::ConstNeighborKey3(void){neighbors=NULL;}
1472 template<class NodeData,class Real>
1473 OctNode<NodeData,Real>::ConstNeighborKey3::~ConstNeighborKey3(void){
1474  if(neighbors){delete[] neighbors;}
1475  neighbors=NULL;
1476 }
1477 
1478 template<class NodeData,class Real>
1479 void OctNode<NodeData,Real>::ConstNeighborKey3::set(int d){
1480  if(neighbors){delete[] neighbors;}
1481  neighbors=NULL;
1482  if(d<0){return;}
1483  neighbors=new ConstNeighbors3[d+1];
1484 }
1485 template<class NodeData,class Real>
1486 typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors(const OctNode<NodeData,Real>* node){
1487  int d=node->depth();
1488  if(node!=neighbors[d].neighbors[1][1][1]){
1489  neighbors[d].clear();
1490 
1491  if(!node->parent){neighbors[d].neighbors[1][1][1]=node;}
1492  else{
1493  int i,j,k,x1,y1,z1,x2,y2,z2;
1494  int idx=int(node-node->parent->children);
1495  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1496  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1497  for(i=0;i<2;i++){
1498  for(j=0;j<2;j++){
1499  for(k=0;k<2;k++){
1500  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1501  }
1502  }
1503  }
1504  ConstNeighbors3& temp=getNeighbors(node->parent);
1505 
1506  // Set the neighbors from across the faces
1507  i=x1<<1;
1508  if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){
1509  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)];}}
1510  }
1511  j=y1<<1;
1512  if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){
1513  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)];}}
1514  }
1515  k=z1<<1;
1516  if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){
1517  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)];}}
1518  }
1519 
1520  // Set the neighbors from across the edges
1521  i=x1<<1; j=y1<<1;
1522  if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){
1523  for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1524  }
1525  i=x1<<1; k=z1<<1;
1526  if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){
1527  for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1528  }
1529  j=y1<<1; k=z1<<1;
1530  if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){
1531  for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1532  }
1533 
1534  // Set the neighbor from across the corner
1535  i=x1<<1; j=y1<<1; k=z1<<1;
1536  if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){
1537  neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1538  }
1539  }
1540  }
1541  return neighbors[node->depth()];
1542 }
1543 template<class NodeData,class Real>
1544 typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors( const OctNode<NodeData,Real>* node , int minDepth )
1545 {
1546  int d=node->depth();
1547  if( d<minDepth ) fprintf( stderr , "[ERROR] Node depth lower than min-depth: %d < %d\n" , d , minDepth ) , exit( 0 );
1548  if( node!=neighbors[d].neighbors[1][1][1] )
1549  {
1550  neighbors[d].clear();
1551 
1552  if( d==minDepth ) neighbors[d].neighbors[1][1][1]=node;
1553  else
1554  {
1555  int i,j,k,x1,y1,z1,x2,y2,z2;
1556  int idx = int(node-node->parent->children);
1557  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1558  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1559 
1560  ConstNeighbors3& temp=getNeighbors( node->parent , minDepth );
1561 
1562  // Set the syblings
1563  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1564  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &node->parent->children[ Cube::CornerIndex(i,j,k) ];
1565 
1566  // Set the neighbors from across the faces
1567  i=x1<<1;
1568  if( temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children )
1569  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)];
1570 
1571  j=y1<<1;
1572  if( temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children )
1573  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)];
1574 
1575  k=z1<<1;
1576  if( temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children )
1577  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)];
1578 
1579  // Set the neighbors from across the edges
1580  i=x1<<1 , j=y1<<1;
1581  if( temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children )
1582  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1583 
1584  i=x1<<1 , k=z1<<1;
1585  if( temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children )
1586  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1587 
1588  j=y1<<1 , k=z1<<1;
1589  if( temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children )
1590  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1591 
1592  // Set the neighbor from across the corner
1593  i=x1<<1 , j=y1<<1 , k=z1<<1;
1594  if( temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children )
1595  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1596  }
1597  }
1598  return neighbors[node->depth()];
1599 }
1600 
1601 template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); }
1602 template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); }
1603 template< class NodeData , class Real >
1604 void OctNode< NodeData , Real >::Neighbors5::clear( void )
1605 {
1606  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;
1607 }
1608 template< class NodeData , class Real >
1609 void OctNode< NodeData , Real >::ConstNeighbors5::clear( void )
1610 {
1611  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;
1612 }
1613 template< class NodeData , class Real >
1614 OctNode< NodeData , Real >::NeighborKey5::NeighborKey5( void )
1615 {
1616  _depth = -1;
1617  neighbors = NULL;
1618 }
1619 template< class NodeData , class Real >
1620 OctNode< NodeData , Real >::ConstNeighborKey5::ConstNeighborKey5( void )
1621 {
1622  _depth = -1;
1623  neighbors = NULL;
1624 }
1625 template< class NodeData , class Real >
1626 OctNode< NodeData , Real >::NeighborKey5::~NeighborKey5( void )
1627 {
1628  if( neighbors ) delete[] neighbors;
1629  neighbors = NULL;
1630 }
1631 template< class NodeData , class Real >
1632 OctNode< NodeData , Real >::ConstNeighborKey5::~ConstNeighborKey5( void )
1633 {
1634  if( neighbors ) delete[] neighbors;
1635  neighbors = NULL;
1636 }
1637 template< class NodeData , class Real >
1638 void OctNode< NodeData , Real >::NeighborKey5::set( int d )
1639 {
1640  if( neighbors ) delete[] neighbors;
1641  neighbors = NULL;
1642  if(d<0) return;
1643  _depth = d;
1644  neighbors=new Neighbors5[d+1];
1645 }
1646 template< class NodeData , class Real >
1647 void OctNode< NodeData , Real >::ConstNeighborKey5::set( int d )
1648 {
1649  if( neighbors ) delete[] neighbors;
1650  neighbors = NULL;
1651  if(d<0) return;
1652  _depth = d;
1653  neighbors=new ConstNeighbors5[d+1];
1654 }
1655 template< class NodeData , class Real >
1656 typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::getNeighbors( OctNode* node )
1657 {
1658  int d=node->depth();
1659  if( node!=neighbors[d].neighbors[2][2][2] )
1660  {
1661  neighbors[d].clear();
1662 
1663  if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1664  else
1665  {
1666  getNeighbors( node->parent );
1667  Neighbors5& temp = neighbors[d-1];
1668  int x1 , y1 , z1 , x2 , y2 , z2;
1669  int idx = int( node - node->parent->children );
1670  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1671 
1672  Neighbors5& n = neighbors[d];
1673  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1674  int i , j , k;
1675  int fx0 = x2+1 , fy0 = y2+1 , fz0 = z2+1; // Indices of the bottom left corner of the parent within the 5x5x5
1676  int cx1 = x1*2+1 , cy1 = y1*2+1 , cz1 = z1*2+1;
1677  int cx2 = x2*2+1 , cy2 = y2*2+1 , cz2 = z2*2+1;
1678  int fx1 = x1*3 , fy1 = y1*3 , fz1 = z1*3;
1679  int fx2 = x2*4 , fy2 = y2*4 , fz2 = z2*4;
1680 
1681  // Set the syblings
1682  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1683  n.neighbors[fx0+i][fy0+j][fz0+k] = node->parent->children + Cube::CornerIndex( i , j , k );
1684 
1685  // Set the neighbors from across the faces
1686  if( temp.neighbors[cx1][2][2] && temp.neighbors[cx1][2][2]->children )
1687  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1688  n.neighbors[fx1+i][fy0+j][fz0+k] = temp.neighbors[cx1][2][2]->children + Cube::CornerIndex( i , j , k );
1689  if( temp.neighbors[2][cy1][2] && temp.neighbors[2][cy1][2]->children )
1690  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1691  n.neighbors[fx0+i][fy1+j][fz0+k] = temp.neighbors[2][cy1][2]->children + Cube::CornerIndex( i , j , k );
1692  if( temp.neighbors[2][2][cz1] && temp.neighbors[2][2][cz1]->children )
1693  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1694  n.neighbors[fx0+i][fy0+j][fz1+k] = temp.neighbors[2][2][cz1]->children + Cube::CornerIndex( i , j , k );
1695  if( temp.neighbors[cx2][2][2] && temp.neighbors[cx2][2][2]->children )
1696  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1697  n.neighbors[fx2 ][fy0+j][fz0+k] = temp.neighbors[cx2][2][2]->children + Cube::CornerIndex( x1 , j , k );
1698  if( temp.neighbors[2][cy2][2] && temp.neighbors[2][cy2][2]->children )
1699  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1700  n.neighbors[fx0+i][fy2 ][fz0+k] = temp.neighbors[2][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1701  if( temp.neighbors[2][2][cz2] && temp.neighbors[2][2][cz2]->children )
1702  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1703  n.neighbors[fx0+i][fy0+j][fz2 ] = temp.neighbors[2][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1704 
1705  // Set the neighbors from across the edges
1706  if( temp.neighbors[cx1][cy1][2] && temp.neighbors[cx1][cy1][2]->children )
1707  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1708  n.neighbors[fx1+i][fy1+j][fz0+k] = temp.neighbors[cx1][cy1][2]->children + Cube::CornerIndex( i , j , k );
1709  if( temp.neighbors[cx1][2][cz1] && temp.neighbors[cx1][2][cz1]->children )
1710  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1711  n.neighbors[fx1+i][fy0+j][fz1+k] = temp.neighbors[cx1][2][cz1]->children + Cube::CornerIndex( i , j , k );
1712  if( temp.neighbors[2][cy1][cz1] && temp.neighbors[2][cy1][cz1]->children )
1713  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1714  n.neighbors[fx0+i][fy1+j][fz1+k] = temp.neighbors[2][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1715  if( temp.neighbors[cx1][cy2][2] && temp.neighbors[cx1][cy2][2]->children )
1716  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1717  n.neighbors[fx1+i][fy2 ][fz0+k] = temp.neighbors[cx1][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1718  if( temp.neighbors[cx1][2][cz2] && temp.neighbors[cx1][2][cz2]->children )
1719  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1720  n.neighbors[fx1+i][fy0+j][fz2 ] = temp.neighbors[cx1][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1721  if( temp.neighbors[cx2][cy1][2] && temp.neighbors[cx2][cy1][2]->children )
1722  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1723  n.neighbors[fx2 ][fy1+j][fz0+k] = temp.neighbors[cx2][cy1][2]->children + Cube::CornerIndex( x1 , j , k );
1724  if( temp.neighbors[2][cy1][cz2] && temp.neighbors[2][cy1][cz2]->children )
1725  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1726  n.neighbors[fx0+i][fy1+j][fz2 ] = temp.neighbors[2][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1727  if( temp.neighbors[cx2][2][cz1] && temp.neighbors[cx2][2][cz1]->children )
1728  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1729  n.neighbors[fx2 ][fy0+j][fz1+k] = temp.neighbors[cx2][2][cz1]->children + Cube::CornerIndex( x1 , j , k );
1730  if( temp.neighbors[2][cy2][cz1] && temp.neighbors[2][cy2][cz1]->children )
1731  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1732  n.neighbors[fx0+i][fy2 ][fz1+k] = temp.neighbors[2][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1733  if( temp.neighbors[cx2][cy2][2] && temp.neighbors[cx2][cy2][2]->children )
1734  for( k=0 ; k<2 ; k++ )
1735  n.neighbors[fx2 ][fy2 ][fz0+k] = temp.neighbors[cx2][cy2][2]->children + Cube::CornerIndex( x1 , y1 , k );
1736  if( temp.neighbors[cx2][2][cz2] && temp.neighbors[cx2][2][cz2]->children )
1737  for( j=0 ; j<2 ; j++ )
1738  n.neighbors[fx2 ][fy0+j][fz2 ] = temp.neighbors[cx2][2][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1739  if( temp.neighbors[2][cy2][cz2] && temp.neighbors[2][cy2][cz2]->children )
1740  for( i=0 ; i<2 ; i++ )
1741  n.neighbors[fx0+i][fy2 ][fz2 ] = temp.neighbors[2][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1742 
1743  // Set the neighbor from across the corners
1744  if( temp.neighbors[cx1][cy1][cz1] && temp.neighbors[cx1][cy1][cz1]->children )
1745  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1746  n.neighbors[fx1+i][fy1+j][fz1+k] = temp.neighbors[cx1][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1747  if( temp.neighbors[cx1][cy1][cz2] && temp.neighbors[cx1][cy1][cz2]->children )
1748  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1749  n.neighbors[fx1+i][fy1+j][fz2 ] = temp.neighbors[cx1][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1750  if( temp.neighbors[cx1][cy2][cz1] && temp.neighbors[cx1][cy2][cz1]->children )
1751  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1752  n.neighbors[fx1+i][fy2 ][fz1+k] = temp.neighbors[cx1][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1753  if( temp.neighbors[cx2][cy1][cz1] && temp.neighbors[cx2][cy1][cz1]->children )
1754  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1755  n.neighbors[fx2 ][fy1+j][fz1+k] = temp.neighbors[cx2][cy1][cz1]->children + Cube::CornerIndex( x1 , j , k );
1756  if( temp.neighbors[cx1][cy2][cz2] && temp.neighbors[cx1][cy2][cz2]->children )
1757  for( i=0 ; i<2 ; i++ )
1758  n.neighbors[fx1+i][fy2 ][fz2 ] = temp.neighbors[cx1][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1759  if( temp.neighbors[cx2][cy1][cz2] && temp.neighbors[cx2][cy1][cz2]->children )
1760  for( j=0 ; j<2 ; j++ )
1761  n.neighbors[fx2 ][fy1+j][fz2 ] = temp.neighbors[cx2][cy1][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1762  if( temp.neighbors[cx2][cy2][cz1] && temp.neighbors[cx2][cy2][cz1]->children )
1763  for( k=0 ; k<2 ; k++ )
1764  n.neighbors[fx2 ][fy2 ][fz1+k] = temp.neighbors[cx2][cy2][cz1]->children + Cube::CornerIndex( x1 , y1 , k );
1765  if( temp.neighbors[cx2][cy2][cz2] && temp.neighbors[cx2][cy2][cz2]->children )
1766  n.neighbors[fx2 ][fy2 ][fz2 ] = temp.neighbors[cx2][cy2][cz2]->children + Cube::CornerIndex( x1 , y1 , z1 );
1767  }
1768  }
1769  return neighbors[d];
1770 }
1771 template< class NodeData , class Real >
1772 typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd )
1773 {
1774  int d=node->depth();
1775  if( node!=neighbors[d].neighbors[2][2][2] )
1776  {
1777  neighbors[d].clear();
1778 
1779  if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1780  else
1781  {
1782  setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1783  Neighbors5& temp = neighbors[d-1];
1784  int x1 , y1 , z1 , x2 , y2 , z2 , ii , jj , kk;
1785  int idx = int( node-node->parent->children );
1786  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1787 
1788  for( int i=xStart ; i<xEnd ; i++ )
1789  {
1790  x2 = i+x1;
1791  ii = x2&1;
1792  x2 = 1+(x2>>1);
1793  for( int j=yStart ; j<yEnd ; j++ )
1794  {
1795  y2 = j+y1;
1796  jj = y2&1;
1797  y2 = 1+(y2>>1);
1798  for( int k=zStart ; k<zEnd ; k++ )
1799  {
1800  z2 = k+z1;
1801  kk = z2&1;
1802  z2 = 1+(z2>>1);
1803  if(temp.neighbors[x2][y2][z2] )
1804  {
1805  if( !temp.neighbors[x2][y2][z2]->children ) temp.neighbors[x2][y2][z2]->initChildren();
1806  neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1807  }
1808  }
1809  }
1810  }
1811  }
1812  }
1813  return neighbors[d];
1814 }
1815 template< class NodeData , class Real >
1816 typename OctNode< NodeData , Real >::ConstNeighbors5& OctNode< NodeData , Real >::ConstNeighborKey5::getNeighbors( const OctNode* node )
1817 {
1818  int d=node->depth();
1819  if( node!=neighbors[d].neighbors[2][2][2] )
1820  {
1821  neighbors[d].clear();
1822 
1823  if(!node->parent) neighbors[d].neighbors[2][2][2]=node;
1824  else
1825  {
1826  getNeighbors( node->parent );
1827  ConstNeighbors5& temp = neighbors[d-1];
1828  int x1,y1,z1,x2,y2,z2,ii,jj,kk;
1829  int idx=int(node-node->parent->children);
1830  Cube::FactorCornerIndex(idx,x1,y1,z1);
1831 
1832  for(int i=0;i<5;i++)
1833  {
1834  x2=i+x1;
1835  ii=x2&1;
1836  x2=1+(x2>>1);
1837  for(int j=0;j<5;j++)
1838  {
1839  y2=j+y1;
1840  jj=y2&1;
1841  y2=1+(y2>>1);
1842  for(int k=0;k<5;k++)
1843  {
1844  z2=k+z1;
1845  kk=z2&1;
1846  z2=1+(z2>>1);
1847  if(temp.neighbors[x2][y2][z2] && temp.neighbors[x2][y2][z2]->children)
1848  neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1849  }
1850  }
1851  }
1852  }
1853  }
1854  return neighbors[d];
1855 }
1856 
1857 
1858 template <class NodeData,class Real>
1859 int OctNode<NodeData,Real>::write(const char* fileName) const{
1860  FILE* fp=fopen(fileName,"wb");
1861  if(!fp){return 0;}
1862  int ret=write(fp);
1863  fclose(fp);
1864  return ret;
1865 }
1866 template <class NodeData,class Real>
1867 int OctNode<NodeData,Real>::write(FILE* fp) const{
1868  fwrite(this,sizeof(OctNode<NodeData,Real>),1,fp);
1869  if(children){for(unsigned int i=0;i<Cube::CORNERS;i++){children[i].write(fp);}}
1870  return 1;
1871 }
1872 template <class NodeData,class Real>
1873 int OctNode<NodeData,Real>::read(const char* fileName){
1874  FILE* fp=fopen(fileName,"rb");
1875  if(!fp){return 0;}
1876  int ret=read(fp);
1877  fclose(fp);
1878  return ret;
1879 }
1880 template <class NodeData,class Real>
1881 int OctNode<NodeData,Real>::read(FILE* fp){
1882  fread(this,sizeof(OctNode<NodeData,Real>),1,fp);
1883  parent=NULL;
1884  if(children){
1885  children=NULL;
1886  initChildren();
1887  for(unsigned int i=0;i<Cube::CORNERS;i++){
1888  children[i].read(fp);
1889  children[i].parent=this;
1890  }
1891  }
1892  return 1;
1893 }
1894 template<class NodeData,class Real>
1895 int OctNode<NodeData,Real>::width(int maxDepth) const {
1896  int d=depth();
1897  return 1<<(maxDepth-d);
1898 }
1899 template<class NodeData,class Real>
1900 void OctNode<NodeData,Real>::centerIndex(int maxDepth,int index[DIMENSION]) const {
1901  int d,o[3];
1902  depthAndOffset(d,o);
1903  for(int i=0;i<DIMENSION;i++){index[i]=BinaryNode<Real>::CornerIndex(maxDepth,d+1,o[i]<<1,1);}
1904 }
1905 #endif
OctNode
Definition: Octree.h:39