00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef VCG_SPACE_INDEX_OCTREE_H
00025 #define VCG_SPACE_INDEX_OCTREE_H
00026
00027 #include <stdlib.h>
00028 #include <algorithm>
00029 #include <vector>
00030 #include <iterator>
00031
00032 #ifdef __glut_h__
00033 #include <vcg/space/color4.h>
00034 #include <wrap/gl/space.h>
00035 #endif
00036
00037 #include <vcg/space/index/base.h>
00038 #include <vcg/space/index/octree_template.h>
00039 #include <vcg/space/box3.h>
00040 #include <wrap/callback.h>
00041
00042 namespace vcg
00043 {
00047 template <typename TYPE>
00048 struct Dereferencer
00049 {
00050 static TYPE& Ref(TYPE &t) { return ( t); }
00051 static TYPE& Ref(TYPE* &t) { return (*t); }
00052 static const TYPE& Ref(const TYPE &t) { return ( t); }
00053 static const TYPE& Ref(const TYPE* &t) { return (*t); }
00054 };
00055
00056
00060 template <typename T>
00061 class ReferenceType
00062 {
00063 public:
00064 typedef T Type;
00065 };
00066
00067
00071 template <typename T>
00072 class ReferenceType<T *>
00073 {
00074 public:
00075 typedef typename ReferenceType<T>::Type Type;
00076 };
00077
00078
00079
00083 struct Voxel
00084 {
00085 Voxel() { count = begin = end = -1; }
00086
00087 void SetRange(const int begin, const int end)
00088 {
00089 this->begin = begin;
00090 this->end = end;
00091 count = end-begin;
00092 };
00093
00094 void AddRange(const Voxel *voxel)
00095 {
00096 assert(voxel->end>end);
00097
00098 count += voxel->count;
00099 end = voxel->end;
00100 };
00101
00102 int begin;
00103 int end;
00104 int count;
00105 };
00106
00107
00108 template < class OBJECT_TYPE, class SCALAR_TYPE>
00109 class Octree : public vcg::OctreeTemplate< Voxel, SCALAR_TYPE >, public vcg::SpatialIndex< OBJECT_TYPE, SCALAR_TYPE >
00110 {
00111 protected:
00112 struct Neighbour;
00113
00114 public:
00115 typedef SCALAR_TYPE ScalarType;
00116 typedef OBJECT_TYPE ObjectType;
00117 typedef typename Octree::Leaf * LeafPointer;
00118 typedef typename Octree::InnerNode * InnerNodePointer;
00119 typedef typename ReferenceType<OBJECT_TYPE>::Type * ObjectPointer;
00120
00121 typedef vcg::Voxel VoxelType;
00122 typedef VoxelType * VoxelPointer;
00123
00124 typedef vcg::OctreeTemplate< VoxelType, SCALAR_TYPE > TemplatedOctree;
00125 typedef typename TemplatedOctree::ZOrderType ZOrderType;
00126
00127 typedef typename TemplatedOctree::BoundingBoxType BoundingBoxType;
00128 typedef typename TemplatedOctree::CenterType CenterType;
00129 typedef typename TemplatedOctree::CoordinateType CoordType;
00130
00131 typedef typename TemplatedOctree::NodeType NodeType;
00132 typedef typename TemplatedOctree::NodePointer NodePointer;
00133 typedef typename TemplatedOctree::NodeIndex NodeIndex;
00134
00135 typedef typename std::vector< Neighbour >::iterator NeighbourIterator;
00136
00137 protected:
00138
00139
00140
00144 template < typename LEAF_TYPE >
00145 struct ObjectPlaceholder
00146 {
00147 typedef LEAF_TYPE* LeafPointer;
00148
00149 ObjectPlaceholder() { z_order = object_index = -1, leaf_pointer = NULL;}
00150
00151 ObjectPlaceholder(ZOrderType zOrder, void* leafPointer, unsigned int objectIndex)
00152 {
00153 z_order = zOrder;
00154 leaf_pointer = leafPointer;
00155 object_index = objectIndex;
00156 }
00157
00158 ZOrderType z_order;
00159 LeafPointer leaf_pointer;
00160 unsigned int object_index;
00161 };
00162
00163
00167 template <typename LEAF_TYPE >
00168 struct ObjectSorter
00169 {
00170 inline bool operator()(const ObjectPlaceholder< LEAF_TYPE > &first, const ObjectPlaceholder< LEAF_TYPE > &second)
00171 {
00172 return (first.z_order<second.z_order);
00173 }
00174 };
00175
00179 struct ObjectReference
00180 {
00181 ObjectReference() {pMark=NULL; pObject=NULL;}
00182
00183 unsigned char *pMark;
00184 ObjectPointer pObject;
00185 };
00186
00187
00188
00189
00190 struct Neighbour
00191 {
00192 Neighbour()
00193 {
00194 this->object = NULL;
00195 this->distance = -1.0f;
00196 };
00197
00198 Neighbour(ObjectPointer &object, CoordType &point, ScalarType distance)
00199 {
00200 this->object = object;
00201 this->point = point;
00202 this->distance = distance;
00203 }
00204
00205 inline bool operator<(const Neighbour &n) const
00206 {
00207 return distance<n.distance;
00208 }
00209
00210
00211 ObjectPointer object;
00212 CoordType point;
00213 ScalarType distance;
00214 };
00215
00216 public:
00217 Octree()
00218 {
00219 marks=0;
00220 }
00221 ~Octree()
00222 {
00223 if(marks) delete []marks;
00224 int node_count = TemplatedOctree::NodeCount();
00225 for (int i=0; i<node_count; i++)
00226 delete TemplatedOctree::nodes[i];
00227 TemplatedOctree::nodes.clear();
00228 }
00229
00230
00234 template < class OBJECT_ITERATOR >
00235 void Set(const OBJECT_ITERATOR & bObj, const OBJECT_ITERATOR & eObj )
00236 {
00237
00238 typedef Dereferencer<typename ReferenceType<typename OBJECT_ITERATOR::value_type>::Type > DereferencerType;
00239 BoundingBoxType bounding_box, obj_bb;
00240 bounding_box.SetNull();
00241 for (OBJECT_ITERATOR iObj=bObj; iObj!=eObj; iObj++)
00242 {
00243 (*iObj).GetBBox(obj_bb);
00244 bounding_box.Add(obj_bb);
00245 }
00246
00247
00248 BoundingBoxType resulting_bb(bounding_box);
00249 CoordType offset = bounding_box.Dim()*Octree::EXPANSION_FACTOR;
00250 CoordType center = bounding_box.Center();
00251 resulting_bb.Offset(offset);
00252 ScalarType longest_side = vcg::math::Max( resulting_bb.DimX(), vcg::math::Max(resulting_bb.DimY(), resulting_bb.DimZ()) )/2.0f;
00253 resulting_bb.Set(center);
00254 resulting_bb.Offset(longest_side);
00255 TemplatedOctree::boundingBox = resulting_bb;
00256
00257
00258 int dataset_dimension = int(std::distance(bObj, eObj));
00259
00260 int primitives_per_voxel;
00261 int depth = 4;
00262 do
00263 {
00264 int number_of_voxel = 1<<(3*depth);
00265 float density = float(number_of_voxel)/float(depth);
00266 primitives_per_voxel = int(float(dataset_dimension)/density);
00267 depth++;
00268 }
00269 while (primitives_per_voxel>25 && depth<15);
00270 TemplatedOctree::Initialize(++depth);
00271
00272
00273 std::string message("Indexing dataset...");
00274 NodePointer *route = new NodePointer[depth+1];
00275 OBJECT_ITERATOR iObj = bObj;
00276
00277
00278
00279 std::vector< ObjectPlaceholder< NodeType > > placeholders;
00280 vcg::Box3<ScalarType> object_bb;
00281 vcg::Point3<ScalarType> hit_leaf;
00282 for (int i=0; i<dataset_dimension; i++, iObj++)
00283 {
00284 (*iObj).GetBBox(object_bb);
00285 hit_leaf = object_bb.min;
00286
00287 while (object_bb.IsIn(hit_leaf))
00288 {
00289 int placeholder_index = int(placeholders.size());
00290 placeholders.push_back( ObjectPlaceholder< NodeType >() );
00291 placeholders[placeholder_index].z_order = BuildRoute(hit_leaf, route);
00292 placeholders[placeholder_index].leaf_pointer = route[depth];
00293 placeholders[placeholder_index].object_index = i;
00294
00295 hit_leaf.X() += TemplatedOctree::leafDimension.X();
00296 if (hit_leaf.X()>object_bb.max.X())
00297 {
00298 hit_leaf.X() = object_bb.min.X();
00299 hit_leaf.Z()+= TemplatedOctree::leafDimension.Z();
00300 if (hit_leaf.Z()>object_bb.max.Z())
00301 {
00302 hit_leaf.Z() = object_bb.min.Z();
00303 hit_leaf.Y()+= TemplatedOctree::leafDimension.Y();
00304 }
00305 }
00306 }
00307 }
00308 delete []route;
00309
00310 int placeholder_count = int(placeholders.size());
00311
00312
00313 global_mark = 1;
00314 marks = new unsigned char[placeholder_count];
00315 memset(&marks[0], 0, sizeof(unsigned char)*placeholder_count);
00316
00317 std::sort(placeholders.begin(), placeholders.end(), ObjectSorter< NodeType >());
00318 std::vector< NodePointer > filled_leaves(placeholder_count);
00319 sorted_dataset.resize( placeholder_count );
00320 for (int i=0; i<placeholder_count; i++)
00321 {
00322 std::advance((iObj=bObj), placeholders[i].object_index);
00323 sorted_dataset[i].pObject = &DereferencerType::Ref(*iObj);
00324 sorted_dataset[i].pMark = &marks[i];
00325 filled_leaves[i] = placeholders[i].leaf_pointer;
00326 }
00327
00328
00329
00330 int begin = -1;
00331 NodePointer initial_leaf = NULL;
00332 for (int end=0; end<placeholder_count; )
00333 {
00334 begin = end;
00335 initial_leaf = filled_leaves[begin];
00336 do end++;
00337 while (end<placeholder_count && initial_leaf==filled_leaves[end]);
00338
00339 VoxelType *voxel = TemplatedOctree::Voxel(initial_leaf);
00340 voxel->SetRange(begin, end);
00341 }
00342
00343
00344
00345 IndexInnerNodes( TemplatedOctree::Root() );
00346 }
00347
00351 template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER>
00352 ObjectPointer GetClosest
00353 (
00354 OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor,
00355 OBJECT_MARKER & ,
00356 const CoordType & query_point,
00357 const ScalarType & max_distance,
00358 ScalarType & distance,
00359 CoordType & point,
00360 bool allow_zero_distance = true
00361 )
00362 {
00363 BoundingBoxType query_bb;
00364 ScalarType sphere_radius;
00365 if (!GuessInitialBoundingBox(query_point, max_distance, sphere_radius, query_bb))
00366 return NULL;
00367
00368 std::vector< NodePointer > leaves;
00369
00370
00371
00372
00373 IncrementMark();
00374 AdjustBoundingBox(query_bb, sphere_radius, max_distance, leaves, 1);
00375
00376 if (sphere_radius>max_distance)
00377 return NULL;
00378
00379 std::vector< Neighbour > neighbors;
00380 RetrieveContainedObjects(query_point, distance_functor, max_distance, allow_zero_distance, leaves, neighbors);
00381
00382 typename std::vector< Neighbour >::iterator first = neighbors.begin();
00383 typename std::vector< Neighbour >::iterator last = neighbors.end();
00384 std::partial_sort(first, first+1, last);
00385
00386 distance = neighbors[0].distance;
00387 point = neighbors[0].point;
00388 return neighbors[0].object;
00389 };
00390
00394 template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_CONTAINER>
00395 unsigned int GetKClosest
00396 (
00397 OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor,
00398 OBJECT_MARKER & ,
00399 unsigned int k,
00400 const CoordType & query_point,
00401 const ScalarType & max_distance,
00402 OBJECT_POINTER_CONTAINER & objects,
00403 DISTANCE_CONTAINER & distances,
00404 POINT_CONTAINER & points,
00405 bool sort_per_distance = true,
00406 bool allow_zero_distance = true
00407 )
00408 {
00409 BoundingBoxType query_bb;
00410 ScalarType sphere_radius;
00411 if (!GuessInitialBoundingBox(query_point, max_distance, sphere_radius, query_bb))
00412 return 0;
00413
00414 std::vector< NodePointer > leaves;
00415 std::vector< Neighbour > neighbors;
00416
00417 unsigned int object_count;
00418 float k_distance;
00419
00420 OBJECT_RETRIEVER:
00421 IncrementMark();
00422 AdjustBoundingBox(query_bb, sphere_radius, max_distance, leaves, k);
00423 object_count = RetrieveContainedObjects(query_point, distance_functor, max_distance, allow_zero_distance, leaves, neighbors);
00424
00425 if (sphere_radius<max_distance && object_count<k)
00426 goto OBJECT_RETRIEVER;
00427
00428 NeighbourIterator first = neighbors.begin();
00429 NeighbourIterator last = neighbors.end();
00430
00431 object_count = vcg::math::Min(k, object_count);
00432 if (sort_per_distance) std::partial_sort< NeighbourIterator >(first, first+object_count, last );
00433 else std::nth_element < NeighbourIterator >(first, first+object_count, last );
00434
00435 k_distance = neighbors[object_count-1].distance;
00436 if (k_distance>sphere_radius && sphere_radius<max_distance)
00437 goto OBJECT_RETRIEVER;
00438
00439 return CopyQueryResults<OBJECT_POINTER_CONTAINER, DISTANCE_CONTAINER, POINT_CONTAINER>(neighbors, object_count, objects, distances, points);
00440 };
00441
00442
00446 template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_CONTAINER>
00447 unsigned int GetInSphere
00448 (
00449 OBJECT_POINT_DISTANCE_FUNCTOR &distance_functor,
00450 OBJECT_MARKER &,
00451 const CoordType &sphere_center,
00452 const ScalarType &sphere_radius,
00453 OBJECT_POINTER_CONTAINER &objects,
00454 DISTANCE_CONTAINER &distances,
00455 POINT_CONTAINER &points,
00456 bool sort_per_distance = false,
00457 bool allow_zero_distance = false
00458 )
00459 {
00460
00461 BoundingBoxType query_bb(sphere_center, sphere_radius);
00462
00463
00464 if (!TemplatedOctree::boundingBox.Collide(query_bb))
00465 return 0;
00466
00467 std::vector< NodePointer > leaves;
00468 std::vector< Neighbour > neighbors;
00469
00470 IncrementMark();
00471 ContainedLeaves(query_bb, leaves, TemplatedOctree::Root(), TemplatedOctree::boundingBox);
00472
00473 int leaves_count = int(leaves.size());
00474 if (leaves_count==0)
00475 return 0;
00476
00477 int object_count = RetrieveContainedObjects(sphere_center, distance_functor, sphere_radius, allow_zero_distance, leaves, neighbors);
00478
00479 NeighbourIterator first = neighbors.begin();
00480 NeighbourIterator last = neighbors.end();
00481 if (sort_per_distance) std::partial_sort< NeighbourIterator >(first, first+object_count, last );
00482 else std::nth_element < NeighbourIterator >(first, first+object_count, last );
00483
00484 return CopyQueryResults<OBJECT_POINTER_CONTAINER, DISTANCE_CONTAINER, POINT_CONTAINER>(neighbors, object_count, objects, distances, points);
00485 };
00486
00490 template <class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER>
00491 unsigned int GetInBox
00492 (
00493 OBJECT_MARKER &,
00494 const BoundingBoxType &query_bounding_box,
00495 OBJECT_POINTER_CONTAINER &objects
00496 )
00497 {
00498
00499 if (!query_bounding_box.Collide())
00500 {
00501 objects.clear();
00502 return 0;
00503 }
00504
00505
00506 std::vector< NodePointer > leaves;
00507 unsigned int object_count;
00508 int leaves_count;
00509
00510 TemplatedOctree::ContainedLeaves(query_bounding_box, leaves, TemplatedOctree::Root(), TemplatedOctree::boundingBox);
00511 leaves_count = int(leaves.size());
00512 if (leaves_count==0)
00513 return 0;
00514
00515 IncrementMark();
00516 for (int i=0; i<leaves_count; i++)
00517 {
00518 VoxelType *voxel = TemplatedOctree::Voxel(leaves[i]);
00519 int begin = voxel->begin;
00520 int end = voxel->end;
00521 for ( ; begin<end; begin++)
00522 {
00523 ObjectReference *ref = &sorted_dataset[begin];
00524 if (IsMarked(ref))
00525 continue;
00526
00527 Mark(ref);
00528 objects.push_back(ref->pObject);
00529 }
00530 }
00531
00532 return int(objects.size());
00533 };
00534
00535 protected:
00540 std::vector< ObjectReference > sorted_dataset;
00541
00545 unsigned char *marks;
00546 unsigned char global_mark;
00547
00555
00556
00557
00558
00559
00560 protected:
00563 inline void IncrementMark()
00564 {
00565
00566 global_mark = (global_mark+1)%255;
00567 if (global_mark == 0)
00568 {
00569 memset(&marks[0], 0, sizeof(unsigned char)*int(sorted_dataset.size()));
00570 global_mark++;
00571 }
00572 };
00573
00574
00575
00576 inline bool IsMarked(const ObjectReference *ref) const
00577 { return *ref->pMark == global_mark; };
00578
00579
00580
00581 inline void Mark(const ObjectReference *ref)
00582 { *ref->pMark = global_mark;};
00583
00588 inline bool GuessInitialBoundingBox(const CoordType &query_point, const ScalarType max_distance, ScalarType &sphere_radius, BoundingBoxType &query_bb)
00589 {
00590
00591
00592
00593 query_bb.Set(query_point);
00594
00595
00596 sphere_radius = 0.0f;
00597
00598
00599 if (!query_bb.IsIn(query_point))
00600 {
00601 do
00602 {
00603 query_bb.Offset(TemplatedOctree::leafDiagonal);
00604 sphere_radius += TemplatedOctree::leafDiagonal;
00605 }
00606 while ( !TemplatedOctree::boundingBox.Collide(query_bb) || sphere_radius>max_distance);
00607 }
00608 return (sphere_radius<=max_distance);
00609 };
00610
00616 inline int AdjustBoundingBox
00617 (
00618 BoundingBoxType & query_bb,
00619 ScalarType & sphere_radius,
00620 const ScalarType max_allowed_distance,
00621 std::vector< NodePointer > & leaves,
00622 const int required_object_count
00623 )
00624 {
00625 int leaves_count;
00626 int object_count;
00627 do
00628 {
00629 leaves.clear();
00630
00631 query_bb.Offset(TemplatedOctree::leafDiagonal);
00632 sphere_radius+= TemplatedOctree::leafDiagonal;
00633
00634 ContainedLeaves(query_bb, leaves, TemplatedOctree::Root(), TemplatedOctree::boundingBox);
00635
00636 leaves_count = int(leaves.size());
00637 object_count = 0;
00638 for (int i=0; i<leaves_count; i++)
00639 object_count += TemplatedOctree::Voxel( leaves[i] )->count;
00640 }
00641 while (object_count<required_object_count && sphere_radius<max_allowed_distance);
00642
00643 return leaves_count;
00644 }
00645
00650 template < class OBJECT_POINT_DISTANCE_FUNCTOR >
00651 inline int RetrieveContainedObjects
00652 (
00653 const CoordType query_point,
00654 OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor,
00655 const ScalarType max_allowed_distance,
00656 bool allow_zero_distance,
00657 std::vector< NodePointer > & leaves,
00658 std::vector< Neighbour > & neighbors
00659 )
00660 {
00661 CoordType closest_point;
00662 neighbors.clear();
00663 for (int i=0, leaves_count=int(leaves.size()); i<leaves_count; i++)
00664 {
00665 VoxelType *voxel = TemplatedOctree::Voxel(leaves[i]);
00666 int begin = voxel->begin;
00667 int end = voxel->end;
00668 for ( ; begin<end; begin++)
00669 {
00670 ObjectReference * ref = &sorted_dataset[begin];
00671 if (IsMarked(ref))
00672 continue;
00673
00674 ScalarType distance = max_allowed_distance;
00675 if (!distance_functor(*ref->pObject, query_point, distance, closest_point))
00676 continue;
00677
00678 Mark(ref);
00679 if ((distance!=ScalarType(0.0) || allow_zero_distance))
00680 neighbors.push_back( Neighbour(ref->pObject, closest_point, distance) );
00681 }
00682 }
00683 return int(neighbors.size());
00684 };
00685
00689 template <class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_CONTAINER>
00690 inline int CopyQueryResults
00691 (
00692 std::vector< Neighbour > &neighbors,
00693 const unsigned int object_count,
00694 OBJECT_POINTER_CONTAINER &objects,
00695 DISTANCE_CONTAINER &distances,
00696 POINT_CONTAINER &points
00697 )
00698 {
00699
00700 points.resize( object_count );
00701 distances.resize( object_count );
00702 objects.resize( object_count );
00703
00704 typename POINT_CONTAINER::iterator iPoint = points.begin();
00705 typename DISTANCE_CONTAINER::iterator iDistance = distances.begin();
00706 typename OBJECT_POINTER_CONTAINER::iterator iObject = objects.begin();
00707 for (unsigned int n=0; n<object_count; n++, iPoint++, iDistance++, iObject++)
00708 {
00709 (*iPoint) = neighbors[n].point;
00710 (*iDistance) = neighbors[n].distance;
00711 (*iObject) = neighbors[n].object;
00712 }
00713 return object_count;
00714 }
00715
00719 void IndexInnerNodes(NodePointer n)
00720 {
00721 assert(n!=NULL);
00722
00723 VoxelPointer current_voxel = TemplatedOctree::Voxel(n);
00724 VoxelPointer son_voxel;
00725 for (int s=0; s<8; s++)
00726 {
00727 NodePointer son_index = Son(n, s);
00728 if (son_index!=NULL)
00729 {
00730 if (Level(son_index)!=TemplatedOctree::maximumDepth)
00731 IndexInnerNodes(son_index);
00732
00733 son_voxel = TemplatedOctree::Voxel(son_index);
00734 current_voxel->AddRange( son_voxel );
00735 }
00736 }
00737 };
00738 };
00739
00740 #ifdef __glut_h__
00741
00742
00743
00744 protected:
00748 struct OcreeRenderingSetting
00749 {
00750 OcreeRenderingSetting()
00751 {
00752 color = vcg::Color4b(155, 155, 155, 255);
00753 isVisible = false;
00754 minVisibleDepth = 1;
00755 maxVisibleDepth = 4;
00756 };
00757
00758 int isVisible;
00759 int minVisibleDepth;
00760 int maxVisibleDepth;
00761 vcg::Color4b color;
00762 };
00763
00764 public:
00765
00766
00767
00768 void DrawOctree(vcg::Box3f &boundingBox, NodePointer n)
00769 {
00770 char level = TemplatedOctree::Level(n);
00771 NodePointer son;
00772 if (rendering_settings.minVisibleDepth>level)
00773 {
00774 for (int s=0; s<8; s++)
00775 if ((son=Son(n, s))!=0)
00776 DrawOctree(TemplatedOctree::SubBox(boundingBox, s), son);
00777 }
00778 else
00779 {
00780 vcg::glBoxWire(boundingBox);
00781 if (level<rendering_settings.maxVisibleDepth)
00782 for (int s=0; s<8; s++)
00783 if ((son=Son(n, s))!=0)
00784 DrawOctree(TemplatedOctree::SubBox(boundingBox, s), son);
00785 }
00786 };
00787
00788 OcreeRenderingSetting rendering_settings;
00789 #endif
00790 }
00791
00792 #endif //VCG_SPACE_INDEX_OCTREE_H