247 lines
9.5 KiB
C++
247 lines
9.5 KiB
C++
/****************************************************************************
|
|
* VCGLib o o *
|
|
* Visual and Computer Graphics Library o o *
|
|
* _ O _ *
|
|
* Copyright(C) 2005 \/)\/ *
|
|
* Visual Computing Lab /\/| *
|
|
* ISTI - Italian National Research Council | *
|
|
* \ *
|
|
* All rights reserved. *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; either version 2 of the License, or *
|
|
* (at your option) any later version. *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
|
* for more details. *
|
|
* *
|
|
****************************************************************************/
|
|
/****************************************************************************
|
|
History
|
|
|
|
$Log: not supported by cvs2svn $
|
|
|
|
|
|
|
|
****************************************************************************/
|
|
|
|
#ifndef __VCGLIB_AABBBINARYTREESEARCH
|
|
#define __VCGLIB_AABBBINARYTREESEARCH
|
|
|
|
// stl headers
|
|
#include <limits>
|
|
#include <vector>
|
|
|
|
// vcg headers
|
|
#include <vcg/space/index/aabb_binary_tree.h>
|
|
|
|
|
|
/***************************************************************************************/
|
|
|
|
namespace vcg {
|
|
|
|
/*
|
|
|
|
Class AABBBinaryTreeSearch
|
|
|
|
SAMPLE USAGE:
|
|
|
|
NOTES:
|
|
|
|
*/
|
|
|
|
template <class OBJTYPE, class SCALARTYPE, class NODEUSERATATYPE>
|
|
class AABBBinaryTreeSearch {
|
|
public:
|
|
struct NodeSearchDataType {
|
|
typename SCALARTYPE minDist;
|
|
};
|
|
struct NodeAuxDataType {
|
|
NodeSearchDataType searchData;
|
|
NODEUSERATATYPE userData;
|
|
};
|
|
typedef AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEUSERATATYPE> ClassType;
|
|
typedef OBJTYPE ObjType;
|
|
typedef SCALARTYPE ScalarType;
|
|
typedef NODEUSERATATYPE NodeUserDataType;
|
|
typedef AABBBinaryTree<ObjType, ScalarType, NodeAuxDataType> TreeType;
|
|
typedef typename TreeType::ObjPtr ObjPtr;
|
|
typedef typename TreeType::CoordType CoordType;
|
|
|
|
inline AABBBinaryTreeSearch(void);
|
|
inline ~AABBBinaryTreeSearch(void);
|
|
|
|
inline void Clear(void);
|
|
|
|
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);
|
|
|
|
template <class OBJPOINTDISTANCEFUNCT>
|
|
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<typename TreeType::NodeType *> & n, const CoordType & p);
|
|
|
|
};
|
|
|
|
/***************************************************************************************/
|
|
|
|
|
|
|
|
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
|
|
AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeSearch(void) {
|
|
|
|
}
|
|
|
|
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
|
|
AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::~AABBBinaryTreeSearch(void) {
|
|
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>
|
|
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::TreeType & AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Tree(void) {
|
|
return (this->tree);
|
|
}
|
|
|
|
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
|
|
const typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::TreeType & AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Tree(void) const {
|
|
return (this->tree);
|
|
}
|
|
|
|
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
|
|
template <class OBJPOINTDISTANCEFUNCT>
|
|
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ObjPtr AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::GetClosest(OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, ScalarType & minDist, CoordType & res) 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);
|
|
}
|
|
|
|
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<ScalarType>::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<ScalarType>::max();
|
|
ScalarType closestDistSq = std::numeric_limits<ScalarType>::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 <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
|
|
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])));
|
|
}
|
|
|
|
template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
|
|
typename AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::CoordType AABBBinaryTreeSearch<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::LowerClamp(const CoordType & p, const ScalarType & r) {
|
|
return (CoordType(math::Max<ScalarType>(p[0], r), math::Max<ScalarType>(p[1], r), math::Max<ScalarType>(p[2], r)));
|
|
}
|
|
|
|
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) {
|
|
typedef std::vector<typename TreeType::NodeType *> 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
|