work on raytracing

wifi/estimate/ray3/ObstacleTree.h

@@ -0,0 +1,139 @@
+#ifndef OBSTACLETREE_H
+#define OBSTACLETREE_H
+
+#include "../../../geo/Sphere3.h"
+#include "Obstacle3.h"
+#include <algorithm>
+
+struct ObstacleNode {
+	bool isLeaf = true;
+	Sphere3 boundSphere;
+	std::vector<ObstacleNode*> sub;
+	ObstacleNode(bool isLeaf = false) : isLeaf(isLeaf) {;}
+};
+
+struct ObstacleLeaf : public ObstacleNode {
+	Obstacle3D obs;
+	ObstacleLeaf() : ObstacleNode(true) {;}
+};
+
+
+
+class ObstacleTree {
+
+	ObstacleNode root;
+
+public:
+
+	/** append a new leaf */
+	void add(ObstacleLeaf* leaf) {
+		root.sub.push_back(leaf);
+	}
+
+	void optimize() {
+		while(true) {
+			bool did = concat();
+			if (!did) {break;}
+		}
+	}
+
+	std::vector<Obstacle3D*> getHits(const Ray3 ray) const {
+		std::vector<Obstacle3D*> obs;
+		getHits(ray, &root, obs);
+		return obs;
+	}
+
+	void getHits(const Ray3 ray, const ObstacleNode* node, std::vector<Obstacle3D*>& hits) const {
+		for (const ObstacleNode* sub : node->sub) {
+			if (sub->boundSphere.intersects(ray)) {
+				if (sub->isLeaf) {
+					ObstacleLeaf* leaf = (ObstacleLeaf*)(sub);
+					hits.push_back(&leaf->obs);
+				} else {
+					if (sub->boundSphere.intersects(ray)) {getHits(ray, sub, hits);}
+				}
+			}
+		}
+	}
+
+
+	bool concat() {
+
+		bool concated = false;
+
+		// first, sort all elements by radius (smallest first)
+		auto compRadius = [] (const ObstacleNode* l1, const ObstacleNode* l2) {
+			return l1->boundSphere.radius < l2->boundSphere.radius;
+		};
+		std::sort(root.sub.begin(), root.sub.end(), compRadius);
+
+		ObstacleNode newRoot;
+
+		// combine nearby elements
+		//for (size_t i = 0; i < root.sub.size(); ++i) {
+		while(true) {
+
+			// get [and remove] the next element
+			ObstacleLeaf* l0 = (ObstacleLeaf*) root.sub[0];
+			root.sub.erase(root.sub.begin()+0);
+
+			// find another element that yields minimal increase in volume
+			auto compNear = [l0] (const ObstacleNode* l1, const ObstacleNode* l2) {
+				const float d1 = Sphere3::join(l0->boundSphere, l1->boundSphere).radius;
+				const float d2 = Sphere3::join(l0->boundSphere, l2->boundSphere).radius;
+				return d1 < d2;
+			};
+			auto it = std::min_element(root.sub.begin(), root.sub.end(), compNear);
+			ObstacleNode* l1 = *it;
+
+			float increment = Sphere3::join(l0->boundSphere, l1->boundSphere).radius / l0->boundSphere.radius;
+
+			const bool combine = (root.sub.size() > 1) && (it != root.sub.end()) && (increment < 1.75);
+
+			if (combine) {
+
+				// combine both into a new node
+				ObstacleNode* node = new ObstacleNode();
+				node->sub.push_back(l0);
+				node->sub.push_back(*it);
+				node->boundSphere = Sphere3::join(l0->boundSphere, (*it)->boundSphere);
+				root.sub.erase(it);
+				newRoot.sub.push_back(node);
+
+				concated = true;
+
+			} else {
+
+				ObstacleNode* node = new ObstacleNode();
+				node->sub.push_back(l0);
+				node->boundSphere = l0->boundSphere;
+				newRoot.sub.push_back(node);
+
+			}
+
+			// done?
+			if (root.sub.size() == 1) {
+
+				ObstacleNode* node = new ObstacleNode();
+				node->sub.push_back(root.sub.front());
+				node->boundSphere = root.sub.front()->boundSphere;
+				newRoot.sub.push_back(node);
+				break;
+
+			} else if (root.sub.size() == 0) {
+				break;
+			}
+
+			//--i;
+
+		}
+
+		root = newRoot;
+
+		return concated;
+
+	}
+
+};
+
+#endif // OBSTACLETREE_H