added new data-structures

added new test-cases
added flexible dijkstra calculation
added debugging log
modified: plotting, grid-generation, grid-importance,
refactoring

CMakeLists.txt

@@ -36,6 +36,7 @@ FILE(GLOB SOURCES
 	./*.cpp
 	./*/*.cpp
 	./*/*/*.cpp
+	./*/*/*/*.cpp
 	../KLib/inc/tinyxml/*.cpp
 )
 
@@ -59,8 +60,9 @@ ADD_DEFINITIONS(
 	-Wall
 	-Werror=return-type
 	-Wextra
+	-Wpedantic
 
-	-g
+	-g3
 	-O0
 
 	-DWITH_TESTS

geo/Point3.h

@@ -24,6 +24,18 @@ struct Point3 {
 
 	Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);}
 
+	Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
+
+	float length() const {return std::sqrt(x*x + y*y + z*z);}
+
+	float length(const float norm) const {
+		return std::pow(
+					(std::pow(std::abs(x),norm) +
+					 std::pow(std::abs(y),norm) +
+					 std::pow(std::abs(z),norm)
+				), 1.0f/norm);
+	}
+
 };
 
 #endif // POINT3_H

grid/Grid.h

@@ -2,15 +2,16 @@
 #define GRID_H
 
 #include <vector>
+#include <iostream>
 #include <unordered_map>
 
 #include "../Exception.h"
 #include "GridPoint.h"
 #include "GridNode.h"
-#include <iostream>
 
 #include <KLib/Assertions.h>
 #include "../geo/BBox3.h"
+#include "../misc/Debug.h"
 
 /**
  * grid of the given grid-size, storing some value which
@@ -18,7 +19,11 @@
  */
 template <int gridSize_cm, typename T> class Grid {
 
-typedef uint64_t UID;
+	static constexpr const char* name = "Grid";
+
+	#include "GridNeighborIterator.h"
+
+	typedef uint64_t UID;
 
 private:
 
@@ -72,10 +77,7 @@ public:
 	void connectUniDir(T& n1, const T& n2) {
 		n1._neighbors[n1._numNeighbors] = n2._idx;
 		++n1._numNeighbors;
-		if (n1._numNeighbors > 12) {
+		_assertBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
-			int i = 0;
-		}
-		_assertBetween(n1._numNeighbors, 0, 12, "number of neighbors out of bounds!");
 	}
 
 	/**
@@ -154,14 +156,21 @@ public:
 	 *
 	 */
 	UID getUID(const GridPoint& p) const {
-		const uint64_t x = std::round(p.x_cm / gridSize_cm);
+		const uint64_t x = std::round(p.x_cm / (float)gridSize_cm);
-		const uint64_t y = std::round(p.y_cm / gridSize_cm);
+		const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
-		const uint64_t z = std::round(p.z_cm / gridSize_cm);
+		const uint64_t z = std::round(p.z_cm / (float)gridSize_cm);
 		return (z << 40) | (y << 20) | (x << 0);
 	}
 
 	/** array access */
 	T& operator [] (const int idx) {
+		_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
+		return nodes[idx];
+	}
+
+	/** const array access */
+	const T& operator [] (const int idx) const {
+		_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
 		return nodes[idx];
 	}
 
@@ -223,48 +232,57 @@ public:
 	 */
 	void cleanup() {
 
-		for (size_t i = 0; i < nodes.size(); ++i) {
+		Log::add(name, "running grid cleanup");
+
+		// check every single node
+		for (int i = (int)nodes.size() - 1; i >= 0; --i) {
+
+			// is this node marked as "deleted"? (idx == -1)
 			if (nodes[i]._idx == -1) {
-				nodes.erase(nodes.begin()+i);
+
-				moveDown(i);
+				// remove this node
-				--i;
+				deleteNode(i);
+				++i;
+
 			}
 		}
 
+		// rebuild hashes
+		Log::add(name, "rebuilding UID hashes");
+		hashes.clear();
+		for (size_t idx = 0; idx < nodes.size(); ++idx) {
+			hashes[getUID(nodes[idx])] = idx;
+		}
+
 	}
 
-	void moveDown(const int idx) {
+private:
+
+	/** hard-delete the given node */
+	void deleteNode(const int idx) {
+
+		// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
+
+		// remove him from the node list (reclaim its memory and its index)
+		nodes.erase(nodes.begin()+idx);
+
+		// decrement the index for all of the following nodes and adjust neighbor references
 		for (size_t i = 0; i < nodes.size(); ++i) {
-			if (nodes[i]._idx >= idx) {--nodes[i]._idx;}
+
+			// decrement the higher indices (reclaim the free one)
+			if (nodes[i]._idx >= idx) { --nodes[i]._idx;}
+
+			// adjust the neighbor references (decrement by one)
 			for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
 				if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
 			}
+
 		}
 	}
 
-	class NeighborIter : std::iterator<std::input_iterator_tag, int> {
+public:
-	private:
+
-		Grid<gridSize_cm, T>& grid;
-		int nodeIdx;
-		int nIdx;
-	public:
-		NeighborIter(Grid<gridSize_cm, T>& grid, const int nodeIdx, const int nIdx) : grid(grid), nodeIdx(nodeIdx), nIdx(nIdx) {;}
-		NeighborIter& operator++() {++nIdx; return *this;}
-		NeighborIter operator++(int) {NeighborIter tmp(*this); operator++(); return tmp;}
-		bool operator==(const NeighborIter& rhs) {return nodeIdx == rhs.nodeIdx && nIdx == rhs.nIdx;}
-		bool operator!=(const NeighborIter& rhs) {return nodeIdx != rhs.nodeIdx || nIdx != rhs.nIdx;}
-		T& operator*() {return (T&) grid.nodes[nodeIdx]._neighbors[nIdx];}
-	};
 
-	class NeighborForEach {
-	private:
-		Grid<gridSize_cm, T>& grid;
-		int nodeIdx;
-	public:
-		NeighborForEach(Grid<gridSize_cm, T>& grid, const int nodeIdx) : grid(grid), nodeIdx(nodeIdx) {;}
-		NeighborIter begin() {return NeighborIter(grid, nodeIdx, 0);}
-		NeighborIter end() {return NeighborIter(grid, nodeIdx, grid[nodeIdx]._numNeighbors);}
-	};
 
 	NeighborForEach neighbors(const int idx) {
 		return neighbors(nodes[idx]);
@@ -287,7 +305,7 @@ public:
 		return nodes.size();
 	}
 
-	template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { return false; }
+	template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { (void) bb; return false; }
 
 	inline float kdtree_get_pt(const size_t idx, const int dim) const {
 		const T& p = nodes[idx];

grid/GridNeighborIterator.h

@@ -0,0 +1,66 @@
+#ifndef GRIDNEIGHBORITERATOR_H
+#define GRIDNEIGHBORITERATOR_H
+
+
+/** allows iterating over all neighbors of one node */
+class NeighborIter : std::iterator<std::input_iterator_tag, int> {
+
+private:
+
+	/** the grid the src-node belongs to */
+	const Grid<gridSize_cm, T>& grid;
+
+	/** index of the source-node within its grid */
+	const int srcNodeIdx;
+
+	/** index of the current neighbor [0:10] */
+	int nIdx;
+
+public:
+
+	/** ctor */
+	NeighborIter(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx, const int nIdx) :
+		grid(grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
+
+	/** next neighbor */
+	NeighborIter& operator++() {++nIdx; return *this;}
+
+	/** next neighbor */
+	NeighborIter operator++(int) {NeighborIter tmp(*this); operator++(); return tmp;}
+
+	/** compare with other iterator */
+	bool operator==(const NeighborIter& rhs) {return srcNodeIdx == rhs.srcNodeIdx && nIdx == rhs.nIdx;}
+
+	/** compare with other iterator */
+	bool operator!=(const NeighborIter& rhs) {return srcNodeIdx != rhs.srcNodeIdx || nIdx != rhs.nIdx;}
+
+	/** get the neighbor the iterator currently points to */
+	T& operator*() {return (T&) grid.getNeighbor(srcNodeIdx, nIdx);}
+
+};
+
+/** allows for-each iteration over all neighbors of one node */
+class NeighborForEach {
+private:
+
+	/** the grid the src-node belongs to */
+	const Grid<gridSize_cm, T>& grid;
+
+	/** index of the source-node within its grid */
+	const int srcNodeIdx;
+
+public:
+
+	/** ctor */
+	NeighborForEach(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx) :
+		grid(grid), srcNodeIdx(srcNodeIdx) {;}
+
+	/** starting point */
+	NeighborIter begin() {return NeighborIter(grid, srcNodeIdx, 0);}
+
+	/** end point */
+	NeighborIter end() {return NeighborIter(grid, srcNodeIdx, grid[srcNodeIdx]._numNeighbors);}
+
+};
+
+#endif // GRIDNEIGHBORITERATOR_H

grid/GridNode.h

@@ -13,22 +13,24 @@ template<int, typename> class Grid;
  * to store additional information for each node besides
  * the user's requested data-structure
  */
-class GridNode {
+struct GridNode {
+
+private:
 
 	template<int, typename> friend class Grid;
 
 	/** INTERNAL: array-index */
-	int _idx = -1;
+	int _idx;
 
 	/** INTERNAL: number of neighbors */
-	int _numNeighbors = 0;
+	int _numNeighbors;
 
 	/** INTERNAL: store neighbors (via index) */
-	int _neighbors[12] = {};
+	int _neighbors[10];
 
 public:
 
-	GridNode() {;}
+	GridNode() : _idx(-1), _numNeighbors(0), _neighbors() {;}
 
 	/** get the node's index within its grid */
 	int getIdx() const {return _idx;}
@@ -36,10 +38,10 @@ public:
 	/** get the number of neighbors for this node */
 	int getNumNeighbors() const {return _numNeighbors;}
 
-	/** get the n-th neighbor for this node */
+//	/** get the n-th neighbor for this node */
-	template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
+//	template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
-		return grid.getNeighbor(_idx, nth);
+//		return grid.getNeighbor(_idx, nth);
-	}
+//	}
 
 };
 

grid/GridPoint.h

@@ -2,6 +2,7 @@
 #define GRIDPOINT_H
 
 #include <cmath>
+#include "../geo/Point3.h"
 
 struct GridPoint {
 
@@ -35,6 +36,12 @@ struct GridPoint {
 		return std::sqrt(dx*dx + dy*dy + dz*dz) / 100.0f;
 	}
 
+	/** cast to Point3 */
+	operator Point3() const {return Point3(x_cm, y_cm, z_cm);}
+
+	/** cast to string */
+	operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
+
 
 };
 

grid/factory/GridFactory.h

@@ -9,10 +9,16 @@
 #include "../GridNodeBBox.h"
 #include "../Grid.h"
 
+#include "../../misc/Debug.h"
+
 template <int gridSize_cm, typename T> class GridFactory {
 
+	/** logging name */
+	static constexpr const char* name = "GridFac";
+
 private:
 
+	/** the grid to build into */
 	Grid<gridSize_cm, T>& grid;
 
 
@@ -24,6 +30,7 @@ public:
 	/** 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));
 
 		// build grid-points
 		for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
@@ -43,8 +50,11 @@ public:
 
 	}
 
+	/** connect all neighboring nodes located on the given height-plane */
 	void connectAdjacent(const float z_cm) {
 
+		Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm));
+
 		// connect adjacent grid-points
 		for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
 
@@ -79,6 +89,8 @@ public:
 
 	void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
 
+		Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm));
+
 		for (const Stair& s : stairs) {
 
 			for (int i = 0; i < grid.getNumNodes(); ++i) {
@@ -100,7 +112,7 @@ public:
 						buildStair(n, n2);
 
 					}
-					int i = 0;
+
 				}
 			}
 
@@ -182,16 +194,21 @@ public:
 
 	void removeIsolated() {
 
+		Log::add(name, "searching for isolated nodes");
+
 		// get largest connected region
 		std::set<int> set;
 		do {
 			const int idxStart = rand() % grid.getNumNodes();
 			set.clear();
+			Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
 			getConnected(idxStart, set);
+			Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
+
 		} while (set.size() < 0.5 * grid.getNumNodes());
 
-
 		// remove all other
+		Log::add(name, "removing the isolated nodes");
 		for (int i = 0; i < grid.getNumNodes(); ++i) {
 			if (set.find(i) == set.end()) {grid.remove(i);}
 		}
@@ -206,10 +223,14 @@ 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];
+		// get the node behind idx
+		const T& n1 = (T&) grid[idx];
+
+		// add him to the current region
 		set.insert(n1.getIdx());
 
-		for (T& n2 : grid.neighbors(n1)) {
+		// get all his (unprocessed) neighbors and add them to the region
+		for (const T& n2 : grid.neighbors(n1)) {
 			if (set.find(n2.getIdx()) == set.end()) {
 				getConnected(n2.getIdx(), set);
 			}

grid/factory/GridImportance.h

@@ -4,16 +4,31 @@
 #include "../Grid.h"
 #include "GridFactory.h"
 #include "../../misc/KNN.h"
+#include "../../math/MiniMat2.h"
+
+#include "../../misc/Debug.h"
 
 #include <KLib/math/distribution/Normal.h>
 
+/**
+ * add an importance factor to each node within the grid.
+ * the importance is calculated based on several facts:
+ * - nodes that belong to a door or narrow path are more important
+ * - nodes directly located at walls are less important
+ */
 class GridImportance {
 
+private:
+
+	static constexpr const char* name = "GridImp";
+
 public:
 
 	/** attach importance-factors to the grid */
 	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, 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);
@@ -22,29 +37,28 @@ public:
 		// construct KNN search
 		KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
 
+		// the number of neighbors to use
+		static constexpr int numNeighbors = 8;
+
 		for (int idx = 0; idx < g.getNumNodes(); ++idx) {
 
 			// process each point
 			T& n1 = (T&) g[idx];
 
-//			// get its nearest neighbor
+			// get the 10 nearest neighbors and their distance
-//			size_t idxNear;
+			size_t indices[numNeighbors];
-//			float distSquared;
+			float squaredDist[numNeighbors];
-//			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);
+			knn.get(point, numNeighbors, indices, squaredDist);
 
-			const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
+			// get the neighbors
-			const float imp2 = door( indices );
+			std::vector<T*> neighbors;
-			n1.imp = (imp1 + imp2)/2;
+			for (int i = 0; i < numNeighbors; ++i) {
+				neighbors.push_back(&inv[indices[i]]);
+			}
+
+			addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
+			addDoor(n1, neighbors);
 
 		}
 
@@ -52,31 +66,67 @@ public:
 
 	}
 
-	float door( size_t* indices ) {
 
-		// build covariance
+	/** add importance to nSrc if it is part of a door */
+	template <typename T> void addDoor( T& nSrc, std::vector<T*> neighbors ) {
 
-		//if (dist1_m > 1.0) {return 1;}
+		MiniMat2 m;
-		//return 1.0 - std::abs(dist1_m - dist2_m);
+		Point3 center = nSrc;
-		return 1;
+
+		// 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() > 60) {return;}
+
+		// build covariance of the nearest-neighbors
+		int used = 0;
+		for (const T* n : neighbors) {
+			Point3 d = (Point3)*n - center;
+			if (d.length() > 100) {continue;}	// radius search
+			m.addSquared(d.x, d.y);
+			++used;
 		}
 
-	float importance(float dist_m) {
+		// we need at least two points for the covariance
+		if (used < 2) {return;}
 
-		static K::NormalDistribution d1(0.0, 0.5);
+		// check eigenvalues
-		//if (dist_m > 1.5) {dist_m = 1.5;}
+		MiniMat2::EV ev = m.getEigenvalues();
-		return 1.0 - d1.getProbability(dist_m) * 0.5;
 
-//		static K::NormalDistribution d1(1.0, 0.75);
+		// ensure e1 > e2
-//		//static K::NormalDistribution d2(3.0, 0.75);
+		if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
+
+		// door?
+		if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.2; }
+
+	}
+
+	/** get the importance of the given node depending on its nearest wall */
+	template <typename T> void addImportance(T& nSrc, float dist_m) {
+
+		// avoid sticking too close to walls (unlikely)
+		static K::NormalDistribution avoidWalls(0.0, 0.3);
+
+		// favour walking near walls (likely)
+		static K::NormalDistribution sticToWalls(0.9, 0.5);
+
+		// favour walking far away (likely)
+		static K::NormalDistribution farAway(2.2, 0.5);
+		if (dist_m > 2.0) {dist_m = 2.0;}
+
+		// overall importance
+		nSrc.imp *=  1.0
+				- avoidWalls.getProbability(dist_m) * 0.35		// avoid walls
+				+ sticToWalls.getProbability(dist_m) * 0.15		// walk near walls
+				+ farAway.getProbability(dist_m) * 0.20			// walk in the middle
+		;
 
-//		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;}
 	}
 
 };

main.cpp

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

math/MiniMat2.h

@@ -0,0 +1,36 @@
+#ifndef MINIMAT2_H
+#define MINIMAT2_H
+
+/**
+ * very simple 2x2 matrix
+ */
+struct MiniMat2 {
+
+	/** store eigenvalues */
+	struct EV {float e1,e2;};
+
+	/** data */
+	float a,b,c,d;
+
+	/** ctor */
+	MiniMat2() : a(0), b(0), c(0), d(0) {;}
+
+	/** get the matrix' eigenvalues */
+	EV getEigenvalues() const {
+		const float T = a+d;
+		const float D = a*d - b*c;
+		EV res;
+		res.e1 = T/2 + std::sqrt(T*T/4-D);
+		res.e2 = T/2 - std::sqrt(T*T/4-D);
+		return res;
+	}
+
+	/** add (x,y) * (x,y)T to the matrix */
+	void addSquared(const float x, const float y) {
+		a += (x*x);	b += (x*y);
+		c += (x*y);	d += (y*y);
+	}
+
+};
+
+#endif // MINIMAT2_H

misc/Debug.h

@@ -0,0 +1,30 @@
+#ifndef DEBUG_H
+#define DEBUG_H
+
+#include <string>
+#include <iostream>
+#include <iomanip>
+
+class Log {
+
+public:
+
+	static void add(const char* comp, const std::string what) {
+		addComp(comp);
+		std::cout << what << std::endl;
+	}
+
+	static void add(const std::string& component, const std::string what) {
+		addComp(component.c_str());
+		std::cout << what << std::endl;
+	}
+
+private:
+
+	static void addComp(const char* component) {
+		std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
+	}
+
+};
+
+#endif // DEBUG_H

nav/dijkstra/Dijkstra.h

@@ -0,0 +1,129 @@
+#ifndef DIJKSTRA_H
+#define DIJKSTRA_H
+
+
+#include <vector>
+#include <algorithm>
+#include <unordered_set>
+
+#include "DijkstraStructs.h"
+#include "../../misc/Debug.h"
+
+#include <KLib/Assertions.h>
+
+template <typename T> class Dijkstra {
+
+	/** all allocated nodes for the user-data inputs */
+	std::unordered_map<const T*, DijkstraNode<T>*> nodes;
+
+	/** all already processed edges */
+	std::unordered_set<DijkstraEdge<T>> usedEdges;
+
+	/** to-be-processed nodes (USE LINKED LIST INSTEAD?!) */
+	std::vector<DijkstraNode<T>*> toBeProcessedNodes;
+
+
+public:
+
+	/** get (or create) a new node for the given user-node */
+	DijkstraNode<T>* getNode(const T* userNode) {
+		if (nodes.find(userNode) == nodes.end()) {
+			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
+			nodes[userNode] = dn;
+		}
+		return nodes[userNode];
+	}
+
+	/** get the edge (bi-directional) between the two given nodes */
+	DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {
+		return DijkstraEdge<T>(n1, n2);
+	}
+
+	/** get the dijkstra-pendant for the given user-node */
+	DijkstraNode<T>* getNode(const T& userNode) {
+		return nodes[&userNode];
+	}
+
+	/** build shortest path from start to end using the provided wrapper-class */
+	template <typename Access> void build(const T& start, const T& end, const Access& acc) {
+
+		// NOTE: end is currently ignored!
+		// runs until all nodes were evaluated
+
+		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start);
+
+		// cleanup
+		toBeProcessedNodes.clear();
+		usedEdges.clear();
+		nodes.clear();
+
+		// run from start
+		const T* cur = &start;
+
+		// create a node for the start element
+		DijkstraNode<T>* dnStart = getNode(cur);
+		dnStart->cumWeight = 0;
+
+		// add this node to the processing list
+		toBeProcessedNodes.push_back(dnStart);
+
+		// until we are done
+		while(!toBeProcessedNodes.empty()) {
+
+			// get the next to-be-processed node
+			DijkstraNode<T>* dnSrc = toBeProcessedNodes[0];
+
+			// and remove him from the list
+			toBeProcessedNodes.erase(toBeProcessedNodes.begin());
+
+			// process each neighbor of the current element
+			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
+
+				// get the neighbor itself
+				const T* dst = acc.getNeighbor(*dnSrc->element, i);
+
+				// get the distance-weight to the neighbor
+				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
+				_assertTrue(weight >= 0, "edge-weight must not be negative!");
+
+				// get-or-create a node for the neighbor
+				DijkstraNode<T>* dnDst = getNode(dst);
+
+				// get-or-create the edge describing the connection
+				DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
+
+				// was this edge already processed? -> skip it
+				if (usedEdges.find(edge) != usedEdges.end()) {continue;}
+
+				// otherwise: remember it
+				usedEdges.insert(edge);
+
+				// and add the node for later processing
+				toBeProcessedNodes.push_back(dnDst);
+
+				// update the weight to the destination?
+				const float potentialWeight = dnSrc->cumWeight + weight;
+				if (potentialWeight < dnDst->cumWeight) {
+					dnDst->cumWeight = potentialWeight;
+					dnDst->previous = dnSrc;
+				}
+
+			}
+
+			// sort the nodes by distance-from-start (shortest first)
+			auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
+			std::sort(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
+
+		}
+
+		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
+
+		// cleanup
+		toBeProcessedNodes.clear();
+		usedEdges.clear();
+
+	}
+
+};
+
+#endif // DIJKSTRA_H

nav/dijkstra/DijkstraStructs.h

@@ -0,0 +1,77 @@
+#ifndef DIJKSTRANODE_H
+#define DIJKSTRANODE_H
+
+/**
+ * wrapper around a user data structure
+ * adds additional fields needed for dijkstra calculation
+ */
+template <typename T> struct DijkstraNode {
+
+	/** pos infinity */
+	static constexpr float INF = +99999999;
+
+	/** the user-element this node describes */
+	const T* element;
+
+	/** the previous dijkstra node (navigation path) */
+	DijkstraNode<T>* previous;
+
+	/** the weight from the start up to this element */
+	float cumWeight;
+
+
+//	/** ctor */
+//	DijkstraNode() : element(nullptr), previous(), cumWeight(INF) {;}
+
+	/** ctor */
+	DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;}
+
+
+	/** equal? (bi-dir) */
+	bool operator == (const DijkstraNode<T>& other) {
+		return element == other.element;
+	}
+
+};
+
+/**
+ * data structure describing the connection between two nodes
+ * only used to track already processed connections!
+ */
+template <typename T> struct DijkstraEdge {
+
+	/** the edge's source */
+	const DijkstraNode<T>* src;
+
+	/** the edge's destination */
+	const DijkstraNode<T>* dst;
+
+	/** ctor */
+	DijkstraEdge(const DijkstraNode<T>* src, const DijkstraNode<T>* dst) : src(src), dst(dst) {;}
+
+	/** equal? (bi-dir) */
+	bool operator == (const DijkstraEdge& other) const {
+		return	((dst == other.dst) && (src == other.src)) ||
+				((src == other.dst) && (dst == other.src));
+	}
+
+};
+
+//template <typename T> struct DijkstraEdgeWeighted : public DijkstraEdge<T> {
+
+//	/** the edge's weight */
+//	float weight;
+
+//	DijkstraEdgeWeighted(const DijkstraNode<T>* src, const DijkstraNode<T>* dst, const float weight) : DijkstraEdge<T>(src,dst), weight(weight) {;}
+
+//};
+
+namespace std {
+	template <typename T> struct hash<DijkstraEdge<T>>{
+		size_t operator()(const DijkstraEdge<T>& e) const {
+			return hash<size_t>()( (size_t)e.src^(size_t)e.dst);
+		}
+	};
+}
+
+#endif // DIJKSTRANODE_H

tests/Tests.h

@@ -6,7 +6,7 @@
 #include <gtest/gtest.h>
 
 static inline std::string getDataFile(const std::string& name) {
-	return "/apps/workspaces/Indoor/tests/data/" + name;
+	return "/mnt/data/workspaces/Indoor/tests/data/" + name;
 }
 
 #endif

tests/floorplan/TestFloorplanFactorySVG.cpp

@@ -1,26 +1,26 @@
 #ifdef WITH_TESTS
 
-//#include "../Tests.h"
+#include "../Tests.h"
-//#include "../../floorplan/FloorplanFactorySVG.h"
+#include "../../floorplan/FloorplanFactorySVG.h"
-//#include <cstdlib>
+#include <cstdlib>
 
 
 
-//TEST(FloorplanFactorySVG, parse) {
+TEST(FloorplanFactorySVG, parse) {
 
-//	const std::string filename = getDataFile("test.svg");
+	const std::string filename = getDataFile("test.svg");
-//	FloorplanFactorySVG factory(filename, 1.0);
+	FloorplanFactorySVG factory(filename, 1.0);
 
-//	Floor f1 = factory.getFloor("1");
+	Floor f1 = factory.getFloor("1");
-//	ASSERT_EQ(30, f1.getObstacles().size());
+	ASSERT_EQ(30, f1.getObstacles().size());
 
-//	Floor f2 = factory.getFloor("2");
+	Floor f2 = factory.getFloor("2");
-//	ASSERT_EQ(30, f2.getObstacles().size());
+	ASSERT_EQ(30, f2.getObstacles().size());
 
-//	Floor f3 = factory.getFloor("1_2");
+	Floor f3 = factory.getFloor("1_2");
-//	ASSERT_EQ(12, f3.getObstacles().size());
+	ASSERT_EQ(12, f3.getObstacles().size());
 
-//}
+}
 
 
 

tests/grid/GridImportance.cpp

@@ -22,7 +22,7 @@ TEST(GridImportance, a) {
 	GridImportance gi;
 	gi.addImportance(g, 20);
 
-	plot(g);
+	Plot p; p.build(g).fire();
 
 }
 

tests/grid/Plot.h

@@ -1,6 +1,8 @@
 #ifndef PLOT_H
 #define PLOT_H
 
+#include "../../grid/Grid.h"
+#include <set>
 
 #include <KLib/misc/gnuplot/Gnuplot.h>
 #include <KLib/misc/gnuplot/GnuplotSplot.h>
@@ -18,16 +20,21 @@ public:
 
 };
 
-template <int gridSize_cm, typename T> void plot(Grid<gridSize_cm, T>& g) {
+class Plot {
+
+public:
 
 	K::Gnuplot gp;
 	K::GnuplotSplot splot;
+
 	K::GnuplotSplotElementColorPoints points;
 	K::GnuplotSplotElementLines lines;
 
+	template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
+
 		gp << "set ticslevel 0\n";
 		gp << "set view equal xyz\n";
-	gp << "set cbrange[0.5:1.0]\n";
+		gp << "set cbrange[0.5:1.5]\n";
 		gp << "set palette gray negative\n";
 
 		std::set<uint64_t> done;
@@ -37,8 +44,9 @@ template <int gridSize_cm, typename T> void plot(Grid<gridSize_cm, T>& g) {
 			const GP& n1 = g[i];
 			points.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), n1.imp);
 
-		for (int n = 0; n < n1.getNumNeighbors(); ++n) {
+			for (const T& n2 : g.neighbors(n1)) {
-			const GP& n2 = n1.getNeighbor(n, g);
+			//for (int n = 0; n < n1.getNumNeighbors(); ++n) {
+			//	const GP& n2 = n1.getNeighbor(n, g);
 				if (done.find(g.getUID(n2)) == done.end()) {
 					K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
 					K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
@@ -55,12 +63,19 @@ template <int gridSize_cm, typename T> void plot(Grid<gridSize_cm, T>& g) {
 		//splot.add(&lines);
 		splot.add(&points);
 
+
+		return *this;
+
+	}
+
+	Plot& fire() {
 		gp.draw(splot);
 		gp.flush();
+		sleep(1000);
+		return *this;
+	}
 
-	sleep(100);
+};
-
-}
 
 
 #endif // PLOT_H

tests/grid/TestAll.cpp

@@ -0,0 +1,69 @@
+#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 "Plot.h"
+
+TEST(TestAll, Nav) {
+
+
+	Grid<20, GP> g;
+
+	// dijkstra mapper
+	class TMP {
+		Grid<20, GP>& grid;
+	public:
+		TMP(Grid<20, 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 {
+			float d = ((Point3)n1 - (Point3)n2).length(2.5);
+			//if (d > 20) {d*= 1.30;}
+			return d / std::pow(n2.imp, 3);
+		}
+	} tmp(g);
+
+	GridFactory<20, GP> gf(g);
+	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
+
+	Floor f1 = fpf.getFloor("1");
+	Floor f2 = fpf.getFloor("2");
+	Stairs s1_2 = fpf.getStairs("1_2");
+
+	gf.addFloor(f1, 20);
+	gf.addFloor(f2, 340);
+	gf.addStairs(s1_2, 20, 340);
+	gf.removeIsolated();
+
+
+	GridImportance gi;
+	gi.addImportance(g, 20);
+	gi.addImportance(g, 340);
+
+	Dijkstra<GP> d;
+	const GP& start = g.getNodeFor(GridPoint(500,200,20));
+	//const GP& end = g.getNodeFor(GridPoint(1400,1400,20));
+	const GP& end = g.getNodeFor(GridPoint(1200,200,340));
+	d.build(start, end, tmp);
+
+	// plot path
+	K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
+	DijkstraNode<GP>* dn = d.getNode(end);
+	while (dn->previous != nullptr) {
+		path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm));
+		dn = dn->previous;
+	}
+
+	Plot p;
+	p.build(g);
+	p.splot.add(&path);
+	p.fire();
+
+}
+
+#endif

tests/grid/TestGrid.cpp

@@ -1,17 +1,11 @@
 #ifdef WITH_TESTS
 
+#include "Plot.h"
 #include "../Tests.h"
 #include "../../grid/Grid.h"
 #include "../../grid/GridPoint.h"
 #include "../../grid/GridNode.h"
 
-class GP : public GridNode, public GridPoint {
-public:
-	GP() : GridNode(), GridPoint() {;}
-	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
-
-};
-
 TEST(Grid, add) {
 
 	Grid<20, GP> grid;
@@ -191,11 +185,11 @@ TEST(Grid, bbox) {
 
 	Grid<1, GP> grid;
 
-	int idx1 = grid.add(GP( 0, 0, 0));
+	grid.add(GP( 0, 0, 0));
-	int idx2 = grid.add(GP( 0, 1, 0));
+	grid.add(GP( 0, 1, 0));
-	int idx3 = grid.add(GP( 0,-1, 0));
+	grid.add(GP( 0,-1, 0));
-	int idx4 = grid.add(GP( 1, 0, 0));
+	grid.add(GP( 1, 0, 0));
-	int idx5 = grid.add(GP(-1, 0, 0));
+	grid.add(GP(-1, 0, 0));
 
 	BBox3 bb = grid.getBBox();
 

tests/grid/TestGridFactory.cpp

@@ -28,7 +28,7 @@ TEST(GridFactory, create) {
 	gfInv.addInverted(g, 20);
 	gfInv.addInverted(g, 340);
 
-//	plot(gInv);
+	//plot(gInv);
 
 
 }

tests/nav/dijkstra/TestDijkstra.cpp

@@ -0,0 +1,51 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../grid/Grid.h"
+#include "../../../nav/dijkstra/Dijkstra.h"
+#include "../../grid/Plot.h"
+
+TEST(Dijkstra, build) {
+
+	Grid<1, GP> grid;
+
+	int idx1 = grid.add(GP( 0, 0, 0));
+	int idx2 = grid.add(GP( 0, 1, 0));
+	int idx3 = grid.add(GP( 0,-1, 0));
+	int idx4 = grid.add(GP( 1, 0, 0));
+	int idx5 = grid.add(GP(-1, 0, 0));
+
+	grid.connectBiDir(idx1, idx2);
+	grid.connectBiDir(idx1, idx3);
+	grid.connectBiDir(idx1, idx4);
+	grid.connectBiDir(idx1, idx5);
+
+	class TMP {
+		Grid<1, GP>& grid;
+	public:
+		TMP(Grid<1, 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();}
+	} tmp(grid);
+
+	Dijkstra<GP> d;
+	d.build(grid[idx5], grid[idx3], tmp);
+
+	// start node must be "idx5"
+	DijkstraNode<GP>* n = d.getNode(grid[idx5]);
+	ASSERT_EQ(&grid[idx5], n->element);	ASSERT_EQ(nullptr, n->previous); ASSERT_EQ(0, n->cumWeight);
+
+	// "idx1" (the center) is reached via idx5
+	DijkstraNode<GP>* n2 = d.getNode(grid[idx1]);
+	ASSERT_EQ(&grid[idx1], n2->element);	ASSERT_EQ(&grid[idx5], n2->previous->element);
+
+	// "idx3" (the target) is reached via idx1 (the center)
+	DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
+	ASSERT_EQ(&grid[idx3], n3->element);	ASSERT_EQ(&grid[idx1], n3->previous->element);
+
+
+}
+
+#endif