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
2016-01-26 18:13:30 +01:00
parent b503fb9bdc
commit e6329e1db4
26 changed files with 824 additions and 179 deletions
--- a/grid/walk/GridWalkRandomHeadingUpdate.h
+++ b/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