00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef VCG_SPACE_INDEX_OCTREETEMPLATE_H
00026 #define VCG_SPACE_INDEX_OCTREETEMPLATE_H
00027
00028 #include <vcg/space/point3.h>
00029 #include <vcg/space/box3.h>
00030 #include <vector>
00031
00032
00033 namespace vcg
00034 {
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 template <typename VOXEL_TYPE, class SCALAR_TYPE>
00052 class OctreeTemplate
00053 {
00054 protected:
00055 struct Node;
00056
00057 public:
00058
00059 typedef unsigned long long ZOrderType;
00060 typedef SCALAR_TYPE ScalarType;
00061 typedef VOXEL_TYPE VoxelType;
00062 typedef VoxelType * VoxelPointer;
00063 typedef vcg::Point3i CenterType;
00064 static const ScalarType EXPANSION_FACTOR;
00065 typedef Node NodeType;
00066 typedef int NodeIndex;
00067 typedef NodeType * NodePointer;
00068 typedef vcg::Box3<ScalarType> BoundingBoxType;
00069 typedef vcg::Point3<ScalarType> CoordinateType;
00070
00071 protected:
00072
00073
00074
00075
00076 struct Node
00077 {
00078
00079 Node()
00080 {
00081 parent = NULL;
00082 level = -1;
00083 }
00084
00085
00086 Node(NodePointer parent, int level)
00087 {
00088 this->parent = parent;
00089 this->level = (char) level;
00090 }
00091
00092 virtual NodePointer &Son(int sonIndex) = 0;
00093
00094 virtual bool IsLeaf() = 0;
00095
00096
00097
00098 CenterType center;
00099 char level;
00100 NodePointer parent;
00101 VoxelType voxel;
00102 };
00103
00104
00105
00106
00107 struct InnerNode : public Node
00108 {
00109 InnerNode() : Node() {};
00110 InnerNode(NodePointer parent, int level) : Node(parent, level)
00111 {
00112 memset(&sons[0], 0, 8*sizeof(Node*));
00113 }
00114
00115 inline NodePointer &Son(int sonIndex)
00116 {
00117 assert(0<=sonIndex && sonIndex<=8);
00118 return sons[sonIndex];
00119 }
00120
00121 inline bool IsLeaf()
00122 {
00123 return false;
00124 }
00125
00126 NodePointer sons[8];
00127 };
00128
00129
00130
00131
00132 struct Leaf : public Node
00133 {
00134 Leaf() : Node() {};
00135 Leaf(NodePointer parent, int level) : Node(parent, level) {}
00136
00137 inline NodePointer &Son(int )
00138 {
00139 assert(false);
00140 static NodePointer p = NULL;
00141 return p;
00142 }
00143
00144 inline bool IsLeaf()
00145 {
00146 return true;
00147 }
00148 };
00149
00150 public:
00151
00152 void Initialize(int maximumDepth)
00153 {
00154 this->maximumDepth = maximumDepth;
00155 size = 1<< maximumDepth;
00156 lSize = 1<<(maximumDepth+1);
00157
00158 InnerNode *root = new InnerNode(NULL,0);
00159 nodes.clear();
00160 nodes.push_back( root );
00161 root->center = CenterType(size, size, size);
00162
00163 ScalarType szf = (ScalarType) size;
00164 leafDimension = boundingBox.Dim();
00165 leafDimension /= szf;
00166 leafDiagonal = leafDimension.Norm();
00167 };
00168
00169
00170 inline BoundingBoxType BoundingBox() { return boundingBox; }
00171
00172
00173 inline VoxelPointer Voxel(const NodePointer n) { return &(n->voxel); }
00174
00175
00176 inline int NodeCount() const { return int(nodes.size()); }
00177
00178
00179 inline const NodePointer Root() const { return nodes[0]; }
00180
00181
00182 inline char Level(const NodePointer n) const { return n->level; }
00183
00184
00185 inline NodePointer& Son(NodePointer n, int i) const { return n->Son(i); }
00186
00187
00188 inline NodePointer Parent(const NodePointer n) const { return n->parent; }
00189
00190
00191 int WhatSon(NodePointer n) const
00192 {
00193 if(n==Root())
00194 assert(false);
00195
00196 NodePointer parent = Parent(n);
00197 for(int i=0;i<8;++i)
00198 if(parent->Son(i)==n)
00199 return i;
00200
00201 return -1;
00202 }
00203
00204
00205 inline CenterType CenterInOctreeCoordinates(const NodePointer n) const { return n->center;}
00206
00211 inline void CenterInWorldCoordinates(const NodePointer n, CoordinateType &wc_Center) const
00212 {
00213 assert(0<=n && n<NodeCount());
00214
00215 int shift = maximumDepth - Level(n) + 1;
00216 CoordinateType ocCenter = CenterInOctreeCoordinates(n);
00217 CoordinateType nodeSize = boundingBox.Dim()/float(1<<Level(n));
00218 wc_Center.X() = boundingBox.min.X() + (nodeSize.X()*(0.5f+(ocCenter.X()>>shift)));
00219 wc_Center.Y() = boundingBox.min.Y() + (nodeSize.Y()*(0.5f+(ocCenter.Y()>>shift)));
00220 wc_Center.Z() = boundingBox.min.Z() + (nodeSize.Z()*(0.5f+(ocCenter.Z()>>shift)));
00221 };
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273 CoordinateType Center(NodePointer n) const
00274 {
00275 CoordinateType center;
00276 center.Import(GetPath(n));
00277 center+=Point3f(.5f,.5f,.5f);
00278
00279
00280 assert(center==nodes[n]->center);
00281
00282 return center;
00283 }
00284
00285
00286 BoundingBoxType BoundingBoxInWorldCoordinates(const NodePointer n)
00287 {
00288 char level = Level(n);
00289 int shift = maximumDepth-level+1;
00290 CoordinateType nodeDim = boundingBox.Dim()/float(1<<level);
00291 CenterType center = CenterInOctreeCoordinates(n);
00292 BoundingBoxType nodeBB;
00293 nodeBB.min.X() = boundingBox.min.X() + (nodeDim.X()*(center.X()>>shift));
00294 nodeBB.min.Y() = boundingBox.min.Y() + (nodeDim.Y()*(center.Y()>>shift));
00295 nodeBB.min.Z() = boundingBox.min.Z() + (nodeDim.Z()*(center.Z()>>shift));
00296 nodeBB.max = nodeBB.min+nodeDim;
00297 return nodeBB;
00298 };
00299
00305 inline void BoundingBoxInWorldCoordinates(const NodePointer n, BoundingBoxType &wc_bb) const
00306 {
00307 char level = Level(n);
00308 int shift = maximumDepth - level + 1;
00309 CoordinateType node_dimension = boundingBox.Dim()/ScalarType(1<<level);
00310 wc_bb.min.X() = boundingBox.min.X()+(node_dimension.X()*(n->center.X()>>shift));
00311 wc_bb.min.Y() = boundingBox.min.Y()+(node_dimension.Y()*(n->center.Y()>>shift));
00312 wc_bb.min.Z() = boundingBox.min.Z()+(node_dimension.Z()*(n->center.Z()>>shift));
00313 wc_bb.max = wc_bb.min+node_dimension;
00314 };
00315
00316
00317 BoundingBoxType SubBox(BoundingBoxType &lbb, int i)
00318 {
00319 BoundingBoxType bs;
00320 if (i&1) bs.min.X()=(lbb.min.X()+(bs.max.X()=lbb.max.X()))/2.0f;
00321 else bs.max.X()=((bs.min.X()=lbb.min.X())+lbb.max.X())/2.0f;
00322 if (i&2) bs.min.Y()=(lbb.min.Y()+(bs.max.Y()=lbb.max.Y()))/2.0f;
00323 else bs.max.Y()=((bs.min.Y()=lbb.min.Y())+lbb.max.Y())/2.0f;
00324 if (i&4) bs.min.Z()=(lbb.min.Z()+(bs.max.Z()=lbb.max.Z()))/2.0f;
00325 else bs.max.Z()=((bs.min.Z()=lbb.min.Z())+lbb.max.Z())/2.0f;
00326
00327 return bs;
00328 }
00329
00330
00331
00332 BoundingBoxType SubBoxAndCenterInWorldCoordinates(BoundingBoxType &lbb, CoordinateType ¢er, int i)
00333 {
00334 BoundingBoxType bs;
00335 if (i&1)
00336 {
00337 bs.min[0]=center[0];
00338 bs.max[0]=lbb.max[0];
00339 }
00340 else
00341 {
00342 bs.min[0]=lbb.min[0];
00343 bs.max[0]=center[0];
00344 }
00345 if (i&2)
00346 {
00347 bs.min[1]=center[1];
00348 bs.max[1]=lbb.max[1];
00349 }
00350 else
00351 {
00352 bs.max[1]=center[1];
00353 bs.min[1]=lbb.min[1];
00354 }
00355 if (i&4)
00356 {
00357 bs.min[2]=center[2];
00358 bs.max[2]=lbb.max[2];
00359 }
00360 else
00361 {
00362 bs.max[2]=center[2];
00363 bs.min[2]=lbb.min[2];
00364 }
00365 return bs;
00366 };
00367
00368
00369
00370
00371
00372
00373 NodePointer NewNode(NodePointer parent, int i)
00374 {
00375 assert(0<=i && i<8);
00376 assert(Son(parent, i)==NULL);
00377
00378
00379 char level = Level(parent)+1;
00380
00381 Node *node = (level<maximumDepth)? (Node*) new InnerNode(parent, level) : (Node*) new Leaf(parent, level);
00382 nodes.push_back( node );
00383 Son(parent, i) = node;
00384
00385 CenterType *parentCenter = &(parent->center);
00386 int displacement = 1<<(maximumDepth-level);
00387 node->center.X() = parentCenter->X() + ((i&1)? displacement : -displacement);
00388 node->center.Y() = parentCenter->Y() + ((i&2)? displacement : -displacement);
00389 node->center.Z() = parentCenter->Z() + ((i&4)? displacement : -displacement);
00390
00391 return node;
00392 }
00393
00394
00395
00396
00397 NodePointer AddNode(CenterType path)
00398 {
00399
00400 assert(path[0]>=0 && path[0]<size);
00401 assert(path[1]>=0 && path[1]<size);
00402 assert(path[2]>=0 && path[2]<size);
00403
00404 NodePointer curNode = Root();
00405 int rootLevel = 0;
00406 int shiftLevel = maximumDepth-1;
00407
00408 while(shiftLevel >= rootLevel)
00409 {
00410 int nextSon=0;
00411 if((path[0]>>shiftLevel)%2) nextSon +=1;
00412 if((path[1]>>shiftLevel)%2) nextSon +=2;
00413 if((path[2]>>shiftLevel)%2) nextSon +=4;
00414 NodePointer nextNode = Son(curNode, nextSon);
00415 if(nextNode!=NULL)
00416 curNode = nextNode;
00417 else
00418 {
00419 NodePointer newNode = NewNode(curNode, nextSon);
00420 assert(Son(curNode, nextSon)==newNode);
00421 curNode=newNode;
00422 }
00423 --shiftLevel;
00424 }
00425 return curNode;
00426 }
00427
00432
00433
00434 CenterType Interize(const CoordinateType &pf) const
00435 {
00436 CenterType pi;
00437
00438 assert(pf.X()>=boundingBox.min.X() && pf.X()<=boundingBox.max.X());
00439 assert(pf.Y()>=boundingBox.min.Y() && pf.Y()<=boundingBox.max.Y());
00440 assert(pf.Z()>=boundingBox.min.Z() && pf.Z()<=boundingBox.max.Z());
00441
00442 pi.X() = int((pf.X() - boundingBox.min.X()) * size / (boundingBox.max.X() - boundingBox.min.X()));
00443 pi.Y() = int((pf.Y() - boundingBox.min.Y()) * size / (boundingBox.max.Y() - boundingBox.min.Y()));
00444 pi.Z() = int((pf.Z() - boundingBox.min.Z()) * size / (boundingBox.max.Z() - boundingBox.min.Z()));
00445
00446 return pi;
00447 }
00448
00449
00450
00451 CoordinateType DeInterize(const CenterType &pi ) const
00452 {
00453 CoordinateType pf;
00454
00455 assert(pi.X()>=0 && pi.X()<size);
00456 assert(pi.Y()>=0 && pi.Y()<size);
00457 assert(pi.Z()>=0 && pi.Z()<size);
00458
00459 pf.X() = pi.X() * (boundingBox.max.X() - boundingBox.min.X()) / size + boundingBox.min.X();
00460 pf.Y() = pi.Y() * (boundingBox.max.Y() - boundingBox.min.Y()) / size + boundingBox.min.Y();
00461 pf.Z() = pi.Z() * (boundingBox.max.Z() - boundingBox.min.Z()) / size + boundingBox.min.Z();
00462
00463 return pf;
00464 }
00465
00466
00467
00468
00469 ZOrderType ZOrder(NodePointer n) const { return ZOrder(GetPath(n), Level(n)); }
00470 ZOrderType ComputeZOrder(const CoordinateType &query) const { return ZOrder(CenterType::Construct(Interize(query)), maximumDepth); };
00471
00472 inline ZOrderType ZOrder(const CenterType &path, const char level) const
00473 {
00474 ZOrderType finalPosition = 0;
00475 ZOrderType currentPosition;
00476
00477 for(int i=0; i<level; ++i)
00478 {
00479 currentPosition = 0;
00480 int mask=1<<i;
00481 if(path[0]&mask) currentPosition|=1;
00482 if(path[1]&mask) currentPosition|=2;
00483 if(path[2]&mask) currentPosition|=4;
00484 currentPosition = currentPosition<<(i*3);
00485 finalPosition |= currentPosition;
00486 }
00487 return finalPosition;
00488 };
00489
00490
00491
00492
00493 NodePointer DeepestNode(CenterType path, int MaxLev)
00494 {
00495 assert(path[0]>=0 && path[0]<size);
00496 assert(path[1]>=0 && path[1]<size);
00497 assert(path[2]>=0 && path[2]<size);
00498
00499 NodePointer curNode = Root();
00500 int shift = maximumDepth-1;
00501
00502 while(shift && Level(curNode) < MaxLev)
00503 {
00504 int son = 0;
00505 if((path[0]>>shift)%2) son +=1;
00506 if((path[1]>>shift)%2) son +=2;
00507 if((path[2]>>shift)%2) son +=4;
00508 NodePointer nextNode = Son(curNode, son);
00509 if(nextNode!=NULL)
00510 curNode=nextNode;
00511 else
00512 break;
00513
00514 --shift;
00515 }
00516 return curNode;
00517 }
00518
00519
00520
00521
00522
00523
00524
00525 CenterType GetPath(NodePointer n) const
00526 {
00527 if(n==Root())
00528 return CenterType(0,0,0);
00529
00530 CenterType path(0,0,0);
00531
00532 int shift, mask, son;
00533 int startingLevel = int(Level(n));
00534 while (n!=Root())
00535 {
00536 shift = startingLevel-Level(n);
00537 mask = 1 << shift;
00538 son = WhatSon(n);
00539 if(son&1) path[0] |= mask;
00540 if(son&2) path[1] |= mask;
00541 if(son&4) path[2] |= mask;
00542 n = Parent(n);
00543 }
00544 return path;
00545 }
00546
00547
00548
00549
00550
00551
00552 ZOrderType BuildRoute(const CoordinateType &p, NodePointer *&route)
00553 {
00554 assert( boundingBox.min.X()<=p.X() && p.X()<=boundingBox.max.X() );
00555 assert( boundingBox.min.Y()<=p.Y() && p.Y()<=boundingBox.max.Y() );
00556 assert( boundingBox.min.Z()<=p.Z() && p.Z()<=boundingBox.max.Z() );
00557
00558 route[0] = Root();
00559 NodePointer curNode = Root();
00560 int shift = maximumDepth-1;
00561 CenterType path = CenterType::Construct(Interize(p));
00562 while(shift >= 0)
00563 {
00564 int son = 0;
00565 if((path[0]>>shift)%2) son +=1;
00566 if((path[1]>>shift)%2) son +=2;
00567 if((path[2]>>shift)%2) son +=4;
00568 NodePointer nextNode = Son(curNode, son);
00569
00570 if(nextNode!=NULL)
00571 {
00572 route[maximumDepth-shift] = nextNode;
00573 curNode = nextNode;
00574 }
00575 else
00576 {
00577 NodePointer newNode = NewNode(curNode, son);
00578 route[maximumDepth-shift] = newNode;
00579 curNode = newNode;
00580 }
00581 --shift;
00582 }
00583 return ZOrder(route[maximumDepth]);
00584 };
00585
00586
00587
00588
00589
00590
00591
00592
00593 bool GetRoute(const CoordinateType &p, NodePointer *&route)
00594 {
00595 assert( boundingBox.min.X()<=p.X() && p.X()<=boundingBox.max.X() );
00596 assert( boundingBox.min.Y()<=p.Y() && p.Y()<=boundingBox.max.Y() );
00597 assert( boundingBox.min.Z()<=p.Z() && p.Z()<=boundingBox.max.Z() );
00598
00599 memset(route, NULL, maximumDepth*sizeof(NodePointer));
00600
00601 CenterType path = CenterType::Construct(Interize(p));
00602 int shift = maximumDepth-1;
00603 NodePointer finalLevel = Root();
00604 NodePointer curNode = Root();
00605
00606 while(shift >= finalLevel)
00607 {
00608 int son=0;
00609 if((path[0]>>shift)%2) son +=1;
00610 if((path[1]>>shift)%2) son +=2;
00611 if((path[2]>>shift)%2) son +=4;
00612 NodePointer nextNode = Son(curNode, son);
00613 if(nextNode!=NULL)
00614 {
00615 route[maximumDepth-shift] = nextNode;
00616 curNode = nextNode;
00617 }
00618 else
00619 return false;
00620
00621 --shift;
00622 }
00623 return true;
00624 };
00625
00626
00627
00628
00629
00630
00631 void ContainedNodes
00632 (
00633 BoundingBoxType &query,
00634 std::vector< NodePointer > &nodes,
00635 int depth,
00636 NodePointer n,
00637 BoundingBoxType &nodeBB)
00638 {
00639 if (!query.Collide(nodeBB))
00640 return;
00641
00642 if (Level(n)==depth)
00643 nodes.push_back(n);
00644 else
00645 {
00646 NodePointer son;
00647 BoundingBoxType sonBB;
00648 CoordinateType nodeCenter = nodeBB.Center();
00649 for (int s=0; s<8; s++)
00650 {
00651 son = Son(n, s);
00652 if (son!=NULL)
00653 {
00654 sonBB = SubBoxAndCenterInWorldCoordinates(nodeBB, nodeCenter, s);
00655 ContainedNodes(query, nodes, depth, son, sonBB);
00656 }
00657 }
00658 }
00659 };
00660
00661
00662
00663
00664
00665 void ContainedLeaves(
00666 BoundingBoxType &query,
00667 std::vector< NodePointer > &leaves,
00668 NodePointer node,
00669 BoundingBoxType &nodeBB
00670 )
00671 {
00672 NodePointer son;
00673 BoundingBoxType sonBB;
00674 CoordinateType nodeCenter = nodeBB.Center();
00675 for (int s=0; s<8; s++)
00676 {
00677 son = Son(node, s);
00678 if (son!=NULL)
00679 {
00680 sonBB = SubBoxAndCenterInWorldCoordinates(nodeBB, nodeCenter, s);
00681 if ( query.Collide(sonBB) )
00682 {
00683 if ( son->IsLeaf() )
00684 leaves.push_back(son);
00685 else
00686 ContainedLeaves(query, leaves, son, sonBB);
00687 }
00688 }
00689 }
00690 };
00691
00692
00693
00694
00695
00696 public:
00697
00698 int size;
00699
00700
00701 int lSize;
00702
00703
00704 int maximumDepth;
00705
00706
00707 CoordinateType leafDimension;
00708
00709
00710 ScalarType leafDiagonal;
00711
00712
00713 std::vector< Node* > nodes;
00714
00715
00716 BoundingBoxType boundingBox;
00717 };
00718
00719 template <typename VOXEL_TYPE, class SCALAR_TYPE>
00720 const SCALAR_TYPE OctreeTemplate<VOXEL_TYPE, SCALAR_TYPE>::EXPANSION_FACTOR = SCALAR_TYPE(0.035);
00721 }
00722
00723 #endif //VCG_SPACE_INDEX_OCTREETEMPLATE_H