huge commit

- worked on about everything
- grid walker using plugable modules
- wifi models
- new distributions
- worked on geometric data-structures
- added typesafe timestamps
- worked on grid-building
- added sensor-classes
- added sensor analysis (step-detection, turn-detection)
- offline data reader
- many test-cases

grid/Grid.h

@@ -54,7 +54,10 @@ public:
 	/** no-copy */
 	Grid(const Grid& o) = delete;
 
-	/** reset the grid (empty) */
+	/**
+	 * reset (clear) the grid
+	 * remove all nodes, hashes, ..
+	 */
 	void reset() {
 		nodes.clear();
 		hashes.clear();
@@ -211,10 +214,23 @@ public:
 	 *
 	 */
 	UID getUID(const GridPoint& p) const {
-		const uint64_t x = std::round(p.x_cm / (float)gridSize_cm);
-		const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
-		const uint64_t z = std::round(p.z_cm / (float)gridSize_cm * 5);		// z is usually much lower and not always aligned -> allow more room for hashes
+
+		// sanity check (region between -1^19 and +1^19
+		const int32_t max = 1 << 19;
+		Assert::isBetween((int32_t)p.x_cm, -max, +max, "x out of bounds");
+		Assert::isBetween((int32_t)p.y_cm, -max, +max, "y out of bounds");
+		Assert::isBetween((int32_t)p.z_cm, -max, +max, "z out of bounds");
+
+		// shift by half of the allowed width of 20 bit to allow negative regions
+		const uint64_t center = 1 << 19;
+
+		// build
+		const uint64_t x = std::round((p.x_cm+center) / (float)gridSize_cm);
+		const uint64_t y = std::round((p.y_cm+center) / (float)gridSize_cm);
+		const uint64_t z = std::round((p.z_cm+center) / (float)gridSize_cm * 5);		// z is usually much lower and not always aligned -> allow more room for hashes
+
 		return (z << 40) | (y << 20) | (x << 0);
+
 	}
 
 	/** array access */
@@ -363,63 +379,47 @@ public:
 		Log::add(name, "memory: " + std::to_string(bytes/1024.0f/1024.0f) + " MB in " + std::to_string(numNodes) + " nodes");
 	}
 
-//	/**
-//	 * remove all nodes, marked for deletion.
-//	 * BEWARE: this will invalidate all indices used externally!
-//	 */
-//	void cleanupOld() {
 
-//		Log::add(name, "running grid cleanup");
+public:
 
-//		// check every single node
-//		for (size_t i = 0; i < nodes.size(); ++i) {
+	/** serialize into the given stream */
+	void write(std::ostream& out) {
 
-//			// is this node marked as "deleted"? (idx == -1)
-//			if (nodes[i]._idx == -1) {
+		// serialize static
+		T::staticSerialize(out);
 
-//				// remove this node
-//				deleteNode(i);
-//				--i;
+		// number of nodes
+		const int numNodes = nodes.size();
+		out.write((const char*) &numNodes, sizeof(numNodes));
 
-//			}
-//		}
+		// serialize
+		for (const T& node : nodes) {
+			out.write((const char*) &node, sizeof(T));
+		}
 
-//		// rebuild hashes
-//		Log::add(name, "rebuilding UID hashes", false);
-//		Log::tick();
-//		hashes.clear();
-//		for (size_t idx = 0; idx < nodes.size(); ++idx) {
-//			hashes[getUID(nodes[idx])] = idx;
-//		}
-//		Log::tock();
+	}
 
-//	}
+	/** deserialize from the given stream */
+	void read(std::istream& inp) {
 
-private:
+		// deserialize static
+		T::staticDeserialize(inp);
 
-//	/** hard-delete the given node */
-//	void deleteNode(const int idx) {
+		// number of nodes
+		int numNodes;
+		inp.read((char*) &numNodes, sizeof(numNodes));
 
-//		_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
+		// allocate node-space
+		nodes.resize(numNodes);
 
-//		// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
+		// deserialize
+		inp.read((char*) nodes.data(), numNodes*sizeof(T));
 
-//		// remove him from the node list (reclaim its memory and its index)
-//		nodes.erase(nodes.begin()+idx);
+		// update
+		rebuildHashes();
 
-//		// decrement the index for all of the following nodes and adjust neighbor references
-//		for (size_t i = 0; i < nodes.size(); ++i) {
+	}
 
-//			// 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];}
-//			}
-
-//		}
-//	}
 
 public:
 

grid/GridNode.h

@@ -4,6 +4,8 @@
 #include "GridNodeBBox.h"
 #include "GridPoint.h"
 
+#include <functional>
+
 /** forward decl. */
 template<typename> class Grid;
 
@@ -35,7 +37,8 @@ private:
 
 
 
-
+	static std::vector<std::function<void(std::ostream&)>> serializers;
+	static std::vector<std::function<void(std::istream&)>> deserializers;
 
 
 public:

grid/GridPoint.h

@@ -35,6 +35,12 @@ struct GridPoint {
 		return x_cm != o.x_cm || y_cm != o.y_cm || z_cm != o.z_cm;
 	}
 
+	GridPoint operator * (const float f) const {return GridPoint(x_cm*f, y_cm*f, z_cm*f);}
+
+	GridPoint& operator += (const GridPoint& o) {x_cm += o.x_cm; y_cm += o.y_cm; z_cm += o.z_cm; return *this;}
+
+	GridPoint& operator /= (const float f) {x_cm /= f; y_cm /= f; z_cm /= f; return *this;}
+
 	/** get the distance (in meter) betwen this and the given point */
 	float getDistanceInMeter(const GridPoint& other) const {
 		return getDistanceInCM(other) / 100.0f;

grid/factory/v2/GridNodeImportance.h

@@ -0,0 +1,18 @@
+#ifndef GRIDNODEIMPORTANCE_H
+#define GRIDNODEIMPORTANCE_H
+
+
+struct GridNodeImportance {
+
+	/** importance-weight for dijkstra calculation */
+	float navImportance;
+
+	/** get the node's nav importance */
+	float getNavImportance() const {return navImportance;}
+
+	/** ctor */
+	GridNodeImportance() : navImportance(1.0f) {;}
+
+};
+
+#endif // GRIDNODEIMPORTANCE_H

grid/factory/v2/Importance.h

@@ -1,6 +1,7 @@
 #ifndef IMPORTANCE_H
 #define IMPORTANCE_H
 
+#include "../../../geo/Units.h"
 #include "../../Grid.h"
 
 #include "../../../misc/KNN.h"
@@ -10,6 +11,8 @@
 #include "../../../math/Distributions.h"
 
 
+
+
 class Importance {
 
 private:
@@ -32,9 +35,9 @@ public:
 	}
 
 	/** attach importance-factors to the grid */
-	template <typename T> static void addImportance(Grid<T>& g, const float z_cm) {
+	template <typename T> static void addImportance(Grid<T>& g) {
 
-		Log::add(name, "adding importance information to all nodes at height " + std::to_string(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());
@@ -70,7 +73,7 @@ public:
 		KNNArray<std::vector<T>> knnArrDoors(doors);
 		KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);
 
-		Distribution::Normal<float> favorDoors(0.0f, 0.6f);
+		Distribution::Normal<float> favorDoors(0.0f, 0.5f);
 
 		// process each node again
 		for (T& n1 : g) {
@@ -90,7 +93,7 @@ public:
 				neighbors.push_back(&inv[indices[i]]);
 			}
 
-			n1.imp = 1.0f;
+			n1.navImportance = 1.0f;
 
 			//if (n1.getType() == GridNode::TYPE_FLOOR) {
 
@@ -100,20 +103,20 @@ public:
 			// 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 =
+			n1.navImportance =
 					1 +
 					getWallImportance( distToWall_m ) +
-					favorDoors.getProbability(distToDoor_m);
+					favorDoors.getProbability(distToDoor_m) * 1.5f;
 
 
 			//}
 			//addDoor(n1, neighbors);
 
 			// importance for this node (based on the distance from the next door)
-			//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
+			//n1.navImportance += favorDoors.getProbability(dist_m) * 0.30;
 
 
-			//n1.imp = (dist_m < 0.2) ? (1) : (0.5);
+			//n1.navImportance = (dist_m < 0.2) ? (1) : (0.5);
 		}
 
 	}
@@ -131,73 +134,73 @@ public:
 
 	}
 
-	/** is the given node (and its inverted neighbors) a door? */
-	template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {
+//	/** 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;}
+//		if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}
 
-		MiniMat2 m1;
-//		MiniMat2 m2;
-		Point3 center = nSrc;
+//		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();
+//		// 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;}
+//		// 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) {
+//		// 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 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);
+////			const Point3 d2 = (Point3)*n - center;
+////			if (d2.length() > 100) {continue;}	// radius search
+////			m2.addSquared(d2.x, d2.y);
 
-			++used;
+//			++used;
 
-		}
+//		}
 
-		// we need at least two points for the covariance
-		if (used < 6) {return false;}
+//		// 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();
+//		// 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);}
+//		// 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);
+//		// 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) ;
+//		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);
+		static Distribution::Normal<float> avoidWalls(0.0, 0.35);
 
 		// favour walking near walls (likely)
-		static Distribution::Normal<float> stickToWalls(0.9, 0.5);
+		//static Distribution::Normal<float> stickToWalls(0.9, 0.7);
 
 		// favour walking far away (likely)
-		static Distribution::Normal<float> farAway(2.2, 0.5);
-		if (dist_m > 2.0) {dist_m = 2.0;}
+		//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

grid/factory/v2/Stairs.h

@@ -233,7 +233,8 @@ public:
 
 			// no matching node found -> add a new one to the grid
 			if (iNode.idx == -1) {
-				iNode.idx = grid.add(iNode.node);
+				const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, iNode.node.z_cm));
+				iNode.idx = (n2) ? (n2->getIdx()) : (grid.add(iNode.node));
 			}
 
 			// add semantic information

grid/walk/v2/GridWalker.h

@@ -0,0 +1,125 @@
+#ifndef GRIDWALKER_H
+#define GRIDWALKER_H
+
+#include "../../Grid.h"
+#include "../../../math/DrawList.h"
+#include "modules/WalkModule.h"
+
+/**
+ * modular grid-walker that takes various sub-components to determine
+ * p(e) and thus randomly pick edges
+ */
+template <typename Node, typename WalkState> class GridWalker {
+
+private:
+
+	/** all modules to evaluate */
+	std::vector<WalkModule<Node, WalkState>*> modules;
+
+	DrawList<const Node*> drawer;
+
+public:
+
+	/** add the given module */
+	void addModule(WalkModule<Node, WalkState>* module) {
+		modules.push_back(module);
+	}
+
+	/** perform the walk based on the configured setup */
+	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
+
+		WalkState startState = _startState;
+		updateBefore(startState);
+
+
+
+		// get the node that corresponds to start;
+		const Node* startNode = grid.getNodePtrFor(startState.startPos);
+		Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
+
+		// currently examined node
+		const Node* curNode = startNode;
+
+		// until distance is reached
+		while (dist_m > 0) {
+
+			drawer.reset();
+
+			// evaluate each neighbor
+			for (const Node& neighbor : grid.neighbors(*curNode)) {
+				const double prob = getProbability(startState, *startNode, *curNode, neighbor);
+				drawer.add(&neighbor, prob);
+			}
+
+			// pick a neighbor
+			const Node* nextNode = drawer.get();
+
+			// inform
+			step(startState, *curNode, *nextNode);
+
+			// update
+			dist_m -= nextNode->getDistanceInMeter(*curNode);
+			curNode = nextNode;
+
+		}
+
+		// output state
+		WalkState nextState = startState;
+		nextState.startPos = *curNode;
+
+		// update
+		updateAfter(nextState, *startNode, *curNode);
+
+		// done
+		return nextState;
+
+	}
+
+private:
+
+	/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
+	inline void updateBefore(WalkState& state) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateBefore(state);
+		}
+
+	}
+
+	/** update the WalkState after the random walk (if needed) */
+	inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateAfter(state, startNode, endNode);
+		}
+
+	}
+
+	/** one step taken. inform modules */
+	inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->step(state, curNode, nextNode);
+		}
+
+	}
+
+	/** get the probability for the given random walk (one edge) */
+	inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {
+
+		//double prob = 1.0;
+		double prob = 0;
+
+		for (const WalkModule<Node, WalkState>* mdl : modules) {
+			//prob *= mdl->getProbability(state, start, cur, next);
+			prob += std::log( mdl->getProbability(state, start, cur, next) );
+		}
+
+		//return prob;
+		return std::exp(prob);
+
+	}
+
+};
+
+#endif // GRIDWALKER_H

grid/walk/v2/GridWalkerMulti.h

@@ -0,0 +1,159 @@
+#ifndef GRIDWALKERMULTI_H
+#define GRIDWALKERMULTI_H
+
+#include "../../Grid.h"
+#include "../../../math/DrawList.h"
+#include "modules/WalkModule.h"
+
+// FOR TESTING
+#include "../../../geo/Heading.h"
+
+/**
+ * modular grid-walker that takes various sub-components to determine
+ * p(e) and thus randomly pick edges
+ */
+template <typename Node, typename WalkState> class GridWalkerMulti {
+
+private:
+
+	struct Path {
+		std::vector<const Node*> nodes;
+		const Node* getSrc() const {return nodes.front();}
+		const Node* getDst() const {return nodes.back();}
+	};
+
+
+private:
+
+	/** all modules to evaluate */
+	std::vector<WalkModule<Node, WalkState>*> modules;
+
+public:
+
+	/** add the given module */
+	void addModule(WalkModule<Node, WalkState>* module) {
+		modules.push_back(module);
+	}
+
+	/** perform the walk based on the configured setup */
+	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float dist_m) {
+
+		// get the node that corresponds to start;
+		const Node* const startNode = grid.getNodePtrFor(_startState.startPos);
+		Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
+
+		const uint64_t seed = (uint64_t) startNode + rand();
+		DrawList<Path> drawPath;
+		drawPath.setSeed(seed);
+
+		for (int i = 0; i < 1; ++i) {
+
+			float dist = dist_m;
+
+			// currently examined node
+			const Node* curNode = startNode;
+
+			Path path;
+			path.nodes.push_back(curNode);
+
+			// until distance is reached
+			while (dist > 0) {
+
+				DrawList<const Node*> drawEdge;
+				drawEdge.setSeed(seed+1);
+
+				//if (curNode->getNumNeighbors() < 6) {break;}
+
+				// evaluate each neighbor
+				for (const Node& neighbor : grid.neighbors(*curNode)) {
+					const double prob = getProb(*startNode, *curNode, neighbor);
+					drawEdge.add(&neighbor, prob);
+				}
+
+				// pick a neighbor
+				const Node* nextNode = drawEdge.get();
+
+				dist -= nextNode->getDistanceInMeter(*curNode);
+
+				curNode = nextNode;
+
+				path.nodes.push_back(curNode);
+
+			}
+
+			const double prob = getProb(path);
+			drawPath.add(path, prob);
+
+		}
+
+		const Path& bestPath = drawPath.get();
+
+		WalkState nextState = _startState;
+		nextState.startPos = *(bestPath.getDst());
+		return nextState;
+
+
+	}
+
+private:
+
+	inline double getProb(const Node& n0, const Node& n1, const Node& n2) {
+		static float angle = 0; angle += 0.05;
+		angle = std::fmod(angle, 6);
+		if (&n1 == &n2) {return 1;}
+		const Heading head1(angle);
+		const Heading head2(n1.x_cm, n1.y_cm, n2.x_cm, n2.y_cm);
+		const float diff = head1.getDiffHalfRAD(head2);
+		return rand() % 32;
+	}
+
+	inline double getProb(const Path& path) {
+		const Point3 p1 = path.getSrc()->inMeter();
+		const Point3 p2 = path.getDst()->inMeter();
+		const float dist = p1.getDistance(p2);
+		return 1/dist;
+	}
+
+	/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
+	inline void updateBefore(WalkState& state) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateBefore(state);
+		}
+
+	}
+
+	/** update the WalkState after the random walk (if needed) */
+	inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateAfter(state, startNode, endNode);
+		}
+
+	}
+
+	/** one step taken. inform modules */
+	inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->step(state, curNode, nextNode);
+		}
+
+	}
+
+	/** get the probability for the given random walk (one edge) */
+	inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {
+
+		double prob = 1.0;
+
+		for (const WalkModule<Node, WalkState>* mdl : modules) {
+			prob *= mdl->getProbability(state, start, cur, next);
+		}
+
+		return prob;
+
+	}
+
+};
+
+#endif // GRIDWALKERMULTI_H

grid/walk/v2/modules/WalkModule.h

@@ -0,0 +1,39 @@
+#ifndef WALKMODULE_H
+#define WALKMODULE_H
+
+#include "../../../Grid.h"
+
+/** base-class for all WalkStates */
+struct WalkState {
+
+	/** position where the walk starts */
+	GridPoint startPos;
+
+	/** ctor */
+	WalkState(const GridPoint& startPos) : startPos(startPos) {;}
+
+};
+
+
+/**
+ * base-class for all walk-modules that influence p(e)
+ */
+template <typename Node, typename WalkState> class WalkModule {
+
+public:
+
+	/** update the given WalkState before starting the walk. e.g. based on sensor readings */
+	virtual void updateBefore(WalkState& state) = 0;
+
+	/** get the probability p(e) from curNode to potentialNode */
+	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;
+
+	/** one step (edge) is taken */
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) = 0;
+
+	/** update the walk state based on the given transition (if any update is necssary) */
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) = 0;
+
+};
+
+#endif // WALKMODULE_H

grid/walk/v2/modules/WalkModuleFollowDestination.h

@@ -0,0 +1,68 @@
+#ifndef WALKMODULEFOLLOWDESTINATION_H
+#define WALKMODULEFOLLOWDESTINATION_H
+
+#include "WalkModule.h"
+#include "../../../../nav/dijkstra/Dijkstra.h"
+
+/**
+ * favour edges p(e) that approach the destination
+ */
+template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
+
+private:
+
+	Dijkstra<Node> dijkstra;
+
+	struct DijkstraMapper {
+		const Grid<Node>& grid;
+		DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
+		int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
+		const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
+		float getWeightBetween(const Node& n1, const Node& n2) const {
+			return n1.getDistanceInCM(n2) * n2.navImportance;
+		}
+	};
+
+public:
+
+	/** ctor */
+	WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
+
+		// shortest path calculation
+		dijkstra.build(&destination, DijkstraMapper(grid));
+
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+
+		const float kappa = 0.8;
+		const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
+		const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
+
+		// probability
+		return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+};
+
+#endif // WALKMODULEFOLLOWDESTINATION_H

grid/walk/v2/modules/WalkModuleHeading.h

@@ -0,0 +1,76 @@
+#ifndef WALKMODULEHEADING_H
+#define WALKMODULEHEADING_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
+
+private:
+
+	/** van-Mises distribution */
+	Distribution::LUT<double> dist;
+
+	/** van-Mises draw list */
+	DrawList<double> draw;
+
+public:
+
+	/** ctor */
+	WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+
+		(void) state;
+		state.startHeading += draw.get();
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+
+//		if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) {
+//			Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm);
+//			state.startHeading = head;
+//		}
+
+	}
+
+	/** one step (edge) is taken */
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+		// TODO
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the heading between curNode and potentialNode
+		const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
+
+		// compare the heading against the state's heading
+		const Heading stateHead = state.startHeading;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();
+
+		// determine probability
+		return dist.getProbability(angularDiff);
+
+	}
+
+
+};
+
+#endif // WALKMODULEHEADING_H

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -0,0 +1,78 @@
+#ifndef WALKMODULEHEADINGCONTROL_H
+#define WALKMODULEHEADINGCONTROL_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	/** van-Mises distribution */
+	Distribution::LUT<double> dist;
+
+	/** van-Mises draw list */
+	DrawList<double> draw;
+
+	Control* ctrl;
+
+	//std::unordered_map<WalkState*, float> errorTracker;
+
+public:
+
+	/** ctor 3.0 should be OK! */
+	WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 1.5).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+
+		const float var = draw.get() * 0.15;//0.05;
+		//const float var = 0;
+		state.startHeading += ctrl->turnAngle + var;
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the heading between curNode and potentialNode
+		const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
+
+		// compare the heading against the state's heading - the last error
+		const Heading stateHead = state.startHeading;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+		// determine probability
+		const float prob = dist.getProbability(angularDiff);
+		return prob;
+
+	}
+
+
+
+};
+
+#endif // WALKMODULEHEADINGCONTROL_H

grid/walk/v2/modules/WalkModuleNodeImportance.h

@@ -0,0 +1,55 @@
+#ifndef WALKMODULENODEIMPORTANCE_H
+#define WALKMODULENODEIMPORTANCE_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+
+/**
+ * favor edges based on the importance-factor of the next node.
+ * @see struct GridNodeImportance
+ */
+template <typename Node, typename WalkState> class WalkModuleNodeImportance : public WalkModule<Node, WalkState> {
+
+private:
+
+public:
+
+	/** ctor */
+	WalkModuleNodeImportance() {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+		(void) curNode;
+
+		const double prob = potentialNode.getNavImportance();
+		return std::pow(prob, 10);
+		//return prob;
+
+	}
+
+
+};
+
+#endif // WALKMODULENODEIMPORTANCE_H

grid/walk/v2/modules/WalkModuleRelativePressureControl.h

@@ -0,0 +1,162 @@
+#ifndef WALKMODULERELATIVEPRESSURE_H
+#define WALKMODULERELATIVEPRESSURE_H
+
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+#include "../../../../Assertions.h"
+
+
+/**
+ * a walk-state the contains the pressure relative to time t0
+ */
+struct WalkStateRelativePressure {
+
+	/** the pressure level [relative to time t0] one SHOULD messure at this state */
+	float pressureRelToT0;
+
+	int dirLock = 0;
+
+	/** ctor */
+	WalkStateRelativePressure(const float pressureRelToT0) : pressureRelToT0(pressureRelToT0) {;}
+
+};
+
+
+/**
+ * uses the pressure relative to t=0 within the control-data
+ * to estimate the likelyhood for any z-changes during the transition
+ */
+template <typename Node, typename WalkState, typename Control> class WalkModuleRelativePressureControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	Control* ctrl;
+
+	/** pressure-change (hPa) per meter */
+	const float hPaPerMeter = 0.126f;		// given an average hPa of 938
+
+public:
+
+	/** ctor */
+	WalkModuleRelativePressureControl(Control* ctrl, const float hPaPerMeter) : ctrl(ctrl), hPaPerMeter(hPaPerMeter) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+
+//		// e.g. walking down from the 3rd to the second floor
+//		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
+//		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+//		const int deltaZ_cm = startNode.z_cm - endNode.z_cm;
+//		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+
+//		// update the states pressure
+//		state.pressureRelToT0 += deltaPressure;
+
+//		// sanity checks
+//		Assert::isNotNaN(deltaPressure, "detected NaN!");
+//		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
+
+//		static int xx = 0;
+//		if (++xx % 1024 == 0) {
+//			++xx;
+//		}
+
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+		// e.g. walking down from the 3rd to the second floor
+		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
+		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+		const int deltaZ_cm = curNode.z_cm - nextNode.z_cm;
+		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+		const float expectedPressure = state.pressureRelToT0 + deltaPressure;
+
+		// update the states pressure
+		state.pressureRelToT0 = expectedPressure;
+
+		// sanity checks
+		Assert::isNotNaN(deltaPressure, "detected NaN!");
+		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
+
+		if(std::abs(state.dirLock) > 0) {
+			if (state.dirLock > 0) {--state.dirLock;}
+			if (state.dirLock < 0) {++state.dirLock;}
+		} else {
+			if (deltaZ_cm > 0) {state.dirLock = +25;}
+			if (deltaZ_cm < 0) {state.dirLock = -25;}
+		}
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the values from control-data
+		const float curRelPressure = ctrl->barometer.hPaRelativeToT0;
+		const float pressureSigma = ctrl->barometer.estimatedSigma * 1.50f;
+
+		// not yet available/calibrated/possible -> skip evaluation!
+		if (pressureSigma == 0) {return 1;}
+
+		// e.g. walking down from the 3rd to the second floor
+		// startZ = 10 meter, endZ = 7 meter -> deltaZ = +3 meter;
+		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+		const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
+		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+		const float expectedPressure = state.pressureRelToT0 + deltaPressure;
+
+		const float oldErr = std::abs(state.pressureRelToT0 - curRelPressure);
+		const float newErr = std::abs(expectedPressure - curRelPressure);
+
+		if (std::abs(state.dirLock) > 0) {
+			if (state.dirLock > 0) {
+				if (deltaZ_cm < 0) {return 0;}
+				if (deltaZ_cm == 0) {return 0.1;}
+				return 0.9;
+			} else {
+				if (deltaZ_cm > 0) {return 0;}
+				if (deltaZ_cm == 0) {return 0.1;}
+				return 0.9;
+			}
+		}
+
+
+		if (oldErr > pressureSigma) {
+			if (deltaZ_cm == 0) {return 0.001;}
+		}
+
+		if (newErr > oldErr) {return 0.002;}
+		if (newErr == oldErr) {return 0.005;}
+		if (newErr < oldErr) {return 0.99;}
+		throw 1;
+
+
+
+//		// compare control-data with potential transition
+//		double prob = Distribution::Normal<double>::getProbability(curRelPressure, pressureSigma, expectedPressure);
+////		prob = std::pow(prob, 20);
+
+//		// sanity checks
+//		Assert::isNotNaN(prob, "detected NaN!");
+//		Assert::isNotNaN(deltaPressure, "detected NaN!");
+//		Assert::isNotNaN(expectedPressure, "detected NaN!");
+
+//		return prob;
+
+	}
+
+
+};
+
+#endif // WALKMODULERELATIVEPRESSURE_H

grid/walk/v2/modules/WalkModuleSpread.h

@@ -0,0 +1,62 @@
+#ifndef WALKMODULESPREAD_H
+#define WALKMODULESPREAD_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+
+/**
+ * simply try to move away from the starting node as much as possible
+ */
+template <typename Node, typename WalkState> class WalkModuleSpread : public WalkModule<Node, WalkState> {
+
+private:
+
+	Point3 avg;
+
+public:
+
+	/** ctor */
+	WalkModuleSpread() {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+		avg = avg * 0.999 + state.startPos.inMeter() * 0.001;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+		(void) curNode;
+
+		const float dOld = avg.getDistance(curNode.inMeter());
+		const float dNew = avg.getDistance(potentialNode.inMeter());
+
+		if (dNew > dOld) {return 0.8;}
+		if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
+		if (dNew == dOld) {return 0.2;}
+		return 0;
+
+	}
+
+
+};
+
+
+#endif // WALKMODULESPREAD_H

grid/walk/v2/modules/WalkStateHeading.h

@@ -0,0 +1,16 @@
+#ifndef WALKSTATEHEADING_H
+#define WALKSTATEHEADING_H
+
+#include "../../../../geo/Heading.h"
+
+struct WalkStateHeading {
+
+	Heading startHeading;
+
+	/** ctor */
+	WalkStateHeading(const Heading& curHeading) : startHeading(curHeading) {;}
+
+};
+
+
+#endif // WALKSTATEHEADING_H