merged

grid/walk/v3/Helper.h

@@ -84,43 +84,67 @@ namespace GW3 {
 
 	};
 
+	/**
+	 * data-structure to track to-be-visited nodes
+	 * push_back, pop_front
+	 * as pop_front is costly, we omit the pop and use a head-index instead
+	 * memory-consumption vs speed
+	 */
+	struct ToVisit {
+		size_t nextIdx = 0;
+		std::vector<uint32_t> vec;
+		ToVisit() {vec.reserve(256);}
+		void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
+		uint32_t next() {return vec[nextIdx++];}
+		bool empty() const {return nextIdx >= vec.size();}
+	};
+
 	/** get an iterator over all nodes reachable from the given start */
-	template <typename Node> class ReachableIteratorUnsorted {
+	template <typename Node, typename Conditions> class ReachableIteratorUnsorted {
 
 	const Grid<Node>& grid;
 	const Node& start;
 
 	Node* curNode = nullptr;
 	std::unordered_set<uint32_t> visited;
-	std::vector<uint32_t> toVisit;
+	ToVisit toVisit;
+
+	Conditions cond;
 
 	public:
 
-		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
-			toVisit.push_back(start.getIdx());
+		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) {
+			toVisit.add(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& next(const std::function<bool(const Node&)>& skip) {
+		//template <typename Skip> const Node& next(const Skip skip) {
+		const Node& next() {
 
+			// get the next to-be-visited node
+			const uint32_t curIdx = toVisit.next();			//visit from inside out (needed for correct distance)
 			const Node& curNode = grid[curIdx];
 
-			for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+			// mark as "visited"
+			visited.insert(curIdx);
 
-				const int neighborIdx = curNode.getNeighborIdx(i);
+			// get all neighbors
+			const int numNeighbors = curNode.getNumNeighbors();
+			for (int i = 0; i < numNeighbors; ++i) {
+
+				const uint32_t neighborIdx = curNode.getNeighborIdx(i);
 				const Node& neighbor = grid[neighborIdx];
 
+				const bool visit = cond.visit(neighbor) ;
+
 				// not yet reached -> store distance
-				if (!skip(neighbor)) {
+				if (visit) {
 					if (visited.find(neighborIdx) == visited.end()) {
-						toVisit.push_back(neighborIdx);
+						toVisit.add(neighborIdx);
 					}
 				}
 
@@ -150,10 +174,11 @@ namespace GW3 {
 
 	public:
 
-		static GridPoint p3ToGp(const Point3 p) {
-			const Point3 p100 = p*100;
-			return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
-		}
+//		static GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
+//			const Point3 p100 = p*100;
+//			//return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
+//			return GridPoint( grid.snapX(p100.x), grid.snapY(p100.y), grid.snapZ(p100.z) );
+//		}
 
 		static Point3 gpToP3(const GridPoint gp) {
 			return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
@@ -169,10 +194,22 @@ namespace GW3 {
 			return bbox.contains(pt);
 		}
 
+//		/** does one of the given grid-nodes contains the provided point-in-question? */
+//		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+//			for (const Node* n : nodes) {
+//				if (contains(grid, n, pt)) {
+//					return n;
+//				}
+//			}
+//			return nullptr;
+//		}
+
 		/** does one of the given grid-nodes contains the provided point-in-question? */
-		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+		static const Node* contains(const Grid<Node>& grid, const std::vector<const Node*>& nodes, Point2 pt) {
 			for (const Node* n : nodes) {
-				if (contains(grid, n, pt)) {return n;}
+				if (contains(grid, n, pt)) {
+					return n;
+				}
 			}
 			return nullptr;
 		}

grid/walk/v3/Reachable.h

@@ -0,0 +1,340 @@
+#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;
+
+		}
+
+	};
+
+
+	/**
+	 * data-structure to track to-be-visited nodes
+	 * push_back, pop_front
+	 * as pop_front is costly, we omit the pop and use a head-index instead
+	 * memory-consumption vs speed
+	 */
+	struct _ToVisit {
+		size_t nextIdx = 0;
+		std::vector<uint32_t> vec;
+		_ToVisit() {vec.reserve(256);}
+		void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
+		uint32_t next() {return vec[nextIdx++];}
+		bool empty() const {return nextIdx >= vec.size();}
+	};
+
+
+
+	/** get a list of all nodes that are reachable after checking several conditions */
+	template <typename Node, typename Conditions> class ReachableByConditionUnsorted {
+
+
+	public:
+
+
+		static std::vector<const Node*> get(const Grid<Node>& grid, const Node& start, const Conditions cond) {
+
+			//Node* curNode = nullptr;
+			std::unordered_set<uint32_t> scheduled;
+			_ToVisit toVisit;
+			toVisit.add(start.getIdx());
+
+			std::vector<const Node*> res;
+
+			while(!toVisit.empty()) {
+
+				// get the next to-be-visited node
+				const uint32_t curIdx = toVisit.next();			//visit from inside out (needed for correct distance)
+				const Node& curNode = grid[curIdx];
+
+				// process current node
+				res.push_back(&curNode);
+				scheduled.insert(curIdx);
+
+				// get all neighbors
+				const int numNeighbors = curNode.getNumNeighbors();
+				for (int i = 0; i < numNeighbors; ++i) {
+
+					const uint32_t neighborIdx = curNode.getNeighborIdx(i);
+
+					// already visited?
+					if (scheduled.find(neighborIdx) != scheduled.end()) {continue;}
+					scheduled.insert(neighborIdx);
+
+					// matches the used condition?
+					const Node& neighbor = grid[neighborIdx];
+					if (!cond.visit(neighbor)) {continue;}
+
+					// OK!
+					toVisit.add(neighborIdx);
+
+				}
+
+			}
+
+			// done
+			return res;
+
+		}
+
+
+
+		//const Node& next(const std::function<bool(const Node&)>& skip) {
+		//template <typename Skip> const Node& next(const Skip skip) {
+		const Node& next() {
+
+
+
+		}
+
+	};
+
+
+}
+
+#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/Structs.h

@@ -4,18 +4,55 @@
 #include "../../../geo/Heading.h"
 #include "../../../geo/Point3.h"
 #include <vector>
+#include "../../../math/Distributions.h"
+#include "../../../grid/Grid.h"
 
 namespace GW3 {
 
+	struct StepSizes {
+
+		float stepSizeFloor_m = NAN;
+		float stepSizeStair_m = NAN;
+
+		bool isValid() const {
+			return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
+		}
+
+		template <typename Node> float inMeter(const int steps, const Point3 start, const Grid<Node>& grid) const {
+
+			Assert::isTrue(isValid(), "invalid step-sizes given");
+
+			const GridPoint gp = grid.toGridPoint(start);
+			const Node& n = grid.getNodeFor(gp);
+			if (grid.isPlain(n)) {
+				return stepSizeFloor_m * steps;
+			} else {
+				return stepSizeStair_m * steps;
+			}
+
+		}
+
+	};
+
 	/** paremters for the walk */
 	struct WalkParams {
 
+		//Distribution::Normal<float> dDistFloor;
+		//Distribution::Normal<float> dDistStair;
+
 		Point3 start;
-		float distance_m;
+		//float distance_m;
+		int numSteps;
 		Heading heading = Heading(0);
 
 		float lookFurther_m = 1.5;
 
+		StepSizes stepSizes;
+
+		template <typename Node> float getDistanceInMeter(const Grid<Node>& grid) const {
+			return stepSizes.inMeter(numSteps, start, grid);
+		}
+
 	};
 
 	/** result of the random walk */

grid/walk/v3/WalkEvaluator.h

@@ -9,6 +9,29 @@
 
 namespace GW3 {
 
+	/** describes a potential walk, which can be evaluated */
+	struct PotentialWalk {
+
+		/** initial parameters (requested walk) */
+		const WalkParams& params;
+
+		/** walk started here */
+		Point3 pStart;
+
+		/** walk ended here */
+		Point3 pEnd;
+
+		/** usually the euclidean distance start<->end but not necessarily! */
+		float walkDist_m;
+
+		/** ctor */
+		PotentialWalk(const WalkParams& params, const Point3 pStart, const Point3 pEnd, const float walkedDistance_m) :
+			params(params), pStart(pStart), pEnd(pEnd), walkDist_m(walkedDistance_m) {
+			;
+		}
+
+	};
+
 	/** interface for all evaluators that return a probability for a given walk */
 	template <typename Node> class WalkEvaluator {
 
@@ -17,7 +40,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 PotentialWalk& walk) const = 0;
 
 	};
 
@@ -31,15 +54,13 @@ 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 PotentialWalk& walk) const override {
 
-			(void) params;
-			(void) pStart;
-
-			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
+			const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
 			const Node& node = grid->getNodeFor(gp);
 			const double p = node.getWalkImportance();
-			return std::pow(p,10);
+			return p;
+			//return std::pow(p,10);
 
 		}
 
@@ -50,25 +71,32 @@ namespace GW3 {
 	/** evaluate the difference between head(start,end) and the requested heading */
 	template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
 
-		const double sigma;
+		const double sigma_rad;
+		const double kappa;
+		Distribution::VonMises<double> _dist;
+		Distribution::LUT<double> dist;
 
 	public:
 
-		WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
+		// kappa = 1/var = 1/sigma^2
+		// https://en.wikipedia.org/wiki/Von_Mises_distribution
+		WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
+			sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
+			;
+		}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			(void) params;
-
-			if (pStart == pEnd) {
+			if (walk.pStart == walk.pEnd) {
 				std::cout << "warn! start-position == end-positon" << std::endl;
 				return 0;
 			}
 
-			const Heading head(pStart.xy(), pEnd.xy());
-			const float diff = head.getDiffHalfRAD(params.heading);
+			const Heading head(walk.pStart.xy(), walk.pEnd.xy());
+			const float diff = head.getDiffHalfRAD(walk.params.heading);
 			//const float diff = Heading::getSignedDiff(params.heading, head);
-			return Distribution::Normal<double>::getProbability(0, sigma, diff);
+			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
+			return dist.getProbability(diff);
 
 		}
 
@@ -77,16 +105,23 @@ namespace GW3 {
 	/** evaluate the difference between distance(start, end) and the requested distance */
 	template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
 
+		const Grid<Node>& grid;
+
 		const double sigma;
 
+		const Distribution::Normal<double> dist;
+
 	public:
 
-		WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
+		WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			const float walkedDistance_m = pStart.getDistance(pEnd);
-			return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+			const float requestedDistance_m = walk.params.getDistanceInMeter(grid);
+			const float walkedDistance_m = walk.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 {
 
@@ -26,18 +28,22 @@ namespace GW3 {
 	public:
 
 		/** get a new destination for the given start */
-		virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
+		virtual const WalkResult getDestination(const WalkParams& params) const = 0;
 
 	};
 
 	template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
 
-		//Random::RandomGenerator rnd;
-
+		Grid<Node>& grid;
 		std::vector<WalkEvaluator<Node>*> evals;
 
 	public:
 
+		/** ctor */
+		WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
+			;
+		}
+
 		/** make the code a little more readable */
 		using Helper = GW3::Helper<Node>;
 		using Walk = typename GW3::Walk<Node>;
@@ -50,44 +56,76 @@ namespace GW3 {
 		}
 
 		/** perform the walk based on the configured setup */
-		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
+		const WalkResult getDestination(const WalkParams& params) const override {
+
+			Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
+
+			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
 
-			Assert::isNot0(params.distance_m, "walking distance must be > 0");
 
 			static std::mt19937 rndGen;
 
-			const GridPoint gpStart = Helper::p3ToGp(params.start);
+			const GridPoint gpStart = grid.toGridPoint(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
 
+			// calculate a walk's probability
+			auto getP = [&] (const Point3 dst) {
+				double p = 1;
+				const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst));
+				for (const WalkEvaluator<Node>* eval : evals) {
+					const double p1 = eval->getProbability(pWalk);
+					p *= p1;
+				}
+				return p;
+			};
+
 			// include one additional grid-cell (increased distance)
-			const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
-			ReachableSettings set;
-			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);
+			//const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1);
+			const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15);
+
+//			ReachableSettings set;
+//			set.limitDistance = true;
+//			set.dist_m = params.distance_m + secBuffer_m;
+//			set.limitHeading = false;
+//			set.heading = params.heading;
+//			set.maxHeadingDiff_rad = M_PI/2;
+
+//			// get all nodes that satisfy above constraints
+//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+
+			struct Cond {
+				const float maxDist_m;
+				const Node* startNode;
+				Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
+				bool visit(const Node& n) const {
+					return (startNode->getDistanceInMeter(n)) < maxDist_m;
+				}
+			};
+			Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
+			std::vector<const Node*>  reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
 
 			WalkResult res;
 			res.heading = params.heading;
 			res.position = params.start;
 
-
+			// get the to-be-reached destination's position (using start+distance+heading)
 			const Point2 dir = res.heading.asVector();
-			const Point2 dst = params.start.xy() + (dir * params.distance_m);
+			const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid));
 
-			// is dst reachable?
+			// is above destination reachable?
 			const Node* n = Helper::contains(grid, reachableNodes, dst);
+			//const Node* n = ri.contains(dst);
 			if (n) {
 
 				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
-				const GridPoint gp = Helper::p3ToGp(p3);
+				const GridPoint gp = grid.toGridPoint(p3);
+
+
 
 				if (grid.hasNodeFor(gp)) {
 					res.position = p3;					// update position
 					//res.heading;						// keep as-is
-					//res.probability;					// keep as-is
+					res.probability *= getP(p3);		// keep as-is
 					return res;							// done
 
 				} else {
@@ -111,9 +149,11 @@ namespace GW3 {
 			const Point3 start = params.start;
 			const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
 
+			const PotentialWalk pWalk(params, start, end, start.getDistance(end));
+
 			double p = 1;
 			for (const WalkEvaluator<Node>* eval : evals) {
-				const double p1 = eval->getProbability(start, end, params);
+				const double p1 = eval->getProbability(pWalk);
 				p *= p1;
 			}
 
@@ -123,8 +163,31 @@ namespace GW3 {
 			}
 
 			res.heading = Heading(start.xy(), end.xy());
-			res.probability = p;
+			res.probability *= getP(end);
 			res.position = end;
+
+			if (!grid.hasNodeFor(grid.toGridPoint(res.position))) {
+
+				std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl;
+
+				std::cout << "issue:" << res.position.asString() << std::endl;
+
+				for (int i = -80; i <= +80; i+=10) {
+					Point3 pos = res.position + Point3(0,0,i/100.0f);
+					std::cout << pos.asString() <<  " ----- " << res.position.asString() << std::endl;
+					std::cout << (grid.toGridPoint(pos)).asString() << std::endl;
+					std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl;
+					std::cout << std::endl;
+				}
+
+				int i = 0; (void) i;
+
+			}
+
+#if (GRID_MODE == GM_BOX)
+			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
+#endif
+
 			return res;
 
 		}
@@ -142,8 +205,20 @@ namespace GW3 {
 
 		std::vector<WalkEvaluator<Node>*> evals;
 
+		Grid<Node>& grid;
+		const float gridSize_m;
+
+		mutable std::minstd_rand rndGen;
+		mutable std::uniform_real_distribution<float> dFinal;
+
 	public:
 
+		/** ctor */
+		WalkerWeightedRandom(Grid<Node>& grid) :
+			grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
+			;
+		}
+
 		/** make the code a little more readable */
 		using Helper = GW3::Helper<Node>;
 		using Walk = typename GW3::Walk<Node>;
@@ -156,101 +231,189 @@ namespace GW3 {
 		}
 
 		/** perform the walk based on the configured setup */
-		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
+		const WalkResult getDestination(const WalkParams& params) const override {
 
-			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+			const float walkDist_m = params.getDistanceInMeter(grid);
 
-			static std::minstd_rand rndGen;
+			Assert::isNot0(walkDist_m, "walking distance must be > 0");
 
 			const GridPoint gpStart = Helper::p3ToGp(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
+			if (!startNode) {throw Exception("start node not found!");}
 
-//			// include one additional grid-cell (increased distance)
-//			const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
+			const float maxDist = walkDist_m + gridSize_m;
+			const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
 
-//			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);
-
-			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
-			//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
-
-
-			Point3 best;
-			double bestP = 0;
-//			DrawList<Point3> drawer;
+			Point3 best; double bestP = 0;
+			//DrawList<Point3> drawer;
 
 			const Point3 start = params.start;
 
-			// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
-			//for (int i = 0; i < 500; ++i) {
 
-			//	const Node* dstNode = reachableNodes[dNode(rndGen)];
-
-			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
-
-
-
-			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;
+			struct RICond {
+				const GridPoint gpStart;
+				const float maxDist;
+				RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;}
+				bool visit (const Node& n) const {
+					const float dist_m = n.getDistanceInMeter(gpStart);
+					return dist_m < maxDist;
+				}
 			};
+			RICond riCond(gpStart, maxDist);
 
-			//for (const Node* dstNode : reachableNodes) {
-			while(ri.hasNext()) {
+			// iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance)
+			ReachableIteratorUnsorted<Node, RICond> ri(grid, *startNode, riCond);
 
-				const Node* dstNode = &ri.next(skip);
-//				const float dist_m = dstNode->getDistanceInMeter(gpStart);
+			int numVisitedNodes = 0;
 
-//				if (dist_m > maxDist) {
-//					break;
+
+			#define MODE	2
+
+			#if (MODE == 1)
+
+				double bestNodeP = 0;
+				const Node* bestNode = nullptr;
+
+				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);
+					const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
+					double p = 1.0;
+					for (const WalkEvaluator<Node>* eval : evals) {
+						const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
+						p *= p1;
+					}
+					if (p > bestNodeP) {
+						bestNodeP = p;
+						bestNode = dstNode;
+					}
+				}
+
+//				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 < 25; ++i) {
+				for (int i = 0; i < 10; ++i) {
+
+					const Point3 nodeCenter = Helper::gpToP3(*bestNode);
 
 					// random position within destination-node
-					const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
+					const float ox = dFinal(rndGen);
+					const float oy = dFinal(rndGen);
 
-					// 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");
+					// 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);
+					const float walkDist_m = end.getDistance(start);//*1.05;
 
 					double p = 1;
 					for (const WalkEvaluator<Node>* eval : evals) {
-						const double p1 = eval->getProbability(start, end, params);
+						const double p1 = eval->getProbability(start, end, walkDist_m, params);
 						p *= p1;
 					}
 
 					if (p > bestP) {bestP = p; best = end;}
-					//drawer.add(end, p);
 
 				}
 
-			}
+			#elif (MODE == 2)
 
-			//const Point3 end = drawer.get();
+				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) {
+
+					++numVisitedNodes;
+
+					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
+
+					// try multiple locations within each reachable node
+					for (int i = 0; i < 3; ++i) {
+
+						// random position within destination-node
+						const float ox = dFinal(rndGen);
+						const float oy = dFinal(rndGen);
+
+						// 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);
+
+						// 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");
+						const float walkDist_m = end.getDistance(start);//*1.05;
+
+						const PotentialWalk pWalk(params, start, end, walkDist_m);
+
+						double p = 1;
+						for (const WalkEvaluator<Node>* eval : evals) {
+							const double p1 = eval->getProbability(pWalk);
+							p *= p1;
+						}
+
+						if (p > bestP) {bestP = p; best = end;}
+	//					drawer.add(end, p);
+
+					}
+
+				}
+
+			#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;
+
+			//double drawProb = 0; const Point3 end = drawer.get(drawProb);
 			const Point3 end = best;
 			WalkResult res;
 			if (start == end) {
 				res.probability = 0;
 			} else {
 				res.heading = Heading(start.xy(), end.xy());
-				res.probability = bestP;
+				//res.probability = drawProb;		// when using DrawList
+				res.probability = bestP;	// when using bestP
 			}
 			res.position = end;
 			return res;