added several grid-walks

added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...

CMakeLists.txt

@@ -62,6 +62,8 @@ ADD_DEFINITIONS(
 	-Wextra
 	-Wpedantic
 
+	-fstack-protector-all
+
 	-g3
 	-O0
 

geo/Angle.h

@@ -4,7 +4,8 @@
 #include <cmath>
 #include <KLib/Assertions.h>
 
-class Angle {
+struct Angle {
+
 
 public:
 
@@ -28,12 +29,15 @@ public:
 	static float getDiffRAD_2PI_PI(const float r1, const float r2) {
 		_assertBetween(r1, 0, 2*M_PI, "r1 out of bounds");
 		_assertBetween(r2, 0, 2*M_PI, "r2 out of bounds");
-		return fmod(std::abs(r2 - r1), M_PI);
+		float tmp = std::abs(r1-r2);
+		return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp);
+		//float tmp2 = fmod(tmp, M_PI);
+		//return fmod(std::abs(r2 - r1), M_PI);
 
 	}
 
 	/** convert degrees to radians */
-	static float degToRad(const float deg) {
+	static constexpr float degToRad(const float deg) {
 		return deg / 180 * M_PI;
 	}
 

geo/Heading.h

@@ -0,0 +1,63 @@
+#ifndef HEADING_H
+#define HEADING_H
+
+#include <cmath>
+
+#include "Angle.h"
+
+struct Heading {
+
+#define _2PI (2*M_PI)
+
+private:
+
+	/** heading in radians. 0 = to-the-right */
+	float rad;
+
+public:
+
+	/** ctor with radians */
+	Heading(const float rad) : rad(rad) {
+		_assertBetween(rad, 0, _2PI, "radians out of bounds");
+	}
+
+	/** ctor from(x1,y1) to(x2,y2) */
+	Heading(const float x1, const float y1, const float x2, const float y2) : rad(Angle::getRAD_2PI(x1,y1,x2,y2)) {
+		_assertBetween(rad, 0, _2PI, "radians out of bounds");
+	}
+
+	/** angular difference [0:PI] */
+	float getDiffHalfRAD(const Heading other) const {
+		return Angle::getDiffRAD_2PI_PI(rad, other.rad);
+	}
+
+	/** update the angle but ensure we stay within [0:2PI] */
+	Heading& operator += (const float _rad) {
+		_assertBetween(_rad, -_2PI*0.99, +_2PI*0.99, "radians out of bounds");
+		rad += _rad;
+		if (rad > _2PI)		{rad -= _2PI;}
+		if (rad < 0)		{rad += _2PI;}
+		return *this;
+	}
+
+	/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
+	Heading getInverted() const {
+		Heading out(rad);
+		out += M_PI;
+		return out;
+	}
+
+	float getRAD() const {return rad;}
+
+#undef _2PI
+
+};
+
+namespace Headings {
+	static const Heading RIGHT =	Heading(M_PI*0/2);
+	static const Heading UP =		Heading(M_PI*1/2);
+	static const Heading LEFT =		Heading(M_PI*2/2);
+	static const Heading DOWN =		Heading(M_PI*3/2);
+}
+
+#endif // HEADING_H

geo/Point3.h

@@ -1,6 +1,8 @@
 #ifndef POINT3_H
 #define POINT3_H
 
+#include <KLib/Assertions.h>
+
 /**
  * 3D Point
  */
@@ -26,6 +28,14 @@ struct Point3 {
 
 	Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
 
+	/** read-only array access */
+	float operator [] (const int idx) const {
+		_assertBetween(idx, 0, 2, "index out of bounds");
+		if (0 == idx)	{return x;}
+		if (1 == idx)	{return y;}
+						return z;
+	}
+
 	float length() const {return std::sqrt(x*x + y*y + z*z);}
 
 	float length(const float norm) const {

grid/Grid.h

@@ -15,7 +15,12 @@
 
 /**
  * grid of the given grid-size, storing some value which
- * extends GridPoint
+ * extends GridPoint and GridNode
+ *
+ * Usage:
+ *		for (Node& n : grid) {...}
+ *		for (Node& n2 : grid.neighbors(n)) {...}
+ *
  */
 template <int gridSize_cm, typename T> class Grid {
 
@@ -47,6 +52,12 @@ public:
 	/** no-assign */
 	void operator = (const Grid& o) = delete;
 
+	/** allows for-each iteration over all included nodes */
+	decltype(nodes.begin()) begin() {return nodes.begin();}
+
+	/** allows for-each iteration over all included nodes */
+	decltype(nodes.end()) end() {return nodes.end();}
+
 
 	/**
 	 * add the given element to the grid.
@@ -235,14 +246,14 @@ public:
 		Log::add(name, "running grid cleanup");
 
 		// check every single node
-		for (int i = (int)nodes.size() - 1; i >= 0; --i) {
+		for (size_t i = 0; i < nodes.size(); ++i) {
 
 			// is this node marked as "deleted"? (idx == -1)
 			if (nodes[i]._idx == -1) {
 
 				// remove this node
 				deleteNode(i);
-				++i;
+				--i;
 
 			}
 		}
@@ -261,6 +272,8 @@ private:
 	/** hard-delete the given node */
 	void deleteNode(const int idx) {
 
+		_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
+
 		// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
 
 		// remove him from the node list (reclaim its memory and its index)

grid/GridNeighborIterator.h

@@ -8,10 +8,10 @@ class NeighborIter : std::iterator<std::input_iterator_tag, int> {
 private:
 
 	/** the grid the src-node belongs to */
-	const Grid<gridSize_cm, T>& grid;
+	Grid<gridSize_cm, T>* grid;
 
 	/** index of the source-node within its grid */
-	const int srcNodeIdx;
+	int srcNodeIdx;
 
 	/** index of the current neighbor [0:10] */
 	int nIdx;
@@ -20,7 +20,7 @@ public:
 
 	/** ctor */
 	NeighborIter(const Grid<gridSize_cm, T>& grid, const int srcNodeIdx, const int nIdx) :
-		grid(grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
+		grid((Grid<gridSize_cm, T>*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;}
 
 	/** next neighbor */
 	NeighborIter& operator++() {++nIdx; return *this;}
@@ -35,7 +35,7 @@ public:
 	bool operator!=(const NeighborIter& rhs) {return srcNodeIdx != rhs.srcNodeIdx || nIdx != rhs.nIdx;}
 
 	/** get the neighbor the iterator currently points to */
-	T& operator*() {return (T&) grid.getNeighbor(srcNodeIdx, nIdx);}
+	T& operator*() {return (T&) grid->getNeighbor(srcNodeIdx, nIdx);}
 
 };
 

grid/GridPoint.h

@@ -4,6 +4,8 @@
 #include <cmath>
 #include "../geo/Point3.h"
 
+#include <KLib/Assertions.h>
+
 struct GridPoint {
 
 	/** x-position (in centimeter) */
@@ -42,6 +44,13 @@ struct GridPoint {
 	/** cast to string */
 	operator std::string() const {return "(" + std::to_string(x_cm) + "," + std::to_string(y_cm) + "," + std::to_string(z_cm) + ")";}
 
+	/** read-only array access */
+	float operator [] (const int idx) const {
+		_assertBetween(idx, 0, 2, "index out of bounds");
+		if (0 == idx)	{return x_cm;}
+		if (1 == idx)	{return y_cm;}
+						{return z_cm;}
+	}
 
 };
 

grid/factory/GridFactory.h

@@ -143,8 +143,8 @@ public:
 			int y = n1.y_cm + yDiff * percent;
 
 			// snap (x,y) to the grid???
-			//x = std::round(x / gridSize_cm) * gridSize_cm;
+			x = std::round(x / gridSize_cm) * gridSize_cm;
-			//y = std::round(y / 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/factory/GridImportance.h

@@ -4,12 +4,18 @@
 #include "../Grid.h"
 #include "GridFactory.h"
 #include "../../misc/KNN.h"
+#include "../../misc/KNNArray.h"
 #include "../../math/MiniMat2.h"
 
 #include "../../misc/Debug.h"
+#include "../../nav/dijkstra/Dijkstra.h"
+#include "../../nav/dijkstra/DijkstraPath.h"
 
 #include <KLib/math/distribution/Normal.h>
 
+
+
+
 /**
  * add an importance factor to each node within the grid.
  * the importance is calculated based on several facts:
@@ -35,7 +41,7 @@ public:
 		fac.addInverted(g, z_cm);
 
 		// construct KNN search
-		KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
+		KNN<Grid<gridSize_cm, T>, 3> knn(inv);
 
 		// the number of neighbors to use
 		static constexpr int numNeighbors = 8;
@@ -62,6 +68,45 @@ public:
 
 		}
 
+	}
+
+	/** attach importance-factors to the grid */
+	template <int gridSize_cm, typename T> void addDistanceToTarget(Grid<gridSize_cm, T>& g, Dijkstra<T>& d) {
+
+		//Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));
+
+		for (T& node : g) {
+
+			DijkstraNode<T>* dn = d.getNode(node);
+			if (dn != nullptr) {
+				node.distToTarget = dn->cumWeight / 2000;
+			}
+
+		}
+
+
+	}
+
+	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {
+
+		// routing path
+		DijkstraPath<T> path(end, start);
+
+		// knn search within the path
+		KNN<DijkstraPath<T>, 3> knn(path);
+
+		// update each node from the grid using its distance to the path
+		for (T& n : g) {
+
+			//const int idx = knn.getNearestIndex( {n.x_cm, n.y_cm, n.z_cm} );
+			//T& node = g[idx];
+			const float dist_cm = knn.getNearestDistance( {n.x_cm, n.y_cm, n.z_cm} );
+			const float dist_m = Units::cmToM(dist_cm);
+			n.impPath = 1.0 + K::NormalDistribution::getProbability(0, 1.0, dist_m) * 0.8;
+
+
+		}
+
 
 
 	}
@@ -102,7 +147,7 @@ public:
 		if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}
 
 		// door?
-		if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.2; }
+		if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; }
 
 	}
 

grid/walk/GridWalkHelper.h

@@ -0,0 +1,17 @@
+#ifndef GRIDWALKHELPER_H
+#define GRIDWALKHELPER_H
+
+#include "../../geo/Heading.h"
+
+class GridWalkHelper {
+
+public:
+
+	/** get the heading-change between the two given locations */
+	template <typename T> static Heading getHeading(const T& from, const T& to) {
+		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
+	}
+
+};
+
+#endif // GRIDWALKHELPER_H

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -0,0 +1,160 @@
+#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
+#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+
+/**
+ * perform walks on the grid based on some sort of weighting
+ * and drawing from the weighted elements
+ */
+template <typename T> class GridWalkLightAtTheEndOfTheTunnel {
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+
+	/** per-edge: allowed heading difference */
+	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
+
+
+	/** allows drawing elements according to their probability */
+	DrawList<T&> drawer;
+
+	/** 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;
+
+public:
+
+	/** ctor with the target you want to reach */
+	template <int gridSize_cm, typename Access> GridWalkLightAtTheEndOfTheTunnel(Grid<gridSize_cm, T>& grid, const Access& acc, const T& target) {
+
+		// 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;
+			}
+
+		}
+
+	}
+
+	template <int gridSize_cm> GridWalkState<T> getDestination(Grid<gridSize_cm, T>& grid, GridWalkState<T> start, float distance_m) {
+
+		int retries = 2;
+		GridWalkState<T> res;
+
+		// try to walk the given distance from the start
+		// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
+		do {
+			res = walk(grid, start, distance_m);
+		} while (res.node == nullptr && --retries);
+
+		// still reaching a dead end?
+		// -> try a walk in the opposite direction instead
+		if (res.node == nullptr) {
+			res = walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> keep the start as-is
+		return (res.node == nullptr) ? (start) : (res);
+
+	}
+
+private:
+
+	template <int gridSize_cm> GridWalkState<T> walk(Grid<gridSize_cm, T>& grid, GridWalkState<T> cur, float distRest_m) {
+
+		drawer.reset();;
+
+		// calculate the weight for all possible destinations from "cur"
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// heading when walking from cur to neighbor
+			const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
+
+			// angular difference
+			const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
+
+			// probability for this direction change?
+			double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
+
+			// perfer locations reaching the target
+			const double shortening = cur.node->distToTarget - neighbor.distToTarget;
+			if (shortening > 0) {prob *= 30;} // << importance factor!!
+
+			drawer.add(neighbor, prob);
+
+		}
+
+		GridWalkState<T> next(nullptr, cur.heading);
+
+		// pick a random destination
+		T& nDir = drawer.get();
+		const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
+		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
+		next.heading = hDir;
+		next.heading += headingChangeDist(gen);
+
+		// compare two neighbors according to their implied heading change
+		auto compp = [&] (const T& n1, const T& n2) {
+			Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
+			Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
+			const float d1 = next.heading.getDiffHalfRAD(h1);
+			const float d2 = next.heading.getDiffHalfRAD(h2);
+			// same heading -> prefer nodes nearer to the target. needed for stairs!!!
+			// BAD: leads to straight lines in some palces. see solution B (below)
+			//return (d1 < d2) && (n1.distToTarget < n2.distToTarget);
+
+			// VERY IMPORTANT!
+			// pick the node with the smallest heading change.
+			// if the heading change is the same for two nodes, pick a random one!
+			return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);
+		};
+
+		// pick the neighbor best matching the new heading
+		auto it = grid.neighbors(*cur.node);
+		T& nn = *std::min_element(it.begin(), it.end(), compp);
+		next.node = &nn;
+
+//		// pervent dramatic heading changes. instead: try again
+//		if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
+//			return State(nullptr, 0);
+//		}
+
+		// 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 // GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H

grid/walk/GridWalkState.h

@@ -0,0 +1,21 @@
+#ifndef GRIDWALKSTATE_H
+#define GRIDWALKSTATE_H
+
+#include "../../geo/Heading.h"
+
+template <typename T> struct GridWalkState {
+
+	/** the user-node this state resides at */
+	const T* node;
+
+	/** the current heading */
+	Heading heading;
+
+	/** empty ctor */
+	GridWalkState() : node(nullptr), heading(0) {;}
+
+	/** ctor with user-node and heading */
+	GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;}
+
+};
+#endif // GRIDWALKSTATE_H

grid/walk/GridWalkWeighted.h

@@ -0,0 +1,141 @@
+#ifndef GRIDWALKWEIGHTED_H
+#define GRIDWALKWEIGHTED_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+
+/**
+ * perform walks on the grid based on some sort of weighting
+ * and drawing from the weighted elements
+ */
+template <typename T> class GridWalkWeighted {
+
+public:
+
+	struct State {
+		const T* node;
+		Heading heading;
+
+		State() : node(nullptr), heading(0) {;}
+		State(const T* node, Heading heading) : node(node), heading(heading) {;}
+
+	};
+
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+
+	/** per-edge: allowed heading difference */
+	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
+
+
+	/** allows drawing elements according to their probability */
+	DrawList<T&> drawer;
+
+	/** 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:
+
+	template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
+
+		int retries = 2;
+		State res;
+
+		// try to walk the given distance from the start
+		// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
+		do {
+			res = walk(grid, start, distance_m);
+		} while (res.node == nullptr && --retries);
+
+		// still reaching a dead end?
+		// -> try a walk in the opposite direction instead
+		if (res.node == nullptr) {
+			res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> keep the start as-is
+		return (res.node == nullptr) ? (start) : (res);
+
+	}
+
+private:
+
+	static Heading getHeading(const T& from, const T& to) {
+		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
+	}
+
+	template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
+
+		drawer.reset();;
+
+		// calculate the weight for all possible destinations from "cur"
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// heading when walking from cur to neighbor
+			const Heading potentialHeading = getHeading(*cur.node, neighbor);
+
+			// angular difference
+			const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
+
+			// probability for this direction change?
+			double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
+
+			// perfer locations reaching the target
+			const double shortening = cur.node->distToTarget - neighbor.distToTarget;
+			if (shortening > 0) {prob *= 5;}
+
+			drawer.add(neighbor, prob);
+
+		}
+
+		State next(nullptr, cur.heading);
+
+		// pick a random destination
+		T& nDir = drawer.get();
+		const Heading hDir = getHeading(*cur.node, nDir);
+		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
+		next.heading = hDir;
+		next.heading += headingChangeDist(gen);
+
+		// compare two neighbors according to their implied heading change
+		auto compp = [&] (const T& n1, const T& n2) {
+			Heading h1 = getHeading(*cur.node, n1);
+			Heading h2 = getHeading(*cur.node, n2);
+			const float d1 = next.heading.getDiffHalfRAD(h1);
+			const float d2 = next.heading.getDiffHalfRAD(h2);
+			return d1 < d2;
+		};
+
+		// pick the neighbor best matching the new heading
+		auto it = grid.neighbors(*cur.node);
+		T& nn = *std::min_element(it.begin(), it.end(), compp);
+		next.node = &nn;
+
+//		// pervent dramatic heading changes. instead: try again
+//		if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
+//			return State(nullptr, 0);
+//		}
+
+		// 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 // GRIDWALKWEIGHTED_H

grid/walk/GridWalkWeighted2.h

@@ -0,0 +1,127 @@
+#ifndef GRIDWALKWEIGHTED2_H
+#define GRIDWALKWEIGHTED2_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+
+/**
+ * perform walks on the grid based on some sort of weighting
+ * and drawing from the weighted elements
+ */
+template <typename T> class GridWalkWeighted {
+
+public:
+
+	struct State {
+		const T* node;
+		Heading heading;
+
+		State() : node(nullptr), heading(0) {;}
+		State(const T* node, Heading heading) : node(node), heading(heading) {;}
+
+	};
+
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
+
+	/** per-edge: allowed heading difference */
+	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(20);
+
+
+	/** allows drawing elements according to their probability */
+	DrawList<T&> drawer;
+
+	/** 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:
+
+	template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
+
+		int retries = 2;
+		State res;
+
+		// try to walk the given distance from the start
+		// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
+		do {
+			res = walk(grid, start, distance_m);
+		} while (res.node == nullptr && --retries);
+
+		// still reaching a dead end?
+		// -> try a walk in the opposite direction instead
+		if (res.node == nullptr) {
+			res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> keep the start as-is
+		return (res.node == nullptr) ? (start) : (res);
+
+	}
+
+private:
+
+	static Heading getHeading(const T& from, const T& to) {
+		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
+	}
+
+	template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
+
+		// make the draw-list empty (faster than allocating a new one every time?)
+		drawer.reset();
+
+		// calculate probabilites for the neighboring nodes
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// heading when walking from cur to neighbor
+			Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, neighbor.x_cm, neighbor.y_cm);
+
+			// angular difference
+			const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
+
+			// probability for this change?
+			double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
+
+			// adjust by the neighbors importance factor
+			//prob += std::pow(neighbor.impPath, 2);
+			prob *= neighbor.imp;
+			//prob *= K::NormalDistribution::getProbability(1.4, 0.2, neighbor.imp);
+
+			drawer.add(neighbor, prob);
+
+		}
+
+		// if there is no likely neighbor at all, we reached a dead end
+		// -> start over!
+		if (drawer.getCumProbability() <  0.01) {return State(nullptr, 0);}
+
+		// get the next node
+		T* nn = &drawer.get();
+
+		//Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, nn->x_cm, nn->y_cm);
+		//State next(nn, potentialHeading);
+		State next(&nn, cur.heading);
+		next.heading += headingChangeDist(gen);
+
+		// 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 // GRIDWALKWEIGHTED2_H

grid/walk/TestWalkWeighted3.h

@@ -0,0 +1,139 @@
+#ifndef GRIDWALKWEIGHTED3_H
+#define GRIDWALKWEIGHTED3_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+
+/**
+ * perform walks on the grid based on some sort of weighting
+ * and drawing from the weighted elements
+ */
+template <typename T> class GridWalkWeighted {
+
+public:
+
+	struct State {
+		const T* node;
+		Heading heading;
+
+		State() : node(nullptr), heading(0) {;}
+		State(const T* node, Heading heading) : node(node), heading(heading) {;}
+
+	};
+
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+
+	/** per-edge: allowed heading difference */
+	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
+
+
+	/** allows drawing elements according to their probability */
+	DrawList<T&> drawer;
+
+	/** 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:
+
+	template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
+
+		int retries = 2;
+		State res;
+
+		// try to walk the given distance from the start
+		// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
+		do {
+			res = walk(grid, start, distance_m);
+		} while (res.node == nullptr && --retries);
+
+		// still reaching a dead end?
+		// -> try a walk in the opposite direction instead
+		if (res.node == nullptr) {
+			res = walk(grid, State(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> keep the start as-is
+		return (res.node == nullptr) ? (start) : (res);
+
+	}
+
+private:
+
+	static Heading getHeading(const T& from, const T& to) {
+		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
+	}
+
+	template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
+
+		drawer.reset();;
+
+		// calculate the weight for all possible destinations from "cur"
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// heading when walking from cur to neighbor
+			const Heading potentialHeading = getHeading(*cur.node, neighbor);
+
+			// angular difference
+			const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
+
+			// probability for this change?
+			double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
+
+			prob *= std::pow(neighbor.impPath, 5);
+
+			drawer.add(neighbor, prob);
+
+		}
+
+		State next(nullptr, cur.heading);
+
+		// pick a random destination
+		T& nDir = drawer.get();
+		const Heading hDir = getHeading(*cur.node, nDir);
+		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.95;
+		next.heading = hDir;
+		next.heading += headingChangeDist(gen);
+
+		// compare two neighbors according to their implied heading change
+		auto compp = [&] (const T& n1, const T& n2) {
+			Heading h1 = getHeading(*cur.node, n1);
+			Heading h2 = getHeading(*cur.node, n2);
+			const float d1 = next.heading.getDiffHalfRAD(h1);
+			const float d2 = next.heading.getDiffHalfRAD(h2);
+			return d1 < d2;
+		};
+
+		// pick the neighbor best matching the new heading
+		auto it = grid.neighbors(*cur.node);
+		T& nn = *std::min_element(it.begin(), it.end(), compp);
+		next.node = &nn;
+
+//		// pervent dramatic heading changes. instead: try again
+//		if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
+//			return State(nullptr, 0);
+//		}
+
+		// 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 // GRIDWALKWEIGHTED3_H

main.cpp

@@ -15,7 +15,7 @@ int main(int argc, char** argv) {
 
 #ifdef WITH_TESTS
 	::testing::InitGoogleTest(&argc, argv);
-	::testing::GTEST_FLAG(filter) = "*TestAll*";
+	::testing::GTEST_FLAG(filter) = "*Walk*";
 	return RUN_ALL_TESTS();
 #endif
 

math/DrawList.h

@@ -0,0 +1,82 @@
+#ifndef DRAWLIST_H
+#define DRAWLIST_H
+
+#include <vector>
+#include <random>
+
+/**
+ * add elements of a certain probability
+ * and randomly draw from them
+ */
+template <typename T> class DrawList {
+
+	/** one entry */
+	struct Entry {
+
+		/** the user element */
+		T element;
+
+		/** the cumulative probability up to this element */
+		double cumProbability;
+
+		/** ctor */
+		Entry(T element, const double cumProbability) : element(element), cumProbability(cumProbability) {;}
+
+		/** compare for searches */
+		bool operator < (const double val) const {return cumProbability < val;}
+
+	};
+
+private:
+
+	/** current cumulative probability */
+	double cumProbability;
+
+	/** all contained elements */
+	std::vector<Entry> elements;
+
+	/** random number generator */
+	std::minstd_rand gen;
+
+public:
+
+	/** ctor */
+	DrawList() : cumProbability(0) {
+		;
+	}
+
+	/** reset */
+	void reset() {
+		cumProbability = 0;
+		elements.clear();
+	}
+
+	/** add a new user-element and its probability */
+	void add(T element, const double probability) {
+		cumProbability += probability;
+		elements.push_back(Entry(element, cumProbability));
+	}
+
+	/** get a random element based on its probability */
+	T get() {
+
+		// generate random number between [0:cumProbability]
+		std::uniform_real_distribution<> dist(0, cumProbability);
+
+		// get a random value
+		const double rndVal = dist(gen);
+
+		// binary search for the matching entry O(log(n))
+		const auto tmp = std::lower_bound(elements.begin(), elements.end(), rndVal);
+		return (*tmp).element;
+
+	}
+
+	/** get the current, cumulative probability */
+	double getCumProbability() const {
+		return cumProbability;
+	}
+
+};
+
+#endif // DRAWLIST_H

misc/Debug.h

@@ -4,23 +4,37 @@
 #include <string>
 #include <iostream>
 #include <iomanip>
+#include "Time.h"
 
 class Log {
 
 public:
 
+
+
 	static void add(const char* comp, const std::string what) {
 		addComp(comp);
-		std::cout << what << std::endl;
+		std::cout << what;
+		addTime();
+		std::cout << std::endl;
 	}
 
 	static void add(const std::string& component, const std::string what) {
 		addComp(component.c_str());
-		std::cout << what << std::endl;
+		std::cout << what;
+		addTime();
+		std::cout << std::endl;
 	}
 
 private:
 
+	static void addTime() {
+		static auto last = Time::tick();
+		const auto cur = Time::tick();
+		std::cout << " (+" << Time::diffMS(last, cur) << "ms)";
+		last = cur;
+	}
+
 	static void addComp(const char* component) {
 		std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
 	}

misc/KNN.h

@@ -2,6 +2,7 @@
 #define KNN_H
 
 #include "../lib/nanoflann/nanoflann.hpp"
+#include "Debug.h"
 
 /**
  * helper class to extract k-nearest-neighbors
@@ -9,14 +10,16 @@
  * uses nanoflann
  *
  * usage:
- *	KNN<float, Grid<20, T>, T, 3> knn(theGrid);
+ *		Grid<30, T> theGrid;
- *	float search[] = {0,0,0};
+ *		KNN<Grid<20, T>, 3, float> knn(theGrid);
- *	std::vector<T> elems = knn.get(search, 3);
+ *		std::vector<T> elems = knn.get({0,0,0}, 10);
  */
-template <typename Scalar, typename DataStructure, typename Element, int dim> class KNN {
+template <typename DataStructure, int dim, typename Scalar = float> class KNN {
 
 private:
 
+	static constexpr const char* name = "KNN";
+
 	/** type-definition for the nanoflann KD-Tree used for searching */
 	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<Scalar, DataStructure>, DataStructure, dim> Tree;
 
@@ -33,11 +36,15 @@ public:
 
 	/** ctor */
 	KNN(DataStructure& data) : tree(dim, data, nanoflann::KDTreeSingleIndexAdaptorParams(maxLeafs)), data(data) {
+
+		Log::add(name, "building kd-tree for " + std::to_string(data.kdtree_get_point_count()) +  " elements");
 		tree.buildIndex();
+		Log::add(name, "done");
+
 	}
 
 	/** get the k-nearest-neighbors for the given input point */
-	std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
+	template <typename Element> std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
 
 		// buffer for to-be-fetched neighbors
 		size_t indices[numNeighbors];
@@ -56,6 +63,11 @@ public:
 
 	}
 
+	/** get the k-nearest-neighbors for the given input point */
+	template <typename Element> std::vector<Element> get(std::initializer_list<Scalar> point, const int numNeighbors, const float maxDistSquared = 99999) const {
+		return get(point.begin(), numNeighbors, maxDistSquared);
+	}
+
 	/** get the nearest neighbor and its distance */
 	void getNearest(const Scalar* point, size_t& idx, float& distSquared) {
 
@@ -64,6 +76,30 @@ public:
 
 	}
 
+	/** get the index of the element nearest to the given point */
+	size_t getNearestIndex(const Scalar* point) {
+		size_t idx;
+		float distSquared;
+		tree.knnSearch(point, 1, &idx, &distSquared);
+		return idx;
+	}
+
+	/** get the index of the element nearest to the given point */
+	size_t getNearestIndex(const std::initializer_list<Scalar> lst) {
+		size_t idx;
+		float distSquared;
+		tree.knnSearch(lst.begin(), 1, &idx, &distSquared);
+		return idx;
+	}
+
+	/** get the distance to the element nearest to the given point */
+	float getNearestDistance(const std::initializer_list<Scalar> lst) {
+		size_t idx;
+		float distSquared;
+		tree.knnSearch(lst.begin(), 1, &idx, &distSquared);
+		return std::sqrt(distSquared);
+	}
+
 	void get(const Scalar* point, const int numNeighbors, size_t* indices, float* squaredDist) {
 
 		// find k-nearest-neighbors

misc/KNNArray.h

@@ -0,0 +1,59 @@
+#ifndef KNNARRAY_H
+#define KNNARRAY_H
+
+/**
+ * this wrapper class provides all methods needed for nanoflanns KNN-search.
+ * in order for this wrapper class to work, your data-structure must provide
+ * the following methods:
+ *
+ * PointList:
+ *		size() - return the number of contained points
+ *		operator [] - access points via their index
+ * Point
+ *		operator [] - access each dimension via its index
+ *
+ * example:
+ *		std::vector<Point3> points;
+ *		KNNArray<std::vector<Point3>> arr(points);
+ *		KNN<KNNArray<std::vector<Point3>>, 3> knn(arr);
+ */
+template <typename T> class KNNArray {
+
+private:
+
+	/** the underlying data structure */
+	const T& elem;
+
+public:
+
+	/** ctor with the underlying data structure */
+	KNNArray(const T& elem) : elem(elem) {
+		;
+	}
+
+	/** get the number of elements to search throrugh */
+	inline int kdtree_get_point_count() const {
+		return elem.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 elem[idx][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] - elem[idx_p2][0];
+		const float d1 = p1[1] - elem[idx_p2][1];
+		const float d2 = p1[2] - elem[idx_p2][2];
+		return (d0*d0) + (d1*d1) + (d2*d2);
+	}
+
+};
+
+#endif // KNNARRAY_H

misc/Time.h

@@ -0,0 +1,21 @@
+#ifndef TIME_H
+#define TIME_H
+
+#include <chrono>
+
+class Time {
+
+public:
+
+	static std::chrono::system_clock::time_point tick() {
+		return std::chrono::system_clock::now();
+
+	}
+
+	static int 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();
+	}
+
+};
+
+#endif // TIME_H

nav/dijkstra/Dijkstra.h

@@ -5,9 +5,11 @@
 #include <vector>
 #include <algorithm>
 #include <unordered_set>
+#include <list>
 
 #include "DijkstraStructs.h"
 #include "../../misc/Debug.h"
+#include "../../misc/Time.h"
 
 #include <KLib/Assertions.h>
 
@@ -19,26 +21,11 @@ template <typename T> class Dijkstra {
 	/** all already processed edges */
 	std::unordered_set<DijkstraEdge<T>> usedEdges;
 
-	/** to-be-processed nodes (USE LINKED LIST INSTEAD?!) */
+	/** to-be-processed nodes (NOTE: using std::list here was SLOWER!) */
 	std::vector<DijkstraNode<T>*> toBeProcessedNodes;
 
-
 public:
 
-	/** get (or create) a new node for the given user-node */
-	DijkstraNode<T>* getNode(const T* userNode) {
-		if (nodes.find(userNode) == nodes.end()) {
-			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
-			nodes[userNode] = dn;
-		}
-		return nodes[userNode];
-	}
-
-	/** get the edge (bi-directional) between the two given nodes */
-	DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {
-		return DijkstraEdge<T>(n1, n2);
-	}
-
 	/** get the dijkstra-pendant for the given user-node */
 	DijkstraNode<T>* getNode(const T& userNode) {
 		return nodes[&userNode];
@@ -50,7 +37,10 @@ public:
 		// NOTE: end is currently ignored!
 		// runs until all nodes were evaluated
 
-		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start);
+		// compare two nodes by their distance from the start
+		static auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
+
+		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes");
 
 		// cleanup
 		toBeProcessedNodes.clear();
@@ -71,10 +61,14 @@ public:
 		while(!toBeProcessedNodes.empty()) {
 
 			// get the next to-be-processed node
-			DijkstraNode<T>* dnSrc = toBeProcessedNodes[0];
+			const auto min = std::min_element(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
+			DijkstraNode<T>* dnSrc = *min;
+
+			// stop when end was reached??
+			//if (dnSrc->element == &end) {break;}
 
 			// and remove him from the list
-			toBeProcessedNodes.erase(toBeProcessedNodes.begin());
+			toBeProcessedNodes.erase(min);
 
 			// process each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
@@ -90,7 +84,7 @@ public:
 				DijkstraNode<T>* dnDst = getNode(dst);
 
 				// get-or-create the edge describing the connection
-				DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
+				const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
 
 				// was this edge already processed? -> skip it
 				if (usedEdges.find(edge) != usedEdges.end()) {continue;}
@@ -98,6 +92,8 @@ public:
 				// otherwise: remember it
 				usedEdges.insert(edge);
 
+
+
 				// and add the node for later processing
 				toBeProcessedNodes.push_back(dnDst);
 
@@ -110,18 +106,30 @@ public:
 
 			}
 
-			// sort the nodes by distance-from-start (shortest first)
-			auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
-			std::sort(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
-
 		}
 
-		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
+		// reclaim temporal memory
-
-		// cleanup
 		toBeProcessedNodes.clear();
 		usedEdges.clear();
 
+		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
+
+	}
+
+private:
+
+	/** get (or create) a new node for the given user-node */
+	inline DijkstraNode<T>* getNode(const T* userNode) {
+		if (nodes.find(userNode) == nodes.end()) {
+			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
+			nodes[userNode] = dn;
+		}
+		return nodes[userNode];
+	}
+
+	/** get the edge (bi-directional) between the two given nodes */
+	inline DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) const {
+		return DijkstraEdge<T>(n1, n2);
 	}
 
 };

nav/dijkstra/DijkstraPath.h

@@ -0,0 +1,58 @@
+#ifndef DIJKSTRAPATH_H
+#define DIJKSTRAPATH_H
+
+#include "DijkstraStructs.h"
+
+/**
+ * describes a dijkstra-generated path between end and start.
+ * allows KNN searches for points within this path.
+ *
+ */
+template <typename T> class DijkstraPath {
+
+private:
+
+	/** the constructed path */
+	std::vector<DijkstraNode<T>*> path;
+
+public:
+
+	/** ctor from end- to start-node */
+	DijkstraPath(DijkstraNode<T>* end, DijkstraNode<T>* start) {
+
+		// follow the path from the end to the start
+		DijkstraNode<T>* curNode = end;
+		while (curNode != start) {
+			path.push_back(curNode);
+			curNode = curNode->previous;
+		}
+
+	}
+
+	/** NANOFLANN: number of elements in the path */
+	inline int kdtree_get_point_count() const {
+		return path.size();
+	}
+
+	/** NANOFLANN: use default bbox */
+	template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
+		(void) bb; return false;
+	}
+
+	/** NANOFLANN: get the idx-th elements dim-th dimension */
+	inline float kdtree_get_pt(const size_t idx, const int dim) const {
+		return (*(path[idx]->element))[dim];
+	}
+
+	/** NANOFLANN: get the distance between the given point and the idx-th element */
+	inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
+		const DijkstraNode<T>* n = path[idx_p2];
+		const float d0 = p1[0] - (*(n->element))[0];
+		const float d1 = p1[1] - (*(n->element))[1];
+		const float d2 = p1[2] - (*(n->element))[2];
+		return (d0*d0) + (d1*d1) + (d2*d2);
+	}
+
+};
+
+#endif // DIJKSTRAPATH_H

nav/dijkstra/DijkstraStructs.h

@@ -20,23 +20,20 @@ template <typename T> struct DijkstraNode {
 	float cumWeight;
 
 
-//	/** ctor */
-//	DijkstraNode() : element(nullptr), previous(), cumWeight(INF) {;}
-
 	/** ctor */
 	DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;}
 
 
-	/** equal? (bi-dir) */
+//	/** equal? (bi-dir) */
-	bool operator == (const DijkstraNode<T>& other) {
+//	bool operator == (const DijkstraNode<T>& other) const {
-		return element == other.element;
+//		return element == other.element;
-	}
+//	}
 
 };
 
 /**
- * data structure describing the connection between two nodes
+ * data structure describing the connection between two nodes.
- * only used to track already processed connections!
+ * NOTE: only used to track already processed connections!
  */
 template <typename T> struct DijkstraEdge {
 
@@ -49,7 +46,7 @@ template <typename T> struct DijkstraEdge {
 	/** ctor */
 	DijkstraEdge(const DijkstraNode<T>* src, const DijkstraNode<T>* dst) : src(src), dst(dst) {;}
 
-	/** equal? (bi-dir) */
+	/** equal? (bi-directional! edge direction does NOT matter) */
 	bool operator == (const DijkstraEdge& other) const {
 		return	((dst == other.dst) && (src == other.src)) ||
 				((src == other.dst) && (dst == other.src));
@@ -57,15 +54,7 @@ template <typename T> struct DijkstraEdge {
 
 };
 
-//template <typename T> struct DijkstraEdgeWeighted : public DijkstraEdge<T> {
+/** allows adding DijkstraEdge<T> to hash-maps */
-
-//	/** the edge's weight */
-//	float weight;
-
-//	DijkstraEdgeWeighted(const DijkstraNode<T>* src, const DijkstraNode<T>* dst, const float weight) : DijkstraEdge<T>(src,dst), weight(weight) {;}
-
-//};
-
 namespace std {
 	template <typename T> struct hash<DijkstraEdge<T>>{
 		size_t operator()(const DijkstraEdge<T>& e) const {

tests/data/fp1.svg

@@ -26,17 +26,17 @@
      inkscape:pageopacity="0.0"
      inkscape:pageshadow="2"
      inkscape:zoom="5.6"
-     inkscape:cx="73.937318"
+     inkscape:cx="124.63903"
-     inkscape:cy="144.77356"
+     inkscape:cy="144.84968"
      inkscape:document-units="px"
-     inkscape:current-layer="layer3"
+     inkscape:current-layer="layer1"
      showgrid="true"
      units="px"
      inkscape:object-nodes="true"
-     inkscape:window-width="1920"
+     inkscape:window-width="1600"
-     inkscape:window-height="1021"
+     inkscape:window-height="837"
-     inkscape:window-x="0"
+     inkscape:window-x="-8"
-     inkscape:window-y="0"
+     inkscape:window-y="-8"
      inkscape:window-maximized="1">
     <inkscape:grid
        type="xygrid"
@@ -51,7 +51,7 @@
         <dc:format>image/svg+xml</dc:format>
         <dc:type
            rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
-        <dc:title></dc:title>
+        <dc:title />
       </cc:Work>
     </rdf:RDF>
   </metadata>
@@ -60,8 +60,7 @@
      inkscape:groupmode="layer"
      id="layer1"
      transform="translate(0,-796.36219)"
-     style="display:none"
+     style="display:inline">
-     sodipodi:insensitive="true">
     <path
        style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
        d="m 8,1044.3622 0,-240.00001 240,0 0,168 -128,0 0,72.00001 -112,0"
@@ -151,6 +150,11 @@
        id="path3389"
        inkscape:connector-curvature="0"
        sodipodi:nodetypes="cccc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 160,876.36219 0,24"
+       id="path3381"
+       inkscape:connector-curvature="0" />
   </g>
   <g
      inkscape:groupmode="layer"
@@ -168,8 +172,7 @@
      inkscape:groupmode="layer"
      id="layer2"
      inkscape:label="2"
-     style="display:inline"
+     style="display:none">
-     sodipodi:insensitive="true">
     <path
        style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
        d="m 112,104 40,0 0,-24 -40,0"
@@ -265,5 +268,10 @@
        d="m 72,136 144,0"
        id="path3438"
        inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 120,80 0,24"
+       id="path3379"
+       inkscape:connector-curvature="0" />
   </g>
 </svg>

tests/geo/TestHeading.cpp

@@ -0,0 +1,29 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../geo/Heading.h"
+
+TEST(Heading, diff) {
+
+	// 180 degree turn
+	{
+//		Heading h1(0,0, 0,+1);
+//		Heading h2(0,0, 0,-1);
+//		ASSERT_NEAR(h1.getDiffHalfRAD(h2), M_PI, 0.001);
+//		ASSERT_NEAR(h2.getDiffHalfRAD(h1), M_PI, 0.001);
+	}
+
+	// ~180 degree turn
+	{
+		Heading h1(0,0,  0.00,+1);
+		Heading h2(0,1, -0.01,-1);
+		Heading h3(0,1, +0.01,-1);
+		ASSERT_NEAR(h1.getDiffHalfRAD(h2), M_PI, 0.01);
+		ASSERT_NEAR(h2.getDiffHalfRAD(h1), M_PI, 0.01);
+		ASSERT_NEAR(h1.getDiffHalfRAD(h3), M_PI, 0.01);
+		ASSERT_NEAR(h3.getDiffHalfRAD(h1), M_PI, 0.01);
+	}
+
+}
+
+#endif

tests/grid/Plot.h

@@ -10,14 +10,16 @@
 #include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
 
+#include "../../floorplan/Floor.h"
 
 class GP : public GridNode, public GridPoint {
 public:
 	float imp = 1.0f;
+	float impPath = 1.0f;
+	float distToTarget = 1.0;
 public:
 	GP() : GridNode(), GridPoint() {;}
 	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
-
 };
 
 class Plot {
@@ -27,22 +29,76 @@ public:
 	K::Gnuplot gp;
 	K::GnuplotSplot splot;
 
-	K::GnuplotSplotElementColorPoints points;
+	K::GnuplotSplotElementColorPoints nodes;
 	K::GnuplotSplotElementLines lines;
+	K::GnuplotSplotElementLines floors;
 
-	template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
+	/** ctor */
+	Plot() {
 
 		gp << "set ticslevel 0\n";
 		gp << "set view equal xyz\n";
 		gp << "set cbrange[0.5:1.5]\n";
 		gp << "set palette gray negative\n";
 
+		//gp << "set hidden3d front\n";
+
+		splot.add(&nodes);
+
+		splot.add(&floors);
+		floors.setLineWidth(2);
+		floors.setColorHex("#008800");
+
+	}
+
+	template <int gridSize_cm, typename T> Plot& showGrid(Grid<gridSize_cm, T>& g) {
+		addEdges(g);
+		addNodes(g);
+		return *this;
+	}
+
+	template <int gridSize_cm, typename T> Plot& addEdges(Grid<gridSize_cm, T>& g) {
+
+		// prevent adding edges twice
+		std::set<size_t> done;
+
+		for (GP& n1 : g) {
+			for (const T& n2 : g.neighbors(n1)) {
+				size_t edge = g.getUID(n1) ^ g.getUID(n2);
+				if (done.find(edge) == done.end()) {
+					K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
+					K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
+					lines.addSegment(p1, p2);
+					done.insert(edge);
+				}
+			}
+		}
+
+		return *this;
+
+	}
+
+	template <int gridSize_cm, typename T> Plot& addNodes(Grid<gridSize_cm, T>& g) {
+
+		for (GP& n1 : g) {
+			K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
+			nodes.add(p1, 1.0);
+		}
+
+		return *this;
+
+	}
+
+	template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
+
 		std::set<uint64_t> done;
 
 		int cnt = 0;
 		for (int i = 0; i < g.getNumNodes(); ++i) {
 			const GP& n1 = g[i];
-			points.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), n1.imp);
+			//const float color = std::pow(n1.distToTarget,0.1);
+			const float color = n1.imp * n1.impPath;
+			nodes.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), color);
 
 			for (const T& n2 : g.neighbors(n1)) {
 			//for (int n = 0; n < n1.getNumNeighbors(); ++n) {
@@ -59,15 +115,28 @@ public:
 
 		}
 
-		points.setPointSize(1);
+		nodes.setPointSize(1.0);
-		//splot.add(&lines);
-		splot.add(&points);
-
 
 		return *this;
 
 	}
 
+	/** remove all floors from the plot */
+	Plot& resetFloors() {
+		floors.clear();
+		return *this;
+	}
+
+	/** add the given floor to the plot */
+	Plot& addFloor(Floor& f, const float z_cm) {
+		for (const Line2& l : f.getObstacles()) {
+			K::GnuplotPoint3 p1(l.p1.x, l.p1.y, z_cm+10);
+			K::GnuplotPoint3 p2(l.p2.x, l.p2.y, z_cm+10);
+			floors.addSegment(p1, p2);
+		}
+		return *this;
+	}
+
 	Plot& fire() {
 		gp.draw(splot);
 		gp.flush();

tests/grid/TestAll.cpp

@@ -6,6 +6,7 @@
 #include "../../grid/factory/GridFactory.h"
 #include "../../floorplan/FloorplanFactorySVG.h"
 #include "../../nav/dijkstra/Dijkstra.h"
+#include "../../grid/walk/GridWalkWeighted.h"
 
 #include "Plot.h"
 
@@ -29,38 +30,55 @@ TEST(TestAll, Nav) {
 	} tmp(g);
 
 	GridFactory<20, GP> gf(g);
-	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
 
+	// load floorplan
+	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
 	Floor f1 = fpf.getFloor("1");
 	Floor f2 = fpf.getFloor("2");
 	Stairs s1_2 = fpf.getStairs("1_2");
 
+	// build the grid
 	gf.addFloor(f1, 20);
 	gf.addFloor(f2, 340);
 	gf.addStairs(s1_2, 20, 340);
 	gf.removeIsolated();
 
-
+	// calculate node importance based on the floorplan (walls, ...)
 	GridImportance gi;
-	gi.addImportance(g, 20);
+	//gi.addImportance(g, 20);
-	gi.addImportance(g, 340);
+	//gi.addImportance(g, 340);
 
+	// calculate dijkstra using aforementioned importance
 	Dijkstra<GP> d;
 	const GP& start = g.getNodeFor(GridPoint(500,200,20));
-	//const GP& end = g.getNodeFor(GridPoint(1400,1400,20));
 	const GP& end = g.getNodeFor(GridPoint(1200,200,340));
+	//const GP& end = g.getNodeFor(GridPoint(1300,1300,20));
 	d.build(start, end, tmp);
 
+	// add the path's importance to the grid
+	gi.addImportance(g, d.getNode(start), d.getNode(end));
+
 	// plot path
 	K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
 	DijkstraNode<GP>* dn = d.getNode(end);
 	while (dn->previous != nullptr) {
-		path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm));
+		path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
 		dn = dn->previous;
 	}
 
+//	// walk
+//	GridWalkWeighted<GP> walk;
+//	Log::add("test", "walking");
+//	for (int i = 0; i < 5000; ++i) {
+//		walk.getDestination(g, &start, 1.0, Headings::UP);
+//	}
+//	Log::add("test", "done");
+
 	Plot p;
 	p.build(g);
+	//p.showGrid(g);
+	p.addFloor(f1, 20);
+	p.addFloor(f2, 340);
 	p.splot.add(&path);
 	p.fire();
 

tests/grid/TestWalk.cpp

@@ -0,0 +1,127 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "Plot.h"
+#include "../../grid/factory/GridImportance.h"
+#include "../../grid/factory/GridFactory.h"
+#include "../../floorplan/FloorplanFactorySVG.h"
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "../../grid/walk/GridWalkState.h"
+#include "../../grid/walk/GridWalkWeighted.h"
+#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"
+
+TEST(Walk, plot) {
+
+	Grid<20, GP> g;
+	GridFactory<20, GP> gf(g);
+
+	bool use3D = true;
+
+	// load floorplan
+	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
+	Floor f1 = fpf.getFloor("1");
+	Floor f2 = fpf.getFloor("2");
+	Stairs s1_2 = fpf.getStairs("1_2");
+
+	// build the grid
+	gf.addFloor(f1, 20);
+	if (use3D) {
+		gf.addFloor(f2, 340);
+		gf.addStairs(s1_2, 20, 340);
+	}
+	gf.removeIsolated();
+
+	// calculate node importance based on the floorplan (walls, ...)
+	GridImportance gi;
+	gi.addImportance(g, 20);
+	gi.addImportance(g, 340);
+
+	// dijkstra mapper
+	class TMP {
+		Grid<20, GP>& grid;
+	public:
+		TMP(Grid<20, 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);}
+		float getWeightBetween(const GP& n1, const GP& n2) const {
+			float d = ((Point3)n1 - (Point3)n2).length(2.0);
+			//if (d > 20) {d*= 1.30;}
+			return d / std::pow(n2.imp, 3);
+		}
+	} tmp(g);
+
+
+
+//	// calculate shortest path
+//	Dijkstra<GP> d;
+	const GP* start = &g.getNodeFor(GridPoint(700,200,20));
+	const GP* end;
+	if (use3D) {
+		end = &g.getNodeFor(GridPoint(1200,200,340));
+	} else {
+		end = &g.getNodeFor(GridPoint(1300,1300,20));
+	}
+//	d.build(end, start, tmp);
+
+//	gi.addDistanceToTarget(g, d);
+
+//	// add the path's importance to the grid
+//	gi.addImportance(g, d.getNode(start), d.getNode(end));
+
+//	// plot path
+//	K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
+//	DijkstraNode<GP>* dn = d.getNode(start);
+//	while (dn->previous != nullptr) {
+//		path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
+//		dn = dn->previous;
+//	}
+
+
+
+	// walk
+	GridWalkLightAtTheEndOfTheTunnel<GP> walk(g, tmp, *end);
+
+	std::minstd_rand gen;
+	std::normal_distribution<float> dist(0.3, 0.3);
+
+	K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#880000");
+
+	// setup starting states
+	std::vector<GridWalkState<GP>> states;
+	for (int i = 0; i < 1000; ++i) {
+		states.push_back( GridWalkState<GP>( start, Headings::UP ) );
+	}
+
+	Plot p;
+	//p.build(g);
+	p.addFloor(f1, 20);
+	if (use3D) {
+		p.addFloor(f2, 340);
+	}
+	//p.splot.add(&path);
+	p.splot.add(&pStates);
+	p.nodes.setPointSize(0.2);
+
+	if (!use3D) {
+		p.gp << "set view 0,0\n";
+	}
+	p.gp << "set view equal xy\n";
+
+	// walk random distances
+	for (int i = 0; i < 5000; ++i) {
+		pStates.clear();
+		for (GridWalkState<GP>& state : states) {
+			state = walk.getDestination(g, state, std::abs(dist(gen)));
+			pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10));
+		}
+		p.gp.draw(p.splot);
+		p.gp.flush();
+		usleep(1000*80);
+	}
+
+
+}
+
+#endif

tests/misc/TestKNN.cpp

@@ -0,0 +1,43 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include <vector>
+#include "../../misc/KNN.h"
+#include "../../misc/KNNArray.h"
+#include "../../geo/Point3.h"
+
+/** test knn-search using the array-wrapper */
+TEST(KNN, array) {
+
+	// construct 3D cross
+	std::vector<Point3> points;
+
+	for (int x = -10; x <= 10; ++x) { if (0 == x) {continue;} points.push_back(Point3(x,0,0)); }	// idx 0 - 19
+	for (int y = -10; y <= 10; ++y) { if (0 == y) {continue;} points.push_back(Point3(0,y,0)); }	// idx 20 - 39
+	for (int z = -10; z <= 10; ++z) { if (0 == z) {continue;} points.push_back(Point3(0,0,z)); } // idx 40 - 59
+	points.push_back(Point3(0,0,0)); // idx 60
+
+	// construct KNN search
+	KNNArray<std::vector<Point3>> arr(points);
+	KNN<KNNArray<std::vector<Point3>>, 3> knn(arr);
+
+	// test center
+	ASSERT_EQ(60, knn.getNearestIndex({0,0,0}) );
+
+	// test X min/max
+	ASSERT_EQ(0, knn.getNearestIndex({-20,0,0}) );
+	ASSERT_EQ(19, knn.getNearestIndex({20,0,0}) );
+
+	// test Y min/max
+	ASSERT_EQ(20, knn.getNearestIndex({0,-20,0}) );
+	ASSERT_EQ(39, knn.getNearestIndex({0,20,0}) );
+
+	// test Z min/max
+	ASSERT_EQ(40, knn.getNearestIndex({0,0,-20}) );
+	ASSERT_EQ(59, knn.getNearestIndex({0,0,20}) );
+
+}
+
+
+#endif