worked on floorplan (v2)

worked on grid-generation (v2)
new helper methods for geometry
new test cases

grid/factory/v2/Importance.h

@@ -0,0 +1,217 @@
+#ifndef IMPORTANCE_H
+#define IMPORTANCE_H
+
+#include "../../Grid.h"
+
+#include "../../../misc/KNN.h"
+#include "../../../misc/KNNArray.h"
+
+#include "../../../math/MiniMat2.h"
+#include "../../../math/Distributions.h"
+
+
+class Importance {
+
+private:
+
+	static constexpr const char* name = "GridImp";
+
+public:
+
+
+	template <typename T> static void addOutlineNodes(Grid<T>& dst, Grid<T>& src) {
+
+		for (const T& n : src) {
+			if (n.getNumNeighbors() < 8) {
+				if (!dst.hasNodeFor(n)) {
+					dst.add(n);
+				}
+			}
+		}
+
+	}
+
+	/** attach importance-factors to the grid */
+	template <typename T> static void addImportance(Grid<T>& g, const float z_cm) {
+
+		Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
+
+		// get an inverted version of the grid
+		Grid<T> inv(g.getGridSize_cm());
+		addOutlineNodes(inv, g);
+		//GridFactory<T> fac(inv);
+		//fac.addInverted(g, z_cm);
+
+		// sanity check
+		Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!");
+
+		// construct KNN search
+		KNN<Grid<T>, 3> knn(inv);
+
+		// the number of neighbors to use
+		static constexpr int numNeighbors = 12;
+
+		// create list of all doors
+		std::vector<T> doors;
+
+		// process each node
+		for (T& n1 : g) {
+
+			// is the current node a door?
+			//if (isDoor(n1, neighbors)) {doors.push_back(n1);}					// OLD
+			if (n1.getType() == GridNode::TYPE_DOOR) {doors.push_back(n1);}		// NEW!
+
+			// favor stairs just like doors
+			//if (isStaircase(g, n1)) {doors.push_back(n1);}					// OLD
+			if (n1.getType() == GridNode::TYPE_STAIR) {doors.push_back(n1);}	// NEW
+
+		}
+
+		KNNArray<std::vector<T>> knnArrDoors(doors);
+		KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
+
+		Distribution::Normal<float> favorDoors(0.0f, 0.6f);
+
+		// process each node again
+		for (T& n1 : g) {
+
+			// skip nodes on other than the requested floor-level
+			//if (n1.z_cm != z_cm) {continue;}
+
+			// get the 10 nearest neighbors and their distance
+			size_t indices[numNeighbors];
+			float squaredDist[numNeighbors];
+			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+			knn.get(point, numNeighbors, indices, squaredDist);
+
+			// get the neighbors
+			std::vector<T*> neighbors;
+			for (int i = 0; i < numNeighbors; ++i) {
+				neighbors.push_back(&inv[indices[i]]);
+			}
+
+			n1.imp = 1.0f;
+
+			//if (n1.getType() == GridNode::TYPE_FLOOR) {
+
+			// get the distance to the nearest door
+			const float distToWall_m = Units::cmToM(std::sqrt(squaredDist[0]) + g.getGridSize_cm());
+
+			// get the distance to the nearest door
+			const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
+
+			n1.imp =
+					1 +
+					getWallImportance( distToWall_m ) +
+					favorDoors.getProbability(distToDoor_m);
+
+
+			//}
+			//addDoor(n1, neighbors);
+
+			// importance for this node (based on the distance from the next door)
+			//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
+
+
+			//n1.imp = (dist_m < 0.2) ? (1) : (0.5);
+		}
+
+	}
+
+	/** is the given node connected to a staircase? */
+	template <typename T> static bool isStaircase(Grid<T>& g, T& node) {
+
+		return node.getType() == GridNode::TYPE_STAIR;
+
+//		// if this node has a neighbor with a different z, this is a stair
+//		for (T& neighbor : g.neighbors(node)) {
+//			if (neighbor.z_cm != node.z_cm) {return true;}
+//		}
+//		return false;
+
+	}
+
+	/** is the given node (and its inverted neighbors) a door? */
+	template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
+
+		if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
+
+		MiniMat2 m1;
+//		MiniMat2 m2;
+		Point3 center = nSrc;
+
+		// calculate the centroid of the nSrc's nearest-neighbors
+		Point3 centroid(0,0,0);
+		for (const T* n : neighbors) {
+			centroid = centroid + (Point3)*n;
+		}
+		centroid /= neighbors.size();
+
+		// if nSrc is too far from the centroid, this does not make sense
+		if ((centroid-center).length() > 40) {return false;}
+
+		// build covariance of the nearest-neighbors
+		int used = 0;
+		for (const T* n : neighbors) {
+
+			const Point3 d1 = (Point3)*n - centroid;
+			if (d1.length() > 100) {continue;}	// radius search
+			m1.addSquared(d1.x, d1.y);
+
+//			const Point3 d2 = (Point3)*n - center;
+//			if (d2.length() > 100) {continue;}	// radius search
+//			m2.addSquared(d2.x, d2.y);
+
+			++used;
+
+		}
+
+		// we need at least two points for the covariance
+		if (used < 6) {return false;}
+
+		// check eigenvalues
+		MiniMat2::EV ev1 = m1.getEigenvalues();
+//		MiniMat2::EV ev2 = m2.getEigenvalues();
+
+		// ensure e1 > e2
+		if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
+//		if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}
+
+		// door?
+		const float ratio1 = (ev1.e2/ev1.e1);
+//		const float ratio2 = (ev2.e2/ev2.e1);
+//		const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);
+
+		return (ratio1 < 0.30 && ratio1 > 0.05) ;
+
+	}
+
+	/** get the importance of the given node depending on its nearest wall */
+	static float getWallImportance(float dist_m) {
+
+		// avoid sticking too close to walls (unlikely)
+		static Distribution::Normal<float> avoidWalls(0.0, 0.5);
+
+		// favour walking near walls (likely)
+		static Distribution::Normal<float> stickToWalls(0.9, 0.5);
+
+		// favour walking far away (likely)
+		static Distribution::Normal<float> farAway(2.2, 0.5);
+		if (dist_m > 2.0) {dist_m = 2.0;}
+
+		// overall importance
+//		return	- avoidWalls.getProbability(dist_m) * 0.30		// avoid walls
+//				+ stickToWalls.getProbability(dist_m) * 0.15	// walk near walls
+//				+ farAway.getProbability(dist_m) * 0.15			// walk in the middle
+		return	- avoidWalls.getProbability(dist_m)		// avoid walls
+				//+ stickToWalls.getProbability(dist_m)	// walk near walls
+				//+ farAway.getProbability(dist_m)			// walk in the middle
+		;
+
+
+	}
+
+
+};
+
+#endif // IMPORTANCE_H