You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
851 lines
32 KiB
851 lines
32 KiB
//////////////////////////////////////////////////////////////////// |
|
// Octree.inl |
|
// |
|
// Copyright 2007 cDc@seacave |
|
// Distributed under the Boost Software License, Version 1.0 |
|
// (See http://www.boost.org/LICENSE_1_0.txt) |
|
|
|
|
|
// D E F I N E S /////////////////////////////////////////////////// |
|
|
|
#ifdef _USE_OPENMP |
|
// minimum number of polygons for which we do multi-threading |
|
#define OCTREE_MIN_ITEMS_MINTHREAD 1024*2 |
|
#endif |
|
|
|
|
|
// S T R U C T S /////////////////////////////////////////////////// |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::CELL_TYPE() |
|
: |
|
m_child(NULL) |
|
{ |
|
} // constructor |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::~CELL_TYPE() |
|
{ |
|
delete[] m_child; |
|
} // destructor |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::Release() |
|
{ |
|
delete[] m_child; |
|
m_child = NULL; |
|
} // Release |
|
// swap the two octrees |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::Swap(CELL_TYPE& rhs) |
|
{ |
|
std::swap(m_child, rhs.m_child); |
|
if (IsLeaf()) |
|
std::swap(m_leaf, rhs.m_leaf); |
|
else |
|
std::swap(m_node, rhs.m_node); |
|
} // Swap |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// compute item's index corresponding to the containing cell |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline unsigned TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::ComputeChild(const POINT_TYPE& item) const |
|
{ |
|
ASSERT(!IsLeaf()); |
|
unsigned idx = 0; |
|
if (item[0] >= Node().center[0]) |
|
idx |= (1<<0); |
|
if (DIMS > 1) |
|
if (item[1] >= Node().center[1]) |
|
idx |= (1<<1); |
|
if (DIMS > 2) |
|
if (item[2] >= Node().center[2]) |
|
idx |= (1<<2); |
|
return idx; |
|
} // ComputeChild |
|
/*----------------------------------------------------------------*/ |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::ComputeCenter(POINT_TYPE centers[]) |
|
{ |
|
if (DIMS == 1) { |
|
centers[0] << -1; |
|
centers[1] << 1; |
|
} else |
|
if (DIMS == 2) { |
|
centers[0] << -1,-1; |
|
centers[1] << 1,-1; |
|
centers[2] << -1, 1; |
|
centers[3] << 1, 1; |
|
} else |
|
if (DIMS == 3) { |
|
centers[0] << -1,-1,-1; |
|
centers[1] << 1,-1,-1; |
|
centers[2] << -1, 1,-1; |
|
centers[3] << 1, 1,-1; |
|
centers[4] << -1,-1, 1; |
|
centers[5] << 1,-1, 1; |
|
centers[6] << -1, 1, 1; |
|
centers[7] << 1, 1, 1; |
|
} |
|
} // ComputeCenter |
|
/*----------------------------------------------------------------*/ |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline typename TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::POINT_TYPE TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::ComputeChildCenter(const POINT_TYPE& center, TYPE radius, unsigned idxChild) |
|
{ |
|
struct CENTERARR_TYPE { |
|
POINT_TYPE child[CELL_TYPE::numChildren]; |
|
inline CENTERARR_TYPE() { CELL_TYPE::ComputeCenter(child); } |
|
}; |
|
static const CENTERARR_TYPE centers; |
|
return center + centers.child[idxChild] * radius; |
|
} // ComputeChildCenter |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// count the number of items contained by the given octree-cell |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
size_t TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CELL_TYPE::GetNumItemsHeld() const |
|
{ |
|
if (IsLeaf()) |
|
return GetNumItems(); |
|
size_t numItems = 0; |
|
for (int i=0; i<numChildren; ++i) |
|
numItems += GetChild(i).GetNumItemsHeld(); |
|
return numItems; |
|
} |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
|
|
// S T R U C T S /////////////////////////////////////////////////// |
|
|
|
// build tree with the given items |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename Functor> |
|
inline TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::TOctree(const ITEMARR_TYPE& items, Functor split) |
|
{ |
|
Insert(items, split); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename Functor> |
|
inline TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::TOctree(const ITEMARR_TYPE& items, const AABB_TYPE& aabb, Functor split) |
|
{ |
|
Insert(items, aabb, split); |
|
} // constructor |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// destroy tree |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Release() |
|
{ |
|
m_indices.Release(); |
|
m_root.Release(); |
|
} // Release |
|
// swap the two octrees |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Swap(TOctree& rhs) |
|
{ |
|
std::swap(m_items, rhs.m_items); |
|
m_indices.Swap(rhs.m_indices); |
|
m_root.Swap(rhs.m_root); |
|
std::swap(m_radius, rhs.m_radius); |
|
} // Swap |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// add the given item to the tree |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename Functor, bool bForceSplit> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_Insert(CELL_TYPE& cell, const POINT_TYPE& center, TYPE radius, IDX_TYPE start, IDX_TYPE size, _InsertData<Functor>& insertData) |
|
{ |
|
ASSERT(size > 0); |
|
// if this child cell needs to be divided further |
|
if (bForceSplit || insertData.split(size, radius)) { |
|
// init node and proceed recursively |
|
ASSERT(cell.m_child == NULL); |
|
cell.m_child = new CELL_TYPE[CELL_TYPE::numChildren]; |
|
cell.Node().center = center; |
|
struct ChildData { |
|
enum { ESTART=0, EEND=CELL_TYPE::numChildren, ESIZE=CELL_TYPE::numChildren*2, EALL=CELL_TYPE::numChildren*3}; |
|
IDX_TYPE data[EALL]; |
|
ChildData() { memset(data, 0, sizeof(IDX_TYPE)*EALL); } |
|
inline IDX_TYPE Start(unsigned i) const { return data[ESTART+i]; } |
|
inline IDX_TYPE& Start(unsigned i) { return data[ESTART+i]; } |
|
inline IDX_TYPE End(unsigned i) const { return data[EEND+i]; } |
|
inline IDX_TYPE& End(unsigned i) { return data[EEND+i]; } |
|
inline IDX_TYPE Size(unsigned i) const { return data[ESIZE+i]; } |
|
inline IDX_TYPE& Size(unsigned i) { return data[ESIZE+i]; } |
|
} childD; |
|
IDX_TYPE idx(start); |
|
for (IDX_TYPE i=0; i<size; ++i) { |
|
const unsigned idxChild(cell.ComputeChild(m_items[idx])); |
|
if (childD.Size(idxChild) == 0) |
|
childD.Start(idxChild) = idx; |
|
else |
|
insertData.successors[childD.End(idxChild)] = idx; |
|
childD.End(idxChild) = idx; |
|
++childD.Size(idxChild); |
|
idx = insertData.successors[idx]; |
|
} |
|
ASSERT(idx == _InsertData<Functor>::NO_INDEX); |
|
const TYPE childRadius(radius / TYPE(2)); |
|
for (unsigned i=0; i<CELL_TYPE::numChildren; ++i) { |
|
CELL_TYPE& child = cell.m_child[i]; |
|
if (childD.Size(i) == 0) { |
|
child.Leaf().idxBegin = m_indices.size(); |
|
child.Leaf().size = 0; |
|
continue; |
|
} |
|
insertData.successors[childD.End(i)] = _InsertData<Functor>::NO_INDEX; // mark the end of child successors |
|
const POINT_TYPE childCenter(CELL_TYPE::ComputeChildCenter(center, childRadius, i)); |
|
_Insert<Functor,false>(child, childCenter, childRadius, childD.Start(i), childD.Size(i), insertData); |
|
} |
|
} else { |
|
// init leaf |
|
cell.Leaf().idxBegin = m_indices.size(); |
|
cell.Leaf().size = (SIZE_TYPE)size; |
|
for (IDX_TYPE idx=start; idx!=_InsertData<Functor>::NO_INDEX; idx=insertData.successors[idx]) |
|
m_indices.push_back(idx); |
|
} |
|
} // _Insert |
|
/*----------------------------------------------------------------*/ |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename Functor> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Insert(const ITEMARR_TYPE& items, const AABB_TYPE& aabb, Functor split) |
|
{ |
|
Release(); |
|
m_items = items.data(); |
|
// create root as node, even if we do not need to divide |
|
m_indices.Reserve(items.size()); |
|
// divide cell |
|
const POINT_TYPE center = aabb.GetCenter(); |
|
m_radius = aabb.GetSize().maxCoeff()/Type(2); |
|
// single connected list of next item indices |
|
_InsertData<Functor> insertData = {items.size(), split}; |
|
std::iota(insertData.successors.begin(), insertData.successors.end(), IDX_TYPE(1)); |
|
insertData.successors.back() = _InsertData<Functor>::NO_INDEX; |
|
// setup each cell |
|
_Insert<Functor,true>(m_root, center, m_radius, 0, items.size(), insertData); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename Functor> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Insert(const ITEMARR_TYPE& items, Functor split) |
|
{ |
|
ASSERT(!items.IsEmpty()); |
|
ASSERT(sizeof(POINT_TYPE) == sizeof(typename ITEMARR_TYPE::Type)); |
|
AABB_TYPE aabb((const POINT_TYPE*)items.data(), items.size()); |
|
aabb.Enlarge(ZEROTOLERANCE<TYPE>()*TYPE(10)); |
|
Insert(items, aabb, split); |
|
} // Insert |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CollectCells(const CELL_TYPE& cell, INSERTER& inserter) const |
|
{ |
|
if (cell.IsLeaf()) { |
|
inserter(m_indices.data()+cell.GetFirstItemIdx(), cell.GetNumItems()); |
|
return; |
|
} |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) |
|
CollectCells(cell.m_child[i], inserter); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename PARSER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_ParseCells(CELL_TYPE& cell, TYPE radius, PARSER& parser) |
|
{ |
|
if (cell.IsLeaf()) { |
|
parser(cell, radius); |
|
return; |
|
} |
|
const TYPE childRadius = radius / TYPE(2); |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) |
|
_ParseCells(cell.m_child[i], childRadius, parser); |
|
} |
|
/*----------------------------------------------------------------*/ |
|
|
|
// calls parser for each leaf of the octree (the IDX_TYPE operator has to be defined) |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::CollectCells(INSERTER& inserter) const |
|
{ |
|
CollectCells(m_root, inserter); |
|
} |
|
// calls parser for each leaf of the octree (the CELL_TYPE operator has to be defined) |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename PARSER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::ParseCells(PARSER& parser) |
|
{ |
|
_ParseCells(m_root, m_radius, parser); |
|
} |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// find all items contained by the given bounding box |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_Collect(const CELL_TYPE& cell, const AABB_TYPE& aabb, INSERTER& inserter) const |
|
{ |
|
if (cell.IsLeaf()) { |
|
// add all items contained by the bounding-box |
|
for (IDX_TYPE i=0; i<cell.Leaf().size; ++i) { |
|
const IDX_TYPE idx = m_indices[cell.Leaf().idxBegin + i]; |
|
if (aabb.Intersects(m_items[idx])) |
|
inserter(idx); |
|
} |
|
return; |
|
} |
|
// find the cells containing the box extremes |
|
const unsigned idxMin = cell.ComputeChild(aabb.ptMin); |
|
const unsigned idxMax = cell.ComputeChild(aabb.ptMax); |
|
// if both extremes are in the same cell |
|
if (idxMin == idxMax) { |
|
// all searched items are inside a single child |
|
return _Collect(cell.m_child[idxMin], aabb, inserter); |
|
} |
|
// divide the bounding-box in 2^DIMS boxes split by the cell center |
|
AABB_TYPE aabbs[CELL_TYPE::numChildren]; |
|
unsigned first; |
|
unsigned n = aabb.SplitBy(cell.Node().center, aabbs, first); |
|
if (n == 0) { |
|
// the aabb has one of the sides right on the cell division line |
|
#if 0 |
|
// so find the cell containing the aabb center and add its items |
|
const unsigned idx = cell.ComputeChild(aabb.GetCenter()); |
|
_Collect(cell.m_child[idx], aabb, inserter); |
|
#else |
|
// so collect both intersecting cells |
|
// (seems to work better for points right on the border) |
|
_Collect(cell.m_child[idxMin], aabb, inserter); |
|
_Collect(cell.m_child[idxMax], aabb, inserter); |
|
#endif |
|
return; |
|
} |
|
// collect each cell |
|
for (unsigned i=0; i<n; ++i) { |
|
// all searched items are inside a single child |
|
const AABB_TYPE& aabbChild = aabbs[(first+i)%CELL_TYPE::numChildren]; |
|
const unsigned idx = cell.ComputeChild(aabbChild.ptMin); |
|
_Collect(cell.m_child[idx], aabbChild, inserter); |
|
} |
|
} // Collect |
|
// find all items contained by the cells intersected by the given line |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER, typename COLLECTOR> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_Collect(const CELL_TYPE& cell, TYPE radius, const COLLECTOR& collector, INSERTER& inserter) const |
|
{ |
|
ASSERT(!cell.IsLeaf()); |
|
const TYPE childRadius = radius / TYPE(2); |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) { |
|
const CELL_TYPE& childCell = cell.m_child[i]; |
|
if (childCell.IsLeaf()) { |
|
if (collector.Intersects(CELL_TYPE::ComputeChildCenter(cell.GetCenter(), childRadius, i), childRadius)) |
|
inserter(m_indices.data()+childCell.GetFirstItemIdx(), childCell.GetNumItems()); |
|
} else { |
|
if (collector.Intersects(childCell.Node().center, childRadius)) |
|
_Collect(childCell, childRadius, collector, inserter); |
|
} |
|
} |
|
} // Collect |
|
/*----------------------------------------------------------------*/ |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(INSERTER& inserter, const AABB_TYPE& aabb) const |
|
{ |
|
_Collect(m_root, aabb, inserter); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(IDXARR_TYPE& indices, const AABB_TYPE& aabb) const |
|
{ |
|
IndexInserter inserter(indices); |
|
_Collect(m_root, aabb, inserter); |
|
} |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(INSERTER& inserter, const POINT_TYPE& center, TYPE radius) const |
|
{ |
|
_Collect(m_root, AABB_TYPE(center, radius), inserter); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(IDXARR_TYPE& indices, const POINT_TYPE& center, TYPE radius) const |
|
{ |
|
IndexInserter inserter(indices); |
|
_Collect(m_root, AABB_TYPE(center, radius), inserter); |
|
} |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename INSERTER, typename COLLECTOR> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(INSERTER& inserter, const COLLECTOR& collector) const |
|
{ |
|
_Collect(m_root, m_radius, collector, inserter); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename COLLECTOR> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(IDXARR_TYPE& indices, const COLLECTOR& collector) const |
|
{ |
|
IndexInserter inserter(indices); |
|
_Collect(m_root, m_radius, collector, inserter); |
|
} |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(IDX_TYPE maxNeighbors, IDXARR_TYPE& indices, const AABB_TYPE& aabb) const |
|
{ |
|
_Collect(m_root, aabb, IndexInserter(indices)); |
|
if (indices.size() > maxNeighbors) { |
|
// keep only the closest neighbors |
|
typedef TIndexScore<IDX_TYPE,TYPE> ItemIndexScore; |
|
typedef cList<ItemIndexScore, const ItemIndexScore&, 0> ItemIndexScoreArr; |
|
ItemIndexScoreArr indexscores(indices.size()); |
|
const POINT_TYPE center(aabb.GetCenter()); |
|
FOREACH(i, indices) { |
|
const IDX_TYPE& idx = indices[i]; |
|
const TYPE score(-(center-m_items[idx]).squaredNorm()); |
|
indexscores[i] = ItemIndexScore(idx,score); |
|
} |
|
indices.Empty(); |
|
indexscores.Sort(); |
|
for (IDX_TYPE i=0; i<maxNeighbors; ++i) |
|
indices.Insert(indexscores[i].idx); |
|
} |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Collect(IDX_TYPE maxNeighbors, IDXARR_TYPE& indices, const POINT_TYPE& center, TYPE radius) const |
|
{ |
|
Collect(maxNeighbors, indices, AABB_TYPE(center, radius)); |
|
} // Collect |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// walk through the tree and collect visible indices |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename FTYPE, int FDIMS, typename INSERTER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_Traverse(const CELL_TYPE& cell, TYPE radius, const TFrustum<FTYPE,FDIMS>& frustum, INSERTER& inserter) const |
|
{ |
|
ASSERT(!cell.IsLeaf()); |
|
switch (frustum.Classify(cell.GetAabb(radius))) { |
|
case CLIPPED: { |
|
const TYPE childRadius = radius / TYPE(2); |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) { |
|
const CELL_TYPE& childCell = cell.m_child[i]; |
|
if (childCell.IsLeaf()) { |
|
const AABB_TYPE childAabb(CELL_TYPE::ComputeChildCenter(cell.GetCenter(), childRadius, i), childRadius); |
|
if (frustum.Classify(childAabb) != CULLED) |
|
inserter(m_indices.data()+childCell.GetFirstItemIdx(), childCell.GetNumItems()); |
|
} else { |
|
_Traverse(childCell, childRadius, frustum, inserter); |
|
} |
|
} |
|
break; } |
|
case VISIBLE: { |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) |
|
CollectCells(cell.m_child[i], inserter); |
|
break; } |
|
} |
|
} |
|
// walk through the tree and collect visible leafs |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename FTYPE, int FDIMS, typename PARSER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_TraverseCells(CELL_TYPE& cell, TYPE radius, const TFrustum<FTYPE,FDIMS>& frustum, PARSER& parser) |
|
{ |
|
ASSERT(!cell.IsLeaf()); |
|
switch (frustum.Classify(cell.GetAabb(radius))) { |
|
case CLIPPED: { |
|
const TYPE childRadius = radius / TYPE(2); |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) { |
|
const CELL_TYPE& childCell = cell.m_child[i]; |
|
if (childCell.IsLeaf()) { |
|
const AABB_TYPE childAabb(CELL_TYPE::ComputeChildCenter(cell.GetCenter(), childRadius, i), childRadius); |
|
if (frustum.Classify(childAabb) != CULLED) |
|
parser(childCell); |
|
} else { |
|
_TraverseCells(childCell, childRadius, frustum, parser); |
|
} |
|
} |
|
break; } |
|
case VISIBLE: { |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) |
|
_ParseCells(cell.m_child[i], parser); |
|
break; } |
|
} |
|
} |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename FTYPE, int FDIMS, typename INSERTER> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Traverse(const TFrustum<FTYPE,FDIMS>& frustum, INSERTER& inserter) const |
|
{ |
|
_Traverse(m_root, m_radius, frustum, inserter); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename FTYPE, int FDIMS> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::Traverse(const TFrustum<FTYPE,FDIMS>& frustum, IDXARR_TYPE& indices) const |
|
{ |
|
_Traverse(m_root, m_radius, frustum, IndexInserter(indices)); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename FTYPE, int FDIMS, typename PARSER> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::TraverseCells(const TFrustum<FTYPE,FDIMS>& frustum, PARSER& parser) |
|
{ |
|
_TraverseCells(m_root, m_radius, frustum, parser); |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename FTYPE, int FDIMS> |
|
inline void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::TraverseCells(const TFrustum<FTYPE,FDIMS>& frustum, CELLPTRARR_TYPE& leaves) |
|
{ |
|
_TraverseCells(m_root, m_radius, frustum, CellInserter(leaves)); |
|
} // Traverse |
|
/*----------------------------------------------------------------*/ |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename AREAESTIMATOR, typename CHUNKINSERTER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_SplitVolume(const CELL_TYPE& parentCell, TYPE parentRadius, unsigned idxChild, float maxArea, AREAESTIMATOR& areaEstimator, CHUNKINSERTER& chunkInserter, const UnsignedArr& indices) |
|
{ |
|
ASSERT(!indices.empty()); |
|
typedef std::pair<UnsignedArr,UnsignedArr> PairIndices; |
|
struct GenerateSamples { |
|
const UnsignedArr& indices; |
|
const unsigned numSamples; |
|
const unsigned halfSamples; |
|
const unsigned numCommonAxis; |
|
POINT_TYPE centers[8]; |
|
cList<PairIndices> arrHalfIndices; |
|
GenerateSamples(const UnsignedArr& _indices) |
|
: indices(_indices), numSamples((unsigned)indices.size()), halfSamples(numSamples/2), numCommonAxis(halfSamples==4?1:2), arrHalfIndices(0, numSamples) |
|
{ |
|
ASSERT(indices.size()%2 == 0 && indices.IsSorted()); |
|
ASSERT(halfSamples == 4 || halfSamples == 2); |
|
CELL_TYPE::ComputeCenter(centers); |
|
UnsignedArr samples(halfSamples); |
|
for (unsigned hs=0; hs<halfSamples; ++hs) { |
|
samples[0] = hs; |
|
GenerateIndices(1, samples); |
|
} |
|
} |
|
void GenerateIndices(unsigned idxSample, UnsignedArr& samples) { |
|
if (idxSample == samples.size()) { |
|
InsertIndices(samples); |
|
return; |
|
} |
|
for (unsigned i=samples[idxSample-1]+1; i<numSamples; ++i) { |
|
samples[idxSample] = i; |
|
GenerateIndices(idxSample+1, samples); |
|
} |
|
} |
|
void InsertIndices(const UnsignedArr& samples) { |
|
UnsignedArr halfIndices(halfSamples); |
|
for (unsigned s=0; s<halfSamples; ++s) |
|
halfIndices[s] = indices[samples[s]]; |
|
// check all samples have one/two common axis |
|
unsigned commonAxis(0); |
|
for (unsigned a=0; a<3; ++a) { |
|
unsigned na(1); |
|
for (unsigned s=1; s<halfSamples; ++s) { |
|
if (centers[halfIndices[0]][a] == centers[halfIndices[s]][a]) |
|
++na; |
|
} |
|
if (na == halfSamples && ++commonAxis == numCommonAxis) |
|
goto CommonAxis; |
|
} |
|
return; |
|
CommonAxis: |
|
// check not complementary samples |
|
for (const PairIndices& pairHalfIndices: arrHalfIndices) { |
|
unsigned nCommon(0); |
|
for (unsigned s=0; s<halfSamples; ++s) |
|
if (halfIndices[s] == pairHalfIndices.second[s]) |
|
++nCommon; |
|
if (nCommon == halfSamples) |
|
return; |
|
} |
|
// generate complementary indices |
|
ASSERT(halfIndices.IsSorted()); |
|
UnsignedArr compHalfIndices(0, halfSamples); |
|
unsigned i(0); |
|
for (unsigned idx: indices) { |
|
if (i < halfSamples && idx == halfIndices[i]) |
|
++i; |
|
else |
|
compHalfIndices.push_back(idx); |
|
} |
|
ASSERT(compHalfIndices.size() == halfSamples); |
|
ASSERT(compHalfIndices.IsSorted()); |
|
arrHalfIndices.emplace_back(std::move(halfIndices), std::move(compHalfIndices)); |
|
} |
|
}; |
|
const CELL_TYPE& cell(parentCell.GetChild(idxChild)); |
|
const TYPE radius(parentRadius / TYPE(2)); |
|
// handle particular cases |
|
if (cell.IsLeaf()) { |
|
chunkInserter(parentCell, parentRadius, UnsignedArr{idxChild}); |
|
return; |
|
} |
|
if (indices.size() == 1) { |
|
_SplitVolume(cell, radius, indices.front(), maxArea, areaEstimator, chunkInserter); |
|
return; |
|
} |
|
if (indices.size() == 2) { |
|
_SplitVolume(cell, radius, indices.front(), maxArea, areaEstimator, chunkInserter); |
|
_SplitVolume(cell, radius, indices.back(), maxArea, areaEstimator, chunkInserter); |
|
return; |
|
} |
|
// measure surface area for each child |
|
float childArea[8], totalArea(0); |
|
for (unsigned c=0; c<8; ++c) { |
|
CollectCells(cell.GetChild(c), areaEstimator); |
|
totalArea += childArea[c] = areaEstimator.PopArea(); |
|
} |
|
if (totalArea < maxArea*1.01f) { |
|
chunkInserter(parentCell, parentRadius, UnsignedArr{idxChild}); |
|
return; |
|
} |
|
// check if all parts are over the limit |
|
unsigned numOverAreas(0); |
|
for (unsigned c: indices) |
|
if (childArea[c] == 0 || childArea[c] >= maxArea) |
|
++numOverAreas; |
|
if (numOverAreas == indices.size()) { |
|
for (unsigned c: indices) |
|
if (childArea[c] > 0) |
|
_SplitVolume(cell, radius, c, maxArea, areaEstimator, chunkInserter); |
|
return; |
|
} |
|
// split mesh children and retain the components with surface smaller than the given area |
|
const cList<PairIndices> halfIndices(std::move(GenerateSamples(indices).arrHalfIndices)); |
|
IDX bestSplit(NO_ID); |
|
float bestArea(0); |
|
Point2f bestAs; |
|
FOREACH(idx, halfIndices) { |
|
const PairIndices& pairHalfIndices = halfIndices[idx]; |
|
ASSERT(pairHalfIndices.first.size() == pairHalfIndices.second.size()); |
|
Point2f as(Point2f::ZERO); |
|
for (unsigned i=0; i<pairHalfIndices.first.size(); ++i) { |
|
as[0] += childArea[pairHalfIndices.first[i]]; |
|
as[1] += childArea[pairHalfIndices.second[i]]; |
|
} |
|
for (unsigned i=0; i<2; ++i) { |
|
if (as[i] < maxArea && bestArea < as[i]) { |
|
bestArea = as[i]; |
|
bestAs = as; |
|
bestSplit = idx; |
|
} |
|
} |
|
} |
|
if (bestSplit != NO_ID) { |
|
// store found clusters |
|
const PairIndices& pairHalfIndices = halfIndices[bestSplit]; |
|
if (bestAs[0] < maxArea) |
|
chunkInserter(cell, radius, pairHalfIndices.first); |
|
else |
|
_SplitVolume(parentCell, parentRadius, idxChild, maxArea, areaEstimator, chunkInserter, pairHalfIndices.first); |
|
if (bestAs[1] < maxArea) |
|
chunkInserter(cell, radius, pairHalfIndices.second); |
|
else |
|
_SplitVolume(parentCell, parentRadius, idxChild, maxArea, areaEstimator, chunkInserter, pairHalfIndices.second); |
|
return; |
|
} |
|
// farther split each half into quarters |
|
if (halfIndices.front().first.size() == 4) { |
|
UnsignedArr bestQIndices[4]; |
|
float bestArea(0); |
|
Eigen::Vector4f bestAs; |
|
for (const PairIndices& pairHalfIndices: halfIndices) { |
|
const cList<PairIndices> qIndicesFirst(std::move(GenerateSamples(pairHalfIndices.first).arrHalfIndices)); |
|
const cList<PairIndices> qIndicesSecond(std::move(GenerateSamples(pairHalfIndices.second).arrHalfIndices)); |
|
ASSERT(qIndicesFirst.size() == qIndicesSecond.size()); |
|
FOREACH(q, qIndicesFirst) { |
|
const PairIndices& qFirst = qIndicesFirst[q]; |
|
const PairIndices& qSecond = qIndicesSecond[q]; |
|
Eigen::Vector4f as(Eigen::Vector4f::Zero()); |
|
for (unsigned i=0; i<qFirst.first.size(); ++i) { |
|
as[0] += childArea[qFirst.first[i]]; |
|
as[1] += childArea[qFirst.second[i]]; |
|
as[2] += childArea[qSecond.first[i]]; |
|
as[3] += childArea[qSecond.second[i]]; |
|
} |
|
float area(0); |
|
for (unsigned i=0; i<4; ++i) { |
|
if (as[i] < maxArea) |
|
area += as[i]; |
|
} |
|
if (bestArea < area) { |
|
bestArea = area; |
|
bestAs = as; |
|
bestQIndices[0] = qFirst.first; |
|
bestQIndices[1] = qFirst.second; |
|
bestQIndices[2] = qSecond.first; |
|
bestQIndices[3] = qSecond.second; |
|
} |
|
} |
|
} |
|
if (bestArea > 0) { |
|
// store found clusters |
|
for (unsigned i=0; i<4; ++i) { |
|
if (bestAs[i] < maxArea) { |
|
chunkInserter(cell, radius, bestQIndices[i]); |
|
} else { |
|
_SplitVolume(cell, radius, bestQIndices[i][0], maxArea, areaEstimator, chunkInserter); |
|
_SplitVolume(cell, radius, bestQIndices[i][1], maxArea, areaEstimator, chunkInserter); |
|
} |
|
} |
|
return; |
|
} |
|
} |
|
// split each child |
|
for (unsigned c: indices) { |
|
if (childArea[c] == 0) |
|
continue; |
|
if (childArea[c] < maxArea) |
|
chunkInserter(cell, radius, UnsignedArr{c}); |
|
else |
|
_SplitVolume(cell, radius, c, maxArea, areaEstimator, chunkInserter); |
|
} |
|
} |
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
template <typename AREAESTIMATOR, typename CHUNKINSERTER> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::SplitVolume(float maxArea, AREAESTIMATOR& areaEstimator, CHUNKINSERTER& chunkInserter) |
|
{ |
|
CELL_TYPE parent; |
|
parent.m_child = new CELL_TYPE[1]; |
|
parent.m_child[0].m_child = m_root.m_child; |
|
parent.m_child[0].Node() = m_root.Node(); |
|
parent.Node().center = m_root.Node().center + POINT_TYPE::Constant(m_radius); |
|
_SplitVolume(parent, m_radius*TYPE(2), 0, maxArea, areaEstimator, chunkInserter); |
|
parent.m_child[0].m_child = NULL; |
|
} // SplitVolume |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::_GetDebugInfo(const CELL_TYPE& cell, unsigned nDepth, DEBUGINFO& info) const |
|
{ |
|
if (cell.IsLeaf()) { |
|
if (info.minDepth > nDepth) |
|
info.minDepth = nDepth; |
|
if (info.maxDepth < nDepth) |
|
info.maxDepth = nDepth; |
|
info.avgDepth += nDepth; |
|
info.numLeaves++; |
|
return; |
|
} |
|
nDepth++; |
|
info.numNodes++; |
|
for (int i=0; i<CELL_TYPE::numChildren; ++i) |
|
_GetDebugInfo(cell.m_child[i], nDepth, info); |
|
} |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::GetDebugInfo(DEBUGINFO* pInfo, bool bPrintStats) const |
|
{ |
|
DEBUGINFO localInfo; |
|
DEBUGINFO& info = (pInfo ? *pInfo : localInfo); |
|
info.Init(); |
|
_GetDebugInfo(m_root, 0, info); |
|
info.avgDepth /= info.numLeaves; |
|
info.numItems = GetNumItems(); |
|
info.numNodes += info.numLeaves; |
|
info.memStruct = info.numNodes*sizeof(CELL_TYPE) + sizeof(TOctree); |
|
info.memItems = sizeof(IDX_TYPE)*info.numItems; |
|
info.memSize = info.memStruct + info.memItems; |
|
if (pInfo == NULL || bPrintStats) |
|
LogDebugInfo(info); |
|
} // GetDebugInfo |
|
/*----------------------------------------------------------------*/ |
|
|
|
template <typename ITEMARR_TYPE, typename TYPE, int DIMS, typename DATA_TYPE> |
|
void TOctree<ITEMARR_TYPE,TYPE,DIMS,DATA_TYPE>::LogDebugInfo(const DEBUGINFO& info) |
|
{ |
|
//VERBOSE("NoItems: %d; Mem %s; MemItems %s; MemStruct %s; AvgMemStruct %.2f%%%%; NoNodes %d; NoLeaf %d; AvgLeaf %.2f%%%%; AvgDepth %.2f; MinDepth %d; MaxDepth %d", |
|
VERBOSE("NumItems %d; Mem %s (%s items, %s struct - %.2f%%%%); NumNodes %d (leaves %d - %.2f%%%%); Depth %.2f (%d min, %d max)", |
|
info.numItems, |
|
Util::formatBytes(info.memSize).c_str(), Util::formatBytes(info.memItems).c_str(), Util::formatBytes(info.memStruct).c_str(), double(info.memStruct)*100.0/info.memSize, |
|
info.numNodes, info.numLeaves, float(info.numLeaves*100)/info.numNodes, |
|
info.avgDepth, info.minDepth, info.maxDepth); |
|
} // LogDebugInfo |
|
/*----------------------------------------------------------------*/ |
|
|
|
// if everything works fine, this function should return true |
|
template <typename TYPE, int DIMS> |
|
inline bool OctreeTest(unsigned iters, unsigned maxItems=1000, bool bRandom=true) { |
|
STATIC_ASSERT(DIMS > 0 && DIMS <= 3); |
|
srand(bRandom ? (unsigned)time(NULL) : 0); |
|
typedef Eigen::Matrix<TYPE,DIMS,1> POINT_TYPE; |
|
typedef CLISTDEF0(POINT_TYPE) TestArr; |
|
typedef TOctree<TestArr,TYPE,DIMS,uint32_t> TestTree; |
|
const TYPE ptMinData[] = {0,0,0}, ptMaxData[] = {640,480,240}; |
|
typename TestTree::AABB_TYPE aabb; |
|
aabb.Set(Eigen::Map<const POINT_TYPE>(ptMinData), Eigen::Map<const POINT_TYPE>(ptMaxData)); |
|
aabb.Enlarge(ZEROTOLERANCE<TYPE>()*TYPE(10)); |
|
unsigned nTotalMatches = 0; |
|
unsigned nTotalMissed = 0; |
|
unsigned nTotalExtra = 0; |
|
#ifndef _RELEASE |
|
typename TestTree::DEBUGINFO_TYPE totalInfo; |
|
totalInfo.Init(); |
|
#endif |
|
for (unsigned iter=0; iter<iters; ++iter) { |
|
// generate random items |
|
const unsigned elems = maxItems/10+RAND()%maxItems; |
|
TestArr items(elems); |
|
FOREACH(i, items) |
|
for (int j=0; j<DIMS; ++j) |
|
items[i](j) = static_cast<TYPE>(RAND()%ROUND2INT(ptMaxData[j])); |
|
// random query point |
|
POINT_TYPE pt; |
|
for (int j=0; j<DIMS; ++j) |
|
pt(j) = static_cast<TYPE>(RAND()%ROUND2INT(ptMaxData[j])); |
|
const TYPE radius(TYPE(3+RAND()%30)); |
|
// build octree and find interest items |
|
TestTree tree(items, aabb, [](typename TestTree::IDX_TYPE size, typename TestTree::Type radius) { |
|
return size > 16 && radius > 10; |
|
}); |
|
typename TestTree::IDXARR_TYPE indices; |
|
tree.Collect(indices, pt, radius); |
|
// find interest items by brute force |
|
typename TestTree::IDXARR_TYPE trueIndices; |
|
#if 1 |
|
// use square bound |
|
typename TestTree::AABB_TYPE aabbQuery(pt, radius); |
|
FOREACH(i, items) |
|
if (aabbQuery.Intersects(items[i])) |
|
trueIndices.Insert(i); |
|
#else |
|
// use circle bound |
|
FOREACH(i, items) |
|
if ((items[i]-pt).norm() < radius) |
|
trueIndices.Insert(i); |
|
#endif |
|
// compare results |
|
unsigned nMatches = 0; |
|
FOREACH(i, trueIndices) { |
|
const typename TestTree::IDX_TYPE idx = trueIndices[i]; |
|
FOREACH(j, indices) { |
|
if (indices[j] == idx) { |
|
++nMatches; |
|
break; |
|
} |
|
} |
|
} |
|
nTotalMatches += nMatches; |
|
nTotalMissed += (unsigned)trueIndices.size()-nMatches; |
|
nTotalExtra += (unsigned)indices.size()-nMatches; |
|
#ifndef _RELEASE |
|
// print stats |
|
typename TestTree::DEBUGINFO_TYPE info; |
|
tree.GetDebugInfo(&info); |
|
totalInfo += info; |
|
#endif |
|
} |
|
#ifndef _RELEASE |
|
TestTree::LogDebugInfo(totalInfo); |
|
VERBOSE("Test %s (TotalMissed %d, TotalExtra %d)", (nTotalMissed == 0 && nTotalExtra == 0 ? "successful" : "FAILED"), nTotalMissed, nTotalExtra); |
|
#endif |
|
return (nTotalMissed == 0 && nTotalExtra == 0); |
|
}
|
|
|