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

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