initial version

grid/Grid.h

@@ -0,0 +1,141 @@
+#ifndef GRID_H
+#define GRID_H
+
+#include <vector>
+#include <unordered_map>
+
+#include "../Exception.h"
+#include "GridPoint.h"
+#include "GridNode.h"
+#include <iostream>
+
+/**
+ * grid of the given grid-size, storing some value which
+ * extends GridPoint
+ */
+template <int gridSize_cm, typename T> class Grid {
+
+typedef uint64_t UID;
+
+private:
+
+	/** all elements (nodes) within the grid */
+	std::vector<T> nodes;
+
+	/** UID -> index mapping */
+	std::unordered_map<UID, int> hashes;
+
+public:
+
+	/** ctor */
+	Grid() {
+		static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
+		static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
+	}
+
+	/** no-copy */
+	Grid(const Grid& o) = delete;
+
+	/** no-assign */
+	void operator = (const Grid& o) = delete;
+
+
+	/**
+	 * add the given element to the grid.
+	 * returns the index of the element within the internal data-structure
+	 * @param elem the element to add
+	 */
+	int add(const T& elem) {
+		assertAligned(elem);							// assert that the to-be-added element is aligned to the grid
+		const int idx = nodes.size();					// next free index
+		const UID uid = getUID(elem);					// get the UID for this new element
+		nodes.push_back(elem);							// add it to the grid
+		nodes.back()._idx = idx;
+		hashes[uid] = idx;								// add an UID->index lookup
+		return idx;										// done
+	}
+
+	/**
+	 * connect (bi-directional) the two provided nodes
+	 * @param idx1 index of the first element
+	 * @param idx2 index of the second element
+	 */
+	void connect(const int idx1, const int idx2) {
+		T& n1 = nodes[idx1];				// get the first element
+		T& n2 = nodes[idx2];				// get the second element
+		connect(n1, n2);
+	}
+
+	/**
+	 * connect (bi-directional) the two provided nodes
+	 * @param n1 the first node
+	 * @param n2 the second node
+	 */
+	void connect(T& n1, T& n2) {
+		n1._neighbors[n1._numNeighbors] = n2._idx;				// add them both as neighbors
+		n2._neighbors[n2._numNeighbors] = n1._idx;
+		++n1._numNeighbors;										// increment the neighbor-counter
+		++n2._numNeighbors;
+	}
+
+	/** get the number of contained nodes */
+	int getNumNodes() const {
+		return nodes.size();
+	}
+
+	/** get the number of neighbors for the given element */
+	int getNumNeighbors(const int idx) const {
+		return getNumNeighbors(nodes[idx]);
+	}
+
+	/** get the number of neighbors for the given element */
+	int getNumNeighbors(const T& e) const {
+		return e._numNeighbors;
+	}
+
+	/** get the center-node the given Point belongs to */
+	const T& getNodeFor(const GridPoint& p) {
+		const UID uid = getUID(p);
+		if (hashes.find(uid) == hashes.end()) {throw Exception("element not found!");}
+		return nodes[hashes[uid]];
+	}
+
+	/** get the BBox for the given node */
+	GridNodeBBox getBBox(const int idx) const {
+		return getBBox(nodes[idx]);
+	}
+
+	/** get the BBox for the given node */
+	GridNodeBBox getBBox(const T& node) const {
+		return GridNodeBBox(node, gridSize_cm);
+	}
+
+	/**
+	 * get an UID for the given point.
+	 * this works only for aligned points.
+	 *
+	 */
+	UID getUID(const GridPoint& p) const {
+		const uint64_t x = std::round(p.x_cm / gridSize_cm);
+		const uint64_t y = std::round(p.y_cm / gridSize_cm);
+		const uint64_t z = std::round(p.z_cm / gridSize_cm);
+		return (z << 40) | (y << 20) | (x << 0);
+	}
+
+	/** array access */
+	const T& operator [] (const int idx) const {
+		return nodes[idx];
+	}
+
+private:
+
+	/** asssert that the given element is aligned to the grid */
+	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!");}
+	}
+
+};
+
+#endif // GRID_H