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/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