manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Modified interface, added GetKClosest().

This commit is contained in:
Marco Di Benedetto 2005-09-16 10:04:15 +00:00
parent f8ba5762e2
commit 15ce3360ed
1 changed files with 135 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
vcg/space/index/aabb_binary_tree_search.h
View File

@ -24,6 +24,9 @@
History History
$Log: not supported by cvs2svn $ $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 // stl headers
#include <limits> #include <limits>
#include <vector> #include <vector>
#include <queue>
#include <deque>
// vcg headers // vcg headers
#include <vcg/space/index/aabb_binary_tree.h> #include <vcg/space/index/aabb_binary_tree.h>
@ -75,23 +80,45 @@ class AABBBinaryTreeSearch {
inline AABBBinaryTreeSearch(void); inline AABBBinaryTreeSearch(void);
inline ~AABBBinaryTreeSearch(void); inline ~AABBBinaryTreeSearch(void);
inline void Clear(void); inline TreeType & Tree(void);
inline const TreeType & Tree(void) const;
template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT> template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
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 <class OBJPOINTDISTANCEFUNCT> template <class OBJPOINTDISTANCEFUNCT>
ObjPtr GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const; ObjPtr GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const;
inline TreeType & Tree(void); template <class OBJPOINTDISTANCEFUNCT, class OBJPTRCONTAINER, class DISTCONTAINER, class POINTCONTAINER>
inline const TreeType & Tree(void) const; unsigned int GetKClosests(OBJPOINTDISTANCEFUNCT & getPointDistance, const unsigned int k, const CoordType & p, OBJPTRCONTAINER & closestObjs, DISTCONTAINER & distances, POINTCONTAINER & closestPts) const;
protected: 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::ClosestObjType, std::deque<typename ClassType::ClosestObjType>, typename ClassType::CompareClosest> PQueueType;
TreeType tree; TreeType tree;
static inline CoordType Abs(const CoordType & p); static inline CoordType Abs(const CoordType & p);
static inline CoordType LowerClamp(const CoordType & p, const ScalarType & r); static inline CoordType LowerClamp(const CoordType & p, const ScalarType & r);
static inline ScalarType MinimumMaxDistance(const ScalarType & currMinMaxDist, const std::vector<typename TreeType::NodeType *> & n, const CoordType & p); static inline ScalarType MinimumMaxSquareDistance(const ScalarType & currMinMaxDist, const std::vector<typename TreeType::NodeType *> & n, const CoordType & p);
static inline void MinMaxSquareDistance(const typename TreeType::NodeType * n, const CoordType & p, ScalarType & dmin, ScalarType & dmax);
template <class OBJPOINTDISTANCEFUNCT>
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<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::~AABBBinaryTreeSearc
this->Clear(); this->Clear();
} }
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
void AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Clear(void) {
this->tree.Clear();
}
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
bool AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::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 <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE> template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::TreeType & AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Tree(void) { typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::TreeType & AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Tree(void) {
return (this->tree); return (this->tree);
@ -130,6 +146,17 @@ const typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::TreeT
return (this->tree); return (this->tree);
} }
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
bool AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::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 <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
void AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Clear(void) {
this->tree.Clear();
}
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE> template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
template <class OBJPOINTDISTANCEFUNCT> template <class OBJPOINTDISTANCEFUNCT>
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ObjPtr AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const { typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ObjPtr AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const {
@ -158,13 +185,16 @@ typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ObjPtr AABB
clist2.resize(0); clist2.resize(0);
leaves.resize(0); leaves.resize(0);
#ifdef max
#undef max
#endif
ScalarType minMaxDist = std::numeric_limits<ScalarType>::max(); ScalarType minMaxDist = std::numeric_limits<ScalarType>::max();
candidates->push_back(t.pRoot); candidates->push_back(t.pRoot);
while (!candidates->empty()) { while (!candidates->empty()) {
newCandidates->resize(0); 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) { for (NodePtrVector_ci ci=candidates->begin(); ci!=candidates->end(); ++ci) {
if ((*ci)->auxData.searchData.minDist < minMaxDist) { if ((*ci)->auxData.searchData.minDist < minMaxDist) {
if ((*ci)->IsLeaf()) { if ((*ci)->IsLeaf()) {
@ -190,8 +220,8 @@ typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ObjPtr AABB
ObjPtr closestObject = 0; ObjPtr closestObject = 0;
CoordType closestPoint; CoordType closestPoint;
ScalarType closestDist = std::numeric_limits<ScalarType>::max(); ScalarType closestDist = math::Sqrt(minMaxDist) + std::numeric_limits<ScalarType>::epsilon();
ScalarType closestDistSq = std::numeric_limits<ScalarType>::max(); ScalarType closestDistSq = minMaxDist + std::numeric_limits<ScalarType>::epsilon();
for (NodePtrVector_ci ci=leaves.begin(); ci!=leaves.end(); ++ci) { for (NodePtrVector_ci ci=leaves.begin(); ci!=leaves.end(); ++ci) {
if ((*ci)->auxData.searchData.minDist < closestDistSq) { if ((*ci)->auxData.searchData.minDist < closestDistSq) {
@ -212,6 +242,84 @@ typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ObjPtr AABB
return (closestObject); return (closestObject);
} }
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
template <class OBJPOINTDISTANCEFUNCT>
void AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::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<ScalarType>::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 <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
template <class OBJPOINTDISTANCEFUNCT, class OBJPTRCONTAINER, class DISTCONTAINER, class POINTCONTAINER>
unsigned int AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::GetKClosests(OBJPOINTDISTANCEFUNCT & getPointDistance, const unsigned int k, const CoordType & p, OBJPTRCONTAINER & closestObjs, DISTCONTAINER & distances, POINTCONTAINER & closestPts) const {
typedef std::vector<typename TreeType::NodeType *> 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<ScalarType>::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 <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE> template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::CoordType AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Abs(const CoordType & p) { typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::CoordType AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Abs(const CoordType & p) {
return (CoordType(math::Abs(p[0]), math::Abs(p[1]), math::Abs(p[2]))); return (CoordType(math::Abs(p[0]), math::Abs(p[1]), math::Abs(p[2])));
@ -223,7 +331,7 @@ typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::CoordType A
} }
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE> template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ScalarType AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::MinimumMaxDistance(const ScalarType & currMinMaxDist, const std::vector<typename TreeType::NodeType *> & n, const CoordType & p) { typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ScalarType AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::MinimumMaxSquareDistance(const ScalarType & currMinMaxDist, const std::vector<typename TreeType::NodeType *> & n, const CoordType & p) {
typedef std::vector<typename TreeType::NodeType *> NodePtrVector; typedef std::vector<typename TreeType::NodeType *> NodePtrVector;
typedef typename NodePtrVector::const_iterator NodePtrVector_ci; typedef typename NodePtrVector::const_iterator NodePtrVector_ci;
@ -241,6 +349,13 @@ typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ScalarType
return (minMaxDist); return (minMaxDist);
} }
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
void AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::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 } // end namespace vcg
#endif // #ifndef __VCGLIB_AABBBINARYTREESEARCH #endif // #ifndef __VCGLIB_AABBBINARYTREESEARCH