Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

geo/Heading.h

@@ -46,6 +46,11 @@ public:
 		return (Heading(*this) += _rad);
 	}
 
+        Heading& operator = (const float _rad) {
+            rad = _rad;
+            return *this;
+        }
+
 	/** compare two headings */
 	bool operator == (const Heading other) const {return rad == other.rad;}
 	bool operator != (const Heading other) const {return rad != other.rad;}
@@ -59,13 +64,13 @@ public:
 
 	float getRAD() const {return rad;}
 
-	/** get a random heading */
+//	/** get a random heading */
-	static Heading rnd() {
+//	static Heading rnd() {
-		static std::minstd_rand gen(1234);
+//		static std::minstd_rand gen(1234);
-		static std::uniform_real_distribution<float> dist(0, _2PI);
+//		static std::uniform_real_distribution<float> dist(0, _2PI);
-		const float rnd = dist(gen);
+//		const float rnd = dist(gen);
-		return Heading(rnd);
+//		return Heading(rnd);
-	}
+//	}
 
 #undef _2PI
 

grid/Grid.h

@@ -152,6 +152,11 @@ public:
 		return (hashes.find(uid) != hashes.end());
 	}
 
+	/** get a list of all nodes within the graph */
+	const std::vector<T>& getNodes() const {
+		return nodes;
+	}
+
 	/** get the center-node the given Point belongs to */
 	const T& getNodeFor(const GridPoint& p) {
 		const UID uid = getUID(p);

grid/walk/GridWalk.h

@@ -4,11 +4,21 @@
 #include "GridWalkState.h"
 #include "../Grid.h"
 
+/** all supported acitivites lukas can detect */
+enum class Activity {
+	UNKNOWN,
+	STANDING,
+	WALKING,
+	STAIRS_UP,
+	STAIRS_DOWN,
+	ELEVATOR,
+};
+
 template <typename T> class GridWalk {
 
 public:
 
-	virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) = 0;
+	virtual GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) = 0;
 
 };
 

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -6,6 +6,7 @@
 
 #include "../../math/DrawList.h"
 #include "../../math/Distributions.h"
+#include "../../math/DrawList.h"
 
 #include "../../nav/dijkstra/Dijkstra.h"
 
@@ -34,7 +35,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
@@ -65,7 +66,7 @@ public:
 
 	}
 
-	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) override {
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) override {
 
 		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad);
 

grid/walk/GridWalkPathControl.h

@@ -23,7 +23,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
@@ -61,27 +61,31 @@ public:
 
 	}
 
-	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
+	Distribution::Normal<float> dHead = Distribution::Normal<float>(1, 0.01);
+	Distribution::Normal<float> dWalk = Distribution::Normal<float>(1, 0.10);
+	Distribution::Normal<float> sWalk = Distribution::Normal<float>(0, 0.15);
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
 
 		// proportional change of the heading
-		static 	Distribution::Normal<float> dHead(1, 0.01);
+		//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.10);
 
 		distance_m = distance_m*dWalk.draw()*1.4;		// TODO: why *2?
 		headChange_rad = headChange_rad*dHead.draw();
 
-		static 	Distribution::Normal<float> sWalk(0, 0.15);
+		//static 	Distribution::Normal<float> sWalk(0, 0.15);
 		if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
 
-		return walk(grid, start, distance_m, headChange_rad);
+		return walk(grid, start, distance_m, headChange_rad, act);
 
 	}
 
 private:
 
-	double getProbability(const T& start, const T& prev, const T& possible, const Heading head) const {
+	double getProbability(const T& start, const T& prev, const T& possible, const Heading head, Activity act) const {
 
 		// TODO: WHY?! not only when going back to the start?
 		if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
@@ -97,24 +101,31 @@ private:
 		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 = 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
 		//const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025;		// from start
 		const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther;		// from previous node
 
+		double actProb = 1.0;
+		if (act == Activity::STAIRS_UP)		{actProb = (prev.z_cm < possible.z_cm) ? (0.8) : (0.2);}
+		if (act == Activity::STAIRS_DOWN)	{actProb = (prev.z_cm > possible.z_cm) ? (0.8) : (0.2);}
+		if (act == Activity::WALKING) {actProb = (prev.z_cm == possible.z_cm) ? (0.8) : (0.2);}
+
 		// bring it together
-		return angleProb * nodeProb;
+		return angleProb * nodeProb * actProb;
 
 	}
 
-	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) {
+	Distribution::Uniform<float> dChange = Distribution::Uniform<float>(Angle::degToRad(0), +Angle::degToRad(359));
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) {
 
 		// 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));
+		//	static	Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));
 
 		int retries = 5;
 		float walked_m = 0;
@@ -130,7 +141,7 @@ private:
 			drawer.reset();
 			for (T& neighbor : grid.neighbors(*cur.node)) {
 
-				const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading);
+				const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading, act);
 				drawer.add(neighbor, prob);
 
 			}

grid/walk/GridWalkPushForward.h

@@ -8,6 +8,7 @@
 #include "../Grid.h"
 
 #include "../../math/DrawList.h"
+#include "../../math/Random.h"
 #include "../../nav/dijkstra/Dijkstra.h"
 
 #include "GridWalkState.h"
@@ -31,7 +32,7 @@ private:
 	static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkRandomHeadingUpdate.h

@@ -4,6 +4,8 @@
 #include "../../geo/Heading.h"
 #include "../Grid.h"
 
+
+#include "../../math/Random.h"
 #include "../../nav/dijkstra/Dijkstra.h"
 
 #include "GridWalk.h"
@@ -33,7 +35,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
@@ -47,7 +49,7 @@ public:
 
 
 
-	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) override {
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) override {
 
 
 		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1);

grid/walk/GridWalkRandomHeadingUpdateAdv.h

@@ -8,6 +8,7 @@
 #include "../../math/DrawList.h"
 #include "../../math/Distributions.h"
 #include "../../nav/dijkstra/Dijkstra.h"
+#include "../../math/Random.h"
 
 #include "GridWalk.h"
 #include "GridWalkState.h"
@@ -34,7 +35,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkShortestPathControl.h

@@ -7,6 +7,7 @@
 
 #include "../../math/Distributions.h"
 #include "../../math/DrawList.h"
+#include "../../math/Random.h"
 
 #include "../../nav/dijkstra/Dijkstra.h"
 #include "../../nav/dijkstra/DijkstraPath.h"
@@ -17,6 +18,8 @@
 #include "GridWalkHelper.h"
 #include "GridWalk.h"
 
+
+
 #include <KLib/math/statistics/Statistics.h>
 
 template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {
@@ -76,7 +79,7 @@ protected:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
@@ -117,7 +120,7 @@ public:
 	int times = 3;
 	float pOther = 0.10;
 
-	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
 
 
 		// update the center-of-mass

grid/walk/GridWalkSimpleControl.h

@@ -8,6 +8,7 @@
 
 #include "../../math/Distributions.h"
 #include "../../math/DrawList.h"
+#include "../../math/Random.h"
 
 #include "GridWalkState.h"
 #include "GridWalkHelper.h"
@@ -23,7 +24,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
@@ -39,20 +40,22 @@ public:
 		gen.seed(1234);
 	}
 
+	Distribution::Normal<float> dHead = Distribution::Normal<float>(1, 0.01);
+	Distribution::Normal<float> dWalk = Distribution::Normal<float>(1, 0.10);
+	Distribution::Normal<float> sWalk = Distribution::Normal<float>(0, 0.10);
 
-
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
-	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);
+		//static 	Distribution::Normal<float> dWalk(1, 0.10);
 
 		distance_m = distance_m*dWalk.draw()*1.4;		// TODO: why * X?
 		headChange_rad = headChange_rad*dHead.draw();
 
-		static 	Distribution::Normal<float> sWalk(0, 0.10);
+		//static 	Distribution::Normal<float> sWalk(0, 0.10);
 		if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
 
 		return walk(grid, start, distance_m, headChange_rad);
@@ -90,12 +93,14 @@ private:
 
 	}
 
+	Distribution::Uniform<float> dChange = Distribution::Uniform<float>(Angle::degToRad(0), +Angle::degToRad(359));
+
 	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));
+		//	static	Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));
 
 		int retries = 5;
 		float walked_m = 0;

grid/walk/GridWalkWeighted.h

@@ -6,6 +6,7 @@
 
 #include "../../math/DrawList.h"
 #include "../../math/Distributions.h"
+#include "../../math/DrawList.h"
 
 /**
  * perform walks on the grid based on some sort of weighting
@@ -38,7 +39,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkWeighted2.h

@@ -5,7 +5,9 @@
 #include "../Grid.h"
 
 #include "../../math/DrawList.h"
-#include <KLib/math/distribution/Normal.h>
+#include "../../math/Random.h"
+
+//#include <KLib/math/distribution/Normal.h>
 
 /**
  * perform walks on the grid based on some sort of weighting
@@ -38,7 +40,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/TestWalkWeighted3.h

@@ -5,7 +5,9 @@
 #include "../Grid.h"
 
 #include "../../math/DrawList.h"
-#include <KLib/math/distribution/Normal.h>
+#include "../../math/Random.h"
+
+//#include <KLib/math/distribution/Normal.h>
 
 /**
  * perform walks on the grid based on some sort of weighting
@@ -38,7 +40,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

math/Distributions.h

@@ -5,5 +5,6 @@
 #include "distribution/Exponential.h"
 #include "distribution/Logistic.h"
 #include "distribution/Uniform.h"
+#include "distribution/VonMises.h"
 
 #endif // DISTRIBUTIONS_H

math/DrawList.h

@@ -2,8 +2,8 @@
 #define DRAWLIST_H
 
 #include <vector>
-#include <random>
 
+#include "Random.h"
 #include "../Assertions.h"
 
 /**
@@ -38,7 +38,7 @@ private:
 	std::vector<Entry> elements;
 
 	/** random number generator */
-	std::minstd_rand gen;
+	RandomGenerator gen;
 
 public:
 

math/Interpolator.h

@@ -34,6 +34,12 @@ public:
 		entries.push_back(InterpolatorEntry(key, value));
 	}
 
+	/** get the smallest added key */
+	Key getMinKey() const {return entries.front().key;}
+
+	/** get the largest added key */
+	Key getMaxKey() const {return entries.back().key;}
+
 	/** get the interpolated value for the given key */
 	Value get(const Key key) const {
 

math/Math.h

@@ -0,0 +1,20 @@
+#ifndef K_MATH_MATH_H
+#define K_MATH_MATH_H
+
+#include "../Defines.h"
+
+class Math {
+
+public:
+
+	/** ensure val stays within [min:max] */
+	template <typename Scalar> static inline Scalar clamp(const Scalar min, const Scalar max, const Scalar val) {
+		if (unlikely(val < min)) {return min;}
+		if (unlikely(val > max)) {return max;}
+		return val;
+	}
+
+};
+
+
+#endif // K_MATH_MATH_H

math/Random.h

@@ -0,0 +1,28 @@
+#ifndef RANDOM_H
+#define RANDOM_H
+
+#include <cmath>
+#include <random>
+#include "../misc/Time.h"
+
+#ifdef USE_FIXED_SEED
+	#define RANDOM_SEED 1234
+#else
+	#define RANDOM_SEED ( std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1) )
+#endif
+
+//using RandomGenerator = std::minstd_rand;
+
+class RandomGenerator : public std::minstd_rand {
+
+public:
+
+	/** ctor with default seed */
+	RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
+
+	/** ctor with custom seed */
+	RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;}
+
+};
+
+#endif // RANDOM_H

math/distribution/Bessel.h

@@ -0,0 +1,57 @@
+#ifndef BESSEL_H
+#define BESSEL_H
+
+namespace Distribution {
+
+	/** helper class */
+	template <typename T> class Bessel {
+
+	public:
+
+		/**
+		 * calculation for I_v(z)
+		 * https://en.wikipedia.org/wiki/Bessel_function
+		 * http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html
+		 */
+		T getModified(const int v, const T z) const {
+
+			// running factorials (updated within for-loops)
+			uint64_t runFac1 = 1;
+			uint64_t runFac2 = factorial(v);	// Delta(k+v+1) = (k+v+1-1)! = (k+v)! [k starts at 0] -> (v)!
+
+			// running potential
+			T runPot = 1;
+			const T pot = T(0.25) * z * z;
+
+			// start for-loop at k=1 instead of k=0. allows for running factorial without using if (k==0)
+			T sum = 0;
+			for (int k = 0; k < 16; ) {
+				sum += runPot / runFac1 / runFac2;
+				++k;
+				runFac1 *= k;
+				runFac2 *= k;
+				runPot *= pot;
+			}
+
+			// done
+			return std::pow( (T(0.5) * z), v) * sum;
+
+		}
+
+	private:
+
+
+
+
+
+		static uint64_t factorial(const int n) {
+			uint64_t res = 1;
+			for (int i = 2; i <= n; ++i) {res *= i;}
+			return res;
+		}
+
+	};
+
+}
+
+#endif // BESSEL_H

math/distribution/Exponential.h

@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <random>
-
+#include "../Random.h"
 
 namespace Distribution {
 
@@ -14,13 +14,13 @@ namespace Distribution {
 
 		const T lambda;
 
-		std::minstd_rand gen;
+		RandomGenerator gen;
 		std::exponential_distribution<T> dist;
 
 	public:
 
 		/** ctor */
-		Exponential(const T lambda) : lambda(lambda), dist(lambda) {
+		Exponential(const T lambda) : lambda(lambda), gen(RANDOM_SEED), dist(lambda) {
 			;
 		}
 

math/distribution/LUT.h

@@ -0,0 +1,58 @@
+#ifndef LUT_H
+#define LUT_H
+
+#include <vector>
+#include "../Math.h"
+
+namespace Distribution {
+
+	template <typename Scalar> class LUT {
+
+	private:
+
+		Scalar min;
+		Scalar max;
+		Scalar diff;
+
+		/** number of samples between min and max */
+		int numSamples;
+
+		/** the look-up-table */
+		std::vector<Scalar> samples;
+
+
+	public:
+
+		/** ctor */
+		template <typename Distribution> LUT(const Distribution dist, const Scalar min, const Scalar max, const int numSamples) :
+			min(min), max(max), diff(max-min), numSamples(numSamples) {
+
+			buildLUT(dist);
+
+		}
+
+		/** get the probability of val from the LUT */
+		Scalar getProbability(const Scalar val) const {
+			int idx = ((val - min) * numSamples / diff);
+			idx = Math::clamp(0, numSamples-1, idx);
+			return samples[idx];
+		}
+
+	private:
+
+		/** build the look-up-table */
+		template <typename Distribution> void buildLUT(const Distribution dist) {
+
+			samples.resize(numSamples);
+			for (int idx = 0; idx < numSamples; ++idx) {
+				const Scalar val = min + (idx * diff / numSamples);
+				samples[idx] = dist.getProbability(val);
+			}
+
+		}
+
+	};
+
+}
+
+#endif // LUT_H

math/distribution/Normal.h

@@ -3,6 +3,7 @@
 
 #include <cmath>
 #include <random>
+#include "../Random.h"
 
 namespace Distribution {
 
@@ -15,14 +16,14 @@ namespace Distribution {
 		const T sigma;
 		const T _a;
 
-		std::minstd_rand gen;
+		RandomGenerator gen;
 		std::normal_distribution<T> dist;
 
 	public:
 
 		/** ctor */
 		Normal(const T mu, const T sigma) :
-			mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(1234), dist(mu,sigma) {
+			mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(RANDOM_SEED), dist(mu,sigma) {
 
 		}
 

math/distribution/Uniform.h

@@ -3,6 +3,9 @@
 
 #include <cmath>
 #include <random>
+#include "../Random.h"
+
+#include <type_traits>
 
 namespace Distribution {
 
@@ -11,14 +14,17 @@ namespace Distribution {
 
 	private:
 
-		std::minstd_rand gen;
+		RandomGenerator gen;
-		std::uniform_real_distribution<T> dist;
+
+		/** depending on T, Dist is either a uniform_real or uniform_int distribution */
+		typedef typename std::conditional< std::is_floating_point<T>::value, std::uniform_real_distribution<T>, std::uniform_int_distribution<T> >::type Dist;
+		Dist dist;
+
 
 	public:
 
 		/** ctor */
-		Uniform(const T min, const T max) :
+		Uniform(const T min, const T max) : gen(RANDOM_SEED), dist(min, max) {
-			gen(1234), dist(min, max) {
 
 		}
 		/** get a uniformaly distributed random number */

math/distribution/VonMises.h

@@ -0,0 +1,56 @@
+#ifndef K_MATH_DISTRIBUTION_VONMISES_H
+#define K_MATH_DISTRIBUTION_VONMISES_H
+
+#include <cmath>
+#include <random>
+
+#include "../Math.h"
+#include "Bessel.h"
+#include "LUT.h"
+
+namespace Distribution {
+
+	/** von-mises distribution */
+	template <typename T> class VonMises {
+
+	private:
+
+		/** center of the distribution */
+		const T mu;
+
+		/** like 1.0/variance of the distribution */
+		const T kappa;
+
+		/** pre-calcuated look-up-table */
+		std::vector<T> lut;
+
+		/** helper for modified bessel I_0(kappa) */
+		Bessel<T> bessel;
+
+	public:
+
+		/** ctor */
+		VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) {
+
+		}
+
+		LUT<T> getLUT() const {
+			return LUT<T>(*this, -M_PI, +M_PI, 10000);
+		}
+
+		/** get probability for the given value */
+		T getProbability(const T _val) const {
+
+			const T val = Math::clamp((T)-M_PI, (T)+M_PI, _val);
+
+			return
+					std::exp(kappa * std::cos(val - mu)) /
+					(2 * M_PI * bessel.getModified(0, kappa));
+
+		}
+
+	};
+
+}
+
+#endif // K_MATH_DISTRIBUTION_VONMISES_H

misc/Time.h

@@ -12,7 +12,7 @@ public:
 
 	}
 
-	static int diffMS(std::chrono::system_clock::time_point tick1, std::chrono::system_clock::time_point tick2) {
+	static uint32_t diffMS(std::chrono::system_clock::time_point tick1, std::chrono::system_clock::time_point tick2) {
 		return std::chrono::duration_cast<std::chrono::milliseconds>(tick2 - tick1).count();
 	}
 

nav/a-star/AStar.h

@@ -0,0 +1,133 @@
+#ifndef ASTAR_H
+#define ASTAR_H
+
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <limits>
+#include <queue>
+#include <unordered_map>
+#include <cassert>
+
+#include "../../grid/Grid.h"
+
+
+template <typename T> class AStar {
+
+public:
+
+#define LE_MAX 500000
+
+	//dijkstra with priority queue O(E log V)
+    template <typename Access>
+    static float get(const T* source, const T* destination, Access acc) {
+
+		// track distances from the source to each other node
+		//std::unordered_map<const T*, float> distance;
+
+		float distance[LE_MAX];
+
+
+		// track the previous node for each node along the path
+		//std::unordered_map<const T*, const T*> parent;
+		const T* parent[LE_MAX];
+
+		// all nodes
+        //const std::vector<T>& nodes = acc.getAllNodes();
+
+		// priority queue to check which node is to-be-processed next
+		std::priority_queue<std::pair<T*,float>, std::vector<std::pair<const T*,float>>, Comparator2> Q;
+
+		// start with infinite distance
+//		for(const auto& node : nodes){
+//			distance[node.getIdx()] = std::numeric_limits<float>::max();
+//		}
+        std::fill_n(distance, LE_MAX, std::numeric_limits<float>::max());
+
+
+//        std::cout << (std::string)*source << std::endl;
+//        std::cout << (std::string)*destination << std::endl;
+//        int iter = 0;
+
+		// start at the source
+		distance[source->getIdx()] = 0.0f;
+		Q.push(std::make_pair(source,distance[source->getIdx()]));
+
+		// proceed until there are now new nodes to follow
+		while(!Q.empty()) {
+
+//            ++iter;
+
+			// fetch the next-nearest node from the queue
+			const T* u = Q.top().first;
+
+			// and check whether we reached the destination
+			if (u == destination) {break;}
+
+			// remove from the Queue
+			Q.pop();
+
+			// process all neighbors for the current element
+			for( const T& v : acc.getNeighbors(*u)) {
+
+				// weight (distance) between the current node and its neighbor
+				//const float w = ((Point3)v - (Point3)*u).length();
+				const float w = acc.getWeightBetween(v, *u);
+
+				// found a better route?
+				if (distance[v.getIdx()] > distance[u->getIdx()] + w) {
+					distance[v.getIdx()] = distance[u->getIdx()] + w;
+					parent[v.getIdx()] = u;
+					Q.push(std::make_pair(&v, distance[v.getIdx()] + acc.getHeuristic(v, *destination)));		// SOURCE OR DEST?!
+				}
+
+			}
+		}
+
+
+//        std::cout << iter << std::endl;
+
+//		// construct the path
+//		std::vector<const T*> path;
+//		const T* p = destination;
+//		path.push_back(destination);
+
+//		// until we reached the source-node
+//		while (p!=source) {
+//			if (p) {
+//				p = parent[p->getIdx()];
+//				path.push_back(p);
+//			} else {
+//				return std::vector<const T*>(); //if no path could be found, just return an empty vector.
+//			}
+//		}
+
+		// done
+        return distance[destination->getIdx()];
+
+	}
+
+
+//	template <typename Access> static std::vector<const T*> getShortestPathAStar(const T* src, const T* dst, Access acc){
+
+//		std::vector<const T*> shortestPath;
+
+//		//here we could do some preprocessing. e.g. area of interest of nodes
+
+
+//		// call aStar
+//		shortestPath = aStar(src, dst, acc);
+
+//		return shortestPath;
+//	}
+
+	class Comparator2 {
+	public:
+		int operator() ( const std::pair<const T*,float>& p1, const std::pair<const T*,float>& p2){
+			return p1.second > p2.second;
+		}
+	};
+
+};
+
+#endif // ASTAR_H

nav/dijkstra/Dijkstra.h

@@ -49,7 +49,7 @@ public:
 		// runs until all nodes were evaluated
 		(void) end;
 
-		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to ALL OTHER nodes", false);
+		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to " + ((end)?((std::string)*end):"ALL OTHER nodes"), true);
 		Log::tick();
 
 		// cleanup previous runs
@@ -75,10 +75,10 @@ public:
 			DijkstraNode<T>* dnSrc = toBeProcessedNodes.pop();
 
 			// when an end is given, stop when end was reached
-			if (end != nullptr && dnSrc->element == end) {break;}
+			if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;}
 
 			// when a maximum weight is given, stop when current cum-dist > maxWeight
-			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {break;}
+			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;}
 
 			// visit (and maybe update) each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
@@ -113,8 +113,8 @@ public:
 
 		}
 
+		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes", false);
 		Log::tock();
-		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
 
 	}
 

tests/geo/TestHeading.cpp

@@ -52,13 +52,13 @@ TEST(Heading, eq) {
 
 }
 
-TEST(Heading, random) {
+//TEST(Heading, random) {
 
-	// two random values must not be equal
+//	// two random values must not be equal
-	ASSERT_NE(Heading::rnd(), Heading::rnd());
+//	ASSERT_NE(Heading::rnd(), Heading::rnd());
-	ASSERT_NE(Heading::rnd(), Heading::rnd());
+//	ASSERT_NE(Heading::rnd(), Heading::rnd());
-	ASSERT_NE(Heading::rnd(), Heading::rnd());
+//	ASSERT_NE(Heading::rnd(), Heading::rnd());
 
-}
+//}
 
 #endif

tests/math/TestDistribution.cpp

@@ -2,6 +2,26 @@
 
 #include "../Tests.h"
 #include "../../math/Distributions.h"
+#include "../../misc/Time.h"
+
+template <typename Dist> void benchDist(Dist dist) {
+
+	const int cnt = 1024*1024*32;
+	float f = 0;
+
+	auto tick = Time::tick();
+	for (int i = 0; i < cnt; ++i) {
+		f = dist.getProbability(f);
+	}
+	auto tock = Time::tick();
+
+	std::cout.imbue(std::locale("en_US.UTF-8"));
+	std::cout << (cnt / (Time::diffMS(tick, tock)+1) * 1000) << "/sec" << std::endl;
+
+
+}
+
+
 
 TEST(Distribution, Exp) {
 
@@ -15,4 +35,69 @@ TEST(Distribution, Exp) {
 
 }
 
+TEST(Distribution, Exp_Performance) {
+
+	Distribution::Exponential<float> dist(0.5);
+	benchDist(dist);
+
+}
+
+TEST(Distribution, BesselHelper) {
+
+	Distribution::Bessel<float> bessel;
+
+	const float delta = 0.001;
+	ASSERT_NEAR( 1.0, bessel.getModified(0, 0), delta);
+	ASSERT_NEAR(11.3, bessel.getModified(0, +4), 0.1);
+	ASSERT_NEAR(11.3, bessel.getModified(0, -4), 0.1);
+
+}
+
+TEST(Distribution, VonMises) {
+
+	Distribution::VonMises<float> dist4(0, 4);
+	Distribution::VonMises<float> dist2(0, 2);
+	Distribution::VonMises<float> dist1(0, 1);
+	Distribution::VonMises<float> dist05(0, 0.5);
+
+	const float delta = 0.01;
+	ASSERT_NEAR(0.768f, dist4.getProbability(0.0f), delta);
+	ASSERT_NEAR(0.515f, dist2.getProbability(0.0f), delta);
+	ASSERT_NEAR(0.342f, dist1.getProbability(0.0f), delta);
+	ASSERT_NEAR(0.24f, dist05.getProbability(0.0f), delta);
+
+}
+
+
+
+
+
+//#include <fstream>
+//TEST(Distribution, VonMisesDump) {
+
+//	Distribution::VonMises<float> dist1(0, 4.0);
+//	auto dist2 = dist1.getLUT();
+
+//	std::ofstream f1("/tmp/1.dat");
+//	std::ofstream f2("/tmp/2.dat");
+//	for (float i = -4; i <= +4; i += 0.001) {
+//		f1 << i << " " << dist1.getProbability(i) << std::endl;
+//		f2 << i << " " << dist2.getProbability(i) << std::endl;
+//	}
+//	f1.close();
+//	f2.close();
+
+//}
+
+TEST(Distribution, VonMises_Performance) {
+	Distribution::VonMises<float> dist(0, 2.0);
+	benchDist(dist);
+}
+
+TEST(Distribution, VonMisesLUT_Performance) {
+	Distribution::VonMises<float> src(0, 2.0);
+	auto dist = src.getLUT();
+	benchDist(dist);
+}
+
 #endif

tests/nav/a-star/TestAStar.cpp

@@ -0,0 +1,48 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../grid/Grid.h"
+#include "../../../nav/a-star/AStar.h"
+#include "../../grid/Plot.h"
+
+TEST(AStar, build) {
+
+	Grid<GP> grid(1);
+
+	int idx1 = grid.add(GP( 0, 0, 0));
+	int idx2 = grid.add(GP( 0, 1, 0));
+	int idx3 = grid.add(GP( 0,-1, 0));
+	int idx4 = grid.add(GP( 1, 0, 0));
+	int idx5 = grid.add(GP(-1, 0, 0));
+
+	grid.connectBiDir(idx1, idx2);
+	grid.connectBiDir(idx1, idx3);
+	grid.connectBiDir(idx1, idx4);
+	grid.connectBiDir(idx1, idx5);
+
+	class TMP {
+		Grid<GP>& grid;
+	public:
+		TMP(Grid<GP>& grid) : grid(grid) {;}
+//		int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
+//		const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
+		const std::vector<GP>& getAllNodes() const {return grid.getNodes();}
+		decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);}
+		float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
+		float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);}
+	} tmp(grid);
+
+	AStar<GP> nav;
+	std::vector<const GP*> vec = nav.get(&grid[0], &grid[4], tmp);
+
+	for (const GP* g : vec) {
+		std::cout << g << std::endl;
+	}
+
+
+}
+
+
+
+#endif