added exponential distrbution + tests

new shortest-path walker (still todo)
modified dijkstra-path (new helper methods)

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -159,9 +159,9 @@ private:
 		next.distanceWalked_m += walked_m;
 		++((T*)next.node)->cnt;		// TODO: eval only
 
-		if (next.node->z_cm != cur.node->z_cm) {
-			int i = 0;
-		}
+//		if (next.node->z_cm != cur.node->z_cm) {
+//			int i = 0;
+//		}
 
 
 

grid/walk/GridWalkShortestPathControl.h

@@ -0,0 +1,266 @@
+#ifndef GRIDWALKSHORTESTPATHCONTROL_H
+#define GRIDWALKSHORTESTPATHCONTROL_H
+
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/Distributions.h"
+#include "../../math/DrawList.h"
+
+#include "../../nav/dijkstra/Dijkstra.h"
+#include "../../nav/dijkstra/DijkstraPath.h"
+#include "../../misc/KNN.h"
+#include "../../misc/KNNArray.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+#include "GridWalk.h"
+
+template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {
+
+
+protected:
+
+	/** put dijkstra-nodes into the KNN */
+	class Wrapper {
+
+	private:
+
+		DijkstraPath<T>* path;
+
+	public:
+
+		Wrapper() : path(nullptr) {;}
+
+		/** ctor with the underlying data structure */
+		Wrapper(DijkstraPath<T>* path) : path(path) {
+			;
+		}
+
+		/** get the number of elements to search throrugh */
+		inline int kdtree_get_point_count() const {
+			return path->size();
+		}
+
+		/** use nanoflanns default bbox */
+		template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
+			(void) bb; return false;
+		}
+
+		/** get the idx-th element's dim-th coordinate */
+		inline float kdtree_get_pt(const size_t idx, const int dim) const {
+			return (*((*path)[idx]).element)[dim];
+		}
+
+		/** get the SQUARED distance between the given coordinates and the provided element */
+		inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
+			const float d0 = p1[0] - (*((*path)[idx_p2]).element)[0];
+			const float d1 = p1[1] - (*((*path)[idx_p2]).element)[1];
+			const float d2 = p1[2] - (*((*path)[idx_p2]).element)[2];
+			return (d0*d0) + (d1*d1) + (d2*d2);
+		}
+
+
+	};
+
+
+
+	friend class GridWalkHelper;
+
+protected:
+
+	/** 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;
+	const T& target;
+
+	Point3 centerOfMass = Point3(0,0,0);
+	Wrapper wrapper;
+	DijkstraPath<T>* path = nullptr;
+	KNN<Wrapper,3>* knn = nullptr;
+
+
+	DrawList<T&> drawer;
+
+
+public:
+
+
+
+	/** ctor with the target you want to reach */
+	template <typename Access> GridWalkShortestPathControl(Grid<T>& grid, const Access& acc, const T& target) : target(target) {
+
+		(void) grid;
+
+		gen.seed(1234);
+
+		// build all shortest path to reach th target
+		dijkstra.build(target, target, acc);
+
+	}
+
+	int recalc = 0;
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
+
+		// update the center-of-mass
+		centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
+		if (path == nullptr) {rebuildPath(grid);}
+
+		if (++recalc > 100 * 1000) {
+			recalc = 0;
+			rebuildPath(grid);
+		}
+
+//		if (knn != nullptr) {
+//			const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} );
+//			if (dist > 10000) {
+//				rebuildPath(grid);
+//			}
+//		}
+
+		// 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(25), diff);
+//		const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1;		// favor best 3 angles equally
+
+		// nodes own importance
+		double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025;
+
+		if (knn != nullptr) {
+			const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
+			//const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} );
+			//nodeProb = (pd < sd) ? 1 : 0.0;
+			nodeProb = Distribution::Exponential<float>::getProbability(0.15, pd_m);
+		}
+
+		// 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;
+
+	}
+
+	/** rebuild the path for the given center point */
+	void rebuildPath(Grid<T>& grid) {
+
+		// find the grid node nearest to the current center-of-mass
+		auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); };
+		const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode);
+
+		delete path; path = nullptr;
+		delete knn; knn = nullptr;
+
+		DijkstraNode<T>* dnTarget = dijkstra.getNode(target);
+		DijkstraNode<T>* dnStart =  dijkstra.getNode(currentMass);
+
+		if (dnStart == nullptr) {
+			return;
+		}
+
+		// calculate the path from there to the target
+		try {
+			path = new DijkstraPath<T>(dnStart, dnTarget);
+
+			// create k-nn lookup
+			wrapper = Wrapper(path);
+			knn = new KNN<Wrapper, 3>(wrapper);
+		} catch (...) {
+			knn = nullptr;
+			path = nullptr;
+		}
+
+	}
+
+
+};
+
+#endif // GRIDWALKSHORTESTPATHCONTROL_H

math/distribution/Exponential.h

@@ -1,4 +1,52 @@
 #ifndef EXPONENTIAL_H
 #define EXPONENTIAL_H
 
+#include <cmath>
+#include <random>
+
+
+namespace Distribution {
+
+	/** exponential distribution */
+	template <typename T> class Exponential {
+
+	private:
+
+		const T lambda;
+
+		std::minstd_rand gen;
+		std::exponential_distribution<T> dist;
+
+	public:
+
+		/** ctor */
+		Exponential(const T lambda) : lambda(lambda), dist(lambda) {
+			;
+		}
+
+		/** get probability for the given value */
+		T getProbability(const T val) const {
+			return getProbability(lambda, val);
+		}
+
+		/** get an exponentially distributed random number */
+		T draw() {
+			return dist(gen);
+		}
+
+		/** set the seed to use */
+		void setSeed(const long seed) {
+			gen.seed(seed);
+		}
+
+
+		/** get the probability for the given value */
+		static T getProbability(const T lambda, const T val) {
+			return (val < 0) ? (0) : (lambda * std::exp( -lambda * val ));
+		}
+
+	};
+
+}
+
 #endif // EXPONENTIAL_H

nav/dijkstra/DijkstraPath.h

@@ -43,8 +43,10 @@ public:
 	}
 
 	/** allow iteration */
-	decltype(path.begin()) begin()	{return path.begin();}
-	decltype(path.end()) end()		{return path.end();}
+	decltype(path.begin()) begin()								{return path.begin();}
+	decltype(path.end()) end()									{return path.end();}
+	const DijkstraNode<T>& operator [] (const int idx) const	{return *(path[idx]);}
+	size_t size() const											{return path.size();}
 
 
 	/** NANOFLANN: number of elements in the path */

tests/math/TestDistribution.cpp

@@ -0,0 +1,18 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../math/Distributions.h"
+
+TEST(Distribution, Exp) {
+
+	ASSERT_NEAR(1.5, Distribution::Exponential<float>::getProbability(1.5, 0.0), 0.001);
+	ASSERT_NEAR(1.0, Distribution::Exponential<float>::getProbability(1.0, 0.0), 0.001);
+	ASSERT_NEAR(0.5, Distribution::Exponential<float>::getProbability(0.5, 0.0), 0.001);
+
+	ASSERT_NEAR(0.35, Distribution::Exponential<float>::getProbability(1.5, 1.0), 0.05);
+	ASSERT_NEAR(0.35, Distribution::Exponential<float>::getProbability(1.0, 1.0), 0.05);
+	ASSERT_NEAR(0.35, Distribution::Exponential<float>::getProbability(0.5, 1.0), 0.05);
+
+}
+
+#endif