Point Cloud Library (PCL) 1.12.0
octree_poisson.hpp
1/*
2Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without modification,
6are permitted provided that the following conditions are met:
7
8Redistributions of source code must retain the above copyright notice, this list of
9conditions and the following disclaimer. Redistributions in binary form must reproduce
10the above copyright notice, this list of conditions and the following disclaimer
11in the documentation and/or other materials provided with the distribution.
12
13Neither the name of the Johns Hopkins University nor the names of its contributors
14may be used to endorse or promote products derived from this software without specific
15prior written permission.
16
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26DAMAGE.
27*/
28
29#include <stdlib.h>
30#include <algorithm>
31
32#include "poisson_exceptions.h"
33
34/////////////
35// OctNode //
36/////////////
37
38namespace 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 }
116 }
117 return 1;
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);
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));
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 }
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 >
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);
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;}
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;}
189 return c+1;
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>
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 }
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 }
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 }
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 >
257 if( !current->parent || current==this ) return NULL;
258 if( current-current->parent->children==0 ) return prevBranch( current->parent );
259 else return current-1;
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;
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>
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>
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;
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>
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)
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)
OctNode & operator=(const OctNode< NodeData2, Real > &node)
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])