worked on grid-walker

grid/walk/v3/Helper.h

@@ -6,10 +6,146 @@
 
 #include "Structs.h"
 
+#include <vector>
 #include <set>
 
+#include <KLib/math/random/RandomIterator.h>
+
+//#define SHOW_DEBUG_PLOT
+
+#ifdef SHOW_DEBUG_PLOT
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
+#endif
+
 namespace GW3 {
 
+
+	/** get an iterator over all nodes reachable from the given start */
+	template <typename Node> class ReachableIteratorSorted {
+
+	const Grid<Node>& grid;
+	const Node& start;
+
+	struct Next {
+
+		uint32_t idx;
+		float distToStart;
+
+		Next(uint32_t idx, float distToStart) : idx(idx), distToStart(distToStart) {;}
+
+		/** compare by weight. same weight? : compare by pointer */
+		bool operator < (const Next& o) const	{
+			return (distToStart != o.distToStart) ? (distToStart < o.distToStart) : (idx < o.idx);
+		}
+
+	};
+
+	Node* curNode = nullptr;
+	std::unordered_set<uint32_t> visited;
+	std::set<Next> toVisit;
+
+	public:
+
+		ReachableIteratorSorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
+			toVisit.insert(Next(start.getIdx(),0));
+		}
+
+		bool hasNext() const {
+			return !toVisit.empty();
+		}
+
+		const Node& next() {
+
+			const Next cur = *(toVisit.begin());				// visit from inside out (needed for correct distance)
+			toVisit.erase(toVisit.begin());
+			visited.insert(cur.idx);
+
+			const Node& curNode = grid[cur.idx];
+
+			for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+				const int neighborIdx = curNode.getNeighborIdx(i);
+				const Node& neighbor = grid[neighborIdx];
+				const float newDist = cur.distToStart + curNode.getDistanceInMeter(neighbor);
+
+				// not yet reached -> store distance
+				if (visited.find(neighborIdx) == visited.end()) {
+					toVisit.insert(Next(neighborIdx, newDist));
+				}
+
+			}
+
+			// done
+			return curNode;
+
+		}
+
+	};
+
+	/** get an iterator over all nodes reachable from the given start */
+	template <typename Node> class ReachableIteratorUnsorted {
+
+	const Grid<Node>& grid;
+	const Node& start;
+
+	Node* curNode = nullptr;
+	std::unordered_set<uint32_t> visited;
+	std::vector<uint32_t> toVisit;
+
+	public:
+
+		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
+			toVisit.push_back(start.getIdx());
+		}
+
+		bool hasNext() const {
+			return !toVisit.empty();
+		}
+
+		const Node& next(const std::function<bool(const Node&)>& skip) {
+
+			const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance)
+			toVisit.erase(toVisit.begin());
+			visited.insert(curIdx);
+
+			const Node& curNode = grid[curIdx];
+
+			for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+				const int neighborIdx = curNode.getNeighborIdx(i);
+				const Node& neighbor = grid[neighborIdx];
+
+				// not yet reached -> store distance
+				if (!skip(neighbor)) {
+					if (visited.find(neighborIdx) == visited.end()) {
+						toVisit.push_back(neighborIdx);
+					}
+				}
+
+			}
+
+			// done
+			return curNode;
+
+		}
+
+	};
+
+
+	struct ReachableSettings {
+
+		float dist_m;
+		bool limitDistance = true;
+
+		Heading heading = Heading(0);
+		float maxHeadingDiff_rad;
+		bool limitHeading = false;
+
+	};
+
+
 	template <typename Node> class Helper {
 
 	public:
@@ -74,54 +210,80 @@ namespace GW3 {
 
 			return walks;
 
-		}
+		}		
 
 		/** get all reachable nodes that are within a given range */
-		static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
+		static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const ReachableSettings& set ) {
+
+			//auto tStart = std::chrono::system_clock::now();
 
 			Nodes<Node> res;
 			std::unordered_map<uint32_t, float> distances;
-			std::vector<uint32_t> toVisit;
+			std::vector<uint32_t> toVisit;						// std::queue was only barely faster: 900 vs 880 microseconds
 
 			toVisit.push_back(start->getIdx());
 			distances[start->getIdx()] = 0.0f;
 
+			#ifdef SHOW_DEBUG_PLOT
+				static K::Gnuplot gp;
+				K::GnuplotPlot plot;
+				K::GnuplotPlotElementColorPoints pts1; pts1.setPointType(7); pts1.setPointSize(1);
+				plot.add(&pts1);
+			#endif
+
 			while (!toVisit.empty()) {
 
-				int curIdx = toVisit.front();
+				const int curIdx = toVisit.front();				// visit from inside out (needed for correct distance)
 				toVisit.erase(toVisit.begin());
 
 				const Node& curNode = grid[curIdx];
 				const float curDistance = distances[curIdx];
-				res.push_back(&curNode);				// remember for output
+				res.push_back(&curNode);						// remember for output
 
-				if (curDistance <= dist_m) {
+				#ifdef SHOW_DEBUG_PLOT
-					for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+					pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance);
+					gp.draw(plot);
+					gp.flush();
+				#endif
 
-						const int neighborIdx = curNode.getNeighborIdx(i);
+				for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
-						const Node& neighbor = grid[neighborIdx];
-						const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
-						const float totalDist = curDistance + addDist;
 
-						// this is like in dijkstra. keep the smallest distance to reach a node:
+					const int neighborIdx = curNode.getNeighborIdx(i);
+					const Node& neighbor = grid[neighborIdx];
+					const float addDist = neighbor.getDistanceInMeter(curNode);
+					const float totalDist = curDistance + addDist;
 
-						// not yet reached -> store distance
+					// this is like in dijkstra. keep the smallest distance to reach a node:
-						if (distances.find(neighborIdx) == distances.end()) {
-							toVisit.push_back(neighborIdx);
-							distances[neighborIdx] = totalDist;
 
-						// reached earlier but found shorter way
+					// not yet reached -> store distance
-						} else {
+					if (distances.find(neighborIdx) == distances.end()) {
-							if (distances[neighborIdx] > totalDist) {
+						distances[neighborIdx] = totalDist;
-								distances[neighborIdx] = totalDist;
+
-							}
+						if (set.limitDistance) {
+							if (totalDist > set.dist_m) {continue;}
 						}
+						if (set.limitHeading) {
+							const Heading head(start->x_cm, start->y_cm, neighbor.x_cm, neighbor.y_cm);		// angle between start and current node
+							const float diff = head.getDiffHalfRAD(set.heading);							// difference between above angle and requested angle
+							if (diff > set.maxHeadingDiff_rad) {continue;}									// more than 90 degree difference? -> ignore
+						}
+						toVisit.push_back(neighborIdx);	// needs a visit? (still some distance left)
+
+
+					// reached earlier but found shorter way
+					} else if (distances[neighborIdx] > totalDist) {
+						distances[neighborIdx] = totalDist;
 
 					}
+
 				}
 
 			}
 
+			//auto tEnd = std::chrono::system_clock::now();
+			//auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(tEnd - tStart);
+			//std::cout << elapsed.count() << std::endl;
+
 			return res;
 
 		}

grid/walk/v3/Structs.h

@@ -9,9 +9,13 @@ namespace GW3 {
 
 	/** paremters for the walk */
 	struct WalkParams {
+
 		Point3 start;
 		float distance_m;
 		Heading heading = Heading(0);
+
+		float lookFurther_m = 1.5;
+
 	};
 
 	/** result of the random walk */

grid/walk/v3/WalkEvaluator.h

@@ -50,8 +50,12 @@ namespace GW3 {
 	/** evaluate the difference between head(start,end) and the requested heading */
 	template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
 
+		const double sigma;
+
 	public:
 
+		WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
+
 		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
 
 			(void) params;
@@ -64,7 +68,7 @@ namespace GW3 {
 			const Heading head(pStart.xy(), pEnd.xy());
 			const float diff = head.getDiffHalfRAD(params.heading);
 			//const float diff = Heading::getSignedDiff(params.heading, head);
-			return Distribution::Normal<double>::getProbability(0, 0.04, diff);
+			return Distribution::Normal<double>::getProbability(0, sigma, diff);
 
 		}
 
@@ -73,10 +77,12 @@ namespace GW3 {
 	/** evaluate the difference between distance(start, end) and the requested distance */
 	template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
 
-		const double sigma = 0.1f;
+		const double sigma;
 
 	public:
 
+		WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
+
 		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
 
 			const float walkedDistance_m = pStart.getDistance(pEnd);

grid/walk/v3/Walker.h

@@ -52,24 +52,30 @@ namespace GW3 {
 		/** perform the walk based on the configured setup */
 		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
 
+			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+
 			static std::mt19937 rndGen;
 
 			const GridPoint gpStart = Helper::p3ToGp(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
 
 			// include one additional grid-cell (increased distance)
-			const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
+			const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
-			const float range_m = params.distance_m + secBuffer_m;
+			ReachableSettings set;
-			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
+			set.limitDistance = true;
+			set.dist_m = params.distance_m + secBuffer_m;
+			set.limitHeading = false;
+			set.heading = params.heading;
+			set.maxHeadingDiff_rad = M_PI/2;
+			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
 
 			WalkResult res;
 			res.heading = params.heading;
 			res.position = params.start;
-			float realDist_m = params.distance_m;
 
 
 			const Point2 dir = res.heading.asVector();
-			const Point2 dst = params.start.xy() + (dir * realDist_m);
+			const Point2 dst = params.start.xy() + (dir * params.distance_m);
 
 			// is dst reachable?
 			const Node* n = Helper::contains(grid, reachableNodes, dst);
@@ -80,12 +86,14 @@ namespace GW3 {
 
 				if (grid.hasNodeFor(gp)) {
 					res.position = p3;					// update position
-					res.heading;						// keep as-is
+					//res.heading;						// keep as-is
-					res.probability;					// keep as-is
+					//res.probability;					// keep as-is
 					return res;							// done
+
 				} else {
 					std::cout << "WARN dst not found" << std::endl;
 					//throw "should not happen";
+
 				}
 
 			}
@@ -150,61 +158,99 @@ namespace GW3 {
 		/** perform the walk based on the configured setup */
 		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
 
+			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+
 			static std::minstd_rand rndGen;
 
 			const GridPoint gpStart = Helper::p3ToGp(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
 
-			// include one additional grid-cell (increased distance)
+//			// include one additional grid-cell (increased distance)
-			const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
+//			const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
-			const float range_m = params.distance_m + secBuffer_m;
+
-			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
+//			ReachableSettings set;
+//			set.limitDistance = true;
+//			set.limitHeading = true;
+//			set.dist_m = params.distance_m + secBuffer_m;
+//			set.heading = params.heading;
+//			set.maxHeadingDiff_rad = M_PI/2;
+//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
 
-			// not found -> try random pick among all reachable nodes
 			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
-			std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
+			//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
 
 
 			Point3 best;
 			double bestP = 0;
+//			DrawList<Point3> drawer;
 
 			const Point3 start = params.start;
 
-			// try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles")
+			// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
-			for (int i = 0; i < 25; ++i) {
+			//for (int i = 0; i < 500; ++i) {
 
-				const Node* dstNode = reachableNodes[dNode(rndGen)];
+			//	const Node* dstNode = reachableNodes[dNode(rndGen)];
 
-				// random position within destination-node
+			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
-				std::uniform_real_distribution<float> dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f);
-				const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
 
-				// destination = node-center + offset (within the node's bbox)
-				const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
 
-				// sanity check
-				Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
 
-				if (start == end) {continue;}
+			ReachableIteratorUnsorted<Node> ri(grid, *startNode);
+			const float maxDist = params.distance_m * 1.25 + gridSize_m;
+
+			auto skip = [&] (const Node& n) {
+				const float dist_m = n.getDistanceInMeter(gpStart);
+				return dist_m > maxDist;
+			};
+
+			//for (const Node* dstNode : reachableNodes) {
+			while(ri.hasNext()) {
+
+				const Node* dstNode = &ri.next(skip);
+//				const float dist_m = dstNode->getDistanceInMeter(gpStart);
+
+//				if (dist_m > maxDist) {
+//					break;
+//				}
+
+				for (int i = 0; i < 25; ++i) {
+
+					// random position within destination-node
+					const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
+
+					// destination = node-center + offset (within the node's bbox)
+					const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
+
+					// sanity check
+					if (start == end) {continue;}
+					if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
+						std::cout << "random destination is not part of the grid" << std::endl;
+						continue;
+					}
+
+					//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
+
+					double p = 1;
+					for (const WalkEvaluator<Node>* eval : evals) {
+						const double p1 = eval->getProbability(start, end, params);
+						p *= p1;
+					}
+
+					if (p > bestP) {bestP = p; best = end;}
+					//drawer.add(end, p);
 
-				double p = 1;
-				for (const WalkEvaluator<Node>* eval : evals) {
-					const double p1 = eval->getProbability(start, end, params);
-					p *= p1;
 				}
 
-				if (p > bestP) {bestP = p; best = end;}
-
 			}
 
-			//const Point3 end = lst.get();
+			//const Point3 end = drawer.get();
 			const Point3 end = best;
 			WalkResult res;
 			if (start == end) {
 				res.probability = 0;
 			} else {
 				res.heading = Heading(start.xy(), end.xy());
-				res.probability = bestP;		// 1
+				res.probability = bestP;
 			}
 			res.position = end;
 			return res;