worked on grid creation

fixed some issues with stairs
fixed/added LINT

floorplan/v2/FloorplanLINT.h

@@ -60,7 +60,7 @@ namespace Floorplan {
 				if (i.type == Type::ERROR) {++err;}
 			}
 			if (err > 0) {
-				throw Exception("detected floorplan errors");
+			//	throw Exception("detected floorplan errors");
 			}
 
 		}
@@ -94,7 +94,7 @@ namespace Floorplan {
 
 				// check stairs
 				for (const Stair* s : floor->stairs) {
-					checkStair(res, floor, s);
+					checkStair(res, map, floor, s);
 				}
 
 				// check elevators
@@ -169,7 +169,14 @@ namespace Floorplan {
 
 		}
 
-		static void checkStair(Issues& res, const Floor* floor, const Stair* stair) {
+		static void checkStair(Issues& res, const IndoorMap* map, const Floor* floor, const Stair* stair) {
+
+			// list of all heights where there is a floor;
+			std::vector<int> floorAtHeight_cm;
+			for (const Floor* f : map->floors) {
+				const int floorZ_cm = std::round(f->atHeight * 100);
+				floorAtHeight_cm.push_back(floorZ_cm);	// integer height in cm
+			}
 
 			if (stair->getParts().empty()) {
 				res.push_back(Issue(Type::ERROR, floor, "stair does not contain any parts! [empty stair]"));
@@ -182,13 +189,20 @@ namespace Floorplan {
 			const Floorplan::Quad3& quadS = quads.front();
 			const Floorplan::Quad3& quadE = quads.back();
 
-			// disconnected start?
+			// start == end?
+			if (quadS.p1.z == quadE.p3.z) {
+				res.push_back(Issue(Type::ERROR, floor, "stair start and end must not belong to the same floor!"));
+			}
+
+			// disconnected start? (MUST belong to the floor the stair is attached to)
 			if (quadS.p1.z != floor->getStartingZ()) {
 				res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the starting floor's ground! [open stair start]"));
 			}
 
-			// disconnected end?
+			// disconnected end? (must be long to ANY other floor within the map)
-			if (quadE.p3.z != floor->getEndingZ()) {
+			//if (quadE.p3.z != floor->getEndingZ()) {
+			const int stairEndingZ_cm = std::round( quadE.p3.z * 100 );
+			if(std::find(floorAtHeight_cm.begin(), floorAtHeight_cm.end(), stairEndingZ_cm) == floorAtHeight_cm.end()) {
 				res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the ending floor's ground! [open stair end]"));
 			}
 

grid/Grid.h

@@ -268,7 +268,7 @@ public:
 	}
 
 	/** remove the connection from n1 to n2 (not the other way round!) */
-	void disconnectUniDir(T& n1, T& n2) {
+	void disconnectUniDir(T& n1, const T& n2) {
 		for (int n = 0; n < n1._numNeighbors; ++n) {
 			if (n1._neighbors[n] == n2._idx) {
 				arrayRemove(n1._neighbors, n, n1._numNeighbors);
@@ -301,12 +301,16 @@ public:
 	void remove(T& node) {
 
 		// disconnect from all neighbors
+		int cnt = 0;
 		while (node._numNeighbors) {
 
 			// by removing one neighbor, all others are shifted to close the gap within the array
 			// its therefor ok to always delete array index [0]
 			disconnectBiDir(node._idx, node._neighbors[0]);
-
+			if (++cnt > node.MAX_NEIGHBORS) {
+				Log::add(name, "WARNING! found unsolveable neighboring! " + node.asString(), true);
+				break;
+			}
 		}
 
 		// remove from hash-list

grid/factory/v2/GridFactory.h

@@ -89,7 +89,7 @@ public:
 		if (listener) {listener->onGridBuildUpdateMajor("adding stairs");}
 		if (_buildStairs) {
 			for (Floorplan::Floor* f : map->floors) {
-				buildStairs(f, listener);
+				buildStairs(map, f, listener);
 				if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
 			}
 		}
@@ -215,7 +215,7 @@ public:
 				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());
 
 				// slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit
-				bbox.grow(0.42345);
+				bbox.grow(0.1337);
 				if (intersects(bbox, floor)) {continue;}
 
 				// add to the grid [once]
@@ -246,7 +246,7 @@ public:
 
 	}
 
-	void buildStairs(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
+	void buildStairs(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
 
 		const int total = floor->stairs.size();
 		int cur = 0;
@@ -254,7 +254,7 @@ public:
 		// process each stair within the floor
 		for (const Floorplan::Stair* stair : floor->stairs) {
 			if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " stair " + std::to_string(cur+1));}
-			stairs.build(floor, stair);
+			stairs.build(map, floor, stair);
 			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
 		}
 
@@ -419,6 +419,9 @@ public:
 
 	void removeIsolatedNodes() {
 
+		//std::cout << "todo: remove" << std::endl;
+		//return;
+
 		// try to start at the first stair
 		for (T& n : grid) {
 			if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
@@ -439,6 +442,9 @@ public:
 		getConnected(n1, set);
 		Log::tock();
 
+		//const int numToRemove = grid.getNumNodes() - set.size();
+		//int numRemoved = 0;
+
 		// remove all other
 		Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
 		Log::tick();
@@ -455,6 +461,9 @@ public:
 				// proceed ;)
 				grid.remove(n2);
 
+				//++numRemoved;
+				//std::cout << numRemoved << ":" << numToRemove << std::endl;
+
 			}
 		}
 		Log::tock();

grid/factory/v2/Helper.h

@@ -254,13 +254,13 @@ public:
 	}
 
 	static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
-		Point2 v0 = b - a, v1 = c - a, v2 = p - a;
+		const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
-		float d00 = dot(v0, v0);
+		double d00 = dot(v0, v0);
-		float d01 = dot(v0, v1);
+		double d01 = dot(v0, v1);
-		float d11 = dot(v1, v1);
+		double d11 = dot(v1, v1);
-		float d20 = dot(v2, v0);
+		double d20 = dot(v2, v0);
-		float d21 = dot(v2, v1);
+		double d21 = dot(v2, v1);
-		float denom = d00 * d11 - d01 * d01;
+		double denom = d00 * d11 - d01 * d01;
 		v = (d11 * d20 - d01 * d21) / denom;
 		w = (d00 * d21 - d01 * d20) / denom;
 		u = 1.0f - v - w;

grid/factory/v2/Stairs2.h

@@ -31,19 +31,23 @@ private:
 	// keep a list of all vertices below stairwells and remove them hereafter
 	std::vector<T*> toDelete;
 
+	bool tryImproveStairConnections = true;
+
 private:
 
 	/** helper struct */
 	struct StairNode {
 
-		const int x_cm;
+		int x_cm;
-		const int y_cm;
+		int y_cm;
-		const int belongsToQuadIdx;
+		int belongsToQuadIdx;
 
 		float z_cm;			// interpolated
 		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) {;}
+
+
 	};
 
 
@@ -62,7 +66,15 @@ public:
 
 
 
-	void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
+	void build(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
+
+		// starting-height of all available floors (in cm)
+		// needed to ensure that stairs start and end at floors
+		std::vector<int> floorsAtHeight_cm;
+		for (const Floorplan::Floor* f : map->floors) {
+			const int floorAtHeight_cm = std::round(f->atHeight*100);
+			floorsAtHeight_cm.push_back(floorAtHeight_cm);
+		}
 
 		const int gs_cm = grid.getGridSize_cm();
 
@@ -123,20 +135,56 @@ public:
 
 		}
 
+		const float stairAbsStart_m = floor->atHeight + stair->getParts().front().start.z;
+		const float stairAbsEnd_m = floor->atHeight + stair->getParts().back().end.z;
+		//const float stairHeight_m = stairAbsEnd_m - stairAbsStart_m;
+		const float stairAbsStart_cm = std::round(stairAbsStart_m*100);
+		const float stairAbsEnd_cm = std::round(stairAbsEnd_m*100);
+		const float stairHeight_cm = stairAbsEnd_cm - stairAbsStart_cm;
+
 		// this might lead to stairs the start slightly above the starting-floor
 		// or ending slightly below the ending floor. this would lead to DISCONNECTION!
 		// therefore re-scale the z-values to ensure they start at floor1 and end at floor 2
-		float minZ = +9999999;
+		{
-		float maxZ = -9999999;
+
-		for (const StairNode& sn : stairNodes) {
+			float minZ = +9999999;
-			if (sn.z_cm < minZ) {minZ = sn.z_cm;}
+			float maxZ = -9999999;
-			if (sn.z_cm > maxZ) {maxZ = sn.z_cm;}
+			for (const StairNode& sn : stairNodes) {
-		}
+				if (sn.z_cm < minZ) {minZ = sn.z_cm;}
-		for (StairNode& sn : stairNodes) {
+				if (sn.z_cm > maxZ) {maxZ = sn.z_cm;}
-			const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ);				// current percentage from minZ to maxZ
+			}
-			sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100;		// apply percentage to floorStartZ <-> floorEndZ
+
+			for (StairNode& sn : stairNodes) {
+				const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ);					// current percentage from minZ to maxZ
+				//sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100;		// apply percentage to floorStartZ <-> floorEndZ
+				sn.z_cm = std::round(stairAbsStart_cm + zPercent * stairHeight_cm);			// apply percentage to floorStartZ <-> floorEndZ
+			}
+
+			// snap stair-nodes to nearby grid nodes, if possible
+			if (tryImproveStairConnections) {
+				for (StairNode& sn : stairNodes) {
+					if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) )	{sn.z_cm = stairAbsStart_cm;}
+					if (std::abs(sn.z_cm-stairAbsEnd_cm)   < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm)))	{sn.z_cm = stairAbsEnd_cm;}
+				}
+			}
+
+			// stort all stair-nodes by z (ascending)
+			const auto comp = [] (const StairNode& sn1, const StairNode& sn2) {return sn1.z_cm < sn2.z_cm;};
+			std::sort(stairNodes.begin(), stairNodes.end(), comp);
+
+			// get first and last stair note
+			const bool end1OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(), stairNodes.front().z_cm) != floorsAtHeight_cm.end();
+			const bool end2OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(),  stairNodes.back().z_cm) != floorsAtHeight_cm.end();
+
+			// be sure both are connected to a floor
+			if (!end1OK || !end2OK) {
+				throw Exception("stair's start or end is not directly connectable to a floor");
+			}
+
 		}
 
+
+
 		// track z-positions of already-existing grid nodes we connected the stair to
 		// if this list contains 2 distinctive values, the stair is successfully connected to starting and ending floor!
 		std::unordered_set<float> connectedWithHeights;
@@ -167,15 +215,16 @@ public:
 				// check if there is a nearby floor-node to delete
 				// -> remove nodes directly above/below the stair
 				const int deleteDist_cm = 100;
-				const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
+				const float distToBelow = gp.z_cm - stairAbsStart_cm;//floor->getStartingZ()*100;
-				const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
+				const float distToAbove = stairAbsEnd_cm - gp.z_cm;//floor->getEndingZ()*100 - gp.z_cm;
 				//if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
 				if (distToBelow > 0 && distToBelow < deleteDist_cm) {
-					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
+					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsStart_cm));//floor->getStartingZ()*100));
 					if (n) {toDelete.push_back(n);}
 				//} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
-				} else if (distToAbove > 0 && distToAbove < deleteDist_cm) {
+				}
-					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
+				if (distToAbove > 0 && distToAbove < deleteDist_cm) {
+					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsEnd_cm));//floor->getEndingZ()*100));
 					if (n) {toDelete.push_back(n);}
 				}
 
@@ -191,7 +240,7 @@ public:
 		// one for the starting floor
 		// one for the ending floor
 		// this mainly fails, when there is a REMOVING outline-area that removes to many nodes and the stair can not be connected
-	Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");
+		Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");
 
 		// now connect all new nodes with their neighbors
 		// do not perform normal grid-connection but examine the nodes within the generated vector