new walker (control+path)

added new sanity checks
fixed minor errors
added corresponding test-cases
added moving-median

grid/walk/GridWalkPathControl.h

@@ -0,0 +1,161 @@
+#ifndef GRIDWALKPATHCONTROL_H
+#define GRIDWALKPATHCONTROL_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/Distributions.h"
+#include "../../math/DrawList.h"
+
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+#include "GridWalk.h"
+
+template <typename T> class GridWalkPathControl : public GridWalk<T> {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
+
+	/** 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);
+
+	Dijkstra<T> dijkstra;
+
+	DrawList<T&> drawer;
+
+public:
+
+
+	/** ctor with the target you want to reach */
+	template <typename Access> GridWalkPathControl(Grid<T>& grid, const Access& acc, const T& target) {
+
+		gen.seed(1234);
+
+		// build all shortest path to reach th target
+		dijkstra.build(target, target, acc);
+
+		// attach a corresponding weight-information to each user-grid-node
+		for (T& node : grid) {
+
+			const DijkstraNode<T>* dn = dijkstra.getNode(node);
+
+			// should never be null as all nodes were evaluated
+			if (dn != nullptr) {
+				node.distToTarget = dn->cumWeight/2000;
+			}
+
+		}
+
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
+
+		// proportional change of the heading
+		static 	Distribution::Normal<float> dHead(1, 0.01);
+
+		// proportional change of the to-be-walked distance
+		static 	Distribution::Normal<float> dWalk(1, 0.10);
+
+		distance_m = distance_m*dWalk.draw()*2;		// TODO: why *2?
+		headChange_rad = headChange_rad*dHead.draw();
+
+
+		return walk(grid, start, distance_m, headChange_rad);
+
+	}
+
+private:
+
+	double getProbability(const T& start, const T& possible, const Heading head) const {
+
+		// TODO: WHY?! not only when going back to the start?
+		if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
+			if (start.z_cm == possible.z_cm) {return 0;} // back to the start
+			//throw 1;
+			return 0.5;// stair start/end TODO: fix
+		}
+
+		// get the angle between START and the possible next node
+		const Heading possibleHead = GridWalkHelper::getHeading(start, possible);
+
+		// calculate the difference
+		const float diff = possibleHead.getDiffHalfRAD(head);
+
+//		// compare this heading with the requested one
+		const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(15), diff);
+//		const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1;		// favor best 3 angles equally
+
+		// nodes own importance
+		const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.0;
+
+		// bring it together
+		return angleProb * nodeProb;
+
+	}
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) {
+
+		// try-again distribution
+		//static 	Distribution::Normal<float> dHead(0, Angle::degToRad(10));
+		//static 	Distribution::Normal<float> dUpdate(0, Angle::degToRad(3));
+			static	Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));
+
+		int retries = 5;
+		float walked_m = 0;
+		GridWalkState<T> cur = start;
+
+		// the desired heading
+		Heading reqHeading = start.heading + (headChange_rad);
+
+		// walk until done
+		while(walked_m < distance_m) {
+
+			// evaluate all neighbors
+			drawer.reset();
+			for (T& neighbor : grid.neighbors(*cur.node)) {
+
+				const double prob = getProbability(*start.node, neighbor, reqHeading);
+				drawer.add(neighbor, prob);
+
+			}
+
+			// too bad? start over!
+			if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) {
+				walked_m = 0;
+				cur = start;
+				//WHAT THE HELL
+				if (retries == 0) { reqHeading = dChange.draw(); }
+				continue;
+			}
+
+			// get the neighbor
+			const T& neighbor = drawer.get();
+
+			// update
+			walked_m += neighbor.getDistanceInMeter(*cur.node);
+			cur.node = &neighbor;
+
+		}
+
+		cur.distanceWalked_m = NAN;
+		cur.headingChange_rad = NAN;
+		cur.heading = reqHeading;
+
+		return cur;
+
+	}
+
+};
+
+
+
+#endif // GRIDWALKPATHCONTROL_H