From 9cf176a0ff6bae2676675a898e7b5e9c7ba31a93 Mon Sep 17 00:00:00 2001 From: cnr-isti-vclab Date: Thu, 28 Sep 2006 22:49:15 +0000 Subject: [PATCH] Added methods GetClosest, GetInSphere and GetInBox. Changed signature of Set method to comply with the SpatialIndex interface --- vcg/space/index/octree.h | 440 ++++++++++++++++++++++++------ vcg/space/index/octree_template.h | 91 +++--- 2 files changed, 404 insertions(+), 127 deletions(-) diff --git a/vcg/space/index/octree.h b/vcg/space/index/octree.h index 9f944bfd..3ed06532 100644 --- a/vcg/space/index/octree.h +++ b/vcg/space/index/octree.h @@ -102,11 +102,11 @@ namespace vcg template < class OBJECT_TYPE, class SCALAR_TYPE> - class Octree : public OctreeTemplate< Voxel >, public vcg::SpatialIndex< OBJECT_TYPE, SCALAR_TYPE > + class Octree : public OctreeTemplate< Voxel, SCALAR_TYPE >, public vcg::SpatialIndex< OBJECT_TYPE, SCALAR_TYPE > { public: + typedef SCALAR_TYPE ScalarType; typedef OBJECT_TYPE ObjectType; - typedef vcg::Box3 BoundingBoxType; typedef typename Octree::Leaf * LeafPointer; typedef typename Octree::InnerNode * InnerNodePointer; typedef typename ReferenceType::Type * ObjectPointer; @@ -178,10 +178,10 @@ namespace vcg */ struct ObjectReference { - ObjectReference() {mark_position=-1; pObject=NULL;} + ObjectReference() {pMark=NULL; pObject=NULL;} - int mark_position; - ObjectPointer pObject; + unsigned char *pMark; + ObjectPointer pObject; }; /* @@ -195,7 +195,7 @@ namespace vcg this->distance = -1.0f; }; - Neighbour(ObjectPointer &object, CoordType &point, float distance) + Neighbour(ObjectPointer &object, CoordType &point, ScalarType distance) { this->object = object; this->point = point; @@ -205,7 +205,7 @@ namespace vcg ObjectPointer object; CoordType point; - float distance; + ScalarType distance; }; /* @@ -232,17 +232,25 @@ public: /*! * Populate the octree */ - template - void Set(const OBJECT_ITERATOR & bObj, const OBJECT_ITERATOR & eObj, const BoundingBoxType &bounding_box, const BOUNDING_BOX_FUNCTOR &bb_functor/*, vcg::CallBackPos *callback=NULL*/) + template + void Set(const OBJECT_ITERATOR & bObj, const OBJECT_ITERATOR & eObj /*, const BoundingBoxType &bounding_box*/ /*, const BOUNDING_BOX_FUNCTOR &bb_functor*/ /*, vcg::CallBackPos *callback=NULL*/) { - typedef Dereferencer::Type > DereferencerType; - // Compute the bounding-box enclosing the whole dataset + typedef Dereferencer::Type > DereferencerType; + BoundingBoxType bounding_box, obj_bb; + bounding_box.SetNull(); + for (OBJECT_ITERATOR iObj=bObj; iObj!=eObj; iObj++) + { + (*iObj).GetBBox(obj_bb); + bounding_box.Add(obj_bb); + } + + //...and expand it a bit more BoundingBoxType resulting_bb(bounding_box); CoordType offset = bounding_box.Dim()*Octree::EXPANSION_FACTOR; CoordType center = bounding_box.Center(); resulting_bb.Offset(offset); - float longest_side = vcg::math::Max( resulting_bb.DimX(), vcg::math::Max(resulting_bb.DimY(), resulting_bb.DimZ()) )/2.0f; + ScalarType longest_side = vcg::math::Max( resulting_bb.DimX(), vcg::math::Max(resulting_bb.DimY(), resulting_bb.DimZ()) )/2.0f; resulting_bb.Set(center); resulting_bb.Offset(longest_side); boundingBox = resulting_bb; @@ -250,11 +258,6 @@ public: // Try to find a reasonable octree depth int dataset_dimension = std::distance(bObj, eObj); - // Allocate the mark array - global_mark = 1; - marks = new unsigned char[dataset_dimension]; - memset(&marks[0], 0, sizeof(unsigned char)*dataset_dimension); - int primitives_per_voxel; int depth = 4; do @@ -265,7 +268,7 @@ public: depth++; } while (primitives_per_voxel>25 && depth<15); - Initialize(depth); + Initialize(++depth); // Sort the dataset (using the lebesgue space filling curve...) std::string message("Indexing dataset..."); @@ -279,7 +282,7 @@ public: vcg::Point3 hit_leaf; for (int i=0; i()); std::vector< Octree::Node* > filled_leaves(placeholder_count); - sorted_dataset.resize( dataset_dimension ); + sorted_dataset.resize( placeholder_count ); for (int i=0; i + ObjectPointer GetClosest + ( + OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor, + OBJECT_MARKER & /*marker*/, + const CoordType & query_point, + const ScalarType & max_distance, + ScalarType & distance, + CoordType & point, + bool allow_zero_distance = false + ) + { + BoundingBoxType query_bb; + ScalarType sphere_radius; + if (!GuessInitialBoundingBox(query_point, max_distance, sphere_radius, query_bb)) + return NULL; + + std::vector< NodePointer > leaves; + + unsigned int object_count; + int leaves_count; + ScalarType k_distance = leafDiagonal; + + IncrementMark(); + + do + { + leaves.clear(); + object_count = 0; + query_bb.Offset(k_distance); + sphere_radius += vcg::math::Max(leafDiagonal, k_distance); + + ContainedLeaves(query_bb, leaves, Root(), boundingBox); + + leaves_count = int(leaves.size()); + object_count = 0; + for (int i=0; icount; + } + while (object_count==0 && sphere_radiusmax_distance) + return NULL; + + + CoordType closest_point; + VoxelType *voxel; + ObjectReference *ref; + int begin; + int end; + ScalarType dist; + + std::vector< Neighbour > neighbors; + object_count = 0; + for (int i=0; ibegin; + end = voxel->end; + for ( ; beginpObject, query_point, dist, closest_point)) + continue; + + Mark(ref); + if (dist!=ScalarType(0.0) || allow_zero_distance) + neighbors.push_back( Neighbour(ref->pObject, closest_point, dist) ); + + } //end of for ( ; begin::iterator first = neighbors.begin(); + std::vector< Neighbour >::iterator last = neighbors.end(); + std::partial_sort(first, first+object_count, last, DistanceCompare()); + distance = neighbors[0].distance; + point = neighbors[0].point; + return neighbors[0].object; + }; //end of GetClosest + /*! * Retrive the k closest element to the query point */ - template + template unsigned int GetKClosest ( - OBJECT_POINT_DISTANCE_FUNCTOR &distance_functor, - OBJECT_MARKER &/*marker*/, - const unsigned int k, - const CoordType &query_point, - const ScalarType &max_distance, - OBJECT_POINTER_CONTAINER &objects, - DISTANCE_CONTAINER &distances, - POINT_POINTER_CONTAINER &points, - bool sort_per_distance = false, - bool allow_zero_distance = false + OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor, + OBJECT_MARKER & /*marker*/, + const unsigned int k, + const CoordType & query_point, + const ScalarType & max_distance, + OBJECT_POINTER_CONTAINER & objects, + DISTANCE_CONTAINER & distances, + POINT_CONTAINER & points, + bool sort_per_distance = false, + bool allow_zero_distance = false ) { - // costruisco una bounging box centrata in query di dimensione pari a quella di una foglia. - // e controllo se in tale bounging box sono contenute un numero di elementi >= a k. - // Altrimenti espando il bounding box. BoundingBoxType query_bb; - query_bb.Set(query_point); - - // Il raggio della sfera centrata nel punto query - float sphere_radius = 0.0f; - - // se il bounding-box non interseca il bounding-box dell'octree, lo espando subito - if (!query_bb.IsIn(query_point)) - { - do - { - query_bb.Offset(leafDiagonal); - sphere_radius += leafDiagonal; - } - while ( !boundingBox.Collide(query_bb) || sphere_radius>max_distance); // TODO check the termination condintion - } + ScalarType sphere_radius; + if (!GuessInitialBoundingBox(query_point, max_distance, sphere_radius, query_bb)) + return 0; std::vector< NodePointer > leaves; std::vector< Neighbour > neighbors; @@ -383,13 +465,7 @@ public: float k_distance = leafDiagonal; do { - // update the marks - global_mark = (global_mark+1)%255; - if (global_mark == 0) - { - memset(&marks[0], 0, sizeof(unsigned char)*int(sorted_dataset.size())); - global_mark++; - } + IncrementMark(); do { @@ -405,7 +481,7 @@ public: for (int i=0; icount; } - while (object_countmax_distance); // TODO check the termination condintion + while (object_countmark_position]==global_mark) + if (IsMarked(ref)) continue; distance = max_distance; if (!distance_functor(*ref->pObject, query_point, distance, closest_point)) continue; - marks[ref->mark_position]=global_mark; - if ((distance!=0.0f || allow_zero_distance)/* && distancepObject, closest_point, distance) ); } //end of for ( ; beginsphere_radius && sphere_radius(neighbors, object_count, objects, distances, points); }; //end of GetKClosest /*! + * Returns all the objects contained inside a specified sphere + */ + template + unsigned int GetInSphere + ( + OBJECT_POINT_DISTANCE_FUNCTOR &distance_functor, + OBJECT_MARKER &/*marker*/, + const CoordType &sphere_center, + const ScalarType &sphere_radius, + OBJECT_POINTER_CONTAINER &objects, + DISTANCE_CONTAINER &distances, + POINT_CONTAINER &points, + bool sort_per_distance = false, + bool allow_zero_distance = false + ) + { + // Define the minimum bounding-box containing the sphere + BoundingBoxType query_bb(sphere_center, sphere_radius); + + // If that bounding-box don't collide with the octree bounding-box, simply return 0 + if (!boundingBox.Collide(query_bb)) + return 0; + + std::vector< NodePointer > leaves; + std::vector< Neighbour > neighbors; + + unsigned int object_count = 0; + float k_distance = leafDiagonal; + + IncrementMark(); + ContainedLeaves(query_bb, leaves, Root(), boundingBox); + + int leaves_countleaves_count = int(leaves.size()); + if (leaves_count==0) + return 0; + + CoordType closest_point; + VoxelType *voxel; + ObjectReference *ref; + int begin; + int end; + float distance; + for (int i=0; ibegin; + end = voxel->end; + for ( ; beginpObject, query_point, distance, closest_point)) + continue; + + object_count++; + Mark(ref); + if ((distance!=0.0f || allow_zero_distance) && distancepObject, closest_point, distance) ); + } //end of for ( ; begin::iterator first = neighbors.begin(); + std::vector< Neighbour >::iterator last = neighbors.end(); + if (sort_per_distance) std::partial_sort(first, first+object_count, last, DistanceCompare()); + else std::nth_element (first, first+object_count, last, DistanceCompare()); + + return CopyQueryResults(neighbors, object_count, objects, distances, points); + };//end of GetInSphere + + /*! + * Returns all the objects lying inside the specified bbox + */ + template + unsigned int GetInBox + ( + OBJECT_MARKER &/*marker*/, + const BoundingBoxType &query_bounding_box, + OBJECT_POINTER_CONTAINER &objects + ) + { + //if the query bounding-box don't collide with the octree bounding-box, simply return 0 + if (!query_bounding_box.Collide()) + { + objects.clear(); + return 0; + } + + //otherwise, retrieve the leaves and fill the container with the objects contained + std::vector< NodePointer > leaves; + unsigned int object_count; + int leaves_count; + + ContainedLeaves(query_bounding_box, leaves, Root(), boundingBox); + leaves_count = int(leaves.size()); + if (leaves_count==0) + { + objects.clear(); + return 0; + } + + IncrementMark(); + + VoxelType *voxel; + ObjectReference *ref; + int begin; + int end; + object_count = 0; + for (int i=0; ibegin; + end = voxel->end; + for ( ; beginpObject); + } //end of for ( ; beginpMark == global_mark; }; + + /* + */ + inline void Mark(const ObjectReference *ref) + { *ref->pMark = global_mark;}; + + /*! + * Guess an initial bounding-box from which starting the research of the closest point(s). + * \return true iff it's possibile to find a sphere, centered in query_point and having radius max_distance at most, which collide the octree bounding-box. + */ + inline bool GuessInitialBoundingBox(const CoordType &query_point, const ScalarType max_distance, ScalarType &sphere_radius, BoundingBoxType &query_bb) + { + // costruisco una bounging box centrata in query di dimensione pari a quella di una foglia. + // e controllo se in tale bounging box sono contenute un numero di elementi >= a k. + // Altrimenti espando il bounding box. + query_bb.Set(query_point); + + // Il raggio della sfera centrata nel punto query + sphere_radius = 0.0f; + + // se il bounding-box non interseca il bounding-box dell'octree, lo espando subito + if (!query_bb.IsIn(query_point)) + { + do + { + query_bb.Offset(leafDiagonal); + sphere_radius += leafDiagonal; + } + while ( !boundingBox.Collide(query_bb) || sphere_radius>max_distance); // TODO check the termination condintion + } + return (sphere_radius<=max_distance); + }; + + /*! + * Copy the results of a query + */ + template + inline int CopyQueryResults + ( + std::vector< Neighbour > &neighbors, + const unsigned int object_count, + OBJECT_POINTER_CONTAINER &objects, + DISTANCE_CONTAINER &distances, + POINT_CONTAINER &points + ) + { + // copy the nearest object into + if (int(points.size())!=object_count) + { + points.resize(object_count); + distances.resize(object_count); + objects.resize(object_count); + } + + POINT_CONTAINER::iterator iPoint = points.begin(); + DISTANCE_CONTAINER::iterator iDistance = distances.begin(); + OBJECT_POINTER_CONTAINER::iterator iObject = objects.begin(); + for (unsigned int n=0; n +template class OctreeTemplate { protected: @@ -56,12 +56,15 @@ protected: public: // Octree Type Definitions - typedef typename VOXEL_TYPE VoxelType; - typedef typename VOXEL_TYPE *VoxelPointer; - typedef vcg::Point3i CenterType; - static const float EXPANSION_FACTOR; - typedef int NodeIndex; - typedef Node *NodePointer; + typedef SCALAR_TYPE ScalarType; + typedef typename VOXEL_TYPE VoxelType; + typedef typename VOXEL_TYPE *VoxelPointer; + typedef vcg::Point3i CenterType; + static const ScalarType EXPANSION_FACTOR; + typedef int NodeIndex; + typedef Node *NodePointer; + typedef vcg::Box3 BoundingBoxType; + typedef vcg::Point3 CoordinateType; protected: /* @@ -154,14 +157,14 @@ public: nodes.push_back( root ); root->center = CenterType(size, size, size); - float szf = (float) size; + ScalarType szf = (ScalarType) size; leafDimension = boundingBox.Dim(); leafDimension /= szf; leafDiagonal = leafDimension.Norm(); }; // Return the octree bounding-box - inline vcg::Box3f BoundingBox() { return boundingBox; } + inline BoundingBoxType BoundingBox() { return boundingBox; } // Return the Voxel of the n-th node inline VoxelPointer Voxel(const NodePointer n) { return &(n->voxel); } @@ -199,15 +202,15 @@ public: inline CenterType CenterInOctreeCoordinates(const NodePointer n) const { return n->center;} // Return the center of the n-th node expressed in world-coordinate - inline vcg::Point3f CenterInWorldCoordinates(const NodePointer n) const + inline CoordinateType CenterInWorldCoordinates(const NodePointer n) const { assert(0<=n && nlevel; int shift = maxDepth-level+1; - vcg::Point3f wcCenter; - vcg::Point3f ocCenter = CenterInOctreeCoordinates(n); - vcg::Point3f nodeSize = boundingBox.Dim()/float(1<>shift))); wcCenter.Y() = boundingBox.min.Y() + (nodeSize.Y()*(0.5f+(ocCenter.Y()>>shift))); wcCenter.Z() = boundingBox.min.Z() + (nodeSize.Z()*(0.5f+(ocCenter.Z()>>shift))); @@ -264,9 +267,9 @@ public: The center of each cell can simply be obtained by adding .5 to the path of the leaves. */ - vcg::Point3f Center(NodePointer n) const + CoordinateType Center(NodePointer n) const { - vcg::Point3f center; + CoordinateType center; center.Import(GetPath(n)); center+=Point3f(.5f,.5f,.5f); @@ -277,15 +280,15 @@ public: } // Return the bounding-box of the n-th node expressed in world-coordinate - vcg::Box3f BoundingBoxInWorldCoordinates(const NodePointer n) + BoundingBoxType BoundingBoxInWorldCoordinates(const NodePointer n) { assert(0<=n && n>shift)); nodeBB.min.Y() = boundingBox.min.Y() + (nodeDim.Y()*(center.Y()>>shift)); nodeBB.min.Z() = boundingBox.min.Z() + (nodeDim.Z()*(center.Z()>>shift)); @@ -294,9 +297,9 @@ public: }; // Return one of the 8 subb box of a given box. - vcg::Box3f SubBox(vcg::Box3f &lbb, int i) + BoundingBoxType SubBox(BoundingBoxType &lbb, int i) { - vcg::Box3f bs; + BoundingBoxType bs; if (i&1) bs.min.X()=(lbb.min.X()+(bs.max.X()=lbb.max.X()))/2.0f; else bs.max.X()=((bs.min.X()=lbb.min.X())+lbb.max.X())/2.0f; if (i&2) bs.min.Y()=(lbb.min.Y()+(bs.max.Y()=lbb.max.Y()))/2.0f; @@ -309,9 +312,9 @@ public: // Given the bounding-box and the center (both in world-coordinates) // of a node, return the bounding-box (in world-coordinats) of the i-th son - vcg::Box3f SubBoxAndCenterInWorldCoordinates(vcg::Box3f &lbb, vcg::Point3f ¢er, int i) + BoundingBoxType SubBoxAndCenterInWorldCoordinates(BoundingBoxType &lbb, CoordinateType ¢er, int i) { - vcg::Box3f bs; + BoundingBoxType bs; if (i&1) { bs.min[0]=center[0]; @@ -407,9 +410,9 @@ public: // Convert the point p coordinates to the integer based representation // in the range 0..size, where size is 2^maxdepth - vcg::Point3i Interize(const vcg::Point3f &pf) const + CenterType Interize(const CoordinateType &pf) const { - vcg::Point3i pi; + CenterType pi; assert(pf.X()>=boundingBox.min.X() && pf.X()<=boundingBox.max.X()); assert(pf.Y()>=boundingBox.min.Y() && pf.Y()<=boundingBox.max.Y()); @@ -424,9 +427,9 @@ public: // Inverse function of Interize; // Return to the original coords space (not to the original values!!) - vcg::Point3f DeInterize(const vcg::Point3i &pi ) const + CoordinateType DeInterize(const CenterType &pi ) const { - vcg::Point3f pf; + CoordinateType pf; assert(pi.X()>=0 && pi.X()=0 && pi.Y() &nodes, int depth, NodePointer n, - vcg::Box3f &nodeBB) + BoundingBoxType &nodeBB) { if (!query.Collide(nodeBB)) return; @@ -621,8 +625,8 @@ public: else { NodePointer son; - vcg::Box3f sonBB; - vcg::Point3f nodeCenter = nodeBB.Center(); + BoundingBoxType sonBB; + CoordinateType nodeCenter = nodeBB.Center(); for (int s=0; s<8; s++) { son = Son(n, s); @@ -640,15 +644,15 @@ public: // bounding-box ha intersezione non nulla con bb; i loro indici // sono inseriti all'interno di leaves. void ContainedLeaves( - vcg::Box3f &query, + BoundingBoxType &query, std::vector< NodePointer > &leaves, NodePointer node, - vcg::Box3f &nodeBB + BoundingBoxType &nodeBB ) { NodePointer son; - vcg::Box3f sonBB; - vcg::Point3f nodeCenter = nodeBB.Center(); + BoundingBoxType sonBB; + CoordinateType nodeCenter = nodeBB.Center(); for (int s=0; s<8; s++) { son = Son(node, s); //nodes[nodeIndex].sonIndex[s] @@ -681,21 +685,20 @@ public: int maximumDepth; // The dimension of a leaf - vcg::Point3f leafDimension; + CoordinateType leafDimension; // The diagonal of a leaf - float leafDiagonal; + ScalarType leafDiagonal; // The Octree nodes std::vector< Node* > nodes; // The bounding box containing the octree (in world coordinate) - vcg::Box3f boundingBox; + BoundingBoxType boundingBox; }; //end of class OctreeTemplate -template -const float OctreeTemplate::EXPANSION_FACTOR = 0.035f; - +template +const SCALAR_TYPE OctreeTemplate::EXPANSION_FACTOR = SCALAR_TYPE(0.035); } #endif //OCTREETEMPLATE_H