new walker (control+path)

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

grid/Grid.h

@@ -78,10 +78,11 @@ public:
 
 	/** add the given (not necessarly aligned) element to the grid */
 	int addUnaligned(const T& elem) {
-		const int idx = nodes.size();					// next free index
 		const UID uid = getUID(elem);														// get the UID for this new element
+		Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!");	// avoid potential errors
+		const int idx = nodes.size();														// next free index
 		nodes.push_back(elem);																// add it to the grid
-		nodes.back()._idx = idx;
+		nodes.back()._idx = idx;															// let the node know his own index
 		hashes[uid] = idx;																	// add an UID->index lookup
 		return idx;																			// done
 	}

grid/factory/GridFactory.h

@@ -199,7 +199,7 @@ public:
 	void buildStairLine(T& _n1, T& _n2) {
 
 		// half the grid size = small steps
-		const int gridSize_cm = grid.getGridSize_cm() / 2;
+		const int gridSize_cm = grid.getGridSize_cm();// / std::sqrt(2);
 
 		// local copies, needed for std::swap to work
 		T n1 = _n1; T n2 = _n2;
@@ -216,7 +216,7 @@ public:
 		const int idx3 = n2.getIdx();		// final node
 
 		// move upards in gridSize steps
-		for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) {
+		for (int _z = gridSize_cm; _z < zDiff - gridSize_cm; _z+= gridSize_cm) {
 
 			// calculate the percentage of reached upwards-distance
 			const float percent = _z/zDiff;
@@ -227,8 +227,8 @@ public:
 			int z = n1.z_cm + _z;
 
 			// snap (x,y) to the grid???
-			x = std::round(x / gridSize_cm) * gridSize_cm;
-			y = std::round(y / gridSize_cm) * gridSize_cm;
+			//x = std::round(x / gridSize_cm) * gridSize_cm;
+			//y = std::round(y / gridSize_cm) * gridSize_cm;
 
 			// create a new node add it to the grid, and connect it with the previous one
 			idx2 = grid.addUnaligned(T(x,y,z));

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -46,6 +46,8 @@ public:
 	/** ctor with the target you want to reach */
 	template <typename Access> GridWalkLightAtTheEndOfTheTunnel(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);
 

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

grid/walk/GridWalkSimpleControl.h

@@ -44,12 +44,12 @@ public:
 	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.10);
+		static 	Distribution::Normal<float> dHead(1, 0.01);
 
 		// proportional change of the to-be-walked distance
-		static 	Distribution::Normal<float> dWalk(1, 0.10);
+		static 	Distribution::Normal<float> dWalk(1, 0.50);
 
-		distance_m = distance_m*dWalk.draw();		// TODO: why *2?
+		distance_m = distance_m*dWalk.draw()*2;		// TODO: why *2?
 		headChange_rad = headChange_rad*dHead.draw();
 
 
@@ -62,7 +62,11 @@ 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) {return 0.1;}
+		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);
@@ -71,7 +75,7 @@ private:
 		const float diff = possibleHead.getDiffHalfRAD(head);
 
 //		// compare this heading with the requested one
-		const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(30), diff);
+		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

math/MovingMedian.h

@@ -0,0 +1,65 @@
+#ifndef MOVINGMEDIAN_H
+#define MOVINGMEDIAN_H
+
+#include <vector>
+#include <algorithm>
+
+template <typename T> class MovingMedian {
+
+private:
+
+	/** up to "size" elements */
+	std::vector<T> values;
+
+	/** the number of elements to find the median within */
+	int size;
+
+public:
+
+	/** ctor */
+	MovingMedian(const int size) : size(size) {;}
+
+	/** add a new value */
+	void add(const T val) {
+
+		// add new value
+		values.push_back(val);
+
+		// too many values?
+		if ((int) values.size() > size) {
+			values.erase(values.begin());
+		}
+
+	}
+
+	/** get the current median */
+	T get() const {
+
+		// create a sorted copy (slow, but works)
+		std::vector<T> copy = values;
+		std::sort(copy.begin(), copy.end());
+
+		// get the median
+		if (values.size() % 2 != 0) {
+			return values[(values.size() + 0) / 2];
+		} else {
+			return (values[values.size() / 2 - 1] + values[values.size() / 2 + 0]) / 2;
+		}
+
+	}
+
+
+	/** get the number of used entries */
+	int getNumUsed() const {
+		return (int) values.size();
+	}
+
+	/** get number of entries to get the median for */
+	int getSize() const {
+		return size;
+	}
+
+
+};
+
+#endif // MOVINGMEDIAN_H

math/TestMovingMedian.h

@@ -0,0 +1,6 @@
+#ifndef TESTMOVINGMEDIAN_H
+#define TESTMOVINGMEDIAN_H
+
+
+
+#endif // TESTMOVINGMEDIAN_H

tests/grid/Plot.h

@@ -17,6 +17,7 @@ public:
 	float imp = 1.0f;
 	float impPath = 1.0f;
 	float distToTarget = 1.0;
+	int cnt = 0;
 public:
 	GP() : GridNode(), GridPoint() {;}
 	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}

tests/grid/TestGrid.cpp

@@ -6,23 +6,37 @@
 #include "../../grid/GridPoint.h"
 #include "../../grid/GridNode.h"
 
+TEST(Grid, addTwice) {
+
+	Grid<GP> grid(20);
+	ASSERT_EQ(0, grid.add(GP(0,0,0)));
+	ASSERT_EQ(1, grid.add(GP(20,20,20)));
+
+	// already present -> error
+	ASSERT_THROW(grid.add(GP(0,0,0)), std::exception);
+	ASSERT_THROW(grid.add(GP(20,20,20)), std::exception);
+
+	ASSERT_EQ(2, grid.add(GP(20,20,40)));
+
+	// already present -> error
+	ASSERT_THROW(grid.add(GP(20,20,40)), std::exception);
+
+}
+
 TEST(Grid, add) {
 
 	Grid<GP> grid(20);
 	ASSERT_EQ(0, grid.add(GP()));
-	ASSERT_EQ(1, grid.add(GP()));
-	ASSERT_EQ(2, grid.add(GP()));
-	ASSERT_EQ(3, grid.add(GP()));
 
 	// not aligned? -> error
 	ASSERT_THROW(grid.add(GP(10,10,10)), std::exception);
 	ASSERT_THROW(grid.add(GP(10,20,20)), std::exception);
 	ASSERT_THROW(grid.add(GP(20,10,20)), std::exception);
 	ASSERT_THROW(grid.add(GP(20,20,10)), std::exception);
-	ASSERT_EQ(4, grid.add(GP(20,20,20)));
+	ASSERT_EQ(1, grid.add(GP(20,20,20)));
 
 	// access
-	ASSERT_EQ(GP(20,20,20), grid[4]);
+	ASSERT_EQ(GP(20,20,20), grid[1]);
 
 }
 

tests/math/TestMovingMedian.cpp

@@ -0,0 +1,22 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../math/MovingMedian.h"
+
+TEST(MovingMedian, add) {
+
+	MovingMedian<float> med(3);
+
+	med.add(1); ASSERT_EQ(1, med.get());
+	med.add(2); ASSERT_EQ(1.5, med.get());
+	med.add(3); ASSERT_EQ(2, med.get());
+
+	med.add(3); ASSERT_EQ(3, med.get());
+	med.add(1); ASSERT_EQ(3, med.get());
+	med.add(1); ASSERT_EQ(1, med.get());
+	med.add(2); ASSERT_EQ(1, med.get());
+	med.add(3); ASSERT_EQ(2, med.get());
+
+}
+
+#endif