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