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