OctTree.h

/*
 * OctTree.h
 *
 *  Created on: Feb 27, 2012
 *      Author: TF
 * \copyright
 * Copyright (c) 2015, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#ifndef OCTTREE_H_
#define OCTTREE_H_

#include <limits>

// MathLib
#include "MathTools.h"

namespace GEOLIB
{
template <typename POINT>
class OctTree
{
public:
	static OctTree<POINT>* createOctTree(POINT& ll, POINT& ur, size_t max_points_per_node)
	{
		const double dx(ur[0] - ll[0]);
		const double dy(ur[1] - ll[1]);
		const double dz(ur[2] - ll[2]);

		if (dx >= dy && dx >= dz)
		{
			ll[1] -= (dx - dy) / 2.0;
			ur[1] += (dx - dy) / 2.0;
			ll[2] -= (dx - dz) / 2.0;
			ur[2] += (dx - dz) / 2.0;
		}
		else
		{
			if (dy >= dx && dy >= dz)
			{
				ll[0] -= (dy - dx) / 2.0;
				ur[0] += (dy - dx) / 2.0;
				ll[2] -= (dy - dz) / 2.0;
				ur[2] += (dy - dz) / 2.0;
			}
			else
			{
				ll[0] -= (dz - dx) / 2.0;
				ur[0] += (dz - dx) / 2.0;
				ll[1] -= (dz - dy) / 2.0;
				ur[1] += (dz - dy) / 2.0;
			}
		}

		OctTree<POINT>::_max_points_per_node = max_points_per_node;
		return new OctTree<POINT>(ll, ur);
	}

	virtual ~OctTree()
	{
		for (size_t k(0); k < 8; k++)
			delete _childs[k];
	}

	/**
	 * This method adds the given point to the OctTree. If necessary,
	 * the OctTree will be extended.
	 * @param pnt the point
	 * @return If the point can be inserted the method returns true, else false.
	 */
	bool addPoint(POINT* pnt)
	{
		if ((*pnt)[0] < _ll[0])
			return false;
		if ((*pnt)[0] > _ur[0])
			return false;
		if ((*pnt)[1] < _ll[1])
			return false;
		if ((*pnt)[1] > _ur[1])
			return false;
		if ((*pnt)[2] < _ll[2])
			return false;
		if ((*pnt)[2] > _ur[2])
			return false;

		if (!_is_leaf)
		{
			for (size_t k(0); k < 8; k++)
			{
				if (_childs[k]->addPoint(pnt))
				{
					return true;
				}
			}
		}

		// check if point is already in OctTree
		bool pnt_in_tree(false);
		for (size_t k(0); k < _pnts.size() && !pnt_in_tree; k++)
		{
			const double sqr_dist(MathLib::sqrDist((_pnts[k])->getData(), pnt->getData()));
			if (sqr_dist < std::numeric_limits<double>::epsilon())
				pnt_in_tree = true;
		}
		if (!pnt_in_tree)
			_pnts.push_back(pnt);
		else
			return false;

		if (_pnts.size() > OctTree<POINT>::_max_points_per_node)
			splitNode();
		return true;
	}

	/**
	 * range query - returns all points inside the range (min[0], max[0]) x (min[1], max[1]) x (min[2], max[2])
	 * @param min
	 * @param max
	 * @param pnts
	 */
	void getPointsInRange(POINT const& min, POINT const& max, std::vector<POINT*>& pnts) const
	{
		if (_ur[0] < min[0])
			return;
		if (_ur[1] < min[1])
			return;
		if (_ur[2] < min[2])
			return;

		if (max[0] < _ll[0])
			return;
		if (max[1] < _ll[1])
			return;
		if (max[2] < _ll[2])
			return;

		if (_is_leaf)
		{
			typename std::vector<POINT*>::const_iterator it;
			for (it = (_pnts.begin()); it != _pnts.end(); it++)
			{
				pnts.push_back(*it);
			}
		}
		else
		{
			for (size_t k(0); k < 8; k++)
			{
				_childs[k]->getPointsInRange(min, max, pnts);
			}
		}
	}

private:
	enum OctTreeQuadrant
	{
		NEL = 0, //!< north east lower
		NWL, //!< north west lower
		SWL, //!< south west lower
		SEL, //!< south east lower
		NEU, //!< south west upper
		NWU, //!< south west upper
		SWU, //!< south west upper
		SEU //!< south east upper
	};

	/**
	 * private constructor
	 * @param ll lower left point
	 * @param ur upper right point
	 * @return
	 */
	OctTree(POINT const& ll, POINT const& ur) : _ll(ll), _ur(ur), _is_leaf(true)
	{
		// init childs
		for (size_t k(0); k < 8; k++)
			_childs[k] = NULL;
	}

	void splitNode()
	{
		const double x_mid((_ur[0] + _ll[0]) / 2.0);
		const double y_mid((_ur[1] + _ll[1]) / 2.0);
		const double z_mid((_ur[2] + _ll[2]) / 2.0);
		POINT p0(x_mid, y_mid, _ll[2]), p1(_ur[0], _ur[1], z_mid);

		// create child NEL
		_childs[NEL] = new OctTree<POINT>(p0, p1);

		// create child NWL
		p0[0] = _ll[0];
		p1[0] = x_mid;
		_childs[NWL] = new OctTree<POINT>(p0, p1);

		// create child SWL
		p0[1] = _ll[1];
		p1[1] = y_mid;
		_childs[SWL] = new OctTree<POINT>(_ll, p1);

		// create child NEU
		_childs[NEU] = new OctTree<POINT>(p1, _ur);

		// create child SEL
		p0[0] = x_mid;
		p1[0] = _ur[0];
		_childs[SEL] = new OctTree<POINT>(p0, p1);

		// create child NWU
		p0[0] = _ll[0];
		p0[1] = y_mid;
		p0[2] = z_mid;
		p1[0] = x_mid;
		p1[1] = _ur[1];
		p1[2] = _ur[2];
		_childs[NWU] = new OctTree<POINT>(p0, p1);

		// create child SWU
		p0[1] = _ll[1];
		p1[1] = y_mid;
		_childs[SWU] = new OctTree<POINT>(p0, p1);

		// create child SEU
		p0[0] = x_mid;
		p1[0] = _ur[0];
		p1[1] = y_mid;
		p1[2] = _ur[2];
		_childs[SEU] = new OctTree<POINT>(p0, p1);

		// distribute points to sub quadtrees
		const size_t n_pnts(_pnts.size());
		for (size_t j(0); j < n_pnts; j++)
		{
			bool nfound(true);
			for (size_t k(0); k < 8 && nfound; k++)
			{
				if (_childs[k]->addPoint(_pnts[j]))
				{
					nfound = false;
				}
			}
		}
		_pnts.clear();
		_is_leaf = false;
	}

	/**
	 * childs are sorted:
	 *   _childs[0] is north east lower child
	 *   _childs[1] is north west lower child
	 *   _childs[2] is south west lower child
	 *   _childs[3] is south east lower child
	 *   _childs[4] is north east upper child
	 *   _childs[5] is north west upper child
	 *   _childs[6] is south west upper child
	 *   _childs[7] is south east upper child
	 */
	OctTree<POINT>* _childs[8];
	/**
	 * lower left front face point of the cube
	 */
	POINT const _ll;
	/**
	 * upper right back face point of the cube
	 */
	POINT const _ur;

	std::vector<POINT*> _pnts;
	bool _is_leaf;
	/**
	 * maximum number of points per leaf
	 */
	static size_t _max_points_per_node;
};

template <typename POINT>
size_t OctTree<POINT>::_max_points_per_node = 0;
}

#endif /* OCTTREE_H_ */