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

@@ -12,6 +12,67 @@ public:
 		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
 	}
 
+	/** get the neighbor of "from" best matching the given heading "h" */
+	template <typename T> static T& getBestNeighbor(Grid<T>& grid, const T& from, const Heading h) {
+
+		auto comp = [&] (const T& n1, const T& n2) {
+			const Heading h1 = getHeading(from, n1);
+			const Heading h2 = getHeading(from, n2);
+			const float d1 = h.getDiffHalfRAD(h1);
+			const float d2 = h.getDiffHalfRAD(h2);
+			//return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2));
+			return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);		// same heading? > random decision
+		};
+
+		auto neighbors = grid.neighbors(from);
+		return *std::min_element(neighbors.begin(), neighbors.end(), comp);
+
+	}
+
+	/**
+	 * try to walk the given distance from the provided node.
+	 * if this fails (algorithm cancel walk e.g. due to detected wall collissions)
+	 *  - try again from the start
+	 * if this also fails several times
+	 *  - try to walk in the opposite direction instead (bounce-back)
+	 * if this also fails
+	 *  - add some randomness and try again
+	 */
+	template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m) {
+
+		_assertTrue(distance_m >= 0, "distance must not be negative!");
+
+		GridWalkState<T> res;
+
+		//again:
+
+		int retries = numRetries;
+
+		// 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 = w.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 = w.walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> modify and try again
+		if (res.node == nullptr) {
+//			start.node = &(*grid.neighbors(*start.node).begin());
+//			start.heading += 0.25;
+//			goto again;
+			res.node = &(*grid.neighbors(*start.node).begin());
+			res.heading = start.heading;
+		}
+
+		return res;
+
+	}
+
 };
 
 #endif // GRIDWALKHELPER_H

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -18,10 +18,12 @@
  */
 template <typename T> class GridWalkLightAtTheEndOfTheTunnel {
 
+	friend class GridWalkHelper;
+
 private:
 
 	/** per-edge: change heading with this sigma */
-	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
 
 	/** per-edge: allowed heading difference */
 	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
@@ -62,23 +64,7 @@ public:
 
 	GridWalkState<T> getDestination(Grid<T>& grid, GridWalkState<T> start, float distance_m) {
 
-		int retries = 2;
-		GridWalkState<T> 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, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
-		}
-
-		// still nothing found? -> keep the start as-is
-		return (res.node == nullptr) ? (start) : (res);
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
 
 	}
 
@@ -102,7 +88,17 @@ private:
 
 			// perfer locations reaching the target
 			const double shortening = cur.node->distToTarget - neighbor.distToTarget;
-			if (shortening > 0) {prob *= 30;} // << importance factor!!
+			if (shortening >= 0) {prob *= 5;} // << importance factor!!
+
+//			prob = 0.1;
+//			if		(diff < Angle::degToRad(40)) {prob += 0.2;}
+//			else if	(diff < Angle::degToRad(20)) {prob += 0.5;}
+
+//			if (shortening >= 0) {prob += 0.5;}
+
+			//prob *= std::pow(neighbor.imp, 5);
+
+			//prob = (shortening >= 0) ? (2) : (0.75);
 
 			drawer.add(neighbor, prob);
 
@@ -114,34 +110,38 @@ private:
 		T& nDir = drawer.get();
 		const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
 		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
+		//next.heading = Heading( cur.heading.getRAD() * 0.2 + hDir.getRAD() * 0.8 );
 		next.heading = hDir;
 		next.heading += headingChangeDist(gen);
+		next.node = &nDir;
 
-		// compare two neighbors according to their implied heading change
-		auto compp = [&] (const T& n1, const T& n2) {
-			Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
-			Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
-			const float d1 = next.heading.getDiffHalfRAD(h1);
-			const float d2 = next.heading.getDiffHalfRAD(h2);
-			// same heading -> prefer nodes nearer to the target. needed for stairs!!!
-			// BAD: leads to straight lines in some palces. see solution B (below)
-			//return (d1 < d2) && (n1.distToTarget < n2.distToTarget);
+////		// compare two neighbors according to their implied heading change
+////		auto compp = [&] (const T& n1, const T& n2) {
+////			Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
+////			Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
+////			const float d1 = next.heading.getDiffHalfRAD(h1);
+////			const float d2 = next.heading.getDiffHalfRAD(h2);
+////			// same heading -> prefer nodes nearer to the target. needed for stairs!!!
+////			// BAD: leads to straight lines in some palces. see solution B (below)
+////			//return (d1 < d2) && (n1.distToTarget < n2.distToTarget);
 
-			// VERY IMPORTANT!
-			// pick the node with the smallest heading change.
-			// if the heading change is the same for two nodes, pick a random one!
-			return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);
-		};
+////			// VERY IMPORTANT!
+////			// pick the node with the smallest heading change.
+////			// if the heading change is the same for two nodes, pick a random one!
+////			return (d1 == d2) ? (rand() < RAND_MAX/2) : (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;
+////		// 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);
-//		}
+//		next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
+
+////		// pervent dramatic heading changes. instead: try again
+////		if (cur.heading.getDiffHalfRAD(GridWalkHelper::getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
+////			return GridWalkState<T>(nullptr, 0);
+////		}
 
 		// get the distance up to this neighbor
 		distRest_m -= next.node->getDistanceInMeter(*cur.node);

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

grid/walk/GridWalkRandomHeadingUpdate.h

@@ -0,0 +1,87 @@
+#ifndef GRIDWALKRANDOMHEADINGUPDATE_H
+#define GRIDWALKRANDOMHEADINGUPDATE_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include <KLib/math/distribution/Normal.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
+ *	- very simple
+ *
+ * CONs
+ *	- particles are bad at walking out of rooms for small grid sizes as there are too many options
+ *		to stay within the room..
+ *
+ */
+template <typename T> class GridWalkRandomHeadingUpdate {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(4);
+
+	/** 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 */
+	GridWalkRandomHeadingUpdate() {
+		;
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
+
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
+
+	}
+
+private:
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
+
+		GridWalkState<T> next;
+
+		// get a new random heading
+		next.heading = cur.heading + headingChangeDist(gen);
+
+		// pick the neighbor best matching this new heading
+		next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
+
+		// 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 // GRIDWALKRANDOMHEADINGUPDATE_H

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

grid/walk/GridWalkState.h

@@ -2,6 +2,7 @@
 #define GRIDWALKSTATE_H
 
 #include "../../geo/Heading.h"
+#include "../../geo/Point3.h"
 
 template <typename T> struct GridWalkState {
 
@@ -14,6 +15,8 @@ template <typename T> struct GridWalkState {
 	/** empty ctor */
 	GridWalkState() : node(nullptr), heading(0) {;}
 
+	Point3 avg = Point3(0,0,0);
+
 	/** ctor with user-node and heading */
 	GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;}