dijkstra is now bleching fast

deleting from the grid is now bleaching fast
added new helper methods
many new test-cases
many new methods for geo classes and others
added a bunch of new grid-walkers

grid/walk/GridWalkRandomHeadingUpdateAdv.h

@@ -0,0 +1,126 @@
+#ifndef GRIDWALKRANDOMHEADINGUPDATEADV_H
+#define GRIDWALKRANDOMHEADINGUPDATEADV_H
+
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/distribution/Exponential.h>
+
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+
+/**
+ * for every walked edge: slightly update (scatter) the current heading
+ * pick the edge (neighbor) best matching the current heading
+ * if this neighbor's heading highly differs from the requested heading: start over
+ *
+ * PROs
+ *  - simple
+ *  - fixes the issues of GridWalkRandomHeadingUpdate by incorporating floor information
+ *  - adds additional randomness which should be more stable
+ *
+ */
+template <typename T> class GridWalkRandomHeadingUpdateAdv {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
+
+	/** 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:
+
+	/** ctor */
+	GridWalkRandomHeadingUpdateAdv() {
+		;
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
+
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
+
+	}
+
+private:
+
+	// https://de.wikipedia.org/wiki/Logistische_Verteilung
+	/** alpha = move the center, beta = slope */
+	const float logisticDist(const float x, const float alpha, const float beta) {
+		return 1 / (1 + std::exp( -((x-alpha)/beta) ) );
+	}
+
+
+	// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
+	DrawList<T*> drawer;
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
+
+		drawer.reset();
+		GridWalkState<T> next;
+
+		// get a new random heading
+		next.heading = cur.heading + headingChangeDist(gen);
+
+		// weight all neighbors based on this heading
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// get the heading between the current node and its neighbor
+			const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
+
+			// calculate the difference from the requested heading
+			const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
+
+			// weight this change
+			const float prob1 = K::NormalDistribution::getProbability(0, Angle::degToRad(40), diffRad);
+
+			// add the node's importance factor into the calculation
+			const float prob2 = logisticDist(neighbor.imp, 1.0, 0.05);
+			//const float prob2 = std::pow(neighbor.imp, 10);
+
+			// final importance
+			const float prob = prob1 * prob2;
+
+			// add for drawing
+			drawer.add(&neighbor, prob);
+
+		}
+
+		// all neighbors are unlikely? -> start over
+		if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
+
+		// pick the neighbor best matching this new heading
+		next.node = drawer.get();
+
+//		// if the best matching neighbor is far of this requested heading
+//		// (e.g. no good neighbor due to walls) cancel the walk. to force a retry
+//		const float diff = GridWalkHelper::getHeading(*cur.node, *next.node).getDiffHalfRAD(next.heading);
+//		if (diff > Angle::degToRad(45)) {
+//			return GridWalkState<T>();
+//		}
+
+		// 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 // GRIDWALKRANDOMHEADINGUPDATEADV_H