many changes :P

grid/factory/GridFactory.h

@@ -3,6 +3,8 @@
 
 #include <string>
 #include "../../floorplan/Floor.h"
+#include "../../floorplan/Stairs.h"
+
 #include "../../geo/Units.h"
 #include "../GridNodeBBox.h"
 #include "../Grid.h"
@@ -22,6 +24,8 @@ public:
 	/** add the given floor at the provided height (in cm) */
 	void addFloor(const Floor& floor, const float z_cm) {
 
+
+		// build grid-points
 		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) {
 
@@ -35,7 +39,181 @@ public:
 			}
 		}
 
-		int i = 0;
+		connectAdjacent(z_cm);
+
+	}
+
+	void connectAdjacent(const float z_cm) {
+
+		// connect adjacent grid-points
+		for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
+
+			T& n1 = (T&) grid[idx];
+			if (n1.z_cm != z_cm) {continue;}	// ugly... different floor -> skip
+
+			// square around each point
+			for (int x =  -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
+				for (int y =  -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
+
+					// skip the center (node itself)
+					if ((x == y) && (x == 0)) {continue;}
+
+					// position of the potential neighbor
+					int ox = n1.x_cm + x;
+					int oy = n1.y_cm + y;
+					GridPoint p(ox, oy, n1.z_cm);
+
+					// does the grid contain the potential neighbor?
+					if (grid.hasNodeFor(p)) {
+						T& n2 = (T&) grid.getNodeFor(p);
+						grid.connectUniDir(n1, n2);
+					}
+
+				}
+			}
+
+		}
+
+	}
+
+
+	void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
+
+		for (const Stair& s : stairs) {
+
+			for (int i = 0; i < grid.getNumNodes(); ++i) {
+
+				// potential starting-point for the stair
+				T& n = (T&) grid[i];
+
+				// real starting point for the stair?
+				if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {
+
+					// construct end-point by using the stair's direction
+					const Point3 end = Point3(n.x_cm, n.y_cm, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
+					GridPoint gp(end.x, end.y, end.z);
+
+					// does such and end-point exist within the grap? -> construct stair
+					if (grid.hasNodeFor(gp)) {
+						T& n2 = (T&) grid.getNodeFor(gp);
+
+						buildStair(n, n2);
+
+					}
+					int i = 0;
+				}
+			}
+
+		}
+
+	}
+
+	/** build a stair (z-transition) from n1 to n2 */
+	void buildStair(T& n1, T& n2) {
+
+		//TODO: ensure n1 is below n2
+		const float zDiff = n2.z_cm - n1.z_cm;
+		const float xDiff = n2.x_cm - n1.x_cm;
+		const float yDiff = n2.y_cm - n1.y_cm;
+
+		int idx1 = n1.getIdx();
+		int idx2 = -1;
+		const int idx3 = n2.getIdx();
+
+		// move upards in gridSize steps
+		for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
+
+			// calculate the percentage of reached upwards-distance
+			const float percent = z/zDiff;
+
+			// adjust (x,y) accordingly (interpolate)
+			int x = n1.x_cm + xDiff * percent;
+			int y = n1.y_cm + yDiff * percent;
+
+			// snap (x,y) to the grid???
+			//x = std::round(x / gridSize_cm) * gridSize_cm;
+			//y = std::round(y / gridSize_cm) * gridSize_cm;
+
+			// create a new node add it to the grid, and connect it with the previous one
+			idx2 = grid.addUnaligned(T(x,y,z));
+			grid.connectBiDir(idx1, idx2);
+			idx1 = idx2;
+
+		}
+
+		// add the last segment
+		if (idx2 != -1) {
+			grid.connectBiDir(idx2, idx3);
+		}
+
+	}
+
+	/** add the inverted version of the given z-layer */
+	void addInverted(const Grid<gridSize_cm, T>& gIn, const float z_cm) {
+
+		// get the original grid's bbox
+		BBox3 bb = gIn.getBBox();
+
+		// build new grid-points
+		for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
+			for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {
+
+				// does the input-grid contain such a point?
+				GridPoint gp(x_cm, y_cm, z_cm);
+				if (gIn.hasNodeFor(gp)) {continue;}
+
+				// add to the grid
+				grid.add(T(x_cm, y_cm, z_cm));
+
+			}
+		}
+
+	}
+
+	// TODO: how to determine the starting index?!
+
+	// IDEAS: find all segments:
+	// start at a random point, add all connected points to the set
+	// start at a NEW random point ( not part of the already processed points), add connected points to a new set
+	// repeat until all points processed
+	// how to handle multiple floor layers?!?!
+	// run after all floors AND staircases were added??
+	// OR: random start, check segment size, < 50% of all nodes? start again
+
+	void removeIsolated() {
+
+		// get largest connected region
+		std::set<int> set;
+		do {
+			const int idxStart = rand() % grid.getNumNodes();
+			set.clear();
+			getConnected(idxStart, set);
+		} while (set.size() < 0.5 * grid.getNumNodes());
+
+
+		// remove all other
+		for (int i = 0; i < grid.getNumNodes(); ++i) {
+			if (set.find(i) == set.end()) {grid.remove(i);}
+		}
+
+		// clean the grid
+		grid.cleanup();
+
+	}
+
+private:
+
+	/** recursively get all connected nodes and add them to the set */
+	void getConnected(const int idx, std::set<int>& set) {
+
+		T& n1 = (T&) grid[idx];
+		set.insert(n1.getIdx());
+
+		for (T& n2 : grid.neighbors(n1)) {
+			if (set.find(n2.getIdx()) == set.end()) {
+				getConnected(n2.getIdx(), set);
+			}
+		}
 
 	}
 
@@ -43,7 +221,7 @@ 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()) {
+		for (const Line2& l : floor.getObstacles()) {
 			if (bbox.intersects(l)) {return true;}
 		}
 		return false;

grid/factory/GridImportance.h

@@ -0,0 +1,84 @@
+#ifndef GRIDIMPORTANCE_H
+#define GRIDIMPORTANCE_H
+
+#include "../Grid.h"
+#include "GridFactory.h"
+#include "../../misc/KNN.h"
+
+#include <KLib/math/distribution/Normal.h>
+
+class GridImportance {
+
+public:
+
+	/** attach importance-factors to the grid */
+	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
+
+		// get an inverted version of the grid
+		Grid<gridSize_cm, T> inv;
+		GridFactory<gridSize_cm, T> fac(inv);
+		fac.addInverted(g, z_cm);
+
+		// construct KNN search
+		KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
+
+		for (int idx = 0; idx < g.getNumNodes(); ++idx) {
+
+			// process each point
+			T& n1 = (T&) g[idx];
+
+//			// get its nearest neighbor
+//			size_t idxNear;
+//			float distSquared;
+//			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+//			knn.getNearest(point, idxNear, distSquared);
+
+//			// calculate importante
+//			const float imp = importance( Units::cmToM(std::sqrt(distSquared)) );
+//			n1.imp = imp;
+
+			size_t indices[10];
+			float squaredDist[10];
+			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+			knn.get(point, 10, indices, squaredDist);
+
+			const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
+			const float imp2 = door( indices );
+			n1.imp = (imp1 + imp2)/2;
+
+		}
+
+
+
+	}
+
+	float door( size_t* indices ) {
+
+		// build covariance
+
+		//if (dist1_m > 1.0) {return 1;}
+		//return 1.0 - std::abs(dist1_m - dist2_m);
+		return 1;
+	}
+
+	float importance(float dist_m) {
+
+		static K::NormalDistribution d1(0.0, 0.5);
+		//if (dist_m > 1.5) {dist_m = 1.5;}
+		return 1.0 - d1.getProbability(dist_m) * 0.5;
+
+//		static K::NormalDistribution d1(1.0, 0.75);
+//		//static K::NormalDistribution d2(3.0, 0.75);
+
+//		if (dist_m > 3.0) {dist_m = 3.0;}
+//		return 0.8 + d1.getProbability(dist_m);// + d2.getProbability(dist_m);
+
+//		if (dist_m < 0.5)	{return 0.8;}
+//		if (dist_m < 1.5)	{return 1.2;}
+//		if (dist_m < 2.5)	{return 0.8;}
+//		else				{return 1.2;}
+	}
+
+};
+
+#endif // GRIDIMPORTANCE_H