worked on grid-walking

grid/walk/v3/Reachable.h

@@ -0,0 +1,257 @@
+#ifndef INDOOR_GW3_REACHABLE_H
+#define INDOOR_GW3_REACHABLE_H
+
+#include <vector>
+#include <set>
+#include "../../Grid.h"
+
+namespace GW3 {
+
+#define likely(x)       __builtin_expect((x),1)
+#define unlikely(x)     __builtin_expect((x),0)
+
+
+
+/**
+ * get all grid nodes that are reachable within x-edges (depth)
+ */
+template <typename Node> class ReachableByDepthUnsorted {
+
+	struct VisitEntry {
+		const Node* gn;
+		int depth;
+		VisitEntry() {;}
+		VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;}
+	};
+
+	struct Visits {
+		VisitEntry visits[512];// __attribute__((aligned(16)));
+		size_t head = 0;
+		size_t tail = 0;
+		VisitEntry& getNext() {
+			return visits[tail++];
+		}
+		void add(const VisitEntry& e) {
+			visits[head++] = e;
+			assert(head < 512);
+			//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
+		}
+		bool hasMore() const {
+			return head > tail;
+		}
+	};
+
+	const Grid<Node>& grid;
+
+public:
+
+	ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
+		;
+	}
+
+	/** get all nodes reachable from start using maxDepth steps */
+	std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
+
+		std::unordered_set<const Node*> checked;
+
+		// assuming max 8 neighbors per node, we need
+		// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
+		// which is 1 + (1+2+3+4+5)*neighbors
+		// which is 1 + (n*n + n)/2*neighbors
+		// however this seems to be slow?!
+		//const int n = maxDepth + 1;
+		//const int maxEntries = (n * n + n) / 2 * 10 + 1;
+		//const int toAlloc = 4096 / sizeof(VisitEntry);
+		//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
+		//if (maxDepth > 9) {throw Exception("will not fit!");}
+
+		Visits toVisit;
+
+		// directly start with the node itself and all its neighbors
+		checked.insert(&start);
+		for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
+			const int nIdx = start.getNeighborIdx(i);
+			const Node& gnNext = grid[nIdx];
+			checked.insert(&gnNext);
+			toVisit.add(VisitEntry(&gnNext, 1));
+		}
+
+		// check all to-be-visited nodes
+		while ( likely(toVisit.hasMore()) ) {
+
+			const VisitEntry& e = toVisit.getNext();
+
+			if ( likely(e.depth <= maxDepth) ) {
+
+				const Node* gnCur = e.gn;
+				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+					const int nIdx = gnCur->getNeighborIdx(i);
+					const Node& gnNext = grid[nIdx];
+					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+						toVisit.add(VisitEntry(&gnNext, e.depth+1));
+						checked.insert(&gnNext);
+					}
+				}
+
+			}
+
+		}
+
+		return checked;
+
+	}
+
+};
+
+
+/**
+ * get all grid nodes that are reachable within x-edges (depth)
+ * additionally returns the needed walking distance in meter
+ */
+template <typename Node> class ReachableByDepthWithDistanceSorted {
+
+	struct VisitEntry {
+		const Node* gn;
+		int depth;
+		float dist_m;
+		int myIdx;
+		VisitEntry() {;}
+		VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
+			gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
+	};
+
+	struct Visits {
+		VisitEntry visits[1024];// __attribute__((aligned(16)));
+		size_t head = 0;
+		size_t tail = 0;
+		VisitEntry& getNext() {
+			return visits[tail++];
+		}
+		void add(const VisitEntry& e) {
+			visits[head++] = e;
+			assert(head < 1024);
+			//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
+		}
+		bool hasMore() const {
+			return head > tail;
+		}
+		void sort() {
+			const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
+				return e1.dist_m < e2.dist_m;
+			};
+			std::sort(&visits[tail], &visits[head], comp);
+		}
+	};
+
+	const Grid<Node>& grid;
+
+public:
+
+	/** result */
+	struct Entry {
+
+		const Node* node;
+		const float walkDistToStart_m;
+		const int prevIdx;
+
+		Entry(const Node* node, const float dist, const size_t prevIdx) :
+			node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}
+
+		bool hasPrev() const {
+			return prevIdx >= 0;
+		}
+
+	};
+
+	ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
+		;
+	}
+
+	/** get all nodes reachable from start using maxDepth steps */
+	std::vector<Entry> get(const Node& start, const int maxDepth) {
+
+		std::unordered_set<const Node*> checked;
+		std::vector<Entry> res;
+
+		Visits toVisit;
+
+		// directly start with the node itself and all its neighbors
+		checked.insert(&start);
+		res.push_back(Entry(&start, 0, -1));
+		for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
+			const int nIdx = start.getNeighborIdx(i);
+			const Node& gnNext = grid[nIdx];
+			const float dist_m = gnNext.getDistanceInMeter(start);
+			toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size()));
+			res.push_back(Entry(&gnNext, dist_m, 0));
+			checked.insert(&gnNext);
+		}
+		toVisit.sort();
+
+		// check all to-be-visited nodes
+		while ( likely(toVisit.hasMore()) ) {
+
+			const VisitEntry& e = toVisit.getNext();
+
+			if ( likely(e.depth <= maxDepth) ) {
+
+				const Node* gnCur = e.gn;
+
+//				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+//					const int nIdx = gnCur->getNeighborIdx(i);
+//					const Node& gnNext = grid[nIdx];
+//					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+//						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
+//						const float dist_m = e.dist_m + nodeNodeDist_m;
+//						toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+//						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
+//						checked.insert(&gnNext);
+//					}
+//				}
+
+//				const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
+
+				std::vector<VisitEntry> sub;
+
+				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+					const int nIdx = gnCur->getNeighborIdx(i);
+					const Node& gnNext = grid[nIdx];
+					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
+						const float dist_m = e.dist_m + nodeNodeDist_m;
+						//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+						sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
+						checked.insert(&gnNext);
+
+					}
+				}
+
+				// dijkstra.. sort the new nodes by destination to start
+				// only sorting the 8 new nodes seems enough due to the graph's layout
+				const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
+					return e1.dist_m < e2.dist_m;
+				};
+
+				std::sort(sub.begin(), sub.end(), comp);
+
+				for (const VisitEntry& e : sub) {
+					toVisit.add(e);
+				}
+
+			}
+
+			// slower with same result ;)
+			//toVisit.sort();
+
+		}
+
+		return res;
+
+	}
+
+};
+
+}
+
+#endif // REACHABLE_H

grid/walk/v3/ReachableSampler.h

@@ -0,0 +1,81 @@
+#ifndef INDOOR_GW3_REACHABLESAMPLER_H
+#define INDOOR_GW3_REACHABLESAMPLER_H
+
+#include "../../../math/Random.h"
+
+#include "Reachable.h"
+#include "Helper.h"
+
+namespace GW3 {
+
+	template <typename Node> class ReachableSamplerByDepth {
+
+	public:
+
+		using Entry = typename ReachableByDepthWithDistanceSorted<Node>::Entry;
+
+		struct SampleResult {
+			Point3 pos;
+			float walkDistToStart_m;
+			SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;}
+		};
+
+	private:
+
+		const Grid<Node>& grid;
+		const float gridSize_m;
+
+		const std::vector<Entry>& reachableNodes;
+
+		mutable RandomGenerator gen;
+
+		mutable std::uniform_real_distribution<float> dOffset;
+
+
+	public:
+
+		/** ctor */
+		ReachableSamplerByDepth(const Grid<Node>& grid, const std::vector<Entry>& reachableNodes) :
+			grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) {
+			;
+		}
+
+		SampleResult sample() {
+
+			std::uniform_int_distribution<int> dIdx(0, reachableNodes.size() - 1);
+
+			const int idx = dIdx(gen);
+
+			const Entry* e = &reachableNodes[idx];
+			const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]);
+			const Node* nDst = e->node;
+
+			// center of the destination node
+			const Point3 nodeCenter = Helper<Node>::gpToP3(*nDst);
+
+			// random position within destination-node
+			const float ox = dOffset(gen);
+			const float oy = dOffset(gen);
+
+			// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
+			const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
+
+			// calculate end's walking-distance towards the start
+			float distToStart_m;
+			if (ePrev1) {
+				distToStart_m = ePrev1->walkDistToStart_m + (Helper<Node>::gpToP3(*(ePrev1->node)).getDistance(end));
+			} else {
+				distToStart_m = nodeCenter.getDistance(end);
+			}
+
+			// done
+			return SampleResult(end, distToStart_m);
+
+		}
+
+
+	};
+
+}
+
+#endif // REACHABLESAMPLER_H

grid/walk/v3/WalkEvaluator.h

@@ -17,7 +17,7 @@ namespace GW3 {
 		/** get the probability for the given walk */
 		//virtual double getProbability(const Walk<Node>& walk) const = 0;
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0;
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0;
 
 	};
 
@@ -31,7 +31,7 @@ namespace GW3 {
 
 		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override {
 
 			(void) params;
 			(void) pStart;
@@ -65,7 +65,7 @@ namespace GW3 {
 			;
 		}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
 
 			(void) params;
 
@@ -95,10 +95,10 @@ namespace GW3 {
 
 		WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
 
 			const float requestedDistance_m = params.distance_m;
-			const float walkedDistance_m = pStart.getDistance(pEnd);
+			const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd);
 			const float diff = walkedDistance_m - requestedDistance_m;
 			return dist.getProbability(diff);
 			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);

grid/walk/v3/Walker.h

@@ -14,6 +14,8 @@
 #include "Helper.h"
 #include "Structs.h"
 #include "WalkEvaluator.h"
+#include "Reachable.h"
+#include "ReachableSampler.h"
 
 namespace GW3 {
 
@@ -117,7 +119,7 @@ namespace GW3 {
 
 			double p = 1;
 			for (const WalkEvaluator<Node>* eval : evals) {
-				const double p1 = eval->getProbability(start, end, params);
+				const double p1 = eval->getProbability(start, end, start.getDistance(end), params);
 				p *= p1;
 			}
 
@@ -156,7 +158,7 @@ namespace GW3 {
 
 		/** ctor */
 		WalkerWeightedRandom(Grid<Node>& grid) :
-			grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f) {
+			grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
 			;
 		}
 
@@ -181,6 +183,7 @@ namespace GW3 {
 			if (!startNode) {throw Exception("start node not found!");}
 
 			const float maxDist = params.distance_m + gridSize_m;
+			const int depth = std::ceil(params.distance_m / gridSize_m) + 1;
 
 			Point3 best; double bestP = 0;
 			//DrawList<Point3> drawer;
@@ -206,15 +209,17 @@ namespace GW3 {
 			int numVisitedNodes = 0;
 
 
-			#define MODE	1
+			#define MODE	3
 
 			#if (MODE == 1)
 
 				double bestNodeP = 0;
 				const Node* bestNode = nullptr;
 
-				while(ri.hasNext()) {
-					const Node* dstNode = &ri.next();
+				ReachableByDepthUnsorted<Node> reach(grid);
+				std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
+
+				for (const Node* dstNode : nodes) {
 					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
 					double p = 1.0;
 					for (const WalkEvaluator<Node>* eval : evals) {
@@ -227,6 +232,20 @@ namespace GW3 {
 					}
 				}
 
+//				while(ri.hasNext()) {
+//					const Node* dstNode = &ri.next();
+//					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
+//					double p = 1.0;
+//					for (const WalkEvaluator<Node>* eval : evals) {
+//						const double p1 = eval->getProbability(start, nodeCenter, params);
+//						p *= p1;
+//					}
+//					if (p > bestNodeP) {
+//						bestNodeP = p;
+//						bestNode = dstNode;
+//					}
+//				}
+
 				for (int i = 0; i < 10; ++i) {
 
 					const Point3 nodeCenter = Helper::gpToP3(*bestNode);
@@ -250,16 +269,21 @@ namespace GW3 {
 
 			#elif (MODE == 2)
 
-				// all reachable nodes
-				while(ri.hasNext()) {
+				ReachableByDepthUnsorted<Node> reach(grid);
+				std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
+
+				// all reachable nodes
+				//while(ri.hasNext()) {
+					//const Node* dstNode = &ri.next();
+
+				for (const Node* dstNode : nodes) {
 
-					const Node* dstNode = &ri.next();
 					++numVisitedNodes;
 
 					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
 
 					// try multiple locations within each reachable node
-					for (int i = 0; i < 1; ++i) {
+					for (int i = 0; i < 3; ++i) {
 
 						// random position within destination-node
 						const float ox = dFinal(rndGen);
@@ -289,6 +313,31 @@ namespace GW3 {
 
 				}
 
+			#elif (MODE == 3)
+
+				using Reachable = ReachableByDepthWithDistanceSorted<Node>;
+				using ReachableNode = typename Reachable::Entry;
+				Reachable reach(grid);
+				std::vector<ReachableNode> reachableNodes = reach.get(*startNode, depth);
+
+				using Sampler = ReachableSamplerByDepth<Node>;
+				using SamplerResult = typename Sampler::SampleResult;
+				Sampler sampler(grid, reachableNodes);
+
+				for (int i = 0; i < 1500; ++i) {
+
+					const SamplerResult sample = sampler.sample();
+
+					double p = 1;
+					for (const WalkEvaluator<Node>* eval : evals) {
+						const double p1 = eval->getProbability(start, sample.pos, sample.walkDistToStart_m*0.94, params);
+						p *= p1;
+					}
+
+					if (p > bestP) {bestP = p; best = sample.pos;}
+
+				}
+
 			#endif
 
 			//std::cout << numVisitedNodes << std::endl;