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