////////////////////////////////////////////////////////////////////
// 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(GetItem(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(GetItem(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-GetItem(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);
}