added new sanity checks and compile-time assertions to prevent errors

fixed stair-building issue
new test-cases
added elevator support
fixed/improved some walker modules

grid/factory/v2/Stairs2.h

@@ -0,0 +1,207 @@
+#ifndef STAIRS_H
+#define STAIRS_H
+
+#include <vector>
+#include <set>
+
+#include "../../Grid.h"
+#include "Helper.h"
+#include "../../../floorplan/v2/Floorplan.h"
+
+#include <fstream>
+#include <unordered_set>
+
+/**
+ * this one should be both, simpler and more robus than v1.
+ * - prevents duplicate (x,y) nodes
+ * - slightly grows the stair-quads to ensure they overlap (sometimes the do not by a few mm)
+ * - only connects the corener nodes between adjacent quads (seems better for most stair-situations)
+ *
+ */
+template <typename T> class Stairs {
+
+private:
+
+	/** the grid to build into */
+	Grid<T>& grid;
+
+	/** calculation helper */
+	Helper<T> helper;
+
+	// keep a list of all vertices below stairwells and remove them hereafter
+	std::vector<T*> toDelete;
+
+private:
+
+	/** helper struct */
+	struct StairNode {
+		const int x_cm;
+		const int y_cm;
+		const int belongsToQuadIdx;
+		int gridIdx = -1;
+		StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;}
+	};
+
+
+public:
+
+	/** ctor */
+	Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
+
+	}
+
+	~Stairs() {
+		finalize();
+	}
+
+
+
+
+
+	void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
+
+		const int gs_cm = grid.getGridSize_cm();
+
+		// get all of the parts for the stair
+		const std::vector<Floorplan::StairPart> parts = stair->getParts();
+
+		// convert each part to a quad. hereby, slightly grow each quad, to ensure stair-parts are connected without gaps!
+		const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor, 1.05);
+
+		std::vector<StairNode> stairNodes;
+
+		// process each quad to get a list of all stair-nodes to add
+		for (int i = 0; i < (int) quads.size(); ++i) {
+
+			// create a 2D polygon (ignore z) for the quad
+			const Floorplan::Quad3& quad = quads[i];
+			HelperPoly poly(quad);
+
+			// get a callback for each node that belongs to the quad
+			poly.forEachGridPoint(gs_cm, [&] (int x_cm, int y_cm) {
+
+				const StairNode sn(x_cm, y_cm, i);
+
+				// IMPORTANT
+				// skip nodes that belong to more than one quad -> skip duplicates -> much more stable!
+				// NOTE: currently this will kill circular stairs that are above themselves
+				// FIX: skip dupliactes only between adjacent quads? this should help
+				auto comp = [&] (const StairNode& sn1) { return sn1.x_cm == sn.x_cm && sn1.y_cm == sn.y_cm; };
+				auto it = std::find_if(stairNodes.begin(), stairNodes.end(), comp);
+				if (it == stairNodes.end()) {stairNodes.push_back(sn);}
+
+			});
+
+		}
+
+		// add the new nodes to the grid
+		// nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes!
+		for (StairNode& sn : stairNodes) {
+
+			// use the nodes (x,y) to reconstruct the z-value for this position using barycentric interpolation
+			const Point2 p(sn.x_cm, sn.y_cm);
+
+			// the stair-quads centimeter position
+			const Floorplan::Quad3& quad = quads[sn.belongsToQuadIdx];
+			const Point3 p1 = quad.p1 * 100;
+			const Point3 p2 = quad.p2 * 100;
+			const Point3 p3 = quad.p3 * 100;
+			const Point3 p4 = quad.p4 * 100;
+
+			// get the z-value from one of the both triangles
+			int z_cm;
+			float u,v,w;
+			if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
+				z_cm = p1.z*u + p2.z*v + p3.z*w;
+			} else {
+				helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
+				z_cm = p1.z*u + p3.z*v + p4.z*w;
+			}
+
+			// the to-be-added position
+			GridPoint gp(sn.x_cm, sn.y_cm, z_cm);
+
+			// if a node is near an existing one (the floor above/below) use the existing one!
+			// this ensures the stair is connected to the floor above and below
+			if (grid.hasNodeFor(gp)) {
+				sn.gridIdx = grid.getNodeFor(gp).getIdx();
+			} else  {
+
+				sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm));
+
+				// check if there is a nearby floor-node to delete
+				const int deleteDist_cm = 100;
+				const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
+				const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
+				if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
+					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
+					if (n) {toDelete.push_back(n);}
+				} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
+					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
+					if (n) {toDelete.push_back(n);}
+				}
+
+
+			}
+
+
+
+
+		}
+
+		// now connect all new nodes with their neighbors
+		// do not perform normal grid-connection but examine the nodes within the generated vector
+		// this allows for some additional checks/criteria to be used
+		for (const StairNode& sn1 : stairNodes) {
+			T& n1 = (T&) grid[sn1.gridIdx];
+
+			for (const StairNode& sn2 : stairNodes) {
+
+				// node full?
+				if (n1.fullyConnected()) {continue;}
+
+				T& n2 = (T&) grid[sn2.gridIdx];
+
+				// do not connect node with itself
+				if (n2.getIdx() == n1.getIdx()) {continue;}
+
+				if (n1.hasNeighbor(n2.getIdx())) {continue;}
+
+				// connect adjacent stair-nodes
+				// but only if their stair-parts are also adjacent
+				// this addresses several error-situations with round stairs (end connected to the start) , ...
+				const int dx = sn1.x_cm - sn2.x_cm;
+				const int dy = sn1.y_cm - sn2.y_cm;
+				const int dz = sn1.belongsToQuadIdx - sn2.belongsToQuadIdx;
+
+				// connect
+				if (std::abs(dx) <= gs_cm && std::abs(dy) <= gs_cm && std::abs(dz) <= 1) {
+					grid.connectUniDir(n1, n2);
+				}
+
+			}
+
+
+		}
+
+
+
+	}
+
+
+	void finalize() {
+
+		// delete all pending nodes and perform a cleanup
+		if (!toDelete.empty()) {
+			for (T* n : toDelete) {grid.remove(*n);}
+			toDelete.clear();
+			grid.cleanup();
+		}
+
+	}
+
+
+
+};
+
+#endif // STAIRS_H