Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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 Point3D<Real> child_center;
750 Real child_width;
751 children[i].centerAndWidth(child_center, child_width);
752 temp=SquareDistance(child_center,p);
753 if(!i || temp<dist2){
754 dist2=temp;
755 nearest=i;
756 }
757 }
758 return children[nearest].getNearestLeaf(p);
759 }
760
761 template <class NodeData,class Real>
762 int OctNode<NodeData,Real>::CommonEdge(const OctNode<NodeData,Real>* node1,int eIndex1,const OctNode<NodeData,Real>* node2,int eIndex2){
763 int o1,o2,i1,i2,j1,j2;
764
765 Cube::FactorEdgeIndex(eIndex1,o1,i1,j1);
766 Cube::FactorEdgeIndex(eIndex2,o2,i2,j2);
767 if(o1!=o2){return 0;}
768
769 int dir[2];
770 int idx1[2];
771 int idx2[2];
772 switch(o1){
773 case 0: dir[0]=1; dir[1]=2; break;
774 case 1: dir[0]=0; dir[1]=2; break;
775 case 2: dir[0]=0; dir[1]=1; break;
776 };
777 int d1,d2,off1[3],off2[3];
778 node1->depthAndOffset(d1,off1);
779 node2->depthAndOffset(d2,off2);
780 idx1[0]=off1[dir[0]]+(1<<d1)+i1;
781 idx1[1]=off1[dir[1]]+(1<<d1)+j1;
782 idx2[0]=off2[dir[0]]+(1<<d2)+i2;
783 idx2[1]=off2[dir[1]]+(1<<d2)+j2;
784 if(d1>d2){
785 idx2[0]<<=(d1-d2);
786 idx2[1]<<=(d1-d2);
787 }
788 else{
789 idx1[0]<<=(d2-d1);
790 idx1[1]<<=(d2-d1);
791 }
792 if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;}
793 else {return 0;}
794 }
795
796 template<class NodeData,class Real>
798 int cIndex=0;
799 if(p.coords[0]>center.coords[0]){cIndex|=1;}
800 if(p.coords[1]>center.coords[1]){cIndex|=2;}
801 if(p.coords[2]>center.coords[2]){cIndex|=4;}
802 return cIndex;
803 }
804
805 template <class NodeData,class Real>
806 template<class NodeData2>
808 int i;
809 delete[] children;
810 children=NULL;
811
812 d=node.depth ();
813 for(i=0;i<DIMENSION;i++){this->off[i] = node.off[i];}
814 if(node.children){
815 initChildren();
816 for(i=0;i<Cube::CORNERS;i++){children[i] = node.children[i];}
817 }
818 return *this;
819 }
820
821 template <class NodeData,class Real>
822 int OctNode<NodeData,Real>::CompareForwardDepths(const void* v1,const void* v2){
823 return ((const OctNode<NodeData,Real>*)v1)->depth()-((const OctNode<NodeData,Real>*)v2)->depth();
824 }
825
826 template< class NodeData , class Real >
827 int OctNode< NodeData , Real >::CompareByDepthAndXYZ( const void* v1 , const void* v2 )
828 {
829 const OctNode<NodeData,Real> *n1 = (*(const OctNode< NodeData , Real >**)v1);
830 const OctNode<NodeData,Real> *n2 = (*(const OctNode< NodeData , Real >**)v2);
831 if( n1->d!=n2->d ) return int(n1->d)-int(n2->d);
832 else if( n1->off[0]!=n2->off[0] ) return int(n1->off[0]) - int(n2->off[0]);
833 else if( n1->off[1]!=n2->off[1] ) return int(n1->off[1]) - int(n2->off[1]);
834 else if( n1->off[2]!=n2->off[2] ) return int(n1->off[2]) - int(n2->off[2]);
835 return 0;
836 }
837
838 long long _InterleaveBits( int p[3] )
839 {
840 long long key = 0;
841 long long _p[3] = {p[0],p[1],p[2]};
842 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) );
843 return key;
844 }
845
846 template <class NodeData,class Real>
847 int OctNode<NodeData,Real>::CompareByDepthAndZIndex( const void* v1 , const void* v2 )
848 {
849 const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1);
850 const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2);
851 int d1 , off1[3] , d2 , off2[3];
852 n1->depthAndOffset( d1 , off1 ) , n2->depthAndOffset( d2 , off2 );
853 if ( d1>d2 ) return 1;
854 else if( d1<d2 ) return -1;
855 long long k1 = _InterleaveBits( off1 ) , k2 = _InterleaveBits( off2 );
856 if ( k1>k2 ) return 1;
857 else if( k1<k2 ) return -1;
858 else return 0;
859 }
860
861 template <class NodeData,class Real>
862 int OctNode<NodeData,Real>::CompareForwardPointerDepths( const void* v1 , const void* v2 )
863 {
864 const OctNode<NodeData,Real>* n1 = (*(const OctNode<NodeData,Real>**)v1);
865 const OctNode<NodeData,Real>* n2 = (*(const OctNode<NodeData,Real>**)v2);
866 if(n1->d!=n2->d){return int(n1->d)-int(n2->d);}
867 while( n1->parent!=n2->parent )
868 {
869 n1=n1->parent;
870 n2=n2->parent;
871 }
872 if(n1->off[0]!=n2->off[0]){return int(n1->off[0])-int(n2->off[0]);}
873 if(n1->off[1]!=n2->off[1]){return int(n1->off[1])-int(n2->off[1]);}
874 return int(n1->off[2])-int(n2->off[2]);
875 return 0;
876 }
877
878 template <class NodeData,class Real>
879 int OctNode<NodeData,Real>::CompareBackwardDepths(const void* v1,const void* v2){
880 return ((const OctNode<NodeData,Real>*)v2)->depth()-((const OctNode<NodeData,Real>*)v1)->depth();
881 }
882
883 template <class NodeData,class Real>
885 return (*(const OctNode<NodeData,Real>**)v2)->depth()-(*(const OctNode<NodeData,Real>**)v1)->depth();
886 }
887
888 template <class NodeData,class Real>
889 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){
890 int d=depth2-depth1;
891 Real w=multiplier2+multiplier1*(1<<d);
892 Real w2=Real((1<<(d-1))-0.5);
893 if(
894 fabs(Real(offSet2[0]-(offSet1[0]<<d))-w2)>=w ||
895 fabs(Real(offSet2[1]-(offSet1[1]<<d))-w2)>=w ||
896 fabs(Real(offSet2[2]-(offSet1[2]<<d))-w2)>=w
897 ){return 0;}
898 return 1;
899 }
900
901 template <class NodeData,class Real>
902 inline int OctNode<NodeData,Real>::Overlap(int c1,int c2,int c3,int dWidth){
903 if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;}
904 else{return 1;}
905 }
906
907 template <class NodeData,class Real>
908 OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);}
909 template <class NodeData,class Real>
910 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::faceNeighbor(int faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);}
911 template <class NodeData,class Real>
912 OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off,int forceChildren){
913 if(!parent){return NULL;}
914 int pIndex=int(this-(parent->children));
915 pIndex^=(1<<dir);
916 if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
917 else{
918 OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren);
919 if(!temp){return NULL;}
920 if(!temp->children){
921 if(forceChildren){temp->initChildren();}
922 else{return temp;}
923 }
924 return &temp->children[pIndex];
925 }
926 }
927
928 template <class NodeData,class Real>
929 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off) const {
930 if(!parent){return NULL;}
931 int pIndex=int(this-(parent->children));
932 pIndex^=(1<<dir);
933 if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
934 else{
935 const OctNode* temp=parent->__faceNeighbor(dir,off);
936 if(!temp || !temp->children){return temp;}
937 else{return &temp->children[pIndex];}
938 }
939 }
940
941 template <class NodeData,class Real>
943 int idx[2],o,i[2];
944 Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]);
945 switch(o){
946 case 0: idx[0]=1; idx[1]=2; break;
947 case 1: idx[0]=0; idx[1]=2; break;
948 case 2: idx[0]=0; idx[1]=1; break;
949 };
950 return __edgeNeighbor(o,i,idx,forceChildren);
951 }
952
953 template <class NodeData,class Real>
955 int idx[2],o,i[2];
956 Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]);
957 switch(o){
958 case 0: idx[0]=1; idx[1]=2; break;
959 case 1: idx[0]=0; idx[1]=2; break;
960 case 2: idx[0]=0; idx[1]=1; break;
961 };
962 return __edgeNeighbor(o,i,idx);
963 }
964
965 template <class NodeData,class Real>
966 const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{
967 if(!parent){return NULL;}
968 int pIndex=int(this-(parent->children));
969 int aIndex,x[DIMENSION];
970
971 Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
972 aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3;
973 pIndex^=(7 ^ (1<<o));
974 if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor
975 const OctNode* temp=parent->__faceNeighbor(idx[0],i[0]);
976 if(!temp || !temp->children){return NULL;}
977 else{return &temp->children[pIndex];}
978 }
979 else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor
980 const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]);
981 if(!temp || !temp->children){return NULL;}
982 else{return &temp->children[pIndex];}
983 }
984 else if(aIndex==0) { // I can get the neighbor from the parent
985 return &parent->children[pIndex];
986 }
987 else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor
988 const OctNode* temp=parent->__edgeNeighbor(o,i,idx);
989 if(!temp || !temp->children){return temp;}
990 else{return &temp->children[pIndex];}
991 }
992 else{return NULL;}
993 }
994
995 template <class NodeData,class Real>
996 OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){
997 if(!parent){return NULL;}
998 int pIndex=int(this-(parent->children));
999 int aIndex,x[DIMENSION];
1000
1001 Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
1002 aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3;
1003 pIndex^=(7 ^ (1<<o));
1004 if(aIndex==1) { // I can get the neighbor from the parent's face adjacent neighbor
1005 OctNode* temp=parent->__faceNeighbor(idx[0],i[0],0);
1006 if(!temp || !temp->children){return NULL;}
1007 else{return &temp->children[pIndex];}
1008 }
1009 else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor
1010 OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0);
1011 if(!temp || !temp->children){return NULL;}
1012 else{return &temp->children[pIndex];}
1013 }
1014 else if(aIndex==0) { // I can get the neighbor from the parent
1015 return &parent->children[pIndex];
1016 }
1017 else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor
1018 OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren);
1019 if(!temp){return NULL;}
1020 if(!temp->children){
1021 if(forceChildren){temp->initChildren();}
1022 else{return temp;}
1023 }
1024 return &temp->children[pIndex];
1025 }
1026 else{return NULL;}
1027 }
1028
1029 template <class NodeData,class Real>
1031 int pIndex,aIndex=0;
1032 if(!parent){return NULL;}
1033
1034 pIndex=int(this-(parent->children));
1035 aIndex=(cornerIndex ^ pIndex); // The disagreement bits
1036 pIndex=(~pIndex)&7; // The antipodal point
1037 if(aIndex==7){ // Agree on no bits
1038 return &parent->children[pIndex];
1039 }
1040 else if(aIndex==0){ // Agree on all bits
1041 const OctNode* temp=((const OctNode*)parent)->cornerNeighbor(cornerIndex);
1042 if(!temp || !temp->children){return temp;}
1043 else{return &temp->children[pIndex];}
1044 }
1045 else if(aIndex==6){ // Agree on face 0
1046 const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1);
1047 if(!temp || !temp->children){return NULL;}
1048 else{return & temp->children[pIndex];}
1049 }
1050 else if(aIndex==5){ // Agree on face 1
1051 const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1);
1052 if(!temp || !temp->children){return NULL;}
1053 else{return & temp->children[pIndex];}
1054 }
1055 else if(aIndex==3){ // Agree on face 2
1056 const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2);
1057 if(!temp || !temp->children){return NULL;}
1058 else{return & temp->children[pIndex];}
1059 }
1060 else if(aIndex==4){ // Agree on edge 2
1061 const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) );
1062 if(!temp || !temp->children){return NULL;}
1063 else{return & temp->children[pIndex];}
1064 }
1065 else if(aIndex==2){ // Agree on edge 1
1066 const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) );
1067 if(!temp || !temp->children){return NULL;}
1068 else{return & temp->children[pIndex];}
1069 }
1070 else if(aIndex==1){ // Agree on edge 0
1071 const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 );
1072 if(!temp || !temp->children){return NULL;}
1073 else{return & temp->children[pIndex];}
1074 }
1075 else{return NULL;}
1076 }
1077
1078 template <class NodeData,class Real>
1080 int pIndex,aIndex=0;
1081 if(!parent){return NULL;}
1082
1083 pIndex=int(this-(parent->children));
1084 aIndex=(cornerIndex ^ pIndex); // The disagreement bits
1085 pIndex=(~pIndex)&7; // The antipodal point
1086 if(aIndex==7){ // Agree on no bits
1087 return &parent->children[pIndex];
1088 }
1089 else if(aIndex==0){ // Agree on all bits
1090 OctNode* temp=((OctNode*)parent)->cornerNeighbor(cornerIndex,forceChildren);
1091 if(!temp){return NULL;}
1092 if(!temp->children){
1093 if(forceChildren){temp->initChildren();}
1094 else{return temp;}
1095 }
1096 return &temp->children[pIndex];
1097 }
1098 else if(aIndex==6){ // Agree on face 0
1099 OctNode* temp=((OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1,0);
1100 if(!temp || !temp->children){return NULL;}
1101 else{return & temp->children[pIndex];}
1102 }
1103 else if(aIndex==5){ // Agree on face 1
1104 OctNode* temp=((OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0);
1105 if(!temp || !temp->children){return NULL;}
1106 else{return & temp->children[pIndex];}
1107 }
1108 else if(aIndex==3){ // Agree on face 2
1109 OctNode* temp=((OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0);
1110 if(!temp || !temp->children){return NULL;}
1111 else{return & temp->children[pIndex];}
1112 }
1113 else if(aIndex==4){ // Agree on edge 2
1114 OctNode* temp=((OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) );
1115 if(!temp || !temp->children){return NULL;}
1116 else{return & temp->children[pIndex];}
1117 }
1118 else if(aIndex==2){ // Agree on edge 1
1119 OctNode* temp=((OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) );
1120 if(!temp || !temp->children){return NULL;}
1121 else{return & temp->children[pIndex];}
1122 }
1123 else if(aIndex==1){ // Agree on edge 0
1124 OctNode* temp=((OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 );
1125 if(!temp || !temp->children){return NULL;}
1126 else{return & temp->children[pIndex];}
1127 }
1128 else{return NULL;}
1129 }
1130
1131 ////////////////////////
1132 // OctNodeNeighborKey //
1133 ////////////////////////
1134 template<class NodeData,class Real>
1136
1137 template<class NodeData,class Real>
1139 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;}}}
1140 }
1141
1142 template<class NodeData,class Real>
1144
1145 template<class NodeData,class Real>
1147 {
1148 delete[] neighbors;
1149 neighbors = NULL;
1150 }
1151
1152 template<class NodeData,class Real>
1154 {
1155 delete[] neighbors;
1156 neighbors = NULL;
1157 if( d<0 ) return;
1158 neighbors = new Neighbors3[d+1];
1159 }
1160
1161 template< class NodeData , class Real >
1163 {
1164 if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) )
1165 {
1166 neighbors[d].clear();
1167
1168 if( !d ) neighbors[d].neighbors[1][1][1] = root;
1169 else
1170 {
1171 Neighbors3& temp = setNeighbors( root , p , d-1 );
1172
1173 int i , j , k , x1 , y1 , z1 , x2 , y2 , z2;
1175 Real w;
1176 temp.neighbors[1][1][1]->centerAndWidth( c , w );
1177 int idx = CornerIndex( c , p );
1178 Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1179 Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1180
1181 if( !temp.neighbors[1][1][1]->children ) temp.neighbors[1][1][1]->initChildren();
1182 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1183 neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)];
1184
1185
1186 // Set the neighbors from across the faces
1187 i=x1<<1;
1188 if( temp.neighbors[i][1][1] )
1189 {
1190 if( !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren();
1191 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)];
1192 }
1193 j=y1<<1;
1194 if( temp.neighbors[1][j][1] )
1195 {
1196 if( !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren();
1197 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)];
1198 }
1199 k=z1<<1;
1200 if( temp.neighbors[1][1][k] )
1201 {
1202 if( !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren();
1203 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)];
1204 }
1205
1206 // Set the neighbors from across the edges
1207 i=x1<<1 , j=y1<<1;
1208 if( temp.neighbors[i][j][1] )
1209 {
1210 if( !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren();
1211 for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1212 }
1213 i=x1<<1 , k=z1<<1;
1214 if( temp.neighbors[i][1][k] )
1215 {
1216 if( !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren();
1217 for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1218 }
1219 j=y1<<1 , k=z1<<1;
1220 if( temp.neighbors[1][j][k] )
1221 {
1222 if( !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren();
1223 for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1224 }
1225
1226 // Set the neighbor from across the corner
1227 i=x1<<1 , j=y1<<1 , k=z1<<1;
1228 if( temp.neighbors[i][j][k] )
1229 {
1230 if( !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren();
1231 neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1232 }
1233 }
1234 }
1235 return neighbors[d];
1236 }
1237
1238 template< class NodeData , class Real >
1239 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors( OctNode<NodeData,Real>* root , Point3D< Real > p , int d )
1240 {
1241 if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) )
1242 {
1243 neighbors[d].clear();
1244
1245 if( !d ) neighbors[d].neighbors[1][1][1] = root;
1246 else
1247 {
1248 Neighbors3& temp = getNeighbors( root , p , d-1 );
1249
1250 int i , j , k , x1 , y1 , z1 , x2 , y2 , z2;
1251 Point3D< Real > c;
1252 Real w;
1253 temp.neighbors[1][1][1]->centerAndWidth( c , w );
1254 int idx = CornerIndex( c , p );
1255 Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1256 Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1257
1258 if( !temp.neighbors[1][1][1] || !temp.neighbors[1][1][1]->children )
1259 {
1260 POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Couldn't find node at appropriate depth");
1261 }
1262 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1263 neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)];
1264
1265
1266 // Set the neighbors from across the faces
1267 i=x1<<1;
1268 if( temp.neighbors[i][1][1] )
1269 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];
1270 j=y1<<1;
1271 if( temp.neighbors[1][j][1] )
1272 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)];
1273 k=z1<<1;
1274 if( temp.neighbors[1][1][k] )
1275 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)];
1276
1277 // Set the neighbors from across the edges
1278 i=x1<<1 , j=y1<<1;
1279 if( temp.neighbors[i][j][1] )
1280 for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1281 i=x1<<1 , k=z1<<1;
1282 if( temp.neighbors[i][1][k] )
1283 for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1284 j=y1<<1 , k=z1<<1;
1285 if( temp.neighbors[1][j][k] )
1286 for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1287
1288 // Set the neighbor from across the corner
1289 i=x1<<1 , j=y1<<1 , k=z1<<1;
1290 if( temp.neighbors[i][j][k] )
1291 neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1292 }
1293 }
1294 return neighbors[d];
1295 }
1296
1297 template< class NodeData , class Real >
1298 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* node )
1299 {
1300 int d = node->depth();
1301 if( node==neighbors[d].neighbors[1][1][1] )
1302 {
1303 bool reset = false;
1304 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;
1305 if( reset ) neighbors[d].neighbors[1][1][1] = NULL;
1306 }
1307 if( node!=neighbors[d].neighbors[1][1][1] )
1308 {
1309 neighbors[d].clear();
1310
1311 if( !node->parent ) neighbors[d].neighbors[1][1][1] = node;
1312 else
1313 {
1314 int i,j,k,x1,y1,z1,x2,y2,z2;
1315 int idx=int(node-node->parent->children);
1316 Cube::FactorCornerIndex( idx ,x1,y1,z1);
1317 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1318 for(i=0;i<2;i++){
1319 for(j=0;j<2;j++){
1320 for(k=0;k<2;k++){
1321 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1322 }
1323 }
1324 }
1325 Neighbors3& temp=setNeighbors(node->parent);
1326
1327 // Set the neighbors from across the faces
1328 i=x1<<1;
1329 if(temp.neighbors[i][1][1]){
1330 if(!temp.neighbors[i][1][1]->children){temp.neighbors[i][1][1]->initChildren();}
1331 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)];}}
1332 }
1333 j=y1<<1;
1334 if(temp.neighbors[1][j][1]){
1335 if(!temp.neighbors[1][j][1]->children){temp.neighbors[1][j][1]->initChildren();}
1336 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)];}}
1337 }
1338 k=z1<<1;
1339 if(temp.neighbors[1][1][k]){
1340 if(!temp.neighbors[1][1][k]->children){temp.neighbors[1][1][k]->initChildren();}
1341 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)];}}
1342 }
1343
1344 // Set the neighbors from across the edges
1345 i=x1<<1; j=y1<<1;
1346 if(temp.neighbors[i][j][1]){
1347 if(!temp.neighbors[i][j][1]->children){temp.neighbors[i][j][1]->initChildren();}
1348 for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1349 }
1350 i=x1<<1; k=z1<<1;
1351 if(temp.neighbors[i][1][k]){
1352 if(!temp.neighbors[i][1][k]->children){temp.neighbors[i][1][k]->initChildren();}
1353 for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1354 }
1355 j=y1<<1; k=z1<<1;
1356 if(temp.neighbors[1][j][k]){
1357 if(!temp.neighbors[1][j][k]->children){temp.neighbors[1][j][k]->initChildren();}
1358 for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1359 }
1360
1361 // Set the neighbor from across the corner
1362 i=x1<<1; j=y1<<1; k=z1<<1;
1363 if(temp.neighbors[i][j][k]){
1364 if(!temp.neighbors[i][j][k]->children){temp.neighbors[i][j][k]->initChildren();}
1365 neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1366 }
1367 }
1368 }
1369 return neighbors[d];
1370 }
1371
1372 // Note the assumption is that if you enable an edge, you also enable adjacent faces.
1373 // And, if you enable a corner, you enable adjacent edges and faces.
1374 template< class NodeData , class Real >
1375 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::setNeighbors( OctNode<NodeData,Real>* node , bool flags[3][3][3] )
1376 {
1377 int d = node->depth();
1378 if( node==neighbors[d].neighbors[1][1][1] )
1379 {
1380 bool reset = false;
1381 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;
1382 if( reset ) neighbors[d].neighbors[1][1][1] = NULL;
1383 }
1384 if( node!=neighbors[d].neighbors[1][1][1] )
1385 {
1386 neighbors[d].clear();
1387
1388 if( !node->parent ) neighbors[d].neighbors[1][1][1] = node;
1389 else
1390 {
1391 int x1,y1,z1,x2,y2,z2;
1392 int idx=int(node-node->parent->children);
1393 Cube::FactorCornerIndex( idx ,x1,y1,z1);
1394 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1395 for( int i=0 ; i<2 ; i++ )
1396 for( int j=0 ; j<2 ; j++ )
1397 for( int k=0 ; k<2 ; k++ )
1398 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1399
1400 Neighbors3& temp=setNeighbors( node->parent , flags );
1401
1402 // Set the neighbors from across the faces
1403 {
1404 int i=x1<<1;
1405 if( temp.neighbors[i][1][1] )
1406 {
1407 if( flags[i][1][1] && !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren();
1408 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)];
1409 }
1410 }
1411 {
1412 int j = y1<<1;
1413 if( temp.neighbors[1][j][1] )
1414 {
1415 if( flags[1][j][1] && !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren();
1416 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)];
1417 }
1418 }
1419 {
1420 int k = z1<<1;
1421 if( temp.neighbors[1][1][k] )
1422 {
1423 if( flags[1][1][k] && !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren();
1424 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)];
1425 }
1426 }
1427
1428 // Set the neighbors from across the edges
1429 {
1430 int i=x1<<1 , j=y1<<1;
1431 if( temp.neighbors[i][j][1] )
1432 {
1433 if( flags[i][j][1] && !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren();
1434 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)];
1435 }
1436 }
1437 {
1438 int i=x1<<1 , k=z1<<1;
1439 if( temp.neighbors[i][1][k] )
1440 {
1441 if( flags[i][1][k] && !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren();
1442 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)];
1443 }
1444 }
1445 {
1446 int j=y1<<1 , k=z1<<1;
1447 if( temp.neighbors[1][j][k] )
1448 {
1449 if( flags[1][j][k] && !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren();
1450 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)];
1451 }
1452 }
1453
1454 // Set the neighbor from across the corner
1455 {
1456 int i=x1<<1 , j=y1<<1 , k=z1<<1;
1457 if( temp.neighbors[i][j][k] )
1458 {
1459 if( flags[i][j][k] && !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren();
1460 if( temp.neighbors[i][j][k]->children ) neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1461 }
1462 }
1463 }
1464 }
1465 return neighbors[d];
1466 }
1467
1468 template<class NodeData,class Real>
1469 typename OctNode<NodeData,Real>::Neighbors3& OctNode<NodeData,Real>::NeighborKey3::getNeighbors(OctNode<NodeData,Real>* node){
1470 int d=node->depth();
1471 if(node!=neighbors[d].neighbors[1][1][1]){
1472 neighbors[d].clear();
1473
1474 if(!node->parent){neighbors[d].neighbors[1][1][1]=node;}
1475 else{
1476 int i,j,k,x1,y1,z1,x2,y2,z2;
1477 int idx=int(node-node->parent->children);
1478 Cube::FactorCornerIndex( idx ,x1,y1,z1);
1479 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1480 for(i=0;i<2;i++){
1481 for(j=0;j<2;j++){
1482 for(k=0;k<2;k++){
1483 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1484 }
1485 }
1486 }
1487 Neighbors3& temp=getNeighbors(node->parent);
1488
1489 // Set the neighbors from across the faces
1490 i=x1<<1;
1491 if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){
1492 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)];}}
1493 }
1494 j=y1<<1;
1495 if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){
1496 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)];}}
1497 }
1498 k=z1<<1;
1499 if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){
1500 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)];}}
1501 }
1502
1503 // Set the neighbors from across the edges
1504 i=x1<<1; j=y1<<1;
1505 if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){
1506 for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1507 }
1508 i=x1<<1; k=z1<<1;
1509 if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){
1510 for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1511 }
1512 j=y1<<1; k=z1<<1;
1513 if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){
1514 for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1515 }
1516
1517 // Set the neighbor from across the corner
1518 i=x1<<1; j=y1<<1; k=z1<<1;
1519 if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){
1520 neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1521 }
1522 }
1523 }
1524 return neighbors[node->depth()];
1525 }
1526
1527 ///////////////////////
1528 // ConstNeighborKey3 //
1529 ///////////////////////
1530 template<class NodeData,class Real>
1532
1533 template<class NodeData,class Real>
1535 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;}}}
1536 }
1537
1538 template<class NodeData,class Real>
1540
1541 template<class NodeData,class Real>
1543 delete[] neighbors;
1544 neighbors=NULL;
1545 }
1546
1547 template<class NodeData,class Real>
1549 delete[] neighbors;
1550 neighbors=NULL;
1551 if(d<0){return;}
1552 neighbors=new ConstNeighbors3[d+1];
1553 }
1554
1555 template<class NodeData,class Real>
1557 int d=node->depth();
1558 if(node!=neighbors[d].neighbors[1][1][1]){
1559 neighbors[d].clear();
1560
1561 if(!node->parent){neighbors[d].neighbors[1][1][1]=node;}
1562 else{
1563 int i,j,k,x1,y1,z1,x2,y2,z2;
1564 int idx=int(node-node->parent->children);
1565 Cube::FactorCornerIndex( idx ,x1,y1,z1);
1566 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1567 for(i=0;i<2;i++){
1568 for(j=0;j<2;j++){
1569 for(k=0;k<2;k++){
1570 neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)];
1571 }
1572 }
1573 }
1574 ConstNeighbors3& temp=getNeighbors(node->parent);
1575
1576 // Set the neighbors from across the faces
1577 i=x1<<1;
1578 if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){
1579 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)];}}
1580 }
1581 j=y1<<1;
1582 if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){
1583 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)];}}
1584 }
1585 k=z1<<1;
1586 if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){
1587 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)];}}
1588 }
1589
1590 // Set the neighbors from across the edges
1591 i=x1<<1; j=y1<<1;
1592 if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){
1593 for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];}
1594 }
1595 i=x1<<1; k=z1<<1;
1596 if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){
1597 for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];}
1598 }
1599 j=y1<<1; k=z1<<1;
1600 if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){
1601 for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];}
1602 }
1603
1604 // Set the neighbor from across the corner
1605 i=x1<<1; j=y1<<1; k=z1<<1;
1606 if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){
1607 neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1608 }
1609 }
1610 }
1611 return neighbors[node->depth()];
1612 }
1613
1614 template<class NodeData,class Real>
1615 typename OctNode<NodeData,Real>::ConstNeighbors3& OctNode<NodeData,Real>::ConstNeighborKey3::getNeighbors( const OctNode<NodeData,Real>* node , int minDepth )
1616 {
1617 int d=node->depth();
1618 if (d < minDepth)
1619 {
1620 POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Node depth lower than min-depth: (actual)" << d << " < (minimum)" << minDepth);
1621 }
1622 if( node!=neighbors[d].neighbors[1][1][1] )
1623 {
1624 neighbors[d].clear();
1625
1626 if( d==minDepth ) neighbors[d].neighbors[1][1][1]=node;
1627 else
1628 {
1629 int i,j,k,x1,y1,z1,x2,y2,z2;
1630 int idx = int(node-node->parent->children);
1631 Cube::FactorCornerIndex( idx ,x1,y1,z1);
1632 Cube::FactorCornerIndex((~idx)&7,x2,y2,z2);
1633
1634 ConstNeighbors3& temp=getNeighbors( node->parent , minDepth );
1635
1636 // Set the syblings
1637 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1638 neighbors[d].neighbors[x2+i][y2+j][z2+k] = &node->parent->children[ Cube::CornerIndex(i,j,k) ];
1639
1640 // Set the neighbors from across the faces
1641 i=x1<<1;
1642 if( temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children )
1643 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)];
1644
1645 j=y1<<1;
1646 if( temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children )
1647 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)];
1648
1649 k=z1<<1;
1650 if( temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children )
1651 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)];
1652
1653 // Set the neighbors from across the edges
1654 i=x1<<1 , j=y1<<1;
1655 if( temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children )
1656 for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];
1657
1658 i=x1<<1 , k=z1<<1;
1659 if( temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children )
1660 for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];
1661
1662 j=y1<<1 , k=z1<<1;
1663 if( temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children )
1664 for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];
1665
1666 // Set the neighbor from across the corner
1667 i=x1<<1 , j=y1<<1 , k=z1<<1;
1668 if( temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children )
1669 neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)];
1670 }
1671 }
1672 return neighbors[node->depth()];
1673 }
1674
1675 template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); }
1676
1677 template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); }
1678
1679 template< class NodeData , class Real >
1681 {
1682 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;
1683 }
1684
1685 template< class NodeData , class Real >
1687 {
1688 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;
1689 }
1690
1691 template< class NodeData , class Real >
1693 {
1694 _depth = -1;
1695 neighbors = NULL;
1696 }
1697
1698 template< class NodeData , class Real >
1700 {
1701 _depth = -1;
1702 neighbors = NULL;
1703 }
1704
1705 template< class NodeData , class Real >
1707 {
1708 delete[] neighbors;
1709 neighbors = NULL;
1710 }
1711
1712 template< class NodeData , class Real >
1714 {
1715 delete[] neighbors;
1716 neighbors = NULL;
1717 }
1718
1719 template< class NodeData , class Real >
1721 {
1722 delete[] neighbors;
1723 neighbors = NULL;
1724 if(d<0) return;
1725 _depth = d;
1726 neighbors=new Neighbors5[d+1];
1727 }
1728
1729 template< class NodeData , class Real >
1731 {
1732 delete[] neighbors;
1733 neighbors = NULL;
1734 if(d<0) return;
1735 _depth = d;
1736 neighbors=new ConstNeighbors5[d+1];
1737 }
1738
1739 template< class NodeData , class Real >
1741 {
1742 int d=node->depth();
1743 if( node!=neighbors[d].neighbors[2][2][2] )
1744 {
1745 neighbors[d].clear();
1746
1747 if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1748 else
1749 {
1750 getNeighbors( node->parent );
1751 Neighbors5& temp = neighbors[d-1];
1752 int x1 , y1 , z1 , x2 , y2 , z2;
1753 int idx = int( node - node->parent->children );
1754 Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1755
1756 Neighbors5& n = neighbors[d];
1757 Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 );
1758 int i , j , k;
1759 int fx0 = x2+1 , fy0 = y2+1 , fz0 = z2+1; // Indices of the bottom left corner of the parent within the 5x5x5
1760 int cx1 = x1*2+1 , cy1 = y1*2+1 , cz1 = z1*2+1;
1761 int cx2 = x2*2+1 , cy2 = y2*2+1 , cz2 = z2*2+1;
1762 int fx1 = x1*3 , fy1 = y1*3 , fz1 = z1*3;
1763 int fx2 = x2*4 , fy2 = y2*4 , fz2 = z2*4;
1764
1765 // Set the syblings
1766 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1767 n.neighbors[fx0+i][fy0+j][fz0+k] = node->parent->children + Cube::CornerIndex( i , j , k );
1768
1769 // Set the neighbors from across the faces
1770 if( temp.neighbors[cx1][2][2] && temp.neighbors[cx1][2][2]->children )
1771 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1772 n.neighbors[fx1+i][fy0+j][fz0+k] = temp.neighbors[cx1][2][2]->children + Cube::CornerIndex( i , j , k );
1773 if( temp.neighbors[2][cy1][2] && temp.neighbors[2][cy1][2]->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][fy1+j][fz0+k] = temp.neighbors[2][cy1][2]->children + Cube::CornerIndex( i , j , k );
1776 if( temp.neighbors[2][2][cz1] && temp.neighbors[2][2][cz1]->children )
1777 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1778 n.neighbors[fx0+i][fy0+j][fz1+k] = temp.neighbors[2][2][cz1]->children + Cube::CornerIndex( i , j , k );
1779 if( temp.neighbors[cx2][2][2] && temp.neighbors[cx2][2][2]->children )
1780 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1781 n.neighbors[fx2 ][fy0+j][fz0+k] = temp.neighbors[cx2][2][2]->children + Cube::CornerIndex( x1 , j , k );
1782 if( temp.neighbors[2][cy2][2] && temp.neighbors[2][cy2][2]->children )
1783 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1784 n.neighbors[fx0+i][fy2 ][fz0+k] = temp.neighbors[2][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1785 if( temp.neighbors[2][2][cz2] && temp.neighbors[2][2][cz2]->children )
1786 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1787 n.neighbors[fx0+i][fy0+j][fz2 ] = temp.neighbors[2][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1788
1789 // Set the neighbors from across the edges
1790 if( temp.neighbors[cx1][cy1][2] && temp.neighbors[cx1][cy1][2]->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][fy1+j][fz0+k] = temp.neighbors[cx1][cy1][2]->children + Cube::CornerIndex( i , j , k );
1793 if( temp.neighbors[cx1][2][cz1] && temp.neighbors[cx1][2][cz1]->children )
1794 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1795 n.neighbors[fx1+i][fy0+j][fz1+k] = temp.neighbors[cx1][2][cz1]->children + Cube::CornerIndex( i , j , k );
1796 if( temp.neighbors[2][cy1][cz1] && temp.neighbors[2][cy1][cz1]->children )
1797 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1798 n.neighbors[fx0+i][fy1+j][fz1+k] = temp.neighbors[2][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1799 if( temp.neighbors[cx1][cy2][2] && temp.neighbors[cx1][cy2][2]->children )
1800 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1801 n.neighbors[fx1+i][fy2 ][fz0+k] = temp.neighbors[cx1][cy2][2]->children + Cube::CornerIndex( i , y1 , k );
1802 if( temp.neighbors[cx1][2][cz2] && temp.neighbors[cx1][2][cz2]->children )
1803 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1804 n.neighbors[fx1+i][fy0+j][fz2 ] = temp.neighbors[cx1][2][cz2]->children + Cube::CornerIndex( i , j , z1 );
1805 if( temp.neighbors[cx2][cy1][2] && temp.neighbors[cx2][cy1][2]->children )
1806 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1807 n.neighbors[fx2 ][fy1+j][fz0+k] = temp.neighbors[cx2][cy1][2]->children + Cube::CornerIndex( x1 , j , k );
1808 if( temp.neighbors[2][cy1][cz2] && temp.neighbors[2][cy1][cz2]->children )
1809 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1810 n.neighbors[fx0+i][fy1+j][fz2 ] = temp.neighbors[2][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1811 if( temp.neighbors[cx2][2][cz1] && temp.neighbors[cx2][2][cz1]->children )
1812 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1813 n.neighbors[fx2 ][fy0+j][fz1+k] = temp.neighbors[cx2][2][cz1]->children + Cube::CornerIndex( x1 , j , k );
1814 if( temp.neighbors[2][cy2][cz1] && temp.neighbors[2][cy2][cz1]->children )
1815 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1816 n.neighbors[fx0+i][fy2 ][fz1+k] = temp.neighbors[2][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1817 if( temp.neighbors[cx2][cy2][2] && temp.neighbors[cx2][cy2][2]->children )
1818 for( k=0 ; k<2 ; k++ )
1819 n.neighbors[fx2 ][fy2 ][fz0+k] = temp.neighbors[cx2][cy2][2]->children + Cube::CornerIndex( x1 , y1 , k );
1820 if( temp.neighbors[cx2][2][cz2] && temp.neighbors[cx2][2][cz2]->children )
1821 for( j=0 ; j<2 ; j++ )
1822 n.neighbors[fx2 ][fy0+j][fz2 ] = temp.neighbors[cx2][2][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1823 if( temp.neighbors[2][cy2][cz2] && temp.neighbors[2][cy2][cz2]->children )
1824 for( i=0 ; i<2 ; i++ )
1825 n.neighbors[fx0+i][fy2 ][fz2 ] = temp.neighbors[2][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1826
1827 // Set the neighbor from across the corners
1828 if( temp.neighbors[cx1][cy1][cz1] && temp.neighbors[cx1][cy1][cz1]->children )
1829 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1830 n.neighbors[fx1+i][fy1+j][fz1+k] = temp.neighbors[cx1][cy1][cz1]->children + Cube::CornerIndex( i , j , k );
1831 if( temp.neighbors[cx1][cy1][cz2] && temp.neighbors[cx1][cy1][cz2]->children )
1832 for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ )
1833 n.neighbors[fx1+i][fy1+j][fz2 ] = temp.neighbors[cx1][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 );
1834 if( temp.neighbors[cx1][cy2][cz1] && temp.neighbors[cx1][cy2][cz1]->children )
1835 for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ )
1836 n.neighbors[fx1+i][fy2 ][fz1+k] = temp.neighbors[cx1][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k );
1837 if( temp.neighbors[cx2][cy1][cz1] && temp.neighbors[cx2][cy1][cz1]->children )
1838 for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ )
1839 n.neighbors[fx2 ][fy1+j][fz1+k] = temp.neighbors[cx2][cy1][cz1]->children + Cube::CornerIndex( x1 , j , k );
1840 if( temp.neighbors[cx1][cy2][cz2] && temp.neighbors[cx1][cy2][cz2]->children )
1841 for( i=0 ; i<2 ; i++ )
1842 n.neighbors[fx1+i][fy2 ][fz2 ] = temp.neighbors[cx1][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 );
1843 if( temp.neighbors[cx2][cy1][cz2] && temp.neighbors[cx2][cy1][cz2]->children )
1844 for( j=0 ; j<2 ; j++ )
1845 n.neighbors[fx2 ][fy1+j][fz2 ] = temp.neighbors[cx2][cy1][cz2]->children + Cube::CornerIndex( x1 , j , z1 );
1846 if( temp.neighbors[cx2][cy2][cz1] && temp.neighbors[cx2][cy2][cz1]->children )
1847 for( k=0 ; k<2 ; k++ )
1848 n.neighbors[fx2 ][fy2 ][fz1+k] = temp.neighbors[cx2][cy2][cz1]->children + Cube::CornerIndex( x1 , y1 , k );
1849 if( temp.neighbors[cx2][cy2][cz2] && temp.neighbors[cx2][cy2][cz2]->children )
1850 n.neighbors[fx2 ][fy2 ][fz2 ] = temp.neighbors[cx2][cy2][cz2]->children + Cube::CornerIndex( x1 , y1 , z1 );
1851 }
1852 }
1853 return neighbors[d];
1854 }
1855
1856 template< class NodeData , class Real >
1857 typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd )
1858 {
1859 int d=node->depth();
1860 if( node!=neighbors[d].neighbors[2][2][2] )
1861 {
1862 neighbors[d].clear();
1863
1864 if( !node->parent ) neighbors[d].neighbors[2][2][2]=node;
1865 else
1866 {
1867 setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1868 Neighbors5& temp = neighbors[d-1];
1869 int x1 , y1 , z1 , x2 , y2 , z2 , ii , jj , kk;
1870 int idx = int( node-node->parent->children );
1871 Cube::FactorCornerIndex( idx , x1 , y1 , z1 );
1872
1873 for( int i=xStart ; i<xEnd ; i++ )
1874 {
1875 x2 = i+x1;
1876 ii = x2&1;
1877 x2 = 1+(x2>>1);
1878 for( int j=yStart ; j<yEnd ; j++ )
1879 {
1880 y2 = j+y1;
1881 jj = y2&1;
1882 y2 = 1+(y2>>1);
1883 for( int k=zStart ; k<zEnd ; k++ )
1884 {
1885 z2 = k+z1;
1886 kk = z2&1;
1887 z2 = 1+(z2>>1);
1888 if(temp.neighbors[x2][y2][z2] )
1889 {
1890 if( !temp.neighbors[x2][y2][z2]->children ) temp.neighbors[x2][y2][z2]->initChildren();
1891 neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1892 }
1893 }
1894 }
1895 }
1896 }
1897 }
1898 return neighbors[d];
1899 }
1900
1901 template< class NodeData , class Real >
1903 {
1904 int d=node->depth();
1905 if( node!=neighbors[d].neighbors[2][2][2] )
1906 {
1907 neighbors[d].clear();
1908
1909 if(!node->parent) neighbors[d].neighbors[2][2][2]=node;
1910 else
1911 {
1912 getNeighbors( node->parent );
1913 ConstNeighbors5& temp = neighbors[d-1];
1914 int x1,y1,z1,x2,y2,z2,ii,jj,kk;
1915 int idx=int(node-node->parent->children);
1916 Cube::FactorCornerIndex(idx,x1,y1,z1);
1917
1918 for(int i=0;i<5;i++)
1919 {
1920 x2=i+x1;
1921 ii=x2&1;
1922 x2=1+(x2>>1);
1923 for(int j=0;j<5;j++)
1924 {
1925 y2=j+y1;
1926 jj=y2&1;
1927 y2=1+(y2>>1);
1928 for(int k=0;k<5;k++)
1929 {
1930 z2=k+z1;
1931 kk=z2&1;
1932 z2=1+(z2>>1);
1933 if(temp.neighbors[x2][y2][z2] && temp.neighbors[x2][y2][z2]->children)
1934 neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk);
1935 }
1936 }
1937 }
1938 }
1939 }
1940 return neighbors[d];
1941 }
1942
1943 template <class NodeData,class Real>
1944 int OctNode<NodeData,Real>::write(const char* fileName) const{
1945 FILE* fp=fopen(fileName,"wb");
1946 if(!fp){return 0;}
1947 int ret=write(fp);
1948 fclose(fp);
1949 return ret;
1950 }
1951
1952 template <class NodeData,class Real>
1954 fwrite(this,sizeof(OctNode<NodeData,Real>),1,fp);
1955 if(children){for(int i=0;i<Cube::CORNERS;i++){children[i].write(fp);}}
1956 return 1;
1957 }
1958
1959 template <class NodeData,class Real>
1960 int OctNode<NodeData,Real>::read(const char* fileName){
1961 FILE* fp=fopen(fileName,"rb");
1962 if(!fp){return 0;}
1963 int ret=read(fp);
1964 fclose(fp);
1965 return ret;
1966 }
1967
1968 template <class NodeData,class Real>
1970 fread(this,sizeof(OctNode<NodeData,Real>),1,fp);
1971 parent=NULL;
1972 if(children){
1973 children=NULL;
1974 initChildren();
1975 for(int i=0;i<Cube::CORNERS;i++){
1976 children[i].read(fp);
1977 children[i].parent=this;
1978 }
1979 }
1980 return 1;
1981 }
1982
1983 template<class NodeData,class Real>
1985 int d=depth();
1986 return 1<<(maxDepth-d);
1987 }
1988
1989 template<class NodeData,class Real>
1990 void OctNode<NodeData,Real>::centerIndex(int maxDepth,int index[DIMENSION]) const {
1991 int d,o[3];
1992 depthAndOffset(d,o);
1993 for(int i=0;i<DIMENSION;i++){index[i]=BinaryNode<Real>::CornerIndex(maxDepth,d+1,o[i]<<1,1);}
1994 }
1995 }
1996}
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)
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
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
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])