removed gridSize from the template arguments

- not needed and code much cleaner
some minor changes
new test-cases

grid/Grid.h

@@ -14,20 +14,21 @@
 #include "../misc/Debug.h"
 
 /**
- * grid of the given grid-size, storing some value which
- * extends GridPoint and GridNode
+ * grid of a given-size, storing some user-data-value which
+ * - extends GridPoint and GridNode
  *
  * Usage:
  *		for (Node& n : grid) {...}
  *		for (Node& n2 : grid.neighbors(n)) {...}
  *
  */
-template <int gridSize_cm, typename T> class Grid {
+template <typename T> class Grid {
 
 	static constexpr const char* name = "Grid";
 
 	#include "GridNeighborIterator.h"
 
+	/** UID for nodes */
 	typedef uint64_t UID;
 
 private:
@@ -38,10 +39,13 @@ private:
 	/** UID -> index mapping */
 	std::unordered_map<UID, int> hashes;
 
+	/** the user-given grid-size */
+	const int gridSize_cm;
+
 public:
 
-	/** ctor */
-	Grid() {
+	/** ctor with the grid's size (in cm) */
+	Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) {
 		static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
 		static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
 	}
@@ -52,12 +56,15 @@ public:
 	/** no-assign */
 	void operator = (const Grid& o) = delete;
 
+
 	/** allows for-each iteration over all included nodes */
 	decltype(nodes.begin()) begin() {return nodes.begin();}
 
 	/** allows for-each iteration over all included nodes */
 	decltype(nodes.end()) end() {return nodes.end();}
 
+	/** get the grid's size */
+	int getGridSize_cm() const {return gridSize_cm;}
 
 	/**
 	 * add the given element to the grid.

grid/GridNeighborIterator.h

@@ -8,7 +8,7 @@ class NeighborIter : std::iterator<std::input_iterator_tag, int> {
 private:
 
 	/** the grid the src-node belongs to */
-	Grid<gridSize_cm, T>* grid;
+	Grid<T>* grid;
 
 	/** index of the source-node within its grid */
 	int srcNodeIdx;
@@ -19,8 +19,8 @@ private:
 public:
 
 	/** ctor */
-	NeighborIter(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx, const int nIdx) :
-		grid((Grid<gridSize_cm, T>*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
+	NeighborIter(const Grid<T>& grid, const int srcNodeIdx, const int nIdx) :
+		grid((Grid<T>*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
 
 	/** next neighbor */
 	NeighborIter& operator++() {++nIdx; return *this;}
@@ -44,7 +44,7 @@ class NeighborForEach {
 private:
 
 	/** the grid the src-node belongs to */
-	const Grid<gridSize_cm, T>& grid;
+	const Grid<T>& grid;
 
 	/** index of the source-node within its grid */
 	const int srcNodeIdx;
@@ -52,7 +52,7 @@ private:
 public:
 
 	/** ctor */
-	NeighborForEach(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx) :
+	NeighborForEach(const Grid<T>& grid, const int srcNodeIdx) :
 		grid(grid), srcNodeIdx(srcNodeIdx) {;}
 
 	/** starting point */

grid/GridNode.h

@@ -4,7 +4,8 @@
 #include "GridNodeBBox.h"
 #include "GridPoint.h"
 
-template<int, typename> class Grid;
+/** forward decl. */
+template<typename> class Grid;
 
 
 /**
@@ -17,7 +18,8 @@ struct GridNode {
 
 private:
 
-	template<int, typename> friend class Grid;
+	/** grant full access to the grid */
+	template<typename> friend class Grid;
 
 	/** INTERNAL: array-index */
 	int _idx;

grid/factory/GridFactory.h

@@ -11,7 +11,7 @@
 
 #include "../../misc/Debug.h"
 
-template <int gridSize_cm, typename T> class GridFactory {
+template <typename T> class GridFactory {
 
 	/** logging name */
 	static constexpr const char* name = "GridFac";
@@ -19,19 +19,21 @@ template <int gridSize_cm, typename T> class GridFactory {
 private:
 
 	/** the grid to build into */
-	Grid<gridSize_cm, T>& grid;
+	Grid<T>& grid;
 
 
 public:
 
 	/** ctor with the grid to fill */
-	GridFactory(Grid<gridSize_cm, T>& grid) : grid(grid) {;}
+	GridFactory(Grid<T>& grid) : grid(grid) {;}
 
 	/** add the given floor at the provided height (in cm) */
 	void addFloor(const Floor& floor, const float z_cm) {
 
 		Log::add(name, "adding floor at height " + std::to_string(z_cm));
 
+		const float gridSize_cm = grid.getGridSize_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) {
@@ -55,6 +57,8 @@ public:
 
 		Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm));
 
+		const int gridSize_cm = grid.getGridSize_cm();
+
 		// connect adjacent grid-points
 		for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
 
@@ -132,6 +136,8 @@ public:
 		int idx2 = -1;
 		const int idx3 = n2.getIdx();
 
+		const int gridSize_cm = grid.getGridSize_cm();
+
 		// move upards in gridSize steps
 		for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
 
@@ -161,11 +167,13 @@ public:
 	}
 
 	/** add the inverted version of the given z-layer */
-	void addInverted(const Grid<gridSize_cm, T>& gIn, const float z_cm) {
+	void addInverted(const Grid<T>& gIn, const float z_cm) {
 
 		// get the original grid's bbox
 		BBox3 bb = gIn.getBBox();
 
+		const int gridSize_cm = grid.getGridSize_cm();
+
 		// 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) {

grid/factory/GridImportance.h

@@ -31,25 +31,26 @@ private:
 public:
 
 	/** attach importance-factors to the grid */
-	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
+	template <typename T> 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<gridSize_cm, T> inv;
-		GridFactory<gridSize_cm, T> fac(inv);
+		Grid<T> inv(g.getGridSize_cm());
+		GridFactory<T> fac(inv);
 		fac.addInverted(g, z_cm);
 
 		// construct KNN search
-		KNN<Grid<gridSize_cm, T>, 3> knn(inv);
+		KNN<Grid<T>, 3> knn(inv);
 
 		// the number of neighbors to use
 		static constexpr int numNeighbors = 8;
 
-		for (int idx = 0; idx < g.getNumNodes(); ++idx) {
+		// create list of all doors
+		std::vector<T> doors;
 
-			// process each point
-			T& n1 = (T&) g[idx];
+		// process each node
+		for (T& n1 : g) {
 
 			// get the 10 nearest neighbors and their distance
 			size_t indices[numNeighbors];
@@ -64,14 +65,51 @@ public:
 			}
 
 			addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
-			addDoor(n1, neighbors);
+			//addDoor(n1, neighbors);
+
+			// is the current node a door?
+			if (isDoor(n1, neighbors)) {doors.push_back(n1);}
+
+			// favor stairs just like doors
+			if (isStaircase(g, n1)) {doors.push_back(n1);}
 
 		}
 
+		KNNArray<std::vector<T>> knnArrDoors(doors);
+		KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
+
+		// process each node again
+		for (T& n1 : g) {
+
+			static K::NormalDistribution favorDoors(0.0, 0.6);
+
+			// get the distance to the nearest door
+			const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
+
+			// importance for this node (based on the distance from the next door)
+			const float imp = 1.0 + favorDoors.getProbability(dist_m) * 0.35;
+
+			// adjust
+			n1.imp *= imp;
+
+		}
+
+
+	}
+
+	/** is the given node connected to a staircase? */
+	template <typename T> bool isStaircase(Grid<T>& g, T& node) {
+
+		// 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;
+
 	}
 
 	/** attach importance-factors to the grid */
-	template <int gridSize_cm, typename T> void addDistanceToTarget(Grid<gridSize_cm, T>& g, Dijkstra<T>& d) {
+	template <typename T> void addDistanceToTarget(Grid<T>& g, Dijkstra<T>& d) {
 
 		//Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
 
@@ -87,7 +125,7 @@ public:
 
 	}
 
-	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
+	template <typename T> void addImportance(Grid<T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
 
 		// routing path
 		DijkstraPath<T> path(end, start);
@@ -112,8 +150,8 @@ public:
 	}
 
 
-	/** add importance to nSrc if it is part of a door */
-	template <typename T> void addDoor( T& nSrc, std::vector<T*> neighbors ) {
+	/** is the given node (and its inverted neighbors) a door? */
+	template <typename T> bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
 
 		MiniMat2 m;
 		Point3 center = nSrc;
@@ -126,7 +164,7 @@ public:
 		centroid /= neighbors.size();
 
 		// if nSrc is too far from the centroid, this does not make sense
-		if ((centroid-center).length() > 60) {return;}
+		if ((centroid-center).length() > 20) {return false;}
 
 		// build covariance of the nearest-neighbors
 		int used = 0;
@@ -138,7 +176,7 @@ public:
 		}
 
 		// we need at least two points for the covariance
-		if (used < 2) {return;}
+		if (used < 2) {return false;}
 
 		// check eigenvalues
 		MiniMat2::EV ev = m.getEigenvalues();
@@ -147,7 +185,7 @@ public:
 		if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
 
 		// door?
-		if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; }
+		return ((ev.e2/ev.e1) < 0.15) ;
 
 	}
 

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -41,7 +41,7 @@ private:
 public:
 
 	/** ctor with the target you want to reach */
-	template <int gridSize_cm, typename Access> GridWalkLightAtTheEndOfTheTunnel(Grid<gridSize_cm, T>& grid, const Access& acc, const T& target) {
+	template <typename Access> GridWalkLightAtTheEndOfTheTunnel(Grid<T>& grid, const Access& acc, const T& target) {
 
 		// build all shortest path to reach th target
 		dijkstra.build(target, target, acc);
@@ -60,7 +60,7 @@ public:
 
 	}
 
-	template <int gridSize_cm> GridWalkState<T> getDestination(Grid<gridSize_cm, T>& grid, GridWalkState<T> start, float distance_m) {
+	GridWalkState<T> getDestination(Grid<T>& grid, GridWalkState<T> start, float distance_m) {
 
 		int retries = 2;
 		GridWalkState<T> res;
@@ -84,7 +84,7 @@ public:
 
 private:
 
-	template <int gridSize_cm> GridWalkState<T> walk(Grid<gridSize_cm, T>& grid, GridWalkState<T> cur, float distRest_m) {
+	GridWalkState<T> walk(Grid<T>& grid, GridWalkState<T> cur, float distRest_m) {
 
 		drawer.reset();;
 

grid/walk/GridWalkWeighted.h

@@ -45,7 +45,7 @@ private:
 
 public:
 
-	template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
+	template <int gridSize_cm> State getDestination(Grid<T>& grid, State start, float distance_m) {
 
 		int retries = 2;
 		State res;
@@ -73,7 +73,7 @@ private:
 		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
 	}
 
-	template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
+	State walk(Grid<T>& grid, State cur, float distRest_m) {
 
 		drawer.reset();;
 

main.cpp

@@ -15,7 +15,9 @@ int main(int argc, char** argv) {
 
 #ifdef WITH_TESTS
 	::testing::InitGoogleTest(&argc, argv);
+	//::testing::GTEST_FLAG(filter) = "*Importance*";
 	::testing::GTEST_FLAG(filter) = "*Walk*";
+
 	return RUN_ALL_TESTS();
 #endif
 

tests/grid/GridImportance.cpp

@@ -11,8 +11,8 @@ TEST(GridImportance, a) {
 
 
 
-	Grid<20, GP> g;
-	GridFactory<20, GP> gf(g);
+	Grid<GP> g(20);
+	GridFactory<GP> gf(g);
 	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
 
 	Floor f1 = fpf.getFloor("1");

tests/grid/Plot.h

@@ -51,13 +51,13 @@ public:
 
 	}
 
-	template <int gridSize_cm, typename T> Plot& showGrid(Grid<gridSize_cm, T>& g) {
+	template <typename T> Plot& showGrid(Grid<T>& g) {
 		addEdges(g);
 		addNodes(g);
 		return *this;
 	}
 
-	template <int gridSize_cm, typename T> Plot& addEdges(Grid<gridSize_cm, T>& g) {
+	template <typename T> Plot& addEdges(Grid<T>& g) {
 
 		// prevent adding edges twice
 		std::set<size_t> done;
@@ -78,7 +78,7 @@ public:
 
 	}
 
-	template <int gridSize_cm, typename T> Plot& addNodes(Grid<gridSize_cm, T>& g) {
+	template <typename T> Plot& addNodes(Grid<T>& g) {
 
 		for (GP& n1 : g) {
 			K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
@@ -89,7 +89,7 @@ public:
 
 	}
 
-	template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
+	template <typename T> Plot& build(Grid<T>& g) {
 
 		std::set<uint64_t> done;
 

tests/grid/TestAll.cpp

@@ -13,13 +13,13 @@
 TEST(TestAll, Nav) {
 
 
-	Grid<20, GP> g;
+	Grid<GP> g(20);
 
 	// dijkstra mapper
 	class TMP {
-		Grid<20, GP>& grid;
+		Grid<GP>& grid;
 	public:
-		TMP(Grid<20, GP>& grid) : grid(grid) {;}
+		TMP(Grid<GP>& grid) : grid(grid) {;}
 		int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
 		const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
 		float getWeightBetween(const GP& n1, const GP& n2) const {
@@ -29,7 +29,7 @@ TEST(TestAll, Nav) {
 		}
 	} tmp(g);
 
-	GridFactory<20, GP> gf(g);
+	GridFactory<GP> gf(g);
 
 	// load floorplan
 	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);

tests/grid/TestGrid.cpp

@@ -8,7 +8,7 @@
 
 TEST(Grid, add) {
 
-	Grid<20, GP> grid;
+	Grid<GP> grid(20);
 	ASSERT_EQ(0, grid.add(GP()));
 	ASSERT_EQ(1, grid.add(GP()));
 	ASSERT_EQ(2, grid.add(GP()));
@@ -28,7 +28,7 @@ TEST(Grid, add) {
 
 TEST(Grid, BBox) {
 
-	Grid<20, GP> grid;
+	Grid<GP> grid(20);
 	int idx = grid.add(GP(40,40,40));
 	ASSERT_EQ(30, grid.getBBox(idx).getMin().x);
 	ASSERT_EQ(50, grid.getBBox(idx).getMax().x);
@@ -40,7 +40,7 @@ TEST(Grid, BBox) {
 
 TEST(Grid, connectBiDir) {
 
-	Grid<1, GP> grid;
+	Grid<GP> grid(1);
 
 	int idx1 = grid.add(GP( 0, 0, 0));
 	int idx2 = grid.add(GP( 0, 1, 0));
@@ -68,7 +68,7 @@ TEST(Grid, connectBiDir) {
 
 TEST(Grid, disconnectBiDir) {
 
-	Grid<1, GP> grid;
+	Grid<GP> grid(1);
 
 	int idx1 = grid.add(GP( 0, 0, 0));
 	int idx2 = grid.add(GP( 0, 1, 0));
@@ -111,7 +111,7 @@ TEST(Grid, disconnectBiDir) {
 
 TEST(Grid, uid) {
 
-	Grid<20, GP> grid;
+	Grid<GP> grid(20);
 
 	GP gp(20,40,60);
 	uint64_t uid = grid.getUID(gp);
@@ -125,7 +125,7 @@ TEST(Grid, uid) {
 
 TEST(Grid, remove) {
 
-	Grid<1, GP> grid;
+	Grid<GP> grid(1);
 
 	GP gp1( 0, 0, 0);
 	GP gp2( 0, 1, 0);
@@ -162,7 +162,7 @@ TEST(Grid, remove) {
 
 TEST(Grid, neighborIter) {
 
-	Grid<1, GP> grid;
+	Grid<GP> grid(1);
 
 	int idx1 = grid.add(GP( 0, 0, 0));
 	int idx2 = grid.add(GP( 0, 1, 0));
@@ -183,7 +183,7 @@ TEST(Grid, neighborIter) {
 
 TEST(Grid, bbox) {
 
-	Grid<1, GP> grid;
+	Grid<GP> grid(1);
 
 	grid.add(GP( 0, 0, 0));
 	grid.add(GP( 0, 1, 0));
@@ -206,7 +206,7 @@ TEST(Grid, bbox) {
 
 TEST(Grid, nearest) {
 
-	Grid<20, GP> grid;
+	Grid<GP> grid(20);
 
 	GP c1(20,20,20);
 	GP c2(40,40,40);

tests/grid/TestGridFactory.cpp

@@ -10,10 +10,10 @@
 
 TEST(GridFactory, create) {
 
-	Grid<20, GP> g;
-	Grid<20, GP> gInv;
+	Grid<GP> g(20);
+	Grid<GP> gInv(20);
 
-	GridFactory<20, GP> gf(g);
+	GridFactory<GP> gf(g);
 	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 4);
 	Floor f1 = fpf.getFloor("1");
 	Floor f2 = fpf.getFloor("2");
@@ -24,7 +24,7 @@ TEST(GridFactory, create) {
 	gf.addStairs(s1_2, 20, 340);
 	gf.removeIsolated();
 
-	GridFactory<20, GP> gfInv(gInv);
+	GridFactory<GP> gfInv(gInv);
 	gfInv.addInverted(g, 20);
 	gfInv.addInverted(g, 340);
 

tests/grid/TestImportance.cpp

@@ -0,0 +1,44 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../grid/factory/GridImportance.h"
+#include "../../grid/factory/GridFactory.h"
+#include "../../floorplan/FloorplanFactorySVG.h"
+#include "../../nav/dijkstra/Dijkstra.h"
+#include "../../grid/walk/GridWalkWeighted.h"
+
+#include "Plot.h"
+
+TEST(Importance, Doors) {
+
+
+	Grid<GP> g(20);
+	GridFactory<GP> gf(g);
+
+	// load floorplan
+	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
+	Floor f1 = fpf.getFloor("1");
+	Floor f2 = fpf.getFloor("2");
+	Stairs s1_2 = fpf.getStairs("1_2");
+
+	// build the grid
+	gf.addFloor(f1, 20);
+	//gf.addFloor(f2, 340);
+	gf.addStairs(s1_2, 20, 340);
+	gf.removeIsolated();
+
+	// calculate node importance based on the floorplan (walls, ...)
+	GridImportance gi;
+	gi.addImportance(g, 20);
+
+
+	Plot p;
+	p.gp << "set view 0,0\n";
+	p.build(g);
+	p.addFloor(f1, 20);
+	p.fire();
+
+}
+
+#endif

tests/grid/TestWalk.cpp

@@ -14,8 +14,8 @@
 
 TEST(Walk, plot) {
 
-	Grid<20, GP> g;
-	GridFactory<20, GP> gf(g);
+	Grid<GP> g(20);
+	GridFactory<GP> gf(g);
 
 	bool use3D = true;
 
@@ -40,9 +40,9 @@ TEST(Walk, plot) {
 
 	// dijkstra mapper
 	class TMP {
-		Grid<20, GP>& grid;
+		Grid<GP>& grid;
 	public:
-		TMP(Grid<20, GP>& grid) : grid(grid) {;}
+		TMP(Grid<GP>& grid) : grid(grid) {;}
 		int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
 		const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
 		float getWeightBetween(const GP& n1, const GP& n2) const {

tests/nav/dijkstra/TestDijkstra.cpp

@@ -8,7 +8,7 @@
 
 TEST(Dijkstra, build) {
 
-	Grid<1, GP> grid;
+	Grid<GP> grid(1);
 
 	int idx1 = grid.add(GP( 0, 0, 0));
 	int idx2 = grid.add(GP( 0, 1, 0));
@@ -22,9 +22,9 @@ TEST(Dijkstra, build) {
 	grid.connectBiDir(idx1, idx5);
 
 	class TMP {
-		Grid<1, GP>& grid;
+		Grid<GP>& grid;
 	public:
-		TMP(Grid<1, GP>& grid) : grid(grid) {;}
+		TMP(Grid<GP>& grid) : grid(grid) {;}
 		int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
 		const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
 		float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}