From 15ce3360edaef69ac593bd415a56301d75741e93 Mon Sep 17 00:00:00 2001 From: dibenedetto Date: Fri, 16 Sep 2005 10:04:15 +0000 Subject: [PATCH] Modified interface, added GetKClosest(). --- vcg/space/index/aabb_binary_tree_search.h | 155 +++++++++++++++++++--- 1 file changed, 135 insertions(+), 20 deletions(-) diff --git a/vcg/space/index/aabb_binary_tree_search.h b/vcg/space/index/aabb_binary_tree_search.h index 805fac6d..db257e6b 100644 --- a/vcg/space/index/aabb_binary_tree_search.h +++ b/vcg/space/index/aabb_binary_tree_search.h @@ -24,6 +24,9 @@ History $Log: not supported by cvs2svn $ +Revision 1.2 2005/09/11 11:46:21 m_di_benedetto +First Commit + @@ -35,6 +38,8 @@ $Log: not supported by cvs2svn $ // stl headers #include #include +#include +#include // vcg headers #include @@ -75,23 +80,45 @@ class AABBBinaryTreeSearch { inline AABBBinaryTreeSearch(void); inline ~AABBBinaryTreeSearch(void); - inline void Clear(void); + inline TreeType & Tree(void); + inline const TreeType & Tree(void) const; template - inline bool Set(const OBJITERATOR & oBegin, const OBJITERATOR & oEnd, const unsigned int size, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance, OBJITERATORPTRFUNCT & objPtr, OBJBOXFUNCT & objBox, OBJBARYCENTERFUNCT & objBarycenter); + inline bool Set(const OBJITERATOR & oBegin, const OBJITERATOR & oEnd, OBJITERATORPTRFUNCT & objPtr, OBJBOXFUNCT & objBox, OBJBARYCENTERFUNCT & objBarycenter, const unsigned int maxElemsPerLeaf = 1, const ScalarType & leafBoxMaxVolume = ((ScalarType)0), const bool useVariance = true); + + inline void Clear(void); template ObjPtr GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const; - inline TreeType & Tree(void); - inline const TreeType & Tree(void) const; + template + unsigned int GetKClosests(OBJPOINTDISTANCEFUNCT & getPointDistance, const unsigned int k, const CoordType & p, OBJPTRCONTAINER & closestObjs, DISTCONTAINER & distances, POINTCONTAINER & closestPts) const; protected: + struct ClosestObjType { + ObjPtr pObj; + ScalarType minDist; + CoordType closestPt; + }; + + class CompareClosest { + public: + bool operator () (const ClosestObjType & a, const ClosestObjType & b) { + return (a.minDist < b.minDist); + } + }; + + typedef std::priority_queue, typename ClassType::CompareClosest> PQueueType; + TreeType tree; static inline CoordType Abs(const CoordType & p); static inline CoordType LowerClamp(const CoordType & p, const ScalarType & r); - static inline ScalarType MinimumMaxDistance(const ScalarType & currMinMaxDist, const std::vector & n, const CoordType & p); + static inline ScalarType MinimumMaxSquareDistance(const ScalarType & currMinMaxDist, const std::vector & n, const CoordType & p); + static inline void MinMaxSquareDistance(const typename TreeType::NodeType * n, const CoordType & p, ScalarType & dmin, ScalarType & dmax); + + template + static inline void DepthFirstCollect(const CoordType & p, typename TreeType::NodeType * node, ScalarType & mindmax, const unsigned int k, PQueueType & pq, OBJPOINTDISTANCEFUNCT & getPointDistance); }; @@ -109,17 +136,6 @@ AABBBinaryTreeSearch::~AABBBinaryTreeSearc this->Clear(); } -template -void AABBBinaryTreeSearch::Clear(void) { - this->tree.Clear(); -} - -template -template -bool AABBBinaryTreeSearch::Set(const OBJITERATOR & oBegin, const OBJITERATOR & oEnd, const unsigned int size, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance, OBJITERATORPTRFUNCT & objPtr, OBJBOXFUNCT & objBox, OBJBARYCENTERFUNCT & objBarycenter) { - return (this->tree.Set(oBegin, oEnd, size, maxElemsPerLeaf, leafBoxMaxVolume, useVariance, objPtr, objBox, objBarycenter)); -} - template typename AABBBinaryTreeSearch::TreeType & AABBBinaryTreeSearch::Tree(void) { return (this->tree); @@ -130,6 +146,17 @@ const typename AABBBinaryTreeSearch::TreeT return (this->tree); } +template +template +bool AABBBinaryTreeSearch::Set(const OBJITERATOR & oBegin, const OBJITERATOR & oEnd, OBJITERATORPTRFUNCT & objPtr, OBJBOXFUNCT & objBox, OBJBARYCENTERFUNCT & objBarycenter, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance) { + return (this->tree.Set(oBegin, oEnd, objPtr, objBox, objBarycenter, maxElemsPerLeaf, leafBoxMaxVolume, useVariance)); +} + +template +void AABBBinaryTreeSearch::Clear(void) { + this->tree.Clear(); +} + template template typename AABBBinaryTreeSearch::ObjPtr AABBBinaryTreeSearch::GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const { @@ -158,13 +185,16 @@ typename AABBBinaryTreeSearch::ObjPtr AABB clist2.resize(0); leaves.resize(0); +#ifdef max +#undef max +#endif ScalarType minMaxDist = std::numeric_limits::max(); candidates->push_back(t.pRoot); while (!candidates->empty()) { newCandidates->resize(0); - minMaxDist = ClassType::MinimumMaxDistance(minMaxDist, *candidates, p); + minMaxDist = ClassType::MinimumMaxSquareDistance(minMaxDist, *candidates, p); for (NodePtrVector_ci ci=candidates->begin(); ci!=candidates->end(); ++ci) { if ((*ci)->auxData.searchData.minDist < minMaxDist) { if ((*ci)->IsLeaf()) { @@ -190,8 +220,8 @@ typename AABBBinaryTreeSearch::ObjPtr AABB ObjPtr closestObject = 0; CoordType closestPoint; - ScalarType closestDist = std::numeric_limits::max(); - ScalarType closestDistSq = std::numeric_limits::max(); + ScalarType closestDist = math::Sqrt(minMaxDist) + std::numeric_limits::epsilon(); + ScalarType closestDistSq = minMaxDist + std::numeric_limits::epsilon(); for (NodePtrVector_ci ci=leaves.begin(); ci!=leaves.end(); ++ci) { if ((*ci)->auxData.searchData.minDist < closestDistSq) { @@ -212,6 +242,84 @@ typename AABBBinaryTreeSearch::ObjPtr AABB return (closestObject); } +template +template +void AABBBinaryTreeSearch::DepthFirstCollect(const CoordType & p, typename TreeType::NodeType * node, ScalarType & mindmax, const unsigned int k, PQueueType & pq, OBJPOINTDISTANCEFUNCT & getPointDistance) { + const CoordType dc = ClassType::Abs(p - node->boxCenter); + + if (pq.size() >= k) { + const ScalarType dmin = ClassType::LowerClamp(dc - node->boxHalfDims, (ScalarType)0).SquaredNorm(); + if (dmin >= mindmax) { + return; + } + } + + if (node->IsLeaf()) { + bool someInserted = true; + for (TreeType::ObjPtrVectorConstIterator si=node->oBegin; si!=node->oEnd; ++si) { + ScalarType minDst = (pq.size() >= k) ? (pq.top().minDist) : (std::numeric_limits::max()); + ClosestObjType cobj; + if (getPointDistance(*(*si), p, minDst, cobj.closestPt)) { + someInserted = true; + cobj.pObj = (*si); + cobj.minDist = minDst; + if (pq.size() >= k) { + pq.pop(); + } + pq.push(cobj); + } + } + if (someInserted) { + if (pq.size() >= k) { + const ScalarType dmax = pq.top().minDist; + const ScalarType sqdmax = dmax * dmax; + if (sqdmax < mindmax) { + mindmax = sqdmax; + } + } + } + } + else { + if (node->children[0] != 0) { + DepthFirstCollect(p, node->children[0], mindmax, k, pq, getPointDistance); + } + if (node->children[1] != 0) { + DepthFirstCollect(p, node->children[1], mindmax, k, pq, getPointDistance); + } + } +} + +template +template +unsigned int AABBBinaryTreeSearch::GetKClosests(OBJPOINTDISTANCEFUNCT & getPointDistance, const unsigned int k, const CoordType & p, OBJPTRCONTAINER & closestObjs, DISTCONTAINER & distances, POINTCONTAINER & closestPts) const { + typedef std::vector NodePtrVector; + typedef typename NodePtrVector::const_iterator NodePtrVector_ci; + + const TreeType & t = this->tree; + TreeType::NodeType * pRoot = t.pRoot; + + if (pRoot == 0) { + return (0); + } + + PQueueType pq; + ScalarType mindmax = std::numeric_limits::max(); + + ClassType::DepthFirstCollect(p, pRoot, mindmax, k, pq, getPointDistance); + + const unsigned int sz = pq.size(); + + while (!pq.empty()) { + ClosestObjType cobj = pq.top(); + pq.pop(); + closestObjs.push_back(cobj.pObj); + distances.push_back(cobj.minDist); + closestPts.push_back(cobj.closestPt); + } + + return (sz); +} + template typename AABBBinaryTreeSearch::CoordType AABBBinaryTreeSearch::Abs(const CoordType & p) { return (CoordType(math::Abs(p[0]), math::Abs(p[1]), math::Abs(p[2]))); @@ -223,7 +331,7 @@ typename AABBBinaryTreeSearch::CoordType A } template -typename AABBBinaryTreeSearch::ScalarType AABBBinaryTreeSearch::MinimumMaxDistance(const ScalarType & currMinMaxDist, const std::vector & n, const CoordType & p) { +typename AABBBinaryTreeSearch::ScalarType AABBBinaryTreeSearch::MinimumMaxSquareDistance(const ScalarType & currMinMaxDist, const std::vector & n, const CoordType & p) { typedef std::vector NodePtrVector; typedef typename NodePtrVector::const_iterator NodePtrVector_ci; @@ -241,6 +349,13 @@ typename AABBBinaryTreeSearch::ScalarType return (minMaxDist); } +template +void AABBBinaryTreeSearch::MinMaxSquareDistance(const typename TreeType::NodeType * n, const CoordType & p, ScalarType & dmin, ScalarType & dmax) { + const CoordType dc = ClassType::Abs(p - n->boxCenter); + dmax = (dc + n->boxHalfDims).SquaredNorm(); + dmin = ClassType::LowerClamp(dc - n->boxHalfDims, (ScalarType)0).SquaredNorm(); +} + } // end namespace vcg #endif // #ifndef __VCGLIB_AABBBINARYTREESEARCH