worked on floorplan (v2)

worked on grid-generation (v2)
new helper methods for geometry
new test cases

grid/Grid.h

@@ -4,6 +4,7 @@
 #include <vector>
 #include <iostream>
 #include <unordered_map>
+#include <algorithm>
 
 #include "../Exception.h"
 #include "GridPoint.h"
@@ -53,6 +54,12 @@ public:
 	/** no-copy */
 	Grid(const Grid& o) = delete;
 
+	/** reset the grid (empty) */
+	void reset() {
+		nodes.clear();
+		hashes.clear();
+	}
+
 	/** no-assign */
 	void operator = (const Grid& o) = delete;
 
@@ -77,14 +84,16 @@ public:
 	}
 
 	/** add the given (not necessarly aligned) element to the grid */
-	int addUnaligned(const T& elem) {
-		const UID uid = getUID(elem);														// get the UID for this new element
-		Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!");	// avoid potential errors
-		const int idx = nodes.size();														// next free index
-		nodes.push_back(elem);																// add it to the grid
-		nodes.back()._idx = idx;															// let the node know his own index
-		hashes[uid] = idx;																	// add an UID->index lookup
-		return idx;																			// done
+	int addUnaligned(const T& elem, const bool check=true) {
+		const UID uid = getUID(elem);															// get the UID for this new element
+		if (check) {
+			Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!");	// avoid potential errors
+		}
+		const int idx = nodes.size();															// next free index
+		nodes.push_back(elem);																	// add it to the grid
+		nodes.back()._idx = idx;																// let the node know his own index
+		hashes[uid] = idx;																		// add an UID->index lookup
+		return idx;																				// done
 	}
 
 	/** connect (uni-dir) i1 -> i2 */
@@ -94,9 +103,11 @@ public:
 
 	/** connect (uni-dir) i1 -> i2 */
 	void connectUniDir(T& n1, const T& n2) {
+		Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected");			// already connected?
+		Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself");
 		n1._neighbors[n1._numNeighbors] = n2._idx;
 		++n1._numNeighbors;
-		Assert::isBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
+		Assert::isBetween(n1._numNeighbors, (uint8_t) 0, (uint8_t) 10, "number of neighbors out of bounds!");
 	}
 
 	/**
@@ -165,11 +176,25 @@ public:
 	}
 
 	/** get the center-node the given Point belongs to. or nullptr if not present */
-	const T* getNodePtrFor(const GridPoint& p) {
+	const T* getNodePtrFor(const GridPoint& p) const {
 		auto it = hashes.find(getUID(p));
 		return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]);
 	}
 
+	/** get the node nearest to the given positon */
+	const T& getNearestNode(const GridPoint& p) const {
+		auto comp = [p] (const T& a, const T& b) { return a.getDistanceInMeter(p) < b.getDistanceInMeter(p); };
+		auto it = std::min_element(nodes.begin(), nodes.end(), comp);
+		return nodes[it->getIdx()];
+	}
+
+	/** get the node nearest to the given positon, examining only the nodes given by the provided index-array */
+	const T& getNearestNode(const GridPoint& p, const std::vector<int>& indices) const {
+		auto comp = [&] (const int a, const int b) { return nodes[a].getDistanceInMeter(p) < nodes[b].getDistanceInMeter(p); };
+		auto it = std::min_element(indices.begin(), indices.end(), comp);
+		return nodes[it->getIdx()];
+	}
+
 	/** get the BBox for the given node */
 	GridNodeBBox getBBox(const int idx) const {
 		return getBBox(nodes[idx]);
@@ -188,7 +213,7 @@ public:
 	UID getUID(const GridPoint& p) const {
 		const uint64_t x = std::round(p.x_cm / (float)gridSize_cm);
 		const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
-		const uint64_t z = std::round(p.z_cm / (float)gridSize_cm);
+		const uint64_t z = std::round(p.z_cm / (float)gridSize_cm * 5);		// z is usually much lower and not always aligned -> allow more room for hashes
 		return (z << 40) | (y << 20) | (x << 0);
 	}
 
@@ -446,7 +471,7 @@ private:
 	void assertAligned(const T& elem) {
 		if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
 		if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
-		if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
+		//if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
 	}
 
 };