Indoor/geo/volume/BVH.h

#ifndef BOUNDINGVOLUMEHIERARCHY_H
#define BOUNDINGVOLUMEHIERARCHY_H

#include <vector>
#include <functional>

#include "../Ray2.h"
#include "../Ray3.h"

#include "BoundingVolume.h"

#include "BoundingVolumeAABB2.h"
#include "BoundingVolumeCircle2.h"

#include "BoundingVolumeAABB3.h"
#include "BoundingVolumeSphere3.h"



template <typename Element, typename Ray, typename Point, typename Volume, typename Wrapper> class BVH {

protected:

	/** one node within the tree */
	struct BVHNode {
		bool isLeaf;
		bool check;
		Volume boundingVolume;
		std::vector<BVHNode*> childNodes;
		BVHNode(bool isLeaf = false, bool check = true) : isLeaf(isLeaf), check(check) {;}
	};

	/** one leaf within the tree */
	struct BVHLeaf : public BVHNode {
		Element element;
		BVHLeaf(const Element& e, const bool check) : BVHNode(true, check), element(e) {;}
	};

	/** the tree's root */
	BVHNode root;

public:

	/** get the tree's root node */
	const BVHNode& getRoot() const {
		return root;
	}

	/** add a new volume to the tree */
	void add(const Element& element, const bool leafCheck = true) {

		// create a new leaf for this element
		BVHLeaf* leaf = new BVHLeaf(element, leafCheck);

		// get the element's boundin volume
		leaf->boundingVolume = getBoundingVolume(element);

		// add the leaf to the tree
		root.childNodes.push_back(leaf);

	}

	/** optimize the tree */
	int optimize(const int max = 9999) {
		for (int i = 0; i < max; ++i) {
			//const bool did = concat();		// faster
			const bool did = combineBest();		// better
			if (!did) {return i;}
		}
		return max;
	}


	void getHits(const Ray& ray, const std::function<void(const Element&)>& func) const {
		getHits(ray, &root, func);
	}

	// this one has to be as fast as possible!
	static void getHits(const Ray& ray, const BVHNode* node, const std::function<void(const Element&)>& func) {
		for (const BVHNode* sub : node->childNodes) {
			if (!sub->check || sub->boundingVolume.intersects(ray)) {
				if (sub->isLeaf) {
					const BVHLeaf* leaf = static_cast<const BVHLeaf*>(sub);
					func(leaf->element);
				} else {
					getHits(ray, sub, func);
				}
			}
		}
	}

	/** get the tree's depth */
	int getDepth() const {
		return getDepth(&root, 1);
	}


private:

	/** call the given function for each leaf within the given subtree */
	void forEachLeaf(const BVHNode* n, std::function<void(const BVHNode*)> func) const {
		if (n->isLeaf) {
			func(n);
		} else {
			for (BVHNode* child : n->childNodes) {
				forEachLeaf(child, func);
			}
		}
	}

	/** determine/approximate a new bounding volume around n1+n2 */
	Volume getVolAround(const BVHNode* n1, const BVHNode* n2) const {
		//return getVolAroundExact(n1, n2);
		return getVolAroundAPX(n1, n2);
	}

	/** determine the bounding-volume around n1 and n2 by (slowly) calculating a new, exact volume based on all leaf-elements */
	Volume getVolAroundExact(const BVHNode* n1, const BVHNode* n2) const {
		std::vector<Point> verts;
		auto onLeaf = [&] (const BVHNode* n) {
			BVHLeaf* leaf = (BVHLeaf*) n;
			std::vector<Point> subVerts = Wrapper::getVertices(leaf->element);
			verts.insert(verts.end(),  subVerts.begin(), subVerts.end());
		};
		forEachLeaf(n1, onLeaf);
		forEachLeaf(n2, onLeaf);
		return Volume::fromVertices(verts);
	}

	/** approximate the bounding-volume around n1 and n2 by (quickly) joining their current volumes. the result might be unnecessarily large */
	Volume getVolAroundAPX(const BVHNode* n1, const BVHNode* n2) const {
		return Volume::join(n1->boundingVolume, n2->boundingVolume);
	}


	bool combineBest() {

		// nothing to do?
		if (root.childNodes.size() < 2) {return false;}

		struct Best {
			BVHNode* n1 = nullptr;
			BVHNode* n2 = nullptr;
			Volume vol;
			float volSize = 99999999;
		} best;

		for (size_t i = 0; i < root.childNodes.size(); ++i) {
			for (size_t j = 0; j < root.childNodes.size(); ++j) {

				if (i == j) {continue;}

				BVHNode* n1 = root.childNodes[i];
				BVHNode* n2 = root.childNodes[j];

				const Volume newVol = getVolAround(n1,n2);
				const float newVolSize = newVol.getVolumeSize();
				if (newVolSize < best.volSize) {
					best.vol = newVol;
					best.volSize = newVolSize;
					best.n1 = n1;
					best.n2 = n2;
				}

			}
		}

		root.childNodes.erase(std::remove(root.childNodes.begin(), root.childNodes.end(), best.n1), root.childNodes.end());
		root.childNodes.erase(std::remove(root.childNodes.begin(), root.childNodes.end(), best.n2), root.childNodes.end());

		// combine both into a new node
		BVHNode* newNode = new BVHNode();
		newNode->childNodes.push_back(best.n1);
		newNode->childNodes.push_back(best.n2);
		newNode->boundingVolume = best.vol;

		// does the newly created node contain any other nodes?
		// THIS SHOULD NEVER BE THE CASE!
//		for (size_t i = 0; i < root.childNodes.size(); ++i) {
//			BVHNode* n3 = root.childNodes[i];
//			if (newNode->boundingVolume.contains(n3->boundingVolume)) {
//				newNode->childNodes.push_back(n3);
//				root.childNodes.erase(root.childNodes.begin()+i);
//				--i;
//			}
//		}

		// attach the node
		root.childNodes.push_back(newNode);

		return true;

	}


	bool concat() {

		// nothing to do?
		if (root.childNodes.size() < 2) {return false;}


		bool concated = false;

		// first, sort all elements by volume (smallest first)
		auto compVolume = [] (const BVHNode* n1, const BVHNode* n2) {
			return n1->boundingVolume.getVolumeSize() < n2->boundingVolume.getVolumeSize();
		};
		std::sort(root.childNodes.begin(), root.childNodes.end(), compVolume);


		// elements will be grouped into this new root
		BVHNode newRoot;

		// combine nearby elements
		while(true) {

			// get [and remove] the next element
			BVHNode* n0 = (BVHNode*) root.childNodes[0];
			root.childNodes.erase(root.childNodes.begin()+0);

			// find another element that yields minimal increase in volume
			auto compNear = [n0] (const BVHNode* n1, const BVHNode* n2) {
				const float v1 = Volume::join(n0->boundingVolume, n1->boundingVolume).getVolumeSize();
				const float v2 = Volume::join(n0->boundingVolume, n2->boundingVolume).getVolumeSize();
				return v1 < v2;
			};
			auto it = std::min_element(root.childNodes.begin(), root.childNodes.end(), compNear);
			BVHNode* n1 = *it;

			// calculate the resulting increment in volume
			const Volume joined = Volume::join(n0->boundingVolume, n1->boundingVolume);
			const float increment = joined.getVolumeSize() / n0->boundingVolume.getVolumeSize();
			const bool intersects = n0->boundingVolume.intersects(n1->boundingVolume);

			const bool combine = true; //(intersects); //(increment < 15.0);

			if (combine) {

				// remove from current root
				root.childNodes.erase(it);

				// combine both into a new node
				BVHNode* node = new BVHNode();
				node->childNodes.push_back(n0);
				node->childNodes.push_back(n1);
				node->boundingVolume = joined;
				newRoot.childNodes.push_back(node);

				concated = true;

			} else {

				BVHNode* node = new BVHNode();
				node->childNodes.push_back(n0);
				node->boundingVolume = n0->boundingVolume;
				newRoot.childNodes.push_back(node);

			}

			// done?
			if (root.childNodes.size() == 1) {
				BVHNode* node = new BVHNode();
				node->childNodes.push_back(root.childNodes.front());
				node->boundingVolume = root.childNodes.front()->boundingVolume;
				newRoot.childNodes.push_back(node);
				break;
			} else if (root.childNodes.size() == 0) {
				break;
			}

		}

		root = newRoot;
		return concated;

	}

	int getDepth(const BVHNode* node, const int cur) const {
		if (node->isLeaf) {
			return cur;
		} else {
			int res = cur;
			for (const BVHNode* sub : node->childNodes) {
				const int subDepth = getDepth(sub, cur+1);
				if (subDepth > res) {res = subDepth;}
			}
			return res;
		}
	}

	/** get a bounding-volume for the given element */
	Volume getBoundingVolume(const Element& element) {
		const std::vector<Point> verts = Wrapper::getVertices(element);
		return Volume::fromVertices(verts);
	}

};

template <typename Element, typename Volume, typename Wrapper> class BVH3 : public BVH<Element, Ray3, Point3, Volume, Wrapper> {

};

template <typename Element, typename Volume, typename Wrapper> class BVH2 : public BVH<Element, Ray2, Point2, Volume, Wrapper> {

};

#endif // BOUNDINGVOLUMEHIERARCHY_H