Point Cloud Library (PCL)  1.9.1-dev
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 #include "poisson_exceptions.h"
34 
35 /////////////
36 // OctNode //
37 /////////////
38 
39 namespace pcl
40 {
41  namespace poisson
42  {
43 
44  template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthShift=5;
45  template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift=19;
46  template<class NodeData,class Real> const int OctNode<NodeData,Real>::DepthMask=(1<<DepthShift)-1;
47  template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetMask=(1<<OffsetShift)-1;
48  template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift1=DepthShift;
49  template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift2=OffsetShift1+OffsetShift;
50  template<class NodeData,class Real> const int OctNode<NodeData,Real>::OffsetShift3=OffsetShift2+OffsetShift;
51 
52  template<class NodeData,class Real> int OctNode<NodeData,Real>::UseAlloc=0;
53  template<class NodeData,class Real> Allocator<OctNode<NodeData,Real> > OctNode<NodeData,Real>::internalAllocator;
54 
55  template<class NodeData,class Real>
57  {
58  if(blockSize>0)
59  {
60  UseAlloc=1;
61  internalAllocator.set(blockSize);
62  }
63  else{UseAlloc=0;}
64  }
65  template<class NodeData,class Real>
66  int OctNode<NodeData,Real>::UseAllocator(void){return UseAlloc;}
67 
68  template <class NodeData,class Real>
70  parent=children=NULL;
71  d=off[0]=off[1]=off[2]=0;
72  }
73 
74  template <class NodeData,class Real>
76  if(!UseAlloc){if(children){delete[] children;}}
77  parent=children=NULL;
78  }
79  template <class NodeData,class Real>
81  if( maxDepth )
82  {
83  if( !children ) initChildren();
84  for( int i=0 ; i<8 ; i++ ) children[i].setFullDepth( maxDepth-1 );
85  }
86  }
87 
88  template <class NodeData,class Real>
90  int i,j,k;
91 
92  if(UseAlloc){children=internalAllocator.newElements(8);}
93  else{
94  if(children){delete[] children;}
95  children=NULL;
96  children=new OctNode[Cube::CORNERS];
97  }
98  if(!children){
99  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadInitException, "Failed to initialize OctNode children.");
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>
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>
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>
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 >
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  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Couldn't find node at appropriate depth");
1199  }
1200  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1201  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)];
1202 
1203 
1204  // Set the neighbors from across the faces
1205  i=x1<<1;
1206  if( temp.neighbors[i][1][1] )
1207  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)];
1208  j=y1<<1;
1209  if( temp.neighbors[1][j][1] )
1210  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)];
1211  k=z1<<1;
1212  if( temp.neighbors[1][1][k] )
1213  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)];
1214 
1215  // Set the neighbors from across the edges
1216  i=x1<<1 , j=y1<<1;
1217  if( temp.neighbors[i][j][1] )
1218  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1219  i=x1<<1 , k=z1<<1;
1220  if( temp.neighbors[i][1][k] )
1221  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1222  j=y1<<1 , k=z1<<1;
1223  if( temp.neighbors[1][j][k] )
1224  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1225 
1226  // Set the neighbor from across the corner
1227  i=x1<<1 , j=y1<<1 , k=z1<<1;
1228  if( temp.neighbors[i][j][k] )
1229  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1230  }
1231  }
1232  return neighbors[d];
1233  }
1234 
1235  template< class NodeData , class Real >
1237  {
1238  int d = node->depth();
1239  if( node==neighbors[d].neighbors[1][1][1] )
1240  {
1241  bool reset = false;
1242  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;
1243  if( reset ) neighbors[d].neighbors[1][1][1] = NULL;
1244  }
1245  if( node!=neighbors[d].neighbors[1][1][1] )
1246  {
1247  neighbors[d].clear();
1248 
1249  if( !node->parent ) neighbors[d].neighbors[1][1][1] = node;
1250  else
1251  {
1252  int i,j,k,x1,y1,z1,x2,y2,z2;
1253  int idx=int(node-node->parent->children);
1254  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1255  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1256  for(i=0;i<2;i++){
1257  for(j=0;j<2;j++){
1258  for(k=0;k<2;k++){
1259  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1260  }
1261  }
1262  }
1263  Neighbors3& temp=setNeighbors(node->parent);
1264 
1265  // Set the neighbors from across the faces
1266  i=x1<<1;
1267  if(temp.neighbors[i][1][1]){
1268  if(!temp.neighbors[i][1][1]->children){temp.neighbors[i][1][1]->initChildren();}
1269  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)];}}
1270  }
1271  j=y1<<1;
1272  if(temp.neighbors[1][j][1]){
1273  if(!temp.neighbors[1][j][1]->children){temp.neighbors[1][j][1]->initChildren();}
1274  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)];}}
1275  }
1276  k=z1<<1;
1277  if(temp.neighbors[1][1][k]){
1278  if(!temp.neighbors[1][1][k]->children){temp.neighbors[1][1][k]->initChildren();}
1279  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)];}}
1280  }
1281 
1282  // Set the neighbors from across the edges
1283  i=x1<<1; j=y1<<1;
1284  if(temp.neighbors[i][j][1]){
1285  if(!temp.neighbors[i][j][1]->children){temp.neighbors[i][j][1]->initChildren();}
1286  for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1287  }
1288  i=x1<<1; k=z1<<1;
1289  if(temp.neighbors[i][1][k]){
1290  if(!temp.neighbors[i][1][k]->children){temp.neighbors[i][1][k]->initChildren();}
1291  for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1292  }
1293  j=y1<<1; k=z1<<1;
1294  if(temp.neighbors[1][j][k]){
1295  if(!temp.neighbors[1][j][k]->children){temp.neighbors[1][j][k]->initChildren();}
1296  for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1297  }
1298 
1299  // Set the neighbor from across the corner
1300  i=x1<<1; j=y1<<1; k=z1<<1;
1301  if(temp.neighbors[i][j][k]){
1302  if(!temp.neighbors[i][j][k]->children){temp.neighbors[i][j][k]->initChildren();}
1303  neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1304  }
1305  }
1306  }
1307  return neighbors[d];
1308  }
1309  // Note the assumption is that if you enable an edge, you also enable adjacent faces.
1310  // And, if you enable a corner, you enable adjacent edges and faces.
1311  template< class NodeData , class Real >
1313  {
1314  int d = node->depth();
1315  if( node==neighbors[d].neighbors[1][1][1] )
1316  {
1317  bool reset = false;
1318  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;
1319  if( reset ) neighbors[d].neighbors[1][1][1] = NULL;
1320  }
1321  if( node!=neighbors[d].neighbors[1][1][1] )
1322  {
1323  neighbors[d].clear();
1324 
1325  if( !node->parent ) neighbors[d].neighbors[1][1][1] = node;
1326  else
1327  {
1328  int x1,y1,z1,x2,y2,z2;
1329  int idx=int(node-node->parent->children);
1330  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1331  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1332  for( int i=0 ; i<2 ; i++ )
1333  for( int j=0 ; j<2 ; j++ )
1334  for( int k=0 ; k<2 ; k++ )
1335  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1336 
1337  Neighbors3& temp=setNeighbors( node->parent , flags );
1338 
1339  // Set the neighbors from across the faces
1340  {
1341  int i=x1<<1;
1342  if( temp.neighbors[i][1][1] )
1343  {
1344  if( flags[i][1][1] && !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren();
1345  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)];
1346  }
1347  }
1348  {
1349  int j = y1<<1;
1350  if( temp.neighbors[1][j][1] )
1351  {
1352  if( flags[1][j][1] && !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren();
1353  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)];
1354  }
1355  }
1356  {
1357  int k = z1<<1;
1358  if( temp.neighbors[1][1][k] )
1359  {
1360  if( flags[1][1][k] && !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren();
1361  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)];
1362  }
1363  }
1364 
1365  // Set the neighbors from across the edges
1366  {
1367  int i=x1<<1 , j=y1<<1;
1368  if( temp.neighbors[i][j][1] )
1369  {
1370  if( flags[i][j][1] && !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren();
1371  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)];
1372  }
1373  }
1374  {
1375  int i=x1<<1 , k=z1<<1;
1376  if( temp.neighbors[i][1][k] )
1377  {
1378  if( flags[i][1][k] && !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren();
1379  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)];
1380  }
1381  }
1382  {
1383  int j=y1<<1 , k=z1<<1;
1384  if( temp.neighbors[1][j][k] )
1385  {
1386  if( flags[1][j][k] && !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren();
1387  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)];
1388  }
1389  }
1390 
1391  // Set the neighbor from across the corner
1392  {
1393  int i=x1<<1 , j=y1<<1 , k=z1<<1;
1394  if( temp.neighbors[i][j][k] )
1395  {
1396  if( flags[i][j][k] && !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren();
1397  if( temp.neighbors[i][j][k]->children ) neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1398  }
1399  }
1400  }
1401  }
1402  return neighbors[d];
1403  }
1404 
1405  template<class NodeData,class Real>
1407  int d=node->depth();
1408  if(node!=neighbors[d].neighbors[1][1][1]){
1409  neighbors[d].clear();
1410 
1411  if(!node->parent){neighbors[d].neighbors[1][1][1]=node;}
1412  else{
1413  int i,j,k,x1,y1,z1,x2,y2,z2;
1414  int idx=int(node-node->parent->children);
1415  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1416  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1417  for(i=0;i<2;i++){
1418  for(j=0;j<2;j++){
1419  for(k=0;k<2;k++){
1420  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1421  }
1422  }
1423  }
1424  Neighbors3& temp=getNeighbors(node->parent);
1425 
1426  // Set the neighbors from across the faces
1427  i=x1<<1;
1428  if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){
1429  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)];}}
1430  }
1431  j=y1<<1;
1432  if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){
1433  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)];}}
1434  }
1435  k=z1<<1;
1436  if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){
1437  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)];}}
1438  }
1439 
1440  // Set the neighbors from across the edges
1441  i=x1<<1; j=y1<<1;
1442  if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){
1443  for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1444  }
1445  i=x1<<1; k=z1<<1;
1446  if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){
1447  for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1448  }
1449  j=y1<<1; k=z1<<1;
1450  if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){
1451  for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1452  }
1453 
1454  // Set the neighbor from across the corner
1455  i=x1<<1; j=y1<<1; k=z1<<1;
1456  if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){
1457  neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1458  }
1459  }
1460  }
1461  return neighbors[node->depth()];
1462  }
1463 
1464  ///////////////////////
1465  // ConstNeighborKey3 //
1466  ///////////////////////
1467  template<class NodeData,class Real>
1469  template<class NodeData,class Real>
1471  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;}}}
1472  }
1473  template<class NodeData,class Real>
1475  template<class NodeData,class Real>
1477  if(neighbors){delete[] neighbors;}
1478  neighbors=NULL;
1479  }
1480 
1481  template<class NodeData,class Real>
1483  if(neighbors){delete[] neighbors;}
1484  neighbors=NULL;
1485  if(d<0){return;}
1486  neighbors=new ConstNeighbors3[d+1];
1487  }
1488  template<class NodeData,class Real>
1490  int d=node->depth();
1491  if(node!=neighbors[d].neighbors[1][1][1]){
1492  neighbors[d].clear();
1493 
1494  if(!node->parent){neighbors[d].neighbors[1][1][1]=node;}
1495  else{
1496  int i,j,k,x1,y1,z1,x2,y2,z2;
1497  int idx=int(node-node->parent->children);
1498  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1499  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1500  for(i=0;i<2;i++){
1501  for(j=0;j<2;j++){
1502  for(k=0;k<2;k++){
1503  neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1504  }
1505  }
1506  }
1507  ConstNeighbors3& temp=getNeighbors(node->parent);
1508 
1509  // Set the neighbors from across the faces
1510  i=x1<<1;
1511  if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){
1512  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)];}}
1513  }
1514  j=y1<<1;
1515  if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){
1516  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)];}}
1517  }
1518  k=z1<<1;
1519  if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){
1520  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)];}}
1521  }
1522 
1523  // Set the neighbors from across the edges
1524  i=x1<<1; j=y1<<1;
1525  if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){
1526  for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1527  }
1528  i=x1<<1; k=z1<<1;
1529  if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){
1530  for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1531  }
1532  j=y1<<1; k=z1<<1;
1533  if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){
1534  for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1535  }
1536 
1537  // Set the neighbor from across the corner
1538  i=x1<<1; j=y1<<1; k=z1<<1;
1539  if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){
1540  neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1541  }
1542  }
1543  }
1544  return neighbors[node->depth()];
1545  }
1546  template<class NodeData,class Real>
1548  {
1549  int d=node->depth();
1550  if (d < minDepth)
1551  {
1552  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Node depth lower than min-depth: (actual)" << d << " < (minimum)" << minDepth);
1553  }
1554  if( node!=neighbors[d].neighbors[1][1][1] )
1555  {
1556  neighbors[d].clear();
1557 
1558  if( d==minDepth ) neighbors[d].neighbors[1][1][1]=node;
1559  else
1560  {
1561  int i,j,k,x1,y1,z1,x2,y2,z2;
1562  int idx = int(node-node->parent->children);
1563  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1564  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1565 
1566  ConstNeighbors3& temp=getNeighbors( node->parent , minDepth );
1567 
1568  // Set the syblings
1569  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1570  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &node->parent->children[ Cube::CornerIndex(i,j,k) ];
1571 
1572  // Set the neighbors from across the faces
1573  i=x1<<1;
1574  if( temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children )
1575  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)];
1576 
1577  j=y1<<1;
1578  if( temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children )
1579  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)];
1580 
1581  k=z1<<1;
1582  if( temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children )
1583  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)];
1584 
1585  // Set the neighbors from across the edges
1586  i=x1<<1 , j=y1<<1;
1587  if( temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children )
1588  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1589 
1590  i=x1<<1 , k=z1<<1;
1591  if( temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children )
1592  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1593 
1594  j=y1<<1 , k=z1<<1;
1595  if( temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children )
1596  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1597 
1598  // Set the neighbor from across the corner
1599  i=x1<<1 , j=y1<<1 , k=z1<<1;
1600  if( temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children )
1601  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1602  }
1603  }
1604  return neighbors[node->depth()];
1605  }
1606 
1607  template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); }
1608  template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); }
1609  template< class NodeData , class Real >
1611  {
1612  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;
1613  }
1614  template< class NodeData , class Real >
1616  {
1617  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;
1618  }
1619  template< class NodeData , class Real >
1621  {
1622  _depth = -1;
1623  neighbors = NULL;
1624  }
1625  template< class NodeData , class Real >
1627  {
1628  _depth = -1;
1629  neighbors = NULL;
1630  }
1631  template< class NodeData , class Real >
1633  {
1634  if( neighbors ) delete[] neighbors;
1635  neighbors = NULL;
1636  }
1637  template< class NodeData , class Real >
1639  {
1640  if( neighbors ) delete[] neighbors;
1641  neighbors = NULL;
1642  }
1643  template< class NodeData , class Real >
1645  {
1646  if( neighbors ) delete[] neighbors;
1647  neighbors = NULL;
1648  if(d<0) return;
1649  _depth = d;
1650  neighbors=new Neighbors5[d+1];
1651  }
1652  template< class NodeData , class Real >
1654  {
1655  if( neighbors ) delete[] neighbors;
1656  neighbors = NULL;
1657  if(d<0) return;
1658  _depth = d;
1659  neighbors=new ConstNeighbors5[d+1];
1660  }
1661  template< class NodeData , class Real >
1663  {
1664  int d=node->depth();
1665  if( node!=neighbors[d].neighbors[2][2][2] )
1666  {
1667  neighbors[d].clear();
1668 
1669  if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1670  else
1671  {
1672  getNeighbors( node->parent );
1673  Neighbors5& temp = neighbors[d-1];
1674  int x1 , y1 , z1 , x2 , y2 , z2;
1675  int idx = int( node - node->parent->children );
1676  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1677 
1678  Neighbors5& n = neighbors[d];
1679  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1680  int i , j , k;
1681  int fx0 = x2+1 , fy0 = y2+1 , fz0 = z2+1; // Indices of the bottom left corner of the parent within the 5x5x5
1682  int cx1 = x1*2+1 , cy1 = y1*2+1 , cz1 = z1*2+1;
1683  int cx2 = x2*2+1 , cy2 = y2*2+1 , cz2 = z2*2+1;
1684  int fx1 = x1*3 , fy1 = y1*3 , fz1 = z1*3;
1685  int fx2 = x2*4 , fy2 = y2*4 , fz2 = z2*4;
1686 
1687  // Set the syblings
1688  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1689  n.neighbors[fx0+i][fy0+j][fz0+k] = node->parent->children + Cube::CornerIndex( i , j , k );
1690 
1691  // Set the neighbors from across the faces
1692  if( temp.neighbors[cx1][2][2] && temp.neighbors[cx1][2][2]->children )
1693  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1694  n.neighbors[fx1+i][fy0+j][fz0+k] = temp.neighbors[cx1][2][2]->children + Cube::CornerIndex( i , j , k );
1695  if( temp.neighbors[2][cy1][2] && temp.neighbors[2][cy1][2]->children )
1696  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1697  n.neighbors[fx0+i][fy1+j][fz0+k] = temp.neighbors[2][cy1][2]->children + Cube::CornerIndex( i , j , k );
1698  if( temp.neighbors[2][2][cz1] && temp.neighbors[2][2][cz1]->children )
1699  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1700  n.neighbors[fx0+i][fy0+j][fz1+k] = temp.neighbors[2][2][cz1]->children + Cube::CornerIndex( i , j , k );
1701  if( temp.neighbors[cx2][2][2] && temp.neighbors[cx2][2][2]->children )
1702  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1703  n.neighbors[fx2 ][fy0+j][fz0+k] = temp.neighbors[cx2][2][2]->children + Cube::CornerIndex( x1 , j , k );
1704  if( temp.neighbors[2][cy2][2] && temp.neighbors[2][cy2][2]->children )
1705  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1706  n.neighbors[fx0+i][fy2 ][fz0+k] = temp.neighbors[2][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1707  if( temp.neighbors[2][2][cz2] && temp.neighbors[2][2][cz2]->children )
1708  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1709  n.neighbors[fx0+i][fy0+j][fz2 ] = temp.neighbors[2][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1710 
1711  // Set the neighbors from across the edges
1712  if( temp.neighbors[cx1][cy1][2] && temp.neighbors[cx1][cy1][2]->children )
1713  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1714  n.neighbors[fx1+i][fy1+j][fz0+k] = temp.neighbors[cx1][cy1][2]->children + Cube::CornerIndex( i , j , k );
1715  if( temp.neighbors[cx1][2][cz1] && temp.neighbors[cx1][2][cz1]->children )
1716  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1717  n.neighbors[fx1+i][fy0+j][fz1+k] = temp.neighbors[cx1][2][cz1]->children + Cube::CornerIndex( i , j , k );
1718  if( temp.neighbors[2][cy1][cz1] && temp.neighbors[2][cy1][cz1]->children )
1719  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1720  n.neighbors[fx0+i][fy1+j][fz1+k] = temp.neighbors[2][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1721  if( temp.neighbors[cx1][cy2][2] && temp.neighbors[cx1][cy2][2]->children )
1722  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1723  n.neighbors[fx1+i][fy2 ][fz0+k] = temp.neighbors[cx1][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1724  if( temp.neighbors[cx1][2][cz2] && temp.neighbors[cx1][2][cz2]->children )
1725  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1726  n.neighbors[fx1+i][fy0+j][fz2 ] = temp.neighbors[cx1][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1727  if( temp.neighbors[cx2][cy1][2] && temp.neighbors[cx2][cy1][2]->children )
1728  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1729  n.neighbors[fx2 ][fy1+j][fz0+k] = temp.neighbors[cx2][cy1][2]->children + Cube::CornerIndex( x1 , j , k );
1730  if( temp.neighbors[2][cy1][cz2] && temp.neighbors[2][cy1][cz2]->children )
1731  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1732  n.neighbors[fx0+i][fy1+j][fz2 ] = temp.neighbors[2][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1733  if( temp.neighbors[cx2][2][cz1] && temp.neighbors[cx2][2][cz1]->children )
1734  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1735  n.neighbors[fx2 ][fy0+j][fz1+k] = temp.neighbors[cx2][2][cz1]->children + Cube::CornerIndex( x1 , j , k );
1736  if( temp.neighbors[2][cy2][cz1] && temp.neighbors[2][cy2][cz1]->children )
1737  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1738  n.neighbors[fx0+i][fy2 ][fz1+k] = temp.neighbors[2][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1739  if( temp.neighbors[cx2][cy2][2] && temp.neighbors[cx2][cy2][2]->children )
1740  for( k=0 ; k<2 ; k++ )
1741  n.neighbors[fx2 ][fy2 ][fz0+k] = temp.neighbors[cx2][cy2][2]->children + Cube::CornerIndex( x1 , y1 , k );
1742  if( temp.neighbors[cx2][2][cz2] && temp.neighbors[cx2][2][cz2]->children )
1743  for( j=0 ; j<2 ; j++ )
1744  n.neighbors[fx2 ][fy0+j][fz2 ] = temp.neighbors[cx2][2][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1745  if( temp.neighbors[2][cy2][cz2] && temp.neighbors[2][cy2][cz2]->children )
1746  for( i=0 ; i<2 ; i++ )
1747  n.neighbors[fx0+i][fy2 ][fz2 ] = temp.neighbors[2][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1748 
1749  // Set the neighbor from across the corners
1750  if( temp.neighbors[cx1][cy1][cz1] && temp.neighbors[cx1][cy1][cz1]->children )
1751  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1752  n.neighbors[fx1+i][fy1+j][fz1+k] = temp.neighbors[cx1][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1753  if( temp.neighbors[cx1][cy1][cz2] && temp.neighbors[cx1][cy1][cz2]->children )
1754  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1755  n.neighbors[fx1+i][fy1+j][fz2 ] = temp.neighbors[cx1][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1756  if( temp.neighbors[cx1][cy2][cz1] && temp.neighbors[cx1][cy2][cz1]->children )
1757  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1758  n.neighbors[fx1+i][fy2 ][fz1+k] = temp.neighbors[cx1][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1759  if( temp.neighbors[cx2][cy1][cz1] && temp.neighbors[cx2][cy1][cz1]->children )
1760  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1761  n.neighbors[fx2 ][fy1+j][fz1+k] = temp.neighbors[cx2][cy1][cz1]->children + Cube::CornerIndex( x1 , j , k );
1762  if( temp.neighbors[cx1][cy2][cz2] && temp.neighbors[cx1][cy2][cz2]->children )
1763  for( i=0 ; i<2 ; i++ )
1764  n.neighbors[fx1+i][fy2 ][fz2 ] = temp.neighbors[cx1][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1765  if( temp.neighbors[cx2][cy1][cz2] && temp.neighbors[cx2][cy1][cz2]->children )
1766  for( j=0 ; j<2 ; j++ )
1767  n.neighbors[fx2 ][fy1+j][fz2 ] = temp.neighbors[cx2][cy1][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1768  if( temp.neighbors[cx2][cy2][cz1] && temp.neighbors[cx2][cy2][cz1]->children )
1769  for( k=0 ; k<2 ; k++ )
1770  n.neighbors[fx2 ][fy2 ][fz1+k] = temp.neighbors[cx2][cy2][cz1]->children + Cube::CornerIndex( x1 , y1 , k );
1771  if( temp.neighbors[cx2][cy2][cz2] && temp.neighbors[cx2][cy2][cz2]->children )
1772  n.neighbors[fx2 ][fy2 ][fz2 ] = temp.neighbors[cx2][cy2][cz2]->children + Cube::CornerIndex( x1 , y1 , z1 );
1773  }
1774  }
1775  return neighbors[d];
1776  }
1777  template< class NodeData , class Real >
1778  typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd )
1779  {
1780  int d=node->depth();
1781  if( node!=neighbors[d].neighbors[2][2][2] )
1782  {
1783  neighbors[d].clear();
1784 
1785  if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1786  else
1787  {
1788  setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1789  Neighbors5& temp = neighbors[d-1];
1790  int x1 , y1 , z1 , x2 , y2 , z2 , ii , jj , kk;
1791  int idx = int( node-node->parent->children );
1792  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1793 
1794  for( int i=xStart ; i<xEnd ; i++ )
1795  {
1796  x2 = i+x1;
1797  ii = x2&1;
1798  x2 = 1+(x2>>1);
1799  for( int j=yStart ; j<yEnd ; j++ )
1800  {
1801  y2 = j+y1;
1802  jj = y2&1;
1803  y2 = 1+(y2>>1);
1804  for( int k=zStart ; k<zEnd ; k++ )
1805  {
1806  z2 = k+z1;
1807  kk = z2&1;
1808  z2 = 1+(z2>>1);
1809  if(temp.neighbors[x2][y2][z2] )
1810  {
1811  if( !temp.neighbors[x2][y2][z2]->children ) temp.neighbors[x2][y2][z2]->initChildren();
1812  neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1813  }
1814  }
1815  }
1816  }
1817  }
1818  }
1819  return neighbors[d];
1820  }
1821  template< class NodeData , class Real >
1823  {
1824  int d=node->depth();
1825  if( node!=neighbors[d].neighbors[2][2][2] )
1826  {
1827  neighbors[d].clear();
1828 
1829  if(!node->parent) neighbors[d].neighbors[2][2][2]=node;
1830  else
1831  {
1832  getNeighbors( node->parent );
1833  ConstNeighbors5& temp = neighbors[d-1];
1834  int x1,y1,z1,x2,y2,z2,ii,jj,kk;
1835  int idx=int(node-node->parent->children);
1836  Cube::FactorCornerIndex(idx,x1,y1,z1);
1837 
1838  for(int i=0;i<5;i++)
1839  {
1840  x2=i+x1;
1841  ii=x2&1;
1842  x2=1+(x2>>1);
1843  for(int j=0;j<5;j++)
1844  {
1845  y2=j+y1;
1846  jj=y2&1;
1847  y2=1+(y2>>1);
1848  for(int k=0;k<5;k++)
1849  {
1850  z2=k+z1;
1851  kk=z2&1;
1852  z2=1+(z2>>1);
1853  if(temp.neighbors[x2][y2][z2] && temp.neighbors[x2][y2][z2]->children)
1854  neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1855  }
1856  }
1857  }
1858  }
1859  }
1860  return neighbors[d];
1861  }
1862 
1863 
1864  template <class NodeData,class Real>
1865  int OctNode<NodeData,Real>::write(const char* fileName) const{
1866  FILE* fp=fopen(fileName,"wb");
1867  if(!fp){return 0;}
1868  int ret=write(fp);
1869  fclose(fp);
1870  return ret;
1871  }
1872  template <class NodeData,class Real>
1873  int OctNode<NodeData,Real>::write(FILE* fp) const{
1874  fwrite(this,sizeof(OctNode<NodeData,Real>),1,fp);
1875  if(children){for(int i=0;i<Cube::CORNERS;i++){children[i].write(fp);}}
1876  return 1;
1877  }
1878  template <class NodeData,class Real>
1879  int OctNode<NodeData,Real>::read(const char* fileName){
1880  FILE* fp=fopen(fileName,"rb");
1881  if(!fp){return 0;}
1882  int ret=read(fp);
1883  fclose(fp);
1884  return ret;
1885  }
1886  template <class NodeData,class Real>
1888  fread(this,sizeof(OctNode<NodeData,Real>),1,fp);
1889  parent=NULL;
1890  if(children){
1891  children=NULL;
1892  initChildren();
1893  for(int i=0;i<Cube::CORNERS;i++){
1894  children[i].read(fp);
1895  children[i].parent=this;
1896  }
1897  }
1898  return 1;
1899  }
1900  template<class NodeData,class Real>
1902  int d=depth();
1903  return 1<<(maxDepth-d);
1904  }
1905  template<class NodeData,class Real>
1906  void OctNode<NodeData,Real>::centerIndex(int maxDepth,int index[DIMENSION]) const {
1907  int d,o[3];
1908  depthAndOffset(d,o);
1909  for(int i=0;i<DIMENSION;i++){index[i]=BinaryNode<Real>::CornerIndex(maxDepth,d+1,o[i]<<1,1);}
1910  }
1911 
1912  }
1913 }
void depthAndOffset(int &depth, int offset[DIMENSION]) const
const OctNode * neighbors[5][5][5]
An exception that is thrown when initialization fails.
static void FactorCornerIndex(int idx, int &x, int &y, int &z)
bool isInside(Point3D< Real > p) const
void processNodeNodes(OctNode *node, NodeAdjacencyFunction *F, int processCurrent=1)
static const int OffsetMask
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
OctNode * edgeNeighbor(int edgeIndex, int forceChildren=0)
const OctNode * nextBranch(const OctNode *current) const
long long _InterleaveBits(int p[3])
static int CompareBackwardDepths(const void *v1, const void *v2)
static void ProcessMaxDepthNodeAdjacentNodes(int maxDepth, OctNode *node1, int width1, OctNode *node2, int width2, int depth, NodeAdjacencyFunction *F, int processCurrent=1)
static int CompareBackwardPointerDepths(const void *v1, const void *v2)
static Allocator< OctNode > internalAllocator
int width(int maxDepth) const
void centerAndWidth(Point3D< Real > &center, Real &width) const
static int CommonEdge(const OctNode *node1, int eIndex1, const OctNode *node2, int eIndex2)
static void ProcessNodeAdjacentNodes(int maxDepth, OctNode *node1, int width1, OctNode *node2, int width2, NodeAdjacencyFunction *F, int processCurrent=1)
const OctNode * neighbors[3][3][3]
ConstNeighbors3 & getNeighbors(const OctNode *node)
OctNode * faceNeighbor(int faceIndex, int forceChildren=0)
static void FaceCorners(int idx, int &c1, int &c2, int &c3, int &c4)
static int CornerIndex(const Point3D< Real > &center, const Point3D< Real > &p)
An exception that is thrown when the arguments number or type is wrong/unhandled. ...
static int UseAllocator(void)
void processNodeFaces(OctNode *node, NodeAdjacencyFunction *F, int fIndex, int processCurrent=1)
OctNode * cornerNeighbor(int cornerIndex, int forceChildren=0)
static void FactorEdgeIndex(int idx, int &orientation, int &i, int &j)
int read(const char *fileName)
void setFullDepth(int maxDepth)
static int CompareForwardDepths(const void *v1, const void *v2)
static const int DepthShift
static const int OffsetShift2
static void Index(int depth, const int offset[3], short &d, short off[DIMENSION])
static void EdgeCorners(int idx, int &c1, int &c2)
const OctNode * nextNode(const OctNode *currentNode=NULL) const
static void ProcessFixedDepthNodeAdjacentNodes(int maxDepth, OctNode *node1, int width1, OctNode *node2, int width2, int depth, NodeAdjacencyFunction *F, int processCurrent=1)
OctNode * getNearestLeaf(const Point3D< Real > &p)
void processNodeEdges(OctNode *node, NodeAdjacencyFunction *F, int eIndex, int processCurrent=1)
const OctNode * prevBranch(const OctNode *current) const
static int CompareByDepthAndXYZ(const void *v1, const void *v2)
int maxDepthLeaves(int maxDepth) const
static int CompareForwardPointerDepths(const void *v1, const void *v2)
static int CornerIndex(int depth, int offSet)
Definition: binary_node.h:48
short off[DIMENSION]
int nodes(void) const
ConstNeighbors5 & getNeighbors(const OctNode *node)
OctNode & operator=(const OctNode< NodeData2, Real > &node)
static int Depth(const long long &index)
static void DepthAndOffset(const long long &index, int &depth, int offset[DIMENSION])
void processNodeCorners(OctNode *node, NodeAdjacencyFunction *F, int cIndex, int processCurrent=1)
static void CenterAndWidth(const long long &index, Point3D< Real > &center, Real &width)
static const int OffsetShift
static void ProcessTerminatingNodeAdjacentNodes(int maxDepth, OctNode *node1, int width1, OctNode *node2, int width2, TerminatingNodeAdjacencyFunction *F, int processCurrent=1)
Neighbors3 & setNeighbors(OctNode *root, Point3D< Real > p, int d)
static int Overlap2(const int &depth1, const int offSet1[DIMENSION], const Real &multiplier1, const int &depth2, const int offSet2[DIMENSION], const Real &multiplier2)
static void ProcessPointAdjacentNodes(int maxDepth, const int center1[3], OctNode *node2, int width2, PointAdjacencyFunction *F, int processCurrent=1)
void printRange(void) const
static void SetAllocator(int blockSize)
Neighbors3 & getNeighbors(OctNode *root, Point3D< Real > p, int d)
static const int OffsetShift1
Neighbors5 & getNeighbors(OctNode *node)
static int CornerIndex(int x, int y, int z)
int write(const char *fileName) const
int maxDepth(void) const
void centerIndex(int maxDepth, int index[DIMENSION]) const
const OctNode * nextLeaf(const OctNode *currentLeaf=NULL) const
int depth(void) const
int leaves(void) const
static const int OffsetShift3
static int CompareByDepthAndZIndex(const void *v1, const void *v2)
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:68
static const int DepthMask
const OctNode * root(void) const
Neighbors5 & setNeighbors(OctNode *node, int xStart=0, int xEnd=5, int yStart=0, int yEnd=5, int zStart=0, int zEnd=5)