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/GridWalkPushForward.h

@@ -0,0 +1,120 @@
+#ifndef GRIDWALKPUSHFORWARD_H
+#define GRIDWALKPUSHFORWARD_H
+
+/**
+ * todo
+ */
+#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"
+
+/**
+ * keeps something like an "average position within the last X steps"
+ * and tries to move away from this point as fast as possible
+ *
+ */
+template <typename T> class GridWalkPushForward {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+
+	static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);
+
+	/** 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 */
+	GridWalkPushForward() {
+		;
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
+
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
+
+	}
+
+private:
+
+	// 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();
+
+		// 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, HEADING_ALLOWED_SIGMA, diffRad);
+
+
+			// distance from average? and previous distance from average
+			const float distToAvg = Point3(neighbor.x_cm, neighbor.y_cm, neighbor.z_cm).getDistance(cur.avg);
+			const float prevDistToAvg = Point3(cur.node->x_cm, cur.node->y_cm, cur.node->z_cm).getDistance(cur.avg);
+			const float increase = distToAvg - prevDistToAvg;
+
+			// the distance from the average MUST increase
+			const float prob2 = (increase > 0) ? (1) : (0.1);
+
+			// add floorplan importance information
+			const float prob3 = std::pow(neighbor.imp, 1);
+
+			const float prob = prob1*prob2*prob3;
+
+			// add for drawing
+			drawer.add(&neighbor, prob);
+
+		}
+
+
+		// all neighbors are unlikely? -> start over
+		if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
+
+		GridWalkState<T> next;
+
+		// pick the neighbor best matching this new heading
+		next.node = drawer.get();
+		next.heading = GridWalkHelper::getHeading(*cur.node, *next.node) + headingChangeDist(gen);
+
+		// weighted average.. moves over time
+		next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1);
+
+		// 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 // GRIDWALKPUSHFORWARD_H