added new math-stats

started working on a new grid walker

grid/walk/v3/GridWalker3.h

@@ -0,0 +1,122 @@
+#ifndef GRIDWALKER3_H
+#define GRIDWALKER3_H
+
+#include "../../../data/Timestamp.h"
+#include "../../Grid.h"
+#include "../../../math/DrawList.h"
+#include "../../../math/Distributions.h"
+#include "../../../math/Stats.h"
+#include "../../../geo/Heading.h"
+#include "../../../math/stats/Variance.h"
+
+/**
+ * modular grid-walker that takes various sub-components to determine
+ * p(e) and thus randomly pick edges
+ */
+template <typename Node, typename WalkState, typename Control> class GridWalker3 {
+
+private:
+
+	/** all modules to evaluate */
+//	std::vector<WalkModule<Node, WalkState>*> modules;
+
+	RandomGenerator rnd;
+
+public:
+
+
+	/** one walk along several nodes */
+	struct Walk : public std::vector<const Node*> {
+
+	};
+
+//	struct Walks : std::vector<Walk> {
+
+//	};
+
+
+	/** perform the walk based on the configured setup */
+	const Node* getDestination(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
+
+		const Node* startNode = grid.getNodePtrFor(start);
+
+		Heading desiredHeading = ctrl.heading;
+
+		DrawList<Walk> walks;
+
+		for (int i = 0; i < 100; ++i) {
+			const Walk walk = getRandomWalk(grid, startNode, dist_m);
+			const double prob = eval(walk, desiredHeading);
+			walks.add(walk, prob);
+		}
+
+		Walk res = walks.get();
+		const Node* dst = res.back();
+		return dst;
+
+	}
+
+	double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
+		const Node* nStart = w.front();
+		const Node* nEnd = w.back();
+		if (nStart == nEnd) {
+			std::cout << "warn! start == end" << std::endl;
+			return 0;
+		}
+		Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
+		const float diff = head.getDiffHalfRAD(desiredHeading);
+		return Distribution::Normal<double>::getProbability(0, 0.3, diff);
+	}
+
+	double evalHeadingChanges(const Walk& w) const {
+		Stats::Variance<float> var;
+		for (int i = 0; i < w.size()-2; ++i) {
+			const Node* n0 = w[i+0];
+			const Node* n1 = w[i+1];
+			const Node* n2 = w[i+2];
+			Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
+			Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
+			const float diff = h1.getDiffHalfRAD(h2);
+			var.add(diff);
+		}
+		const float totalVar = var.get();
+		return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
+	}
+
+	double eval(const Walk& w, const Heading desiredHeading) const {
+
+		return 1.0
+				* evalHeadingStartEnd(w, desiredHeading)
+				// * evalHeadingChanges(w);
+		;
+
+	}
+
+	Walk getRandomWalk(Grid<Node>& grid, const Node* start, const float dist_m) const {
+
+		Walk walk;
+
+		float dist = 0;
+
+		const Node* cur = start;
+		while(true) {
+
+			walk.push_back(cur);
+			if (dist > dist_m) {break;}
+
+			const int numNeighbors = cur->getNumNeighbors();
+			//std::cout << "neighbors: " << numNeighbors << std::endl;
+			int idx = rand() % numNeighbors;
+			const Node* next = &grid.getNeighbor(*cur, idx);
+			dist += next->inMeter().getDistance(cur->inMeter());
+			cur = next;
+
+		}
+
+		return walk;
+
+	}
+
+};
+
+#endif // GRIDWALKER3_H