From 574063e0521d1be2713cfe2bc6e0c0e3029dccf4 Mon Sep 17 00:00:00 2001 From: dibenedetto Date: Sat, 10 Sep 2005 13:22:44 +0000 Subject: [PATCH] First Commit --- vcg/space/index/aabb_binary_tree.h | 299 ++++++++++++++++++++++ vcg/space/index/aabb_binary_tree_search.h | 215 ++++++++++++++++ vcg/space/index/aabb_binary_tree_utils.h | 101 ++++++++ 3 files changed, 615 insertions(+) create mode 100644 vcg/space/index/aabb_binary_tree.h create mode 100644 vcg/space/index/aabb_binary_tree_search.h create mode 100644 vcg/space/index/aabb_binary_tree_utils.h diff --git a/vcg/space/index/aabb_binary_tree.h b/vcg/space/index/aabb_binary_tree.h new file mode 100644 index 00000000..4f660aa9 --- /dev/null +++ b/vcg/space/index/aabb_binary_tree.h @@ -0,0 +1,299 @@ +#ifndef __VCGLIB_AABBBINARYTREE +#define __VCGLIB_AABBBINARYTREE + +// stl headers +#include + +// vcg headers +#include +#include + + +/***************************************************************************************/ + +namespace vcg { + +/* + +Class AABBBinaryTree + +SAMPLE USAGE: + +NOTES: + +*/ + +template +class AABBBinaryTree { + public: + typedef AABBBinaryTree ClassType; + typedef OBJTYPE ObjType; + typedef ObjType * ObjPtr; + typedef SCALARTYPE ScalarType; + typedef NODEAUXDATATYPE NodeAuxDataType; + typedef Point3 CoordType; + + typedef std::vector ObjPtrVector; + typedef typename ObjPtrVector::iterator ObjPtrVectorIterator; + typedef typename ObjPtrVector::const_iterator ObjPtrVectorConstIterator; + + public: + class AABBBinaryTreeNode { + public: + AABBBinaryTreeNode * parent; + AABBBinaryTreeNode * children[2]; + CoordType boxCenter; + CoordType boxHalfDims; + ObjPtrVectorIterator oBegin; + ObjPtrVectorIterator oEnd; + unsigned int numObjects; + NodeAuxDataType auxData; + + inline AABBBinaryTreeNode(AABBBinaryTreeNode * pParent = 0); + inline ~AABBBinaryTreeNode(void); + + inline void Clear(void); + + inline bool IsLeaf(void) const; + }; + + typedef AABBBinaryTreeNode NodeType; + + ObjPtrVector pObjects; + NodeType * pRoot; + + inline AABBBinaryTree(void); + inline ~AABBBinaryTree(void); + + inline void Clear(void); + + 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); + + protected: + template + inline static NodeType * BoundObjects(NodeType * parent, const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const unsigned int size, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance, OBJBOXFUNCT & getBox, OBJBARYCENTERFUNCT & getBarycenter); + + template + inline static int BalanceMedian(const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const int size, const int splitAxis, OBJBARYCENTERFUNCT & getBarycenter, ObjPtrVectorIterator & medianIter); +}; + +/***************************************************************************************/ + + + +template +AABBBinaryTree::AABBBinaryTree(void) { + this->pObjects.clear(); + this->pRoot = 0; +} + +template +AABBBinaryTree::~AABBBinaryTree(void) { + this->pObjects.clear(); + delete this->pRoot; +} + +template +void AABBBinaryTree::Clear(void) { + this->pObjects.clear(); + delete this->pRoot; + this->pRoot = 0; +} + +template +template +bool AABBBinaryTree::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) { + this->Clear(); + + if ((maxElemsPerLeaf == 0) && (leafBoxMaxVolume <= ((ScalarType)0))) { + return (false); + } + + this->pObjects.reserve(size); + for (OBJITERATOR oi=oBegin; oi!=oEnd; ++oi) { + this->pObjects.push_back(objPtr(*oi)); + } + + this->pRoot = ClassType::BoundObjects(0, this->pObjects.begin(), this->pObjects.end(), (unsigned int)(this->pObjects.size()), maxElemsPerLeaf, leafBoxMaxVolume, useVariance, objBox, objBarycenter); + + return (this->pRoot != 0); +} + +template +template +typename AABBBinaryTree::NodeType * AABBBinaryTree::BoundObjects(NodeType * parent, const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const unsigned int size, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance, OBJBOXFUNCT & getBox, OBJBARYCENTERFUNCT & getBarycenter) { + if (size <= 0) { + return (0); + } + + NodeType * pNode = new NodeType(parent); + if (pNode == 0) { + return (0); + } + + pNode->children[0] = 0; + pNode->children[1] = 0; + + pNode->oBegin = oBegin; + pNode->oEnd = oEnd; + + Box3 bbox; + bbox.SetNull(); + for (ObjPtrVectorConstIterator oi=pNode->oBegin; oi!=pNode->oEnd; ++oi) { + const Box3 tbox = getBox(*(*oi)); + bbox.Add(tbox); + } + + pNode->boxCenter = bbox.Center(); + pNode->boxHalfDims = bbox.Dim() / ((ScalarType)2); + + const bool bMaxObjectsReached = (((maxElemsPerLeaf > 0) && (size <= maxElemsPerLeaf)) || (size == 1)); + const bool bMaxVolumeReached = ((leafBoxMaxVolume > ((ScalarType)0)) && (bbox.Volume() <= leafBoxMaxVolume)); + const bool isLeaf = bMaxObjectsReached || bMaxVolumeReached; + + if (isLeaf) { + return (pNode); + } + + pNode->numObjects = size; + + CoordType pSplit; + + if (useVariance) { + CoordType mean((ScalarType)0, (ScalarType)0, (ScalarType)0); + CoordType variance((ScalarType)0, (ScalarType)0, (ScalarType)0); + for (ObjPtrVectorIterator oi=oBegin; oi!=oEnd; ++oi) { + const CoordType bc = getBarycenter(*(*oi)); + mean += bc; + variance[0] += bc[0] * bc[0]; + variance[1] += bc[1] * bc[1]; + variance[2] += bc[2] * bc[2]; + } + variance[0] -= (mean[0] * mean[0]) / ((ScalarType)size); + variance[1] -= (mean[1] * mean[1]) / ((ScalarType)size); + variance[2] -= (mean[2] * mean[2]) / ((ScalarType)size); + pSplit = variance; + } + else { + pSplit = pNode->boxHalfDims; + } + + ScalarType maxDim = pSplit[0]; + int splitAxis = 0; + if (maxDim < pSplit[1]) { + maxDim = pSplit[1]; + splitAxis = 1; + } + if (maxDim < pSplit[2]) { + maxDim = pSplit[2]; + splitAxis = 2; + } + + ObjPtrVectorIterator median; + const int lSize = ClassType::BalanceMedian(pNode->oBegin, pNode->oEnd, size, splitAxis, getBarycenter, median); + const int rSize = size - lSize; + + if (lSize > 0) { + pNode->children[0] = ClassType::BoundObjects(pNode, pNode->oBegin, median, lSize, maxElemsPerLeaf, leafBoxMaxVolume, useVariance, getBox, getBarycenter); + if (pNode->children[0] == 0) { + delete pNode; + return (0); + } + } + + if (rSize > 0) { + pNode->children[1] = ClassType::BoundObjects(pNode, median, pNode->oEnd, rSize, maxElemsPerLeaf, leafBoxMaxVolume, useVariance, getBox, getBarycenter); + if (pNode->children[1] == 0) { + delete pNode; + return (0); + } + } + + return (pNode); +} + +template +template +int AABBBinaryTree::BalanceMedian(const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const int size, const int splitAxis, OBJBARYCENTERFUNCT & getBarycenter, ObjPtrVectorIterator & medianIter) { + const int iMedian = (size + 1) / 2; + + ObjPtrVectorIterator l, r, i, j; + ObjPtr iTmp; + ScalarType pos; + ObjPtrVectorIterator median = oBegin + iMedian; + + l = oBegin; + r = oEnd - 1; + + while (l < r) { + pos = getBarycenter(*(*r))[splitAxis]; + + i = l; + j = r - 1; + + while (true) { + while ((getBarycenter(*(*i))[splitAxis] <= pos) && (i < r)) { + i++; + } + while ((getBarycenter(*(*j))[splitAxis] > pos) && (j > l)) { + j--; + } + if (i >= j) { + break; + } + iTmp = (*i); + (*i) = (*j); + (*j) = iTmp; + } + + iTmp = (*i); + (*i) = (*r); + (*r) = iTmp; + + if (i >= (median)) { + r = i - 1; + } + if (i <= (median)) { + l = i + 1; + } + } + + medianIter = median; + + return (iMedian); +} + +template +AABBBinaryTree::AABBBinaryTreeNode::AABBBinaryTreeNode(NodeType * pParent) { + this->parent = pParent; + this->children[0] = 0; + this->children[1] = 0; + this->numObjects = 0; +} + +template +AABBBinaryTree::AABBBinaryTreeNode::~AABBBinaryTreeNode(void) { + delete this->children[0]; + delete this->children[1]; +} + +template +void AABBBinaryTree::AABBBinaryTreeNode::Clear(void) { + delete this->children[0]; + this->children[0] = 0; + + delete this->children[1]; + this->children[1] = 0; + + this->numObjects = 0; +} + +template +bool AABBBinaryTree::AABBBinaryTreeNode::IsLeaf(void) const { + return ((this->children[0] == 0) && (this->children[1] == 0)); +} + +} // end namespace vcg + +#endif // #ifndef __VCGLIB_AABBBINARYTREE diff --git a/vcg/space/index/aabb_binary_tree_search.h b/vcg/space/index/aabb_binary_tree_search.h new file mode 100644 index 00000000..8c518898 --- /dev/null +++ b/vcg/space/index/aabb_binary_tree_search.h @@ -0,0 +1,215 @@ +#ifndef __VCGLIB_AABBBINARYTREESEARCH +#define __VCGLIB_AABBBINARYTREESEARCH + +// stl headers +#include +#include + +// vcg headers +#include + + +/***************************************************************************************/ + +namespace vcg { + +/* + +Class AABBBinaryTreeSearch + +SAMPLE USAGE: + +NOTES: + +*/ + +template +class AABBBinaryTreeSearch { + public: + struct NodeSearchDataType { + typename SCALARTYPE minDist; + }; + struct NodeAuxDataType { + NodeSearchDataType searchData; + NODEUSERATATYPE userData; + }; + typedef AABBBinaryTreeSearch ClassType; + typedef OBJTYPE ObjType; + typedef SCALARTYPE ScalarType; + typedef NODEUSERATATYPE NodeUserDataType; + typedef AABBBinaryTree TreeType; + typedef typename TreeType::ObjPtr ObjPtr; + typedef typename TreeType::CoordType CoordType; + + inline AABBBinaryTreeSearch(void); + inline ~AABBBinaryTreeSearch(void); + + inline void Clear(void); + + 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); + + template + ObjPtr GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) const; + + inline TreeType & Tree(void); + inline const TreeType & Tree(void) const; + + protected: + 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); + +}; + +/***************************************************************************************/ + + + +template +AABBBinaryTreeSearch::AABBBinaryTreeSearch(void) { + +} + +template +AABBBinaryTreeSearch::~AABBBinaryTreeSearch(void) { + 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); +} + +template +const typename AABBBinaryTreeSearch::TreeType & AABBBinaryTreeSearch::Tree(void) const { + return (this->tree); +} + +template +template +typename AABBBinaryTreeSearch::ObjPtr AABBBinaryTreeSearch::GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) 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); + } + + NodePtrVector clist1; + NodePtrVector clist2; + NodePtrVector leaves; + + NodePtrVector * candidates = &clist1; + NodePtrVector * newCandidates = &clist2; + + clist1.reserve(t.pObjects.size()); + clist2.reserve(t.pObjects.size()); + leaves.reserve(t.pObjects.size()); + + clist1.resize(0); + clist2.resize(0); + leaves.resize(0); + + ScalarType minMaxDist = std::numeric_limits::max(); + + candidates->push_back(t.pRoot); + + while (!candidates->empty()) { + newCandidates->resize(0); + minMaxDist = ClassType::MinimumMaxDistance(minMaxDist, *candidates, p); + for (NodePtrVector_ci ci=candidates->begin(); ci!=candidates->end(); ++ci) { + if ((*ci)->auxData.searchData.minDist < minMaxDist) { + if ((*ci)->IsLeaf()) { + leaves.push_back(*ci); + } + else { + if ((*ci)->children[0] != 0) { + newCandidates->push_back((*ci)->children[0]); + } + if ((*ci)->children[1] != 0) { + newCandidates->push_back((*ci)->children[1]); + } + } + } + } + NodePtrVector * cSwap = candidates; + candidates = newCandidates; + newCandidates = cSwap; + } + + clist1.clear(); + clist2.clear(); + + ObjPtr closestObject = 0; + CoordType closestPoint; + ScalarType closestDist = std::numeric_limits::max(); + ScalarType closestDistSq = std::numeric_limits::max(); + + for (NodePtrVector_ci ci=leaves.begin(); ci!=leaves.end(); ++ci) { + if ((*ci)->auxData.searchData.minDist < closestDistSq) { + for (TreeType::ObjPtrVectorConstIterator si=(*ci)->oBegin; si!=(*ci)->oEnd; ++si) { + if (getPointDistance(*(*si), p, closestDist, closestPoint)) { + closestDistSq = closestDist * closestDist; + closestObject = (*si); + } + } + } + } + + leaves.clear(); + + res = closestPoint; + minDist = closestDist; + + return (closestObject); +} + +template +typename AABBBinaryTreeSearch::CoordType AABBBinaryTreeSearch::Abs(const CoordType & p) { + return (CoordType(math::Abs(p[0]), math::Abs(p[1]), math::Abs(p[2]))); +} + +template +typename AABBBinaryTreeSearch::CoordType AABBBinaryTreeSearch::LowerClamp(const CoordType & p, const ScalarType & r) { + return (CoordType(math::Max(p[0], r), math::Max(p[1], r), math::Max(p[2], r))); +} + +template +typename AABBBinaryTreeSearch::ScalarType AABBBinaryTreeSearch::MinimumMaxDistance(const ScalarType & currMinMaxDist, const std::vector & n, const CoordType & p) { + typedef std::vector NodePtrVector; + typedef typename NodePtrVector::const_iterator NodePtrVector_ci; + + ScalarType minMaxDist = currMinMaxDist; + + for (NodePtrVector_ci bv=n.begin(); bv!=n.end(); ++bv) { + const CoordType dc = ClassType::Abs(p - (*bv)->boxCenter); + const ScalarType maxDist = (dc + (*bv)->boxHalfDims).SquaredNorm(); + (*bv)->auxData.searchData.minDist = ClassType::LowerClamp(dc - (*bv)->boxHalfDims, (ScalarType)0).SquaredNorm(); + if (maxDist < minMaxDist) { + minMaxDist = maxDist; + } + } + + return (minMaxDist); +} + +} // end namespace vcg + +#endif // #ifndef __VCGLIB_AABBBINARYTREESEARCH diff --git a/vcg/space/index/aabb_binary_tree_utils.h b/vcg/space/index/aabb_binary_tree_utils.h new file mode 100644 index 00000000..01783b9e --- /dev/null +++ b/vcg/space/index/aabb_binary_tree_utils.h @@ -0,0 +1,101 @@ +#ifndef __VCGLIB_AABBBINARYTREEUTILS +#define __VCGLIB_AABBBINARYTREEUTILS + +// vcg headers +#include +#include + + +/***************************************************************************************/ + +namespace vcg { + +/* + +Class AABBBinaryTreeUtils + +SAMPLE USAGE: + +NOTES: + +*/ + +template +class AABBBinaryTreeUtils { + public: + typedef AABBBinaryTreeUtils ClassType; + typedef SCALARTYPE ScalarType; + + template + static inline Point3 ConvertP3(const Point3 & p) { + return (Point3(S(p[0]), S(p[1]), S(p[2]))); + } + + class EmptyClass { + }; + + class ObjIteratorPtrFunct { + public: + template + inline T * operator () (T & t) { + return (&t); + } + + template + inline T * operator () (T * & t) { + return (t); + } + }; + + template + class FaceBoxFunct { + public: + typedef FACETYPE FaceType; + + inline Box3 operator () (const FaceType & f) { + Box3 box; + box.SetNull(); + + box.Add(AABBBinaryTreeUtils::ConvertP3(f.P(0))); + box.Add(AABBBinaryTreeUtils::ConvertP3(f.P(1))); + box.Add(AABBBinaryTreeUtils::ConvertP3(f.P(2))); + + return (box); + } + }; + + template + class ObjBarycenterFunct { + public: + typedef OBJTYPE ObjType; + + inline Point3 operator () (const ObjType & obj) { + return (AABBBinaryTreeUtils::ConvertP3(obj.Barycenter())); + } + }; + + template + class FacePointDistanceFunct { + public: + typedef FACETYPE FaceType; + + inline bool operator () (const FaceType & f, const Point3 & p, ScalarType & minDist, Point3 & res) { + FaceType::ScalarType fdist; + const AFace::CoordType fp = ConvertP3(p); + FaceType::CoordType fres; + + const bool br = face::PointDistance(f, fp, fdist, fres); + minDist = (ScalarType)(fdist); + res = ConvertP3(fres); + + return (br); + } + }; + +}; + +/***************************************************************************************/ + +} // end namespace vcg + +#endif // #ifndef __VCGLIB_AABBBINARYTREEUTILS