added several grid-walks

added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...

grid/walk/GridWalkWeighted.h

@@ -0,0 +1,141 @@
+#ifndef GRIDWALKWEIGHTED_H
+#define GRIDWALKWEIGHTED_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+
+/**
+ * perform walks on the grid based on some sort of weighting
+ * and drawing from the weighted elements
+ */
+template <typename T> class GridWalkWeighted {
+
+public:
+
+	struct State {
+		const T* node;
+		Heading heading;
+
+		State() : node(nullptr), heading(0) {;}
+		State(const T* node, Heading heading) : node(node), heading(heading) {;}
+
+	};
+
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+
+	/** per-edge: allowed heading difference */
+	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
+
+
+	/** allows drawing elements according to their probability */
+	DrawList<T&> drawer;
+
+	/** fast random-number-generator */
+	std::minstd_rand gen;
+
+	/** 0-mean normal distribution */
+	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
+
+public:
+
+	template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
+
+		int retries = 2;
+		State res;
+
+		// try to walk the given distance from the start
+		// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
+		do {
+			res = walk(grid, start, distance_m);
+		} while (res.node == nullptr && --retries);
+
+		// still reaching a dead end?
+		// -> try a walk in the opposite direction instead
+		if (res.node == nullptr) {
+			res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> keep the start as-is
+		return (res.node == nullptr) ? (start) : (res);
+
+	}
+
+private:
+
+	static Heading getHeading(const T& from, const T& to) {
+		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) {
+
+		drawer.reset();;
+
+		// calculate the weight for all possible destinations from "cur"
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// heading when walking from cur to neighbor
+			const Heading potentialHeading = getHeading(*cur.node, neighbor);
+
+			// angular difference
+			const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
+
+			// probability for this direction change?
+			double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
+
+			// perfer locations reaching the target
+			const double shortening = cur.node->distToTarget - neighbor.distToTarget;
+			if (shortening > 0) {prob *= 5;}
+
+			drawer.add(neighbor, prob);
+
+		}
+
+		State next(nullptr, cur.heading);
+
+		// pick a random destination
+		T& nDir = drawer.get();
+		const Heading hDir = getHeading(*cur.node, nDir);
+		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
+		next.heading = hDir;
+		next.heading += headingChangeDist(gen);
+
+		// compare two neighbors according to their implied heading change
+		auto compp = [&] (const T& n1, const T& n2) {
+			Heading h1 = getHeading(*cur.node, n1);
+			Heading h2 = getHeading(*cur.node, n2);
+			const float d1 = next.heading.getDiffHalfRAD(h1);
+			const float d2 = next.heading.getDiffHalfRAD(h2);
+			return d1 < d2;
+		};
+
+		// pick the neighbor best matching the new heading
+		auto it = grid.neighbors(*cur.node);
+		T& nn = *std::min_element(it.begin(), it.end(), compp);
+		next.node = &nn;
+
+//		// pervent dramatic heading changes. instead: try again
+//		if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
+//			return State(nullptr, 0);
+//		}
+
+		// get the distance up to this neighbor
+		distRest_m -= next.node->getDistanceInMeter(*cur.node);
+
+		// done?
+		if (distRest_m <= 0) {return next;}
+
+		// another round..
+		return walk(grid, next, distRest_m);
+
+	}
+
+};
+
+#endif // GRIDWALKWEIGHTED_H