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