Program Listing for File AABBtree.cc¶
↰ Return to documentation for file (AABBtree.cc)
/*--------------------------------------------------------------------------*\
| |
| Copyright (C) 2018 |
| |
| , __ , __ |
| /|/ \ /|/ \ |
| | __/ _ ,_ | __/ _ ,_ |
| | \|/ / | | | | \|/ / | | | |
| |(__/|__/ |_/ \_/|/|(__/|__/ |_/ \_/|/ |
| /| /| |
| \| \| |
| |
| Paolo Bevilacqua and Enrico Bertolazzi |
| |
| (1) Dipartimento di Ingegneria e Scienza dell'Informazione |
| (2) Dipartimento di Ingegneria Industriale |
| |
| Universita` degli Studi di Trento |
| email: paolo.bevilacqua@unitn.it |
| email: enrico.bertolazzi@unitn.it |
| |
\*--------------------------------------------------------------------------*/
#include "Clothoids.hh"
// Workaround for Visual Studio
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#include <algorithm>
namespace G2lib {
using std::abs;
using std::min;
using std::max;
using std::numeric_limits;
/*\
| ____ ____
| | __ )| __ ) _____ __
| | _ \| _ \ / _ \ \/ /
| | |_) | |_) | (_) > <
| |____/|____/ \___/_/\_\
\*/
void
BBox::join( vector<PtrBBox> const & bboxes ) {
if ( bboxes.empty() ) {
m_xmin = m_ymin = m_xmax = m_ymax = 0;
} else {
vector<PtrBBox>::const_iterator it = bboxes.begin();
m_xmin = (*it)->m_xmin;
m_ymin = (*it)->m_ymin;
m_xmax = (*it)->m_xmax;
m_ymax = (*it)->m_ymax;
for ( ++it; it != bboxes.end(); ++it ) {
BBox const & currBox = **it;
if ( currBox.m_xmin < m_xmin ) m_xmin = currBox.m_xmin;
if ( currBox.m_ymin < m_ymin ) m_ymin = currBox.m_ymin;
if ( currBox.m_xmax > m_xmax ) m_xmax = currBox.m_xmax;
if ( currBox.m_ymax > m_ymax ) m_ymax = currBox.m_ymax;
}
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
real_type
BBox::distance( real_type x, real_type y ) const {
/*\
|
| 6 7 8
| +-------------+
| | |
| 3 | 4 | 5
| | |
| +-------------+
| 0 1 2
|
\*/
int_type icase = 4;
if ( x < m_xmin ) icase = 3;
else if ( x > m_xmax ) icase = 5;
if ( y < m_ymin ) icase -= 3;
else if ( y > m_ymax ) icase += 3;
real_type dst = 0;
switch ( icase ) {
case 0: dst = hypot( x-m_xmin, y-m_ymin); break;
case 1: dst = m_ymin-y; break;
case 2: dst = hypot( x-m_xmax, y-m_ymin); break;
case 3: dst = m_xmin-x; break;
case 4: break;
case 5: dst = x-m_xmax; break;
case 6: dst = hypot( x-m_xmin, y-m_ymax); break;
case 7: dst = y-m_ymax; break;
case 8: dst = hypot( x-m_xmax, y-m_ymax); break;
}
return dst;
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
real_type
BBox::maxDistance( real_type x, real_type y ) const {
real_type dx = max( abs(x-m_xmin), abs(x-m_xmax) );
real_type dy = max( abs(y-m_ymin), abs(y-m_ymax) );
return hypot(dx,dy);
}
/*\
| _ _ ____ ____ _
| / \ / \ | __ )| __ )| |_ _ __ ___ ___
| / _ \ / _ \ | _ \| _ \| __| '__/ _ \/ _ \
| / ___ \ / ___ \| |_) | |_) | |_| | | __/ __/
| /_/ \_\/_/ \_\____/|____/ \__|_| \___|\___|
\*/
#ifdef G2LIB_USE_CXX11
AABBtree::AABBtree() {
pBBox.reset();
children.clear();
}
AABBtree::~AABBtree() {
pBBox.reset();
children.clear();
}
void
AABBtree::clear() {
pBBox.reset();
children.clear();
}
bool
AABBtree::empty() const {
return children.empty() && !pBBox;
}
#else
AABBtree::AABBtree()
: pBBox(nullptr)
{
children.clear();
}
AABBtree::~AABBtree() {
if ( pBBox != nullptr ) {
delete pBBox;
pBBox = nullptr;
}
children.clear();
}
void
AABBtree::clear() {
if ( pBBox != nullptr ) {
delete pBBox;
pBBox = nullptr;
}
children.clear();
}
bool
AABBtree::empty() const {
return children.empty() && pBBox == nullptr;
}
#endif
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
AABBtree::build( vector<PtrBBox> const & bboxes ) {
clear();
if ( bboxes.empty() ) return;
size_t size = bboxes.size();
if ( size == 1 ) {
this -> pBBox = bboxes.front();
return;
}
#ifdef G2LIB_USE_CXX11
pBBox = shared_ptr<BBox>( new BBox(bboxes, 0, 0) );
#else
if ( pBBox != nullptr ) { delete pBBox; pBBox = nullptr; }
pBBox = new BBox( bboxes, 0, 0 );
#endif
real_type xmin = pBBox -> Xmin();
real_type ymin = pBBox -> Ymin();
real_type xmax = pBBox -> Xmax();
real_type ymax = pBBox -> Ymax();
vector<PtrBBox> posBoxes;
vector<PtrBBox> negBoxes;
if ( (ymax - ymin) > (xmax - xmin) ) {
real_type cutPos = (ymax + ymin)/2;
vector<PtrBBox>::const_iterator it;
for ( it = bboxes.begin(); it != bboxes.end(); ++it ) {
real_type ymid = ( (*it) -> Ymin() + (*it) -> Ymax() ) / 2;
if ( ymid > cutPos ) posBoxes.push_back(*it);
else negBoxes.push_back(*it);
}
} else {
real_type cutPos = (xmax + xmin)/2;
vector<PtrBBox>::const_iterator it;
for ( it = bboxes.begin(); it != bboxes.end(); ++it ) {
real_type xmid = ( (*it) -> Xmin() + (*it) -> Xmax() ) / 2;
if ( xmid > cutPos ) posBoxes.push_back(*it);
else negBoxes.push_back(*it);
}
}
if ( negBoxes.empty() ) {
vector<PtrBBox>::iterator midIdx;
midIdx = posBoxes.begin() + posBoxes.size()/2;
negBoxes.insert( negBoxes.end(), midIdx, posBoxes.end() );
posBoxes.erase( midIdx, posBoxes.end() );
} else if ( posBoxes.empty() ) {
vector<PtrBBox>::iterator midIdx;
midIdx = negBoxes.begin() + negBoxes.size()/2;
posBoxes.insert( posBoxes.end(), midIdx, negBoxes.end() );
negBoxes.erase( midIdx, negBoxes.end() );
}
#ifdef G2LIB_USE_CXX11
PtrAABB neg = make_shared<AABBtree>();
PtrAABB pos = make_shared<AABBtree>();
#else
PtrAABB neg = new AABBtree();
PtrAABB pos = new AABBtree();
#endif
neg->build(negBoxes);
if (!neg->empty()) children.push_back(neg);
pos->build(posBoxes);
if (!pos->empty()) children.push_back(pos);
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
AABBtree::print( ostream_type & stream, int level ) const {
if ( empty() ) {
stream << "[EMPTY AABB tree]\n";
} else {
fmt::print( stream,
"BBOX xmin={:<12.4)} ymin={:<12.4)} xmax={:<12.4)} ymax={:<12.4)} level={}\n",
pBBox->m_xmin, pBBox->m_ymin, pBBox->m_xmax, pBBox->m_ymax
);
vector<PtrAABB>::const_iterator it;
for ( it = children.begin(); it != children.end(); ++it )
(*it)->print( stream, level+1 );
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
AABBtree::intersect(
AABBtree const & tree,
VecPairPtrBBox & intersectionList,
bool swap_tree
) const {
// check bbox with
if ( !tree.pBBox->collision(*pBBox) ) return;
int icase = (children.empty() ? 0 : 1) +
(tree.children.empty()? 0 : 2);
switch ( icase ) {
case 0: // both leaf
if ( swap_tree )
intersectionList.push_back( PairPtrBBox(tree.pBBox,pBBox ) );
else
intersectionList.push_back( PairPtrBBox(pBBox,tree.pBBox ) );
break;
case 1: // first is a tree, second is a leaf
{ vector<PtrAABB>::const_iterator it;
for ( it = children.begin(); it != children.end(); ++it )
tree.intersect( **it, intersectionList, !swap_tree );
}
break;
case 2: // first leaf, second is a tree
{ vector<PtrAABB>::const_iterator it;
for ( it = tree.children.begin(); it != tree.children.end(); ++it )
this->intersect( **it, intersectionList, swap_tree );
}
break;
case 3: // first is a tree, second is a tree
{ vector<PtrAABB>::const_iterator c1;
vector<PtrAABB>::const_iterator c2;
for ( c1 = children.begin(); c1 != children.end(); ++c1 )
for ( c2 = tree.children.begin(); c2 != tree.children.end(); ++c2 )
(*c1)->intersect( **c2, intersectionList, swap_tree );
}
break;
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
real_type
AABBtree::min_maxdist(
real_type x,
real_type y,
AABBtree const & tree,
real_type mmDist
) {
vector<PtrAABB> const & children = tree.children;
if ( children.empty() ) {
real_type dst = tree.pBBox->maxDistance( x, y );
return min( dst, mmDist );
}
real_type dmin = tree.pBBox->distance( x, y );
if ( dmin > mmDist ) return mmDist;
// check bbox with
vector<PtrAABB>::const_iterator it;
for ( it = children.begin(); it != children.end(); ++it )
mmDist = min_maxdist( x, y, **it, mmDist );
return mmDist;
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
AABBtree::min_maxdist_select(
real_type x,
real_type y,
real_type mmDist,
AABBtree const & tree,
VecPtrBBox & candidateList
) {
vector<PtrAABB> const & children = tree.children;
real_type dst = tree.pBBox->distance( x, y );
if ( dst <= mmDist ) {
if ( children.empty() ) {
candidateList.push_back( tree.pBBox );
} else {
// check bbox with
vector<PtrAABB>::const_iterator it;
for ( it = children.begin(); it != children.end(); ++it )
min_maxdist_select( x, y, mmDist, **it, candidateList );
}
}
}
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
void
AABBtree::min_distance(
real_type x,
real_type y,
VecPtrBBox & candidateList
) const {
real_type mmDist = min_maxdist(
x, y, *this, numeric_limits<real_type>::infinity()
);
min_maxdist_select( x, y, mmDist, *this, candidateList );
}
}
