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

grid/GridNode.h

@@ -0,0 +1,34 @@
+#ifndef GRIDNODE_H
+#define GRIDNODE_H
+
+#include "GridNodeBBox.h"
+#include "GridPoint.h"
+
+/**
+ * INTERNAL DATASTRUCTURE
+ * this data-structure is internally used by the Grid
+ * to store additional information for each node besides
+ * the user's requested data-structure
+ */
+class GridNode {
+
+	template<int, typename> friend class Grid;
+
+	/** INTERNAL: array-index */
+	int _idx = -1;
+
+	/** INTERNAL: store neighbors (via index) */
+	int _numNeighbors = 0;
+
+	/** INTERNAL: number of neighbors */
+	int _neighbors[10] = {};
+
+public:
+
+	GridNode() {;}
+
+
+
+};
+
+#endif // GRIDNODE_H

grid/GridNodeBBox.h

@@ -0,0 +1,46 @@
+#ifndef GRIDNODEBBOX_H
+#define GRIDNODEBBOX_H
+
+#include "GridPoint.h"
+#include "../geo/Line2D.h"
+
+/**
+ * describes the 2D (one floor)
+ * bounding-box for one node on the grid
+ */
+struct GridNodeBBox {
+
+	/** smaller half */
+	float x1_cm, y1_cm;
+
+	/** larger half */
+	float x2_cm, y2_cm;
+
+	/** ctor */
+	GridNodeBBox(const GridPoint& center, const int gridSize_cm) {
+		x1_cm = center.x_cm - gridSize_cm/2;	// smaller half
+		y1_cm = center.y_cm - gridSize_cm/2;
+		x2_cm = center.x_cm + gridSize_cm/2;	// larger half
+		y2_cm = center.y_cm + gridSize_cm/2;
+	}
+
+	/** equal? */
+	bool operator == (const GridNodeBBox& o) const {
+		return (x1_cm == o.x1_cm) && (y1_cm == o.y1_cm) && (x2_cm == o.x2_cm) && (y2_cm == o.y2_cm);
+	}
+
+	/** does the BBox intersect with the given line? */
+	bool intersects (const Line2D& l) const {
+		Line2D l1(x1_cm, y1_cm, x2_cm, y1_cm);		// upper
+		Line2D l2(x1_cm, y2_cm, x2_cm, y2_cm);		// lower
+		Line2D l3(x1_cm, y1_cm, x1_cm, y2_cm);		// left
+		Line2D l4(x2_cm, y1_cm, x2_cm, y2_cm);		// right
+		return	l.getSegmentIntersection(l1) ||
+				l.getSegmentIntersection(l2) ||
+				l.getSegmentIntersection(l3) ||
+				l.getSegmentIntersection(l4);
+	}
+
+};
+
+#endif // GRIDNODEBBOX_H

grid/GridPoint.h

@@ -0,0 +1,41 @@
+#ifndef GRIDPOINT_H
+#define GRIDPOINT_H
+
+#include <cmath>
+
+struct GridPoint {
+
+	/** x-position (in centimeter) */
+	const float x_cm;
+
+	/** y-position (in centimeter) */
+	const float y_cm;
+
+	/** z-position (in centimeter) */
+	const float z_cm;
+
+
+	/** empty ctor */
+	GridPoint() : x_cm(0), y_cm(0), z_cm(0) {;}
+
+	/** ctor */
+	GridPoint(const float x_cm, const float y_cm, const float z_cm) : x_cm(x_cm), y_cm(y_cm), z_cm(z_cm) {;}
+
+
+	/** equal? */
+	bool operator == (const GridPoint& o) const {
+		return x_cm == o.x_cm && y_cm == o.y_cm && z_cm == o.z_cm;
+	}
+
+	/** get the distance (in meter) betwen this and the given point */
+	float getDistanceInMeter(const GridPoint& other) const {
+		const int dx = x_cm - other.x_cm;
+		const int dy = y_cm - other.y_cm;
+		const int dz = z_cm - other.z_cm;
+		return std::sqrt(dx*dx + dy*dy + dz*dz) / 100.0f;
+	}
+
+
+};
+
+#endif // GRIDPOINT_H

grid/factory/GridFactory.h

@@ -0,0 +1,55 @@
+#ifndef GRIDFACTORY_H
+#define GRIDFACTORY_H
+
+#include <string>
+#include "../../floorplan/Floor.h"
+#include "../../geo/Units.h"
+#include "../GridNodeBBox.h"
+#include "../Grid.h"
+
+template <int gridSize_cm, typename T> class GridFactory {
+
+private:
+
+	Grid<gridSize_cm, T>& grid;
+
+
+public:
+
+	/** ctor with the grid to fill */
+	GridFactory(Grid<gridSize_cm, T>& grid) : grid(grid) {;}
+
+	/** add the given floor at the provided height (in cm) */
+	void addFloor(const Floor& floor, const float z_cm) {
+
+		for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
+			for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {
+
+				// check intersection with the floorplan
+				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
+				if (intersects(bbox, floor)) {continue;}
+
+				// add to the grid
+				grid.add(T(x_cm, y_cm, z_cm));
+
+			}
+		}
+
+		int i = 0;
+
+	}
+
+private:
+
+	/** does the bbox intersect with any of the floor's walls? */
+	bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
+		for (const Line2D l : floor.getObstacles()) {
+			if (bbox.intersects(l)) {return true;}
+		}
+		return false;
+	}
+
+
+};
+
+#endif // GRIDFACTORY_H