Point Cloud Library (PCL)  1.10.0-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  long long _p[3] = {p[0],p[1],p[2]};
798  for( int i=0 ; i<31 ; i++ ) key |= ( ( _p[0] & (1ull<<i) )<<(2*i) ) | ( ( _p[1] & (1ull<<i) )<<(2*i+1) ) | ( ( _p[2] & (1ull<<i) )<<(2*i+2) );
799  return key;
800  }
801  template <class NodeData,class Real>
802  int OctNode<NodeData,Real>::CompareByDepthAndZIndex( const void* v1 , const void* v2 )
803  {
804  const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1);
805  const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2);
806  int d1 , off1[3] , d2 , off2[3];
807  n1->depthAndOffset( d1 , off1 ) , n2->depthAndOffset( d2 , off2 );
808  if ( d1>d2 ) return 1;
809  else if( d1<d2 ) return -1;
810  long long k1 = _InterleaveBits( off1 ) , k2 = _InterleaveBits( off2 );
811  if ( k1>k2 ) return 1;
812  else if( k1<k2 ) return -1;
813  else return 0;
814  }
815 
816  template <class NodeData,class Real>
817  int OctNode<NodeData,Real>::CompareForwardPointerDepths( const void* v1 , const void* v2 )
818  {
819  const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1);
820  const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2);
821  if(n1->d!=n2->d){return int(n1->d)-int(n2->d);}
822  while( n1->parent!=n2->parent )
823  {
824  n1=n1->parent;
825  n2=n2->parent;
826  }
827  if(n1->off[0]!=n2->off[0]){return int(n1->off[0])-int(n2->off[0]);}
828  if(n1->off[1]!=n2->off[1]){return int(n1->off[1])-int(n2->off[1]);}
829  return int(n1->off[2])-int(n2->off[2]);
830  return 0;
831  }
832  template <class NodeData,class Real>
833  int OctNode<NodeData,Real>::CompareBackwardDepths(const void* v1,const void* v2){
834  return ((const OctNode<NodeData,Real>*)v2)->depth-((const OctNode<NodeData,Real>*)v1)->depth;
835  }
836  template <class NodeData,class Real>
837  int OctNode<NodeData,Real>::CompareBackwardPointerDepths(const void* v1,const void* v2){
838  return (*(const OctNode<NodeData,Real>**)v2)->depth()-(*(const OctNode<NodeData,Real>**)v1)->depth();
839  }
840  template <class NodeData,class Real>
841  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){
842  int d=depth2-depth1;
843  Real w=multiplier2+multiplier1*(1<<d);
844  Real w2=Real((1<<(d-1))-0.5);
845  if(
846  fabs(Real(offSet2[0]-(offSet1[0]<<d))-w2)>=w ||
847  fabs(Real(offSet2[1]-(offSet1[1]<<d))-w2)>=w ||
848  fabs(Real(offSet2[2]-(offSet1[2]<<d))-w2)>=w
849  ){return 0;}
850  return 1;
851  }
852  template <class NodeData,class Real>
853  inline int OctNode<NodeData,Real>::Overlap(int c1,int c2,int c3,int dWidth){
854  if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;}
855  else{return 1;}
856  }
857  template <class NodeData,class Real>
858  OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);}
859  template <class NodeData,class Real>
860  const OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);}
861  template <class NodeData,class Real>
862  OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off,int forceChildren){
863  if(!parent){return NULL;}
864  int pIndex=int(this-parent->children);
865  pIndex^=(1<<dir);
866  if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
867  else{
868  OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren);
869  if(!temp){return NULL;}
870  if(!temp->children){
871  if(forceChildren){temp->initChildren();}
872  else{return temp;}
873  }
874  return &temp->children[pIndex];
875  }
876  }
877  template <class NodeData,class Real>
878  const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off) const {
879  if(!parent){return NULL;}
880  int pIndex=int(this-parent->children);
881  pIndex^=(1<<dir);
882  if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
883  else{
884  const OctNode* temp=parent->__faceNeighbor(dir,off);
885  if(!temp || !temp->children){return temp;}
886  else{return &temp->children[pIndex];}
887  }
888  }
889 
890  template <class NodeData,class Real>
892  int idx[2],o,i[2];
893  Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]);
894  switch(o){
895  case 0: idx[0]=1; idx[1]=2; break;
896  case 1: idx[0]=0; idx[1]=2; break;
897  case 2: idx[0]=0; idx[1]=1; break;
898  };
899  return __edgeNeighbor(o,i,idx,forceChildren);
900  }
901  template <class NodeData,class Real>
903  int idx[2],o,i[2];
904  Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]);
905  switch(o){
906  case 0: idx[0]=1; idx[1]=2; break;
907  case 1: idx[0]=0; idx[1]=2; break;
908  case 2: idx[0]=0; idx[1]=1; break;
909  };
910  return __edgeNeighbor(o,i,idx);
911  }
912  template <class NodeData,class Real>
913  const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{
914  if(!parent){return NULL;}
915  int pIndex=int(this-parent->children);
916  int aIndex,x[DIMENSION];
917 
918  Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
919  aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3;
920  pIndex^=(7 ^ (1<<o));
921  if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor
922  const OctNode* temp=parent->__faceNeighbor(idx[0],i[0]);
923  if(!temp || !temp->children){return NULL;}
924  else{return &temp->children[pIndex];}
925  }
926  else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor
927  const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]);
928  if(!temp || !temp->children){return NULL;}
929  else{return &temp->children[pIndex];}
930  }
931  else if(aIndex==0) { // I can get the neighbor from the parent
932  return &parent->children[pIndex];
933  }
934  else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor
935  const OctNode* temp=parent->__edgeNeighbor(o,i,idx);
936  if(!temp || !temp->children){return temp;}
937  else{return &temp->children[pIndex];}
938  }
939  else{return NULL;}
940  }
941  template <class NodeData,class Real>
942  OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){
943  if(!parent){return NULL;}
944  int pIndex=int(this-parent->children);
945  int aIndex,x[DIMENSION];
946 
947  Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
948  aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3;
949  pIndex^=(7 ^ (1<<o));
950  if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor
951  OctNode* temp=parent->__faceNeighbor(idx[0],i[0],0);
952  if(!temp || !temp->children){return NULL;}
953  else{return &temp->children[pIndex];}
954  }
955  else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor
956  OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0);
957  if(!temp || !temp->children){return NULL;}
958  else{return &temp->children[pIndex];}
959  }
960  else if(aIndex==0) { // I can get the neighbor from the parent
961  return &parent->children[pIndex];
962  }
963  else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor
964  OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren);
965  if(!temp){return NULL;}
966  if(!temp->children){
967  if(forceChildren){temp->initChildren();}
968  else{return temp;}
969  }
970  return &temp->children[pIndex];
971  }
972  else{return NULL;}
973  }
974 
975  template <class NodeData,class Real>
977  int pIndex,aIndex=0;
978  if(!parent){return NULL;}
979 
980  pIndex=int(this-parent->children);
981  aIndex=(cornerIndex ^ pIndex); // The disagreement bits
982  pIndex=(~pIndex)&7; // The antipodal point
983  if(aIndex==7){ // Agree on no bits
984  return &parent->children[pIndex];
985  }
986  else if(aIndex==0){ // Agree on all bits
987  const OctNode* temp=((const OctNode*)parent)->cornerNeighbor(cornerIndex);
988  if(!temp || !temp->children){return temp;}
989  else{return &temp->children[pIndex];}
990  }
991  else if(aIndex==6){ // Agree on face 0
992  const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1);
993  if(!temp || !temp->children){return NULL;}
994  else{return & temp->children[pIndex];}
995  }
996  else if(aIndex==5){ // Agree on face 1
997  const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1);
998  if(!temp || !temp->children){return NULL;}
999  else{return & temp->children[pIndex];}
1000  }
1001  else if(aIndex==3){ // Agree on face 2
1002  const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2);
1003  if(!temp || !temp->children){return NULL;}
1004  else{return & temp->children[pIndex];}
1005  }
1006  else if(aIndex==4){ // Agree on edge 2
1007  const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) );
1008  if(!temp || !temp->children){return NULL;}
1009  else{return & temp->children[pIndex];}
1010  }
1011  else if(aIndex==2){ // Agree on edge 1
1012  const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) );
1013  if(!temp || !temp->children){return NULL;}
1014  else{return & temp->children[pIndex];}
1015  }
1016  else if(aIndex==1){ // Agree on edge 0
1017  const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 );
1018  if(!temp || !temp->children){return NULL;}
1019  else{return & temp->children[pIndex];}
1020  }
1021  else{return NULL;}
1022  }
1023  template <class NodeData,class Real>
1025  int pIndex,aIndex=0;
1026  if(!parent){return NULL;}
1027 
1028  pIndex=int(this-parent->children);
1029  aIndex=(cornerIndex ^ pIndex); // The disagreement bits
1030  pIndex=(~pIndex)&7; // The antipodal point
1031  if(aIndex==7){ // Agree on no bits
1032  return &parent->children[pIndex];
1033  }
1034  else if(aIndex==0){ // Agree on all bits
1035  OctNode* temp=((OctNode*)parent)->cornerNeighbor(cornerIndex,forceChildren);
1036  if(!temp){return NULL;}
1037  if(!temp->children){
1038  if(forceChildren){temp->initChildren();}
1039  else{return temp;}
1040  }
1041  return &temp->children[pIndex];
1042  }
1043  else if(aIndex==6){ // Agree on face 0
1044  OctNode* temp=((OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1,0);
1045  if(!temp || !temp->children){return NULL;}
1046  else{return & temp->children[pIndex];}
1047  }
1048  else if(aIndex==5){ // Agree on face 1
1049  OctNode* temp=((OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0);
1050  if(!temp || !temp->children){return NULL;}
1051  else{return & temp->children[pIndex];}
1052  }
1053  else if(aIndex==3){ // Agree on face 2
1054  OctNode* temp=((OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0);
1055  if(!temp || !temp->children){return NULL;}
1056  else{return & temp->children[pIndex];}
1057  }
1058  else if(aIndex==4){ // Agree on edge 2
1059  OctNode* temp=((OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) );
1060  if(!temp || !temp->children){return NULL;}
1061  else{return & temp->children[pIndex];}
1062  }
1063  else if(aIndex==2){ // Agree on edge 1
1064  OctNode* temp=((OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) );
1065  if(!temp || !temp->children){return NULL;}
1066  else{return & temp->children[pIndex];}
1067  }
1068  else if(aIndex==1){ // Agree on edge 0
1069  OctNode* temp=((OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 );
1070  if(!temp || !temp->children){return NULL;}
1071  else{return & temp->children[pIndex];}
1072  }
1073  else{return NULL;}
1074  }
1075  ////////////////////////
1076  // OctNodeNeighborKey //
1077  ////////////////////////
1078  template<class NodeData,class Real>
1080  template<class NodeData,class Real>
1082  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;}}}
1083  }
1084  template<class NodeData,class Real>
1086  template<class NodeData,class Real>
1088  {
1089  if( neighbors ) delete[] neighbors;
1090  neighbors = NULL;
1091  }
1092 
1093  template<class NodeData,class Real>
1095  {
1096  if( neighbors ) delete[] neighbors;
1097  neighbors = NULL;
1098  if( d<0 ) return;
1099  neighbors = new Neighbors3[d+1];
1100  }
1101  template< class NodeData , class Real >
1103  {
1104  if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) )
1105  {
1106  neighbors[d].clear();
1107 
1108  if( !d ) neighbors[d].neighbors[1][1][1] = root;
1109  else
1110  {
1111  Neighbors3& temp = setNeighbors( root , p , d-1 );
1112 
1113  int i , j , k , x1 , y1 , z1 , x2 , y2 , z2;
1114  Point3D< Real > c;
1115  Real w;
1116  temp.neighbors[1][1][1]->centerAndWidth( c , w );
1117  int idx = CornerIndex( c , p );
1118  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1119  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1120 
1121  if( !temp.neighbors[1][1][1]->children ) temp.neighbors[1][1][1]->initChildren();
1122  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1123  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)];
1124 
1125 
1126  // Set the neighbors from across the faces
1127  i=x1<<1;
1128  if( temp.neighbors[i][1][1] )
1129  {
1130  if( !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren();
1131  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)];
1132  }
1133  j=y1<<1;
1134  if( temp.neighbors[1][j][1] )
1135  {
1136  if( !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren();
1137  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)];
1138  }
1139  k=z1<<1;
1140  if( temp.neighbors[1][1][k] )
1141  {
1142  if( !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren();
1143  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)];
1144  }
1145 
1146  // Set the neighbors from across the edges
1147  i=x1<<1 , j=y1<<1;
1148  if( temp.neighbors[i][j][1] )
1149  {
1150  if( !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren();
1151  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1152  }
1153  i=x1<<1 , k=z1<<1;
1154  if( temp.neighbors[i][1][k] )
1155  {
1156  if( !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren();
1157  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1158  }
1159  j=y1<<1 , k=z1<<1;
1160  if( temp.neighbors[1][j][k] )
1161  {
1162  if( !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren();
1163  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1164  }
1165 
1166  // Set the neighbor from across the corner
1167  i=x1<<1 , j=y1<<1 , k=z1<<1;
1168  if( temp.neighbors[i][j][k] )
1169  {
1170  if( !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren();
1171  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1172  }
1173  }
1174  }
1175  return neighbors[d];
1176  }
1177  template< class NodeData , class Real >
1179  {
1180  if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) )
1181  {
1182  neighbors[d].clear();
1183 
1184  if( !d ) neighbors[d].neighbors[1][1][1] = root;
1185  else
1186  {
1187  Neighbors3& temp = getNeighbors( root , p , d-1 );
1188 
1189  int i , j , k , x1 , y1 , z1 , x2 , y2 , z2;
1190  Point3D< Real > c;
1191  Real w;
1192  temp.neighbors[1][1][1]->centerAndWidth( c , w );
1193  int idx = CornerIndex( c , p );
1194  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1195  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1196 
1197  if( !temp.neighbors[1][1][1] || !temp.neighbors[1][1][1]->children )
1198  {
1199  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Couldn't find node at appropriate depth");
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 >
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 >
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>
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>
1549  {
1550  int d=node->depth();
1551  if (d < minDepth)
1552  {
1553  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Node depth lower than min-depth: (actual)" << d << " < (minimum)" << minDepth);
1554  }
1555  if( node!=neighbors[d].neighbors[1][1][1] )
1556  {
1557  neighbors[d].clear();
1558 
1559  if( d==minDepth ) neighbors[d].neighbors[1][1][1]=node;
1560  else
1561  {
1562  int i,j,k,x1,y1,z1,x2,y2,z2;
1563  int idx = int(node-node->parent->children);
1564  Cube::FactorCornerIndex( idx ,x1,y1,z1);
1565  Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1566 
1567  ConstNeighbors3& temp=getNeighbors( node->parent , minDepth );
1568 
1569  // Set the syblings
1570  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1571  neighbors[d].neighbors[x2+i][y2+j][z2+k] = &node->parent->children[ Cube::CornerIndex(i,j,k) ];
1572 
1573  // Set the neighbors from across the faces
1574  i=x1<<1;
1575  if( temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children )
1576  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)];
1577 
1578  j=y1<<1;
1579  if( temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children )
1580  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)];
1581 
1582  k=z1<<1;
1583  if( temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children )
1584  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)];
1585 
1586  // Set the neighbors from across the edges
1587  i=x1<<1 , j=y1<<1;
1588  if( temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children )
1589  for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1590 
1591  i=x1<<1 , k=z1<<1;
1592  if( temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children )
1593  for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1594 
1595  j=y1<<1 , k=z1<<1;
1596  if( temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children )
1597  for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1598 
1599  // Set the neighbor from across the corner
1600  i=x1<<1 , j=y1<<1 , k=z1<<1;
1601  if( temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children )
1602  neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1603  }
1604  }
1605  return neighbors[node->depth()];
1606  }
1607 
1608  template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); }
1609  template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); }
1610  template< class NodeData , class Real >
1612  {
1613  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;
1614  }
1615  template< class NodeData , class Real >
1617  {
1618  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;
1619  }
1620  template< class NodeData , class Real >
1622  {
1623  _depth = -1;
1624  neighbors = NULL;
1625  }
1626  template< class NodeData , class Real >
1628  {
1629  _depth = -1;
1630  neighbors = NULL;
1631  }
1632  template< class NodeData , class Real >
1634  {
1635  if( neighbors ) delete[] neighbors;
1636  neighbors = NULL;
1637  }
1638  template< class NodeData , class Real >
1640  {
1641  if( neighbors ) delete[] neighbors;
1642  neighbors = NULL;
1643  }
1644  template< class NodeData , class Real >
1646  {
1647  if( neighbors ) delete[] neighbors;
1648  neighbors = NULL;
1649  if(d<0) return;
1650  _depth = d;
1651  neighbors=new Neighbors5[d+1];
1652  }
1653  template< class NodeData , class Real >
1655  {
1656  if( neighbors ) delete[] neighbors;
1657  neighbors = NULL;
1658  if(d<0) return;
1659  _depth = d;
1660  neighbors=new ConstNeighbors5[d+1];
1661  }
1662  template< class NodeData , class Real >
1664  {
1665  int d=node->depth();
1666  if( node!=neighbors[d].neighbors[2][2][2] )
1667  {
1668  neighbors[d].clear();
1669 
1670  if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1671  else
1672  {
1673  getNeighbors( node->parent );
1674  Neighbors5& temp = neighbors[d-1];
1675  int x1 , y1 , z1 , x2 , y2 , z2;
1676  int idx = int( node - node->parent->children );
1677  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1678 
1679  Neighbors5& n = neighbors[d];
1680  Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1681  int i , j , k;
1682  int fx0 = x2+1 , fy0 = y2+1 , fz0 = z2+1; // Indices of the bottom left corner of the parent within the 5x5x5
1683  int cx1 = x1*2+1 , cy1 = y1*2+1 , cz1 = z1*2+1;
1684  int cx2 = x2*2+1 , cy2 = y2*2+1 , cz2 = z2*2+1;
1685  int fx1 = x1*3 , fy1 = y1*3 , fz1 = z1*3;
1686  int fx2 = x2*4 , fy2 = y2*4 , fz2 = z2*4;
1687 
1688  // Set the syblings
1689  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1690  n.neighbors[fx0+i][fy0+j][fz0+k] = node->parent->children + Cube::CornerIndex( i , j , k );
1691 
1692  // Set the neighbors from across the faces
1693  if( temp.neighbors[cx1][2][2] && temp.neighbors[cx1][2][2]->children )
1694  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1695  n.neighbors[fx1+i][fy0+j][fz0+k] = temp.neighbors[cx1][2][2]->children + Cube::CornerIndex( i , j , k );
1696  if( temp.neighbors[2][cy1][2] && temp.neighbors[2][cy1][2]->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][fy1+j][fz0+k] = temp.neighbors[2][cy1][2]->children + Cube::CornerIndex( i , j , k );
1699  if( temp.neighbors[2][2][cz1] && temp.neighbors[2][2][cz1]->children )
1700  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1701  n.neighbors[fx0+i][fy0+j][fz1+k] = temp.neighbors[2][2][cz1]->children + Cube::CornerIndex( i , j , k );
1702  if( temp.neighbors[cx2][2][2] && temp.neighbors[cx2][2][2]->children )
1703  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1704  n.neighbors[fx2 ][fy0+j][fz0+k] = temp.neighbors[cx2][2][2]->children + Cube::CornerIndex( x1 , j , k );
1705  if( temp.neighbors[2][cy2][2] && temp.neighbors[2][cy2][2]->children )
1706  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1707  n.neighbors[fx0+i][fy2 ][fz0+k] = temp.neighbors[2][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1708  if( temp.neighbors[2][2][cz2] && temp.neighbors[2][2][cz2]->children )
1709  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1710  n.neighbors[fx0+i][fy0+j][fz2 ] = temp.neighbors[2][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1711 
1712  // Set the neighbors from across the edges
1713  if( temp.neighbors[cx1][cy1][2] && temp.neighbors[cx1][cy1][2]->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][fy1+j][fz0+k] = temp.neighbors[cx1][cy1][2]->children + Cube::CornerIndex( i , j , k );
1716  if( temp.neighbors[cx1][2][cz1] && temp.neighbors[cx1][2][cz1]->children )
1717  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1718  n.neighbors[fx1+i][fy0+j][fz1+k] = temp.neighbors[cx1][2][cz1]->children + Cube::CornerIndex( i , j , k );
1719  if( temp.neighbors[2][cy1][cz1] && temp.neighbors[2][cy1][cz1]->children )
1720  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1721  n.neighbors[fx0+i][fy1+j][fz1+k] = temp.neighbors[2][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1722  if( temp.neighbors[cx1][cy2][2] && temp.neighbors[cx1][cy2][2]->children )
1723  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1724  n.neighbors[fx1+i][fy2 ][fz0+k] = temp.neighbors[cx1][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1725  if( temp.neighbors[cx1][2][cz2] && temp.neighbors[cx1][2][cz2]->children )
1726  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1727  n.neighbors[fx1+i][fy0+j][fz2 ] = temp.neighbors[cx1][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1728  if( temp.neighbors[cx2][cy1][2] && temp.neighbors[cx2][cy1][2]->children )
1729  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1730  n.neighbors[fx2 ][fy1+j][fz0+k] = temp.neighbors[cx2][cy1][2]->children + Cube::CornerIndex( x1 , j , k );
1731  if( temp.neighbors[2][cy1][cz2] && temp.neighbors[2][cy1][cz2]->children )
1732  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1733  n.neighbors[fx0+i][fy1+j][fz2 ] = temp.neighbors[2][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1734  if( temp.neighbors[cx2][2][cz1] && temp.neighbors[cx2][2][cz1]->children )
1735  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1736  n.neighbors[fx2 ][fy0+j][fz1+k] = temp.neighbors[cx2][2][cz1]->children + Cube::CornerIndex( x1 , j , k );
1737  if( temp.neighbors[2][cy2][cz1] && temp.neighbors[2][cy2][cz1]->children )
1738  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1739  n.neighbors[fx0+i][fy2 ][fz1+k] = temp.neighbors[2][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1740  if( temp.neighbors[cx2][cy2][2] && temp.neighbors[cx2][cy2][2]->children )
1741  for( k=0 ; k<2 ; k++ )
1742  n.neighbors[fx2 ][fy2 ][fz0+k] = temp.neighbors[cx2][cy2][2]->children + Cube::CornerIndex( x1 , y1 , k );
1743  if( temp.neighbors[cx2][2][cz2] && temp.neighbors[cx2][2][cz2]->children )
1744  for( j=0 ; j<2 ; j++ )
1745  n.neighbors[fx2 ][fy0+j][fz2 ] = temp.neighbors[cx2][2][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1746  if( temp.neighbors[2][cy2][cz2] && temp.neighbors[2][cy2][cz2]->children )
1747  for( i=0 ; i<2 ; i++ )
1748  n.neighbors[fx0+i][fy2 ][fz2 ] = temp.neighbors[2][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1749 
1750  // Set the neighbor from across the corners
1751  if( temp.neighbors[cx1][cy1][cz1] && temp.neighbors[cx1][cy1][cz1]->children )
1752  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1753  n.neighbors[fx1+i][fy1+j][fz1+k] = temp.neighbors[cx1][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1754  if( temp.neighbors[cx1][cy1][cz2] && temp.neighbors[cx1][cy1][cz2]->children )
1755  for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1756  n.neighbors[fx1+i][fy1+j][fz2 ] = temp.neighbors[cx1][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1757  if( temp.neighbors[cx1][cy2][cz1] && temp.neighbors[cx1][cy2][cz1]->children )
1758  for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1759  n.neighbors[fx1+i][fy2 ][fz1+k] = temp.neighbors[cx1][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1760  if( temp.neighbors[cx2][cy1][cz1] && temp.neighbors[cx2][cy1][cz1]->children )
1761  for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1762  n.neighbors[fx2 ][fy1+j][fz1+k] = temp.neighbors[cx2][cy1][cz1]->children + Cube::CornerIndex( x1 , j , k );
1763  if( temp.neighbors[cx1][cy2][cz2] && temp.neighbors[cx1][cy2][cz2]->children )
1764  for( i=0 ; i<2 ; i++ )
1765  n.neighbors[fx1+i][fy2 ][fz2 ] = temp.neighbors[cx1][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1766  if( temp.neighbors[cx2][cy1][cz2] && temp.neighbors[cx2][cy1][cz2]->children )
1767  for( j=0 ; j<2 ; j++ )
1768  n.neighbors[fx2 ][fy1+j][fz2 ] = temp.neighbors[cx2][cy1][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1769  if( temp.neighbors[cx2][cy2][cz1] && temp.neighbors[cx2][cy2][cz1]->children )
1770  for( k=0 ; k<2 ; k++ )
1771  n.neighbors[fx2 ][fy2 ][fz1+k] = temp.neighbors[cx2][cy2][cz1]->children + Cube::CornerIndex( x1 , y1 , k );
1772  if( temp.neighbors[cx2][cy2][cz2] && temp.neighbors[cx2][cy2][cz2]->children )
1773  n.neighbors[fx2 ][fy2 ][fz2 ] = temp.neighbors[cx2][cy2][cz2]->children + Cube::CornerIndex( x1 , y1 , z1 );
1774  }
1775  }
1776  return neighbors[d];
1777  }
1778  template< class NodeData , class Real >
1779  typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd )
1780  {
1781  int d=node->depth();
1782  if( node!=neighbors[d].neighbors[2][2][2] )
1783  {
1784  neighbors[d].clear();
1785 
1786  if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1787  else
1788  {
1789  setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1790  Neighbors5& temp = neighbors[d-1];
1791  int x1 , y1 , z1 , x2 , y2 , z2 , ii , jj , kk;
1792  int idx = int( node-node->parent->children );
1793  Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1794 
1795  for( int i=xStart ; i<xEnd ; i++ )
1796  {
1797  x2 = i+x1;
1798  ii = x2&1;
1799  x2 = 1+(x2>>1);
1800  for( int j=yStart ; j<yEnd ; j++ )
1801  {
1802  y2 = j+y1;
1803  jj = y2&1;
1804  y2 = 1+(y2>>1);
1805  for( int k=zStart ; k<zEnd ; k++ )
1806  {
1807  z2 = k+z1;
1808  kk = z2&1;
1809  z2 = 1+(z2>>1);
1810  if(temp.neighbors[x2][y2][z2] )
1811  {
1812  if( !temp.neighbors[x2][y2][z2]->children ) temp.neighbors[x2][y2][z2]->initChildren();
1813  neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1814  }
1815  }
1816  }
1817  }
1818  }
1819  }
1820  return neighbors[d];
1821  }
1822  template< class NodeData , class Real >
1824  {
1825  int d=node->depth();
1826  if( node!=neighbors[d].neighbors[2][2][2] )
1827  {
1828  neighbors[d].clear();
1829 
1830  if(!node->parent) neighbors[d].neighbors[2][2][2]=node;
1831  else
1832  {
1833  getNeighbors( node->parent );
1834  ConstNeighbors5& temp = neighbors[d-1];
1835  int x1,y1,z1,x2,y2,z2,ii,jj,kk;
1836  int idx=int(node-node->parent->children);
1837  Cube::FactorCornerIndex(idx,x1,y1,z1);
1838 
1839  for(int i=0;i<5;i++)
1840  {
1841  x2=i+x1;
1842  ii=x2&1;
1843  x2=1+(x2>>1);
1844  for(int j=0;j<5;j++)
1845  {
1846  y2=j+y1;
1847  jj=y2&1;
1848  y2=1+(y2>>1);
1849  for(int k=0;k<5;k++)
1850  {
1851  z2=k+z1;
1852  kk=z2&1;
1853  z2=1+(z2>>1);
1854  if(temp.neighbors[x2][y2][z2] && temp.neighbors[x2][y2][z2]->children)
1855  neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1856  }
1857  }
1858  }
1859  }
1860  }
1861  return neighbors[d];
1862  }
1863 
1864 
1865  template <class NodeData,class Real>
1866  int OctNode<NodeData,Real>::write(const char* fileName) const{
1867  FILE* fp=fopen(fileName,"wb");
1868  if(!fp){return 0;}
1869  int ret=write(fp);
1870  fclose(fp);
1871  return ret;
1872  }
1873  template <class NodeData,class Real>
1874  int OctNode<NodeData,Real>::write(FILE* fp) const{
1875  fwrite(this,sizeof(OctNode<NodeData,Real>),1,fp);
1876  if(children){for(int i=0;i<Cube::CORNERS;i++){children[i].write(fp);}}
1877  return 1;
1878  }
1879  template <class NodeData,class Real>
1880  int OctNode<NodeData,Real>::read(const char* fileName){
1881  FILE* fp=fopen(fileName,"rb");
1882  if(!fp){return 0;}
1883  int ret=read(fp);
1884  fclose(fp);
1885  return ret;
1886  }
1887  template <class NodeData,class Real>
1889  fread(this,sizeof(OctNode<NodeData,Real>),1,fp);
1890  parent=NULL;
1891  if(children){
1892  children=NULL;
1893  initChildren();
1894  for(int i=0;i<Cube::CORNERS;i++){
1895  children[i].read(fp);
1896  children[i].parent=this;
1897  }
1898  }
1899  return 1;
1900  }
1901  template<class NodeData,class Real>
1903  int d=depth();
1904  return 1<<(maxDepth-d);
1905  }
1906  template<class NodeData,class Real>
1907  void OctNode<NodeData,Real>::centerIndex(int maxDepth,int index[DIMENSION]) const {
1908  int d,o[3];
1909  depthAndOffset(d,o);
1910  for(int i=0;i<DIMENSION;i++){index[i]=BinaryNode<Real>::CornerIndex(maxDepth,d+1,o[i]<<1,1);}
1911  }
1912 
1913  }
1914 }
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)
void processNodeNodes(OctNode *node, NodeAdjacencyFunction *F, int processCurrent=1)
int width(int maxDepth) const
void printRange(void) const
const OctNode * root(void) const
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)
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)
const OctNode * nextBranch(const OctNode *current) const
static Allocator< OctNode > internalAllocator
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]
bool isInside(Point3D< Real > p) const
ConstNeighbors3 & getNeighbors(const OctNode *node)
const OctNode * prevBranch(const OctNode *current) const
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)
int maxDepthLeaves(int maxDepth) const
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)
int write(const char *fileName) const
static void FactorEdgeIndex(int idx, int &orientation, int &i, int &j)
void centerAndWidth(Point3D< Real > &center, Real &width) const
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 * nextLeaf(const OctNode *currentLeaf=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)
static int CompareByDepthAndXYZ(const void *v1, const void *v2)
static int CompareForwardPointerDepths(const void *v1, const void *v2)
static int CornerIndex(int depth, int offSet)
Definition: binary_node.h:48
short off[DIMENSION]
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])
int maxDepth(void) const
void processNodeCorners(OctNode *node, NodeAdjacencyFunction *F, int cIndex, int processCurrent=1)
int leaves(void) const
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)
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)
void depthAndOffset(int &depth, int offset[DIMENSION]) const
void centerIndex(int maxDepth, int index[DIMENSION]) const
static const int OffsetShift3
const OctNode * nextNode(const OctNode *currentNode=NULL) const
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
Neighbors5 & setNeighbors(OctNode *node, int xStart=0, int xEnd=5, int yStart=0, int yEnd=5, int zStart=0, int zEnd=5)