dijkstra is now bleching fast

deleting from the grid is now bleaching fast added new helper methods many new test-cases many new methods for geo classes and others added a bunch of new grid-walkers
2016-01-26 18:13:30 +01:00
parent b503fb9bdc
commit e6329e1db4
26 changed files with 824 additions and 179 deletions
--- a/Defines.h
+++ b/Defines.h
@@ -0,0 +1,7 @@
+#ifndef DEFINES_H
+#define DEFINES_H
+
+#define likely(x) __builtin_expect((x),1)
+#define unlikely(x) __builtin_expect((x),0)
+
+#endif // DEFINES_H
--- a/floorplan/FloorplanFactorySVG.h
+++ b/floorplan/FloorplanFactorySVG.h
@@ -184,11 +184,12 @@ private:
 						Stair s;
 						Line2 dir = scaler.scale(line);
 						s.dir = (dir.p2 - dir.p1);
-						const float d = 9;
-						s.from.add(start.p1 + Point2(-d,-d));
-						s.from.add(start.p1 + Point2(+d,+d));
-						s.from.add(start.p2 + Point2(-d,-d));
-						s.from.add(start.p2 + Point2(+d,+d));
+//						const float d = 50;
+						s.start = start;
+//						s.from.add(start.p1 + Point2(-d,-d));
+//						s.from.add(start.p1 + Point2(+d,+d));
+//						s.from.add(start.p2 + Point2(-d,-d));
+//						s.from.add(start.p2 + Point2(+d,+d));
 						stairs.push_back(s);
 					}
 				}
--- a/floorplan/PlatformStair.h
+++ b/floorplan/PlatformStair.h
@@ -0,0 +1,17 @@
+#ifndef PLATFORMSTAIR_H
+#define PLATFORMSTAIR_H
+
+#include "Stair.h";
+
+
+class PlatformStair {
+
+	Stair s1;
+
+	BBox2 platform;
+
+	Stair s2;
+
+};
+
+#endif // PLATFORMSTAIR_H
--- a/floorplan/Stair.h
+++ b/floorplan/Stair.h
@@ -4,10 +4,11 @@
 #include "../geo/BBox2.h"
 #include "../geo/Point2.h"

+/** a simple stair with a slope from A to B */
 struct Stair {

-	/** bbox with all starting points */
-	BBox2 from;
+	/** starting line of the stair */
+	Line2 start;

 	/** the direction to move all the starting points to */
 	Point2 dir;
--- a/geo/Angle.h
+++ b/geo/Angle.h
@@ -27,13 +27,10 @@ public:
 	 * - as a change-in-direction between [0:PI]
 	 */
 	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");
+		_assertBetween(r1, 0, (float)(2*M_PI), "r1 out of bounds");
+		_assertBetween(r2, 0, (float)(2*M_PI), "r2 out of bounds");
 		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 */
--- a/geo/Heading.h
+++ b/geo/Heading.h
@@ -2,6 +2,7 @@
 #define HEADING_H

 #include <cmath>
+#include <random>

 #include "Angle.h"

@@ -35,11 +36,16 @@ public:
 	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;}
+		if		(rad >= _2PI)	{rad -= _2PI;}
+		else if	(rad < 0)		{rad += _2PI;}
 		return *this;
 	}

+	/** update the angle but ensure we stay within [0:2PI] */
+	Heading operator + (const float _rad) const {
+		return (Heading(*this) += _rad);
+	}
+
 	/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
 	Heading getInverted() const {
 		Heading out(rad);
@@ -49,6 +55,13 @@ public:

 	float getRAD() const {return rad;}

+	/** get a random heading */
+	static Heading rnd() {
+		static std::minstd_rand gen; gen.seed(1234);
+		static std::uniform_real_distribution<float> dist(0, _2PI);
+		return Heading(dist(gen));
+	}
+
 #undef _2PI

 };
--- a/geo/Line2.h
+++ b/geo/Line2.h
@@ -26,23 +26,23 @@ public:

 	bool getSegmentIntersection(const Line2& other) const {

-		const double bx = p2.x - p1.x;
-		const double by = p2.y - p1.y;
+		const float bx = p2.x - p1.x;
+		const float by = p2.y - p1.y;

-		const double dx = other.p2.x - other.p1.x;
-		const double dy = other.p2.y - other.p1.y;
+		const float dx = other.p2.x - other.p1.x;
+		const float dy = other.p2.y - other.p1.y;

-		const double b_dot_d_perp = bx*dy - by*dx;
+		const float b_dot_d_perp = bx*dy - by*dx;

 		if (b_dot_d_perp == 0) {return false;}

-		const double cx = other.p1.x - p1.x;
-		const double cy = other.p1.y - p1.y;
+		const float cx = other.p1.x - p1.x;
+		const float cy = other.p1.y - p1.y;

-		const double t = (cx * dy - cy * dx) / b_dot_d_perp;
+		const float t = (cx * dy - cy * dx) / b_dot_d_perp;
 		if(t < 0 || t > 1) {return false;}

-		const double u = (cx * by - cy * bx) / b_dot_d_perp;
+		const float u = (cx * by - cy * bx) / b_dot_d_perp;
 		if(u < 0 || u > 1) {return false;}

 		return true;
--- a/geo/Point3.h
+++ b/geo/Point3.h
@@ -2,6 +2,7 @@
 #define POINT3_H

 #include <KLib/Assertions.h>
+#include <cmath>

 /**
 * 3D Point
@@ -28,6 +29,12 @@ struct Point3 {

 	Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}

+	Point3& operator += (const Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;}
+
+	Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;}
+
+	bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;}
+
 	/** read-only array access */
 	float operator [] (const int idx) const {
 		_assertBetween(idx, 0, 2, "index out of bounds");
@@ -36,6 +43,14 @@ struct Point3 {
 						return z;
 	}

+	/** get the distance between this point and the other one */
+	float getDistance(const Point3& o) const {
+		const float dx = x - o.x;
+		const float dy = y - o.y;
+		const float dz = z - o.z;
+		return std::sqrt(dx*dx + dy*dy + dz*dz);
+	}
+
 	float length() const {return std::sqrt(x*x + y*y + z*z);}

 	float length(const float norm) const {
--- a/grid/Grid.h
+++ b/grid/Grid.h
@@ -250,61 +250,123 @@ public:

 	}

-	/**
-	 * remove all nodes, marked for deletion.
-	 * BEWARE: this will invalidate all indices used externally!
-	 */
 	void cleanup() {

-		Log::add(name, "running grid cleanup");
+		Log::add(name, "running grid cleanup", false);
+		Log::tick();

-		// check every single node
+		// generate a look-up-table for oldIndex (before deletion) -> newIndex (after deletion)
+		std::vector<int> oldToNew; oldToNew.resize(nodes.size());
+		int newIdx = 0;
+		for (size_t oldIdx = 0; oldIdx < nodes.size(); ++oldIdx) {
+			if (nodes[oldIdx].getIdx() != -1) {
+				oldToNew[oldIdx] = newIdx;
+				++newIdx;
+			}
+		}
+
+		// adjust all indices from the old to the new mapping
 		for (size_t i = 0; i < nodes.size(); ++i) {

-			// is this node marked as "deleted"? (idx == -1)
-			if (nodes[i]._idx == -1) {
+			// skip the nodes actually marked for deletion
+			if (nodes[i]._idx == -1) {continue;}

-				// remove this node
-				deleteNode(i);
-				--i;
+			// adjust the node's index
+			nodes[i]._idx = oldToNew[nodes[i]._idx];

-			}
+			// adjust its neighbor's indices
+			for (int j = 0; j < nodes[i]._numNeighbors; ++j) {
+				nodes[i]._neighbors[j] = oldToNew[nodes[i]._neighbors[j]];
 			}

-		// rebuild hashes
-		Log::add(name, "rebuilding UID hashes");
+		}
+
+		// MUCH(!!!) faster than deleting nodes from the existing node-vector
+		// is to build a new one and swap those two
+		std::vector<T> newNodes;
+		for (size_t i = 0; i < nodes.size(); ++i) {
+			if (nodes[i]._idx != -1) {newNodes.push_back(nodes[i]);}
+		}
+		std::swap(nodes, newNodes);
+
+		Log::tock();
+
+		rebuildHashes();
+
+	}
+
+	/** rebuild the UID-hash-list */
+	void rebuildHashes() {
+
+		Log::add(name, "rebuilding UID hashes", false);
+		Log::tick();
+
 		hashes.clear();
 		for (size_t idx = 0; idx < nodes.size(); ++idx) {
 			hashes[getUID(nodes[idx])] = idx;
 		}

+		Log::tock();
+
 	}

+//	/**
+//	 * remove all nodes, marked for deletion.
+//	 * BEWARE: this will invalidate all indices used externally!
+//	 */
+//	void cleanupOld() {
+
+//		Log::add(name, "running grid cleanup");
+
+//		// check every single node
+//		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;
+
+//			}
+//		}
+
+//		// rebuild hashes
+//		Log::add(name, "rebuilding UID hashes", false);
+//		Log::tick();
+//		hashes.clear();
+//		for (size_t idx = 0; idx < nodes.size(); ++idx) {
+//			hashes[getUID(nodes[idx])] = idx;
+//		}
+//		Log::tock();
+
+//	}
+
 private:

-	/** hard-delete the given node */
-	void deleteNode(const int idx) {
+//	/** hard-delete the given node */
+//	void deleteNode(const int idx) {

-		_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
+//		_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");

-		// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
+//		// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT

-		// remove him from the node list (reclaim its memory and its index)
-		nodes.erase(nodes.begin()+idx);
+//		// remove him from the node list (reclaim its memory and its index)
+//		nodes.erase(nodes.begin()+idx);

-		// decrement the index for all of the following nodes and adjust neighbor references
-		for (size_t i = 0; i < nodes.size(); ++i) {
+//		// decrement the index for all of the following nodes and adjust neighbor references
+//		for (size_t i = 0; i < nodes.size(); ++i) {

-			// decrement the higher indices (reclaim the free one)
-			if (nodes[i]._idx >= idx) { --nodes[i]._idx;}
+//			// decrement the higher indices (reclaim the free one)
+//			if (nodes[i]._idx >= idx) { --nodes[i]._idx;}

-			// adjust the neighbor references (decrement by one)
-			for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
-				if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
-			}
+//			// adjust the neighbor references (decrement by one)
+//			for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
+//				if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
+//			}

-		}
-	}
+//		}
+//	}

 public:

--- a/grid/factory/GridFactory.h
+++ b/grid/factory/GridFactory.h
@@ -42,7 +42,7 @@ public:
 			for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {

 				// check intersection with the floorplan
-				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
+				const GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
 				if (intersects(bbox, floor)) {continue;}

 				// add to the grid
@@ -101,20 +101,19 @@ public:

 	void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {

-		Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm));
+		Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm), false);
+		Log::tick();

 		for (const Stair& s : stairs) {

-			for (int i = 0; i < grid.getNumNodes(); ++i) {
-
 			// potential starting-point for the stair
-				T& n = (T&) grid[i];
+			for (T& n : grid) {

-				// real starting point for the stair?
-				if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {
+				// node lies on the stair's starting edge?
+				if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {

 					// construct end-point by using the stair's direction
-					const Point3 end = Point3(n.x_cm, n.y_cm, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
+					const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
 					GridPoint gp(end.x, end.y, end.z);

 					// does such and end-point exist within the grap? -> construct stair
@@ -130,6 +129,8 @@ public:

 		}

+		Log::tock();
+
 	}

 	/** build a stair (z-transition) from n1 to n2 */
@@ -147,14 +148,15 @@ public:
 		const int gridSize_cm = grid.getGridSize_cm();

 		// move upards in gridSize steps
-		for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
+		for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) {

 			// calculate the percentage of reached upwards-distance
-			const float percent = z/zDiff;
+			const float percent = _z/zDiff;

 			// adjust (x,y) accordingly (interpolate)
 			int x = n1.x_cm + xDiff * percent;
 			int y = n1.y_cm + yDiff * percent;
+			int z = n1.z_cm + _z;

 			// snap (x,y) to the grid???
 			x = std::round(x / gridSize_cm) * gridSize_cm;
@@ -218,7 +220,7 @@ public:
 			const int idxStart = rand() % grid.getNumNodes();
 			set.clear();
 			Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
-			getConnected(idxStart, set);
+			getConnected(grid[idxStart], set);
 			Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");

 		} while (set.size() < 0.5 * grid.getNumNodes());
@@ -234,30 +236,94 @@ public:

 	}

+	/** remove all nodes not connected to n1 */
+	void removeIsolated(T& n1) {
+
+		// get the connected region around n1
+		Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
+		Log::tick();
+		std::unordered_set<int> set;
+		getConnected(n1, set);
+		Log::tock();
+
+		// remove all other
+		Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
+		Log::tick();
+		for (T& n2 : grid) {
+			if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
+		}
+		Log::tock();
+
+		// clean the grid (physically delete the removed nodes)
+		grid.cleanup();
+
+	}
+
 private:

 	/** recursively get all connected nodes and add them to the set */
-	void getConnected(const int idx, std::unordered_set<int>& set) {
+	void getConnected(T& n1, std::unordered_set<int>& visited) {

-		// get the node behind idx
-		const T& n1 = (T&) grid[idx];
+		std::unordered_set<int> toVisit;
+		toVisit.insert(n1.getIdx());

-		// add him to the current region
-		set.insert(n1.getIdx());
+		// run while there are new nodes to visit
+		while(!toVisit.empty()) {
+
+			// get the next node
+			int nextIdx = *toVisit.begin();
+			toVisit.erase(nextIdx);
+			visited.insert(nextIdx);
+			T& next = grid[nextIdx];

 			// get all his (unprocessed) neighbors and add them to the region
-		for (const T& n2 : grid.neighbors(n1)) {
-			if (set.find(n2.getIdx()) == set.end()) {
-				getConnected(n2.getIdx(), set);
+			for (const T& n2 : grid.neighbors(next)) {
+				if (visited.find(n2.getIdx()) == visited.end()) {
+					toVisit.insert(n2.getIdx());
 				}
 			}

 		}

+	}
+
+//	/** recursively get all connected nodes and add them to the set */
+//	void getConnected(const int idx, std::unordered_set<int>& set) {
+
+//		// get the node behind idx
+//		const T& n1 = (T&) grid[idx];
+
+//		// add him to the current region
+//		set.insert(n1.getIdx());
+
+//		// get all his (unprocessed) neighbors and add them to the region
+//		for (const T& n2 : grid.neighbors(n1)) {
+//			if (set.find(n2.getIdx()) == set.end()) {
+//				getConnected(n2.getIdx(), set);
+//			}
+//		}
+
+//	}
+
+//	/** recursively get all connected nodes and add them to the set */
+//	void getConnected(const T& n1, std::unordered_set<int>& set) {
+
+//		// add him to the current region
+//		set.insert(n1.getIdx());
+
+//		// get all his (unprocessed) neighbors and add them to the region
+//		for (const T& n2 : grid.neighbors(n1)) {
+//			if (set.find(n2.getIdx()) == set.end()) {
+//				getConnected(n2, set);
+//			}
+//		}
+
+//	}
+
 private:

 	/** does the bbox intersect with any of the floor's walls? */
-	bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
+	static inline bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
 		for (const Line2& l : floor.getObstacles()) {
 			if (bbox.intersects(l)) {return true;}
 		}
--- a/grid/factory/GridImportance.h
+++ b/grid/factory/GridImportance.h
@@ -52,6 +52,9 @@ public:
 		// process each node
 		for (T& n1 : g) {

+			// skip nodes on other than the requested floor-level
+			if (n1.z_cm != z_cm) {continue;}
+
 			// get the 10 nearest neighbors and their distance
 			size_t indices[numNeighbors];
 			float squaredDist[numNeighbors];
@@ -64,7 +67,9 @@ public:
 				neighbors.push_back(&inv[indices[i]]);
 			}

-			addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
+			n1.imp = 1.0f;
+
+			n1.imp += getWallImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
 			//addDoor(n1, neighbors);

 			// is the current node a door?
@@ -81,20 +86,16 @@ public:
 		// process each node again
 		for (T& n1 : g) {

-			static K::NormalDistribution favorDoors(0.0, 0.6);
+			static K::NormalDistribution favorDoors(0.0, 1.0);

 			// get the distance to the nearest door
 			const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));

 			// importance for this node (based on the distance from the next door)
-			const float imp = 1.0 + favorDoors.getProbability(dist_m) * 0.35;
-
-			// adjust
-			n1.imp *= imp;
+			n1.imp += favorDoors.getProbability(dist_m) * 0.30;

 		}

-
 	}

 	/** is the given node connected to a staircase? */
@@ -190,10 +191,10 @@ public:
 	}

 	/** get the importance of the given node depending on its nearest wall */
-	template <typename T> void addImportance(T& nSrc, float dist_m) {
+	template <typename T> float getWallImportance(T& nSrc, float dist_m) {

 		// avoid sticking too close to walls (unlikely)
-		static K::NormalDistribution avoidWalls(0.0, 0.3);
+		static K::NormalDistribution avoidWalls(0.0, 0.4);

 		// favour walking near walls (likely)
 		static K::NormalDistribution sticToWalls(0.9, 0.5);
@@ -203,10 +204,9 @@ public:
 		if (dist_m > 2.0) {dist_m = 2.0;}

 		// overall importance
-		nSrc.imp *=  1.0
-				- avoidWalls.getProbability(dist_m) * 0.35		// avoid walls
+		return	- avoidWalls.getProbability(dist_m) * 0.30		// avoid walls
 				+ sticToWalls.getProbability(dist_m) * 0.15		// walk near walls
-				+ farAway.getProbability(dist_m) * 0.20			// walk in the middle
+				+ farAway.getProbability(dist_m) * 0.15			// walk in the middle
 		;


--- a/grid/walk/GridWalkHelper.h
+++ b/grid/walk/GridWalkHelper.h
@@ -12,6 +12,67 @@ public:
 		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
 	}

+	/** get the neighbor of "from" best matching the given heading "h" */
+	template <typename T> static T& getBestNeighbor(Grid<T>& grid, const T& from, const Heading h) {
+
+		auto comp = [&] (const T& n1, const T& n2) {
+			const Heading h1 = getHeading(from, n1);
+			const Heading h2 = getHeading(from, n2);
+			const float d1 = h.getDiffHalfRAD(h1);
+			const float d2 = h.getDiffHalfRAD(h2);
+			//return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2));
+			return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);		// same heading? > random decision
+		};
+
+		auto neighbors = grid.neighbors(from);
+		return *std::min_element(neighbors.begin(), neighbors.end(), comp);
+
+	}
+
+	/**
+	 * try to walk the given distance from the provided node.
+	 * if this fails (algorithm cancel walk e.g. due to detected wall collissions)
+	 *  - try again from the start
+	 * if this also fails several times
+	 *  - try to walk in the opposite direction instead (bounce-back)
+	 * if this also fails
+	 *  - add some randomness and try again
+	 */
+	template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m) {
+
+		_assertTrue(distance_m >= 0, "distance must not be negative!");
+
+		GridWalkState<T> res;
+
+		//again:
+
+		int retries = numRetries;
+
+		// 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 = w.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 = w.walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
+		}
+
+		// still nothing found? -> modify and try again
+		if (res.node == nullptr) {
+//			start.node = &(*grid.neighbors(*start.node).begin());
+//			start.heading += 0.25;
+//			goto again;
+			res.node = &(*grid.neighbors(*start.node).begin());
+			res.heading = start.heading;
+		}
+
+		return res;
+
+	}
+
 };

 #endif // GRIDWALKHELPER_H
--- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
+++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
@@ -18,10 +18,12 @@
 */
 template <typename T> class GridWalkLightAtTheEndOfTheTunnel {

+	friend class GridWalkHelper;
+
 private:

 	/** per-edge: change heading with this sigma */
-	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);

 	/** per-edge: allowed heading difference */
 	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
@@ -62,23 +64,7 @@ public:

 	GridWalkState<T> getDestination(Grid<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);
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);

 	}

@@ -102,7 +88,17 @@ private:

 			// perfer locations reaching the target
 			const double shortening = cur.node->distToTarget - neighbor.distToTarget;
-			if (shortening > 0) {prob *= 30;} // << importance factor!!
+			if (shortening >= 0) {prob *= 5;} // << importance factor!!
+
+//			prob = 0.1;
+//			if		(diff < Angle::degToRad(40)) {prob += 0.2;}
+//			else if	(diff < Angle::degToRad(20)) {prob += 0.5;}
+
+//			if (shortening >= 0) {prob += 0.5;}
+
+			//prob *= std::pow(neighbor.imp, 5);
+
+			//prob = (shortening >= 0) ? (2) : (0.75);

 			drawer.add(neighbor, prob);

@@ -114,34 +110,38 @@ private:
 		T& nDir = drawer.get();
 		const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
 		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
+		//next.heading = Heading( cur.heading.getRAD() * 0.2 + hDir.getRAD() * 0.8 );
 		next.heading = hDir;
 		next.heading += headingChangeDist(gen);
+		next.node = &nDir;

-		// 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);
+////		// 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);
-		};
+////			// 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;
+////		// 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);
-//		}
+//		next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
+
+////		// pervent dramatic heading changes. instead: try again
+////		if (cur.heading.getDiffHalfRAD(GridWalkHelper::getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
+////			return GridWalkState<T>(nullptr, 0);
+////		}

 		// get the distance up to this neighbor
 		distRest_m -= next.node->getDistanceInMeter(*cur.node);
--- a/grid/walk/GridWalkPushForward.h
+++ b/grid/walk/GridWalkPushForward.h
@@ -0,0 +1,120 @@
+#ifndef GRIDWALKPUSHFORWARD_H
+#define GRIDWALKPUSHFORWARD_H
+
+/**
+ * todo
+ */
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/distribution/Exponential.h>
+
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+
+/**
+ * keeps something like an "average position within the last X steps"
+ * and tries to move away from this point as fast as possible
+ *
+ */
+template <typename T> class GridWalkPushForward {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
+
+	static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);
+
+	/** 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:
+
+	/** ctor */
+	GridWalkPushForward() {
+		;
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
+
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
+
+	}
+
+private:
+
+	// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
+	DrawList<T*> drawer;
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
+
+		drawer.reset();
+
+		// weight all neighbors based on this heading
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// get the heading between the current node and its neighbor
+			const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
+
+			// calculate the difference from the requested heading
+			const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
+
+			// weight this change
+			const float prob1 = K::NormalDistribution::getProbability(0, HEADING_ALLOWED_SIGMA, diffRad);
+
+
+			// distance from average? and previous distance from average
+			const float distToAvg = Point3(neighbor.x_cm, neighbor.y_cm, neighbor.z_cm).getDistance(cur.avg);
+			const float prevDistToAvg = Point3(cur.node->x_cm, cur.node->y_cm, cur.node->z_cm).getDistance(cur.avg);
+			const float increase = distToAvg - prevDistToAvg;
+
+			// the distance from the average MUST increase
+			const float prob2 = (increase > 0) ? (1) : (0.1);
+
+			// add floorplan importance information
+			const float prob3 = std::pow(neighbor.imp, 1);
+
+			const float prob = prob1*prob2*prob3;
+
+			// add for drawing
+			drawer.add(&neighbor, prob);
+
+		}
+
+
+		// all neighbors are unlikely? -> start over
+		if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
+
+		GridWalkState<T> next;
+
+		// pick the neighbor best matching this new heading
+		next.node = drawer.get();
+		next.heading = GridWalkHelper::getHeading(*cur.node, *next.node) + headingChangeDist(gen);
+
+		// weighted average.. moves over time
+		next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1);
+
+		// 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 // GRIDWALKPUSHFORWARD_H
--- a/grid/walk/GridWalkRandomHeadingUpdate.h
+++ b/grid/walk/GridWalkRandomHeadingUpdate.h
@@ -0,0 +1,87 @@
+#ifndef GRIDWALKRANDOMHEADINGUPDATE_H
+#define GRIDWALKRANDOMHEADINGUPDATE_H
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include <KLib/math/distribution/Normal.h>
+
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+
+/**
+ * for every walked edge: slightly update (scatter) the current heading
+ * pick the edge (neighbor) best matching the current heading
+ * if this neighbor's heading highly differs from the requested heading: start over
+ *
+ * PROs
+ *	- very simple
+ *
+ * CONs
+ *	- particles are bad at walking out of rooms for small grid sizes as there are too many options
+ *		to stay within the room..
+ *
+ */
+template <typename T> class GridWalkRandomHeadingUpdate {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(4);
+
+	/** 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:
+
+	/** ctor */
+	GridWalkRandomHeadingUpdate() {
+		;
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
+
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
+
+	}
+
+private:
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
+
+		GridWalkState<T> next;
+
+		// get a new random heading
+		next.heading = cur.heading + headingChangeDist(gen);
+
+		// pick the neighbor best matching this new heading
+		next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
+
+		// if the best matching neighbor is far of this requested heading
+		// (e.g. no good neighbor due to walls) cancel the walk. to force a retry
+		const float diff = GridWalkHelper::getHeading(*cur.node, *next.node).getDiffHalfRAD(next.heading);
+		if (diff > Angle::degToRad(45)) {
+			return GridWalkState<T>();
+		}
+
+		// 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 // GRIDWALKRANDOMHEADINGUPDATE_H
--- a/grid/walk/GridWalkRandomHeadingUpdateAdv.h
+++ b/grid/walk/GridWalkRandomHeadingUpdateAdv.h
@@ -0,0 +1,126 @@
+#ifndef GRIDWALKRANDOMHEADINGUPDATEADV_H
+#define GRIDWALKRANDOMHEADINGUPDATEADV_H
+
+
+#include "../../geo/Heading.h"
+#include "../Grid.h"
+
+#include "../../math/DrawList.h"
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/distribution/Exponential.h>
+
+#include "../../nav/dijkstra/Dijkstra.h"
+
+#include "GridWalkState.h"
+#include "GridWalkHelper.h"
+
+/**
+ * for every walked edge: slightly update (scatter) the current heading
+ * pick the edge (neighbor) best matching the current heading
+ * if this neighbor's heading highly differs from the requested heading: start over
+ *
+ * PROs
+ *  - simple
+ *  - fixes the issues of GridWalkRandomHeadingUpdate by incorporating floor information
+ *  - adds additional randomness which should be more stable
+ *
+ */
+template <typename T> class GridWalkRandomHeadingUpdateAdv {
+
+	friend class GridWalkHelper;
+
+private:
+
+	/** per-edge: change heading with this sigma */
+	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
+
+	/** 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:
+
+	/** ctor */
+	GridWalkRandomHeadingUpdateAdv() {
+		;
+	}
+
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
+
+		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
+
+	}
+
+private:
+
+	// https://de.wikipedia.org/wiki/Logistische_Verteilung
+	/** alpha = move the center, beta = slope */
+	const float logisticDist(const float x, const float alpha, const float beta) {
+		return 1 / (1 + std::exp( -((x-alpha)/beta) ) );
+	}
+
+
+	// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
+	DrawList<T*> drawer;
+
+	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
+
+		drawer.reset();
+		GridWalkState<T> next;
+
+		// get a new random heading
+		next.heading = cur.heading + headingChangeDist(gen);
+
+		// weight all neighbors based on this heading
+		for (T& neighbor : grid.neighbors(*cur.node)) {
+
+			// get the heading between the current node and its neighbor
+			const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
+
+			// calculate the difference from the requested heading
+			const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
+
+			// weight this change
+			const float prob1 = K::NormalDistribution::getProbability(0, Angle::degToRad(40), diffRad);
+
+			// add the node's importance factor into the calculation
+			const float prob2 = logisticDist(neighbor.imp, 1.0, 0.05);
+			//const float prob2 = std::pow(neighbor.imp, 10);
+
+			// final importance
+			const float prob = prob1 * prob2;
+
+			// add for drawing
+			drawer.add(&neighbor, prob);
+
+		}
+
+		// all neighbors are unlikely? -> start over
+		if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
+
+		// pick the neighbor best matching this new heading
+		next.node = drawer.get();
+
+//		// if the best matching neighbor is far of this requested heading
+//		// (e.g. no good neighbor due to walls) cancel the walk. to force a retry
+//		const float diff = GridWalkHelper::getHeading(*cur.node, *next.node).getDiffHalfRAD(next.heading);
+//		if (diff > Angle::degToRad(45)) {
+//			return GridWalkState<T>();
+//		}
+
+		// 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 // GRIDWALKRANDOMHEADINGUPDATEADV_H
--- a/grid/walk/GridWalkState.h
+++ b/grid/walk/GridWalkState.h
@@ -2,6 +2,7 @@
 #define GRIDWALKSTATE_H

 #include "../../geo/Heading.h"
+#include "../../geo/Point3.h"

 template <typename T> struct GridWalkState {

@@ -14,6 +15,8 @@ template <typename T> struct GridWalkState {
 	/** empty ctor */
 	GridWalkState() : node(nullptr), heading(0) {;}

+	Point3 avg = Point3(0,0,0);
+
 	/** ctor with user-node and heading */
 	GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;}

--- a/main.cpp
+++ b/main.cpp
@@ -16,7 +16,7 @@ int main(int argc, char** argv) {
 #ifdef WITH_TESTS
 	::testing::InitGoogleTest(&argc, argv);
 	//::testing::GTEST_FLAG(filter) = "*Importance*";
-	::testing::GTEST_FLAG(filter) = "*Length*";
+	//::testing::GTEST_FLAG(filter) = "*Walk*";

 	return RUN_ALL_TESTS();
 #endif
--- a/math/DrawList.h
+++ b/math/DrawList.h
@@ -4,6 +4,8 @@
 #include <vector>
 #include <random>

+#include <KLib/Assertions.h>
+
 /**
 * add elements of a certain probability
 * and randomly draw from them
@@ -68,6 +70,11 @@ public:

 		// binary search for the matching entry O(log(n))
 		const auto tmp = std::lower_bound(elements.begin(), elements.end(), rndVal);
+
+		// sanity check
+		_assertFalse(tmp == elements.end(), "draw() did not find a valid element");
+
+		// done
 		return (*tmp).element;

 	}
--- a/misc/Debug.h
+++ b/misc/Debug.h
@@ -18,13 +18,13 @@ public:
 	static void add(const char* comp, const std::string what, const bool nl = true) {
 		addComp(comp);
 		std::cout << what;
-		if (nl) {std::cout << std::endl;}
+		if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
 	}

 	static void add(const std::string& component, const std::string what, const bool nl = true) {
 		addComp(component.c_str());
 		std::cout << what;
-		if (nl) {std::cout << std::endl;}
+		if (nl) {std::cout << std::endl;} else {std::cout << std::flush;}
 	}


--- a/misc/KNN.h
+++ b/misc/KNN.h
@@ -37,9 +37,10 @@ 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");
+		Log::add(name, "building kd-tree for " + std::to_string(data.kdtree_get_point_count()) +  " elements", false);
+		Log::tick();
 		tree.buildIndex();
-		Log::add(name, "done");
+		Log::tock();

 	}

--- a/nav/dijkstra/Dijkstra.h
+++ b/nav/dijkstra/Dijkstra.h
@@ -6,10 +6,12 @@
 #include <algorithm>
 #include <unordered_set>
 #include <list>
+#include <set>

 #include "DijkstraStructs.h"
 #include "../../misc/Debug.h"
 #include "../../misc/Time.h"
+#include "../../Defines.h"

 #include <KLib/Assertions.h>

@@ -18,12 +20,6 @@ template <typename T> class Dijkstra {
 	/** all allocated nodes for the user-data inputs */
 	std::unordered_map<const T*, DijkstraNode<T>*> nodes;

-	/** all already processed edges */
-	std::unordered_set<DijkstraEdge<T>> usedEdges;
-
-	/** to-be-processed nodes (NOTE: using std::list here was SLOWER!) */
-	std::vector<DijkstraNode<T>*> toBeProcessedNodes;
-
 public:

 	/** get the dijkstra-pendant for the given user-node */
@@ -37,16 +33,18 @@ public:
 		// NOTE: end is currently ignored!
 		// runs until all nodes were evaluated

-		// 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", false);
+		Log::tick();

-		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes");
-
-		// cleanup
-		toBeProcessedNodes.clear();
-		usedEdges.clear();
+		// cleanup previous runs
 		nodes.clear();

+		// sorted list of all to-be-processed nodes
+		ToProcess toBeProcessedNodes;
+
+		// all already processed edges
+		std::unordered_set<decltype(getEdge(nullptr,nullptr))> usedEdges;
+
 		// run from start
 		const T* cur = &start;

@@ -55,36 +53,29 @@ public:
 		dnStart->cumWeight = 0;

 		// add this node to the processing list
-		toBeProcessedNodes.push_back(dnStart);
+		toBeProcessedNodes.add(dnStart);

 		// until we are done
-		while(!toBeProcessedNodes.empty()) {
+		while(unlikely(!toBeProcessedNodes.empty())) {

 			// get the next to-be-processed node
-			const auto min = std::min_element(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
-			DijkstraNode<T>* dnSrc = *min;
+			DijkstraNode<T>* dnSrc = toBeProcessedNodes.pop();

 			// stop when end was reached??
 			//if (dnSrc->element == &end) {break;}

-			// and remove him from the list
-			toBeProcessedNodes.erase(min);
-
 			// process each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {

 				// get the neighbor itself
 				const T* dst = acc.getNeighbor(*dnSrc->element, i);

-				// get the distance-weight to the neighbor
-				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
-				_assertTrue(weight >= 0, "edge-weight must not be negative!");
-
 				// get-or-create a node for the neighbor
 				DijkstraNode<T>* dnDst = getNode(dst);

 				// get-or-create the edge describing the connection
-				const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
+				//const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
+				const auto edge = getEdge(dnSrc, dnDst);

 				// was this edge already processed? -> skip it
 				if (usedEdges.find(edge) != usedEdges.end()) {continue;}
@@ -92,10 +83,13 @@ public:
 				// otherwise: remember it
 				usedEdges.insert(edge);

-
-
 				// and add the node for later processing
-				toBeProcessedNodes.push_back(dnDst);
+				//toBeProcessedNodes.push_back(dnDst);
+				toBeProcessedNodes.add(dnDst);
+
+				// get the distance-weight to the neighbor
+				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
+				_assertTrue(weight >= 0, "edge-weight must not be negative!");

 				// update the weight to the destination?
 				const float potentialWeight = dnSrc->cumWeight + weight;
@@ -108,23 +102,56 @@ public:

 		}

-		// reclaim temporal memory
-		toBeProcessedNodes.clear();
-		usedEdges.clear();
-
+		Log::tock();
 		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");

 	}

 private:

+	/** helper class to sort to-be-processed nodes by their distance from the start */
+	class ToProcess {
+
+		/** sort comparator */
+		struct setComp {
+			bool operator() (const DijkstraNode<T>* dn1, const DijkstraNode<T>* dn2) {
+				return dn1->cumWeight < dn2->cumWeight;
+			}
+		};
+
+		/** sorted list of to-be-processed nodes */
+		std::set<DijkstraNode<T>*, setComp> toBeProcessedNodes;
+
+	public:
+
+		/** add a new to-be-processed node */
+		void add(DijkstraNode<T>* node) {toBeProcessedNodes.insert(node);}
+
+		/** get the next to-be-processed node (smallest distance) */
+		DijkstraNode<T>* pop() {
+			DijkstraNode<T>* next = *toBeProcessedNodes.begin();
+			toBeProcessedNodes.erase(toBeProcessedNodes.begin());
+			return next;
+		}
+
+		/** set empty? */
+		bool empty() const {return toBeProcessedNodes.empty();}
+
+	};
+
+
+
+
 	/** get (or create) a new node for the given user-node */
 	inline DijkstraNode<T>* getNode(const T* userNode) {
-		if (nodes.find(userNode) == nodes.end()) {
+		auto it = nodes.find(userNode);
+		if (unlikely(it == nodes.end())) {
 			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
 			nodes[userNode] = dn;
+			return dn;
+		} else {
+			return it->second;
 		}
-		return nodes[userNode];
 	}

 	/** get the edge (bi-directional) between the two given nodes */
--- a/nav/dijkstra/DijkstraStructs.h
+++ b/nav/dijkstra/DijkstraStructs.h
@@ -52,13 +52,23 @@ template <typename T> struct DijkstraEdge {
 				((src == other.dst) && (dst == other.src));
 	}

+	// std::set was slower than std::unordered_set
+//	bool operator < (const DijkstraEdge& other) const {
+//		return ((size_t)src * (size_t)dst) < ((size_t)other.src * (size_t)other.dst);
+//	}
+
 };

 /** allows adding DijkstraEdge<T> to hash-maps */
 namespace std {
 	template <typename T> struct hash<DijkstraEdge<T>>{
 		size_t operator()(const DijkstraEdge<T>& e) const {
-			return hash<size_t>()( (size_t)e.src^(size_t)e.dst);
+
+			// dunno why but this one provided the fastet results even though
+			// this should lead to the most hash-collissions?!
+			return hash<size_t>()(  std::min((size_t)e.src, (size_t)e.dst)   );
+			//return hash<size_t>()(  (size_t)e.src * (size_t)e.dst   );
+
 		}
 	};
 }
--- a/tests/geo/TestPoint.cpp
+++ b/tests/geo/TestPoint.cpp
@@ -0,0 +1,24 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../geo/Point3.h"
+
+TEST(Point3, math) {
+
+	Point3 p1(1,2,3);
+	p1 += Point3(2,3,4);
+	ASSERT_EQ(p1, Point3(3,5,7));
+
+	Point3 p2 = Point3(-2,-1,-4) + p1;
+	ASSERT_EQ(p2, Point3(1, 4, 3));
+
+	p2 -= Point3(1, 2, 3);
+	ASSERT_EQ(p2, Point3(0,2,0));
+
+	Point3 p3 = Point3(1,2,3)*2;
+	ASSERT_EQ(p3, Point3(2,4,6));
+
+
+}
+
+#endif
--- a/tests/grid/Plot.h
+++ b/tests/grid/Plot.h
@@ -140,7 +140,7 @@ public:
 	Plot& fire() {
 		gp.draw(splot);
 		gp.flush();
-		sleep(1000);
+		//sleep(1000);
 		return *this;
 	}

--- a/tests/grid/TestWalk.cpp
+++ b/tests/grid/TestWalk.cpp
@@ -12,7 +12,7 @@
 #include "../../grid/walk/GridWalkWeighted.h"
 #include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"

-TEST(Walk, plot) {
+TEST(Walk, DISABLED_plot) {

 	Grid<GP> g(20);
 	GridFactory<GP> gf(g);