Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

grid/GridNode.h

@@ -64,6 +64,9 @@ public:
 	/** get the node's index within its grid */
 	int getIdx() const {return _idx;}
 
+	/** get the x-th neighbor's index within its grid */
+	int getNeighborIdx(const int x) const {return _neighbors[x];}
+
 	/** get the number of neighbors for this node */
 	int getNumNeighbors() const {return _numNeighbors;}
 

grid/factory/v2/GridFactory.h

@@ -12,6 +12,7 @@
 #include "Elevators.h"
 
 #include "../../../geo/Units.h"
+#include "../../../geo/Circle2.h"
 #include "../../GridNodeBBox.h"
 #include "../../Grid.h"
 
@@ -583,7 +584,10 @@ private:
 				if (lineObstacle.getSegmentIntersection(lineNodes)) {return true;}
 
 			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
-				throw Exception("should not happen");
+				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
+				Circle2 circ(circle->center, circle->radius);
+				if (circ.intersects(lineNodes)) {return true;}
+				//throw Exception("should not happen");
 
 			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
 				// DOORS ARE NOT AN OBSTACLE

grid/walk/v2/modules/WalkModuleActivityControl.h

@@ -6,7 +6,7 @@
 #include "../../../../geo/Heading.h"
 #include "../../../../math/Distributions.h"
 
-#include "../../../../sensors/pressure/ActivityButterPressure.h"
+#include "../../../../sensors/activity/ActivityButterPressure.h"
 
 
 /**
@@ -74,20 +74,20 @@ public:
 		const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;
 
 		// TODO: general activity enum and activity-detector based on barometer and accelerometer?
-		const ActivityButterPressure::Activity activity = ctrl->activity;
+        const Activity activity = ctrl->activity;
 
 //		const float kappa = 0.75;
 
 		switch (activity) {
-			case ActivityButterPressure::Activity::DOWN:
+            case Activity::WALKING_DOWN:
 			    if (deltaZ_cm < 0)	{return 0.60;}
 				if (deltaZ_cm == 0)	{return 0.25;}
 		                            {return 0.15;}
-			case ActivityButterPressure::Activity::UP:
+            case Activity::WALKING_UP:
 			    if (deltaZ_cm > 0)	{return 0.60;}
 				if (deltaZ_cm == 0)	{return 0.25;}
 		                            {return 0.15;}
-			case ActivityButterPressure::Activity::STAY:
+            case Activity::WALKING:
 			    if (deltaZ_cm == 0) {return 0.60;}
 		                            {return 0.40;}
 //			case ActivityButterPressure::Activity::DOWN:

grid/walk/v2/modules/WalkModuleHeadingVonMises.h

@@ -48,6 +48,7 @@ public:
         if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
 
         // adjust the state's heading using the control-data
+        //TODO: if motionaxis detects landscaping, do not do this!
         state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
 
         //set kappa of mises

grid/walk/v3/Helper.h

@@ -0,0 +1,295 @@
+#ifndef INDOOR_GW3_HELPER_H
+#define INDOOR_GW3_HELPER_H
+
+#include "../../../nav/dijkstra/Dijkstra.h"
+#include "../../Grid.h"
+
+#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:
+
+		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 Point3 gpToP3(const GridPoint gp) {
+			return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
+		}
+
+		/** does the given grid-node contain the provided point-in-question? */
+		static bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
+			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
+			const float d = gridSize_m / 2.0f;
+			const Point2 pMin = n->inMeter().xy() - Point2(d, d);
+			const Point2 pMax = n->inMeter().xy() + Point2(d, d);
+			const BBox2 bbox(pMin, pMax);
+			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;
+		}
+
+		/** get all possible walks from start within a given region */
+		static Walks<Node> getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
+
+			struct Access {
+				Grid<Node>& grid;
+				Access(Grid<Node>& grid) : grid(grid) {;}
+				int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
+				Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
+				float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
+			} acc(grid);
+
+			const float addDist_m = grid.getGridSize_cm() / 100.0f;
+			const float maxDist_m = dist_m * 1.1 + addDist_m;
+			Dijkstra<Node> dijkstra;
+			dijkstra.build(start, nullptr, acc, maxDist_m);
+
+			const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
+
+			Walks<Node> walks;
+
+			for (const auto& it : nodes) {
+				Walk<Node> walk;
+				DijkstraNode<Node>* node = it.second;
+				do {
+					const Node* gridNode = node->element;
+					walk.insert(walk.begin(), gridNode);		// push_front() as dijkstra is inverted
+					node = node->previous;
+				} while (node);
+				walks.push_back(walk);
+			}
+
+			return walks;
+
+		}		
+
+		/** get all reachable nodes that are within a given range */
+		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::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()) {
+
+				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
+
+				#ifdef SHOW_DEBUG_PLOT
+					pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance);
+					gp.draw(plot);
+					gp.flush();
+				#endif
+
+				for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+					const int neighborIdx = curNode.getNeighborIdx(i);
+					const Node& neighbor = grid[neighborIdx];
+					const float addDist = neighbor.getDistanceInMeter(curNode);
+					const float totalDist = curDistance + addDist;
+
+					// this is like in dijkstra. keep the smallest distance to reach a node:
+
+					// not yet reached -> store distance
+					if (distances.find(neighborIdx) == distances.end()) {
+						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;
+
+		}
+
+	};
+
+}
+
+#endif // INDOOR_GW3_HELPER_H

grid/walk/v3/Structs.h

@@ -0,0 +1,47 @@
+#ifndef INDOOR_GW3_STRUCTS_H
+#define INDOOR_GW3_STRUCTS_H
+
+#include "../../../geo/Heading.h"
+#include "../../../geo/Point3.h"
+#include <vector>
+
+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 */
+	struct WalkResult {
+		Point3 position;
+		Heading heading = Heading(0);
+		double probability = 1.0;
+	};
+
+	/** several nodes */
+	template <typename Node> struct Nodes : public std::vector<const Node*> {
+
+	};
+
+	/** one walk along several nodes */
+	template <typename Node> struct Walk : public std::vector<const Node*> {
+
+	};
+
+	/** several walks */
+	template <typename Node> struct Walks : public std::vector<Walk<Node>> {
+
+	};
+
+
+
+}
+
+#endif // INDOOR_GW3_STRUCTS_H

grid/walk/v3/WalkEvaluator.h

@@ -0,0 +1,97 @@
+#ifndef INDOOR_GW3_WALKEVALUATOR_H
+#define INDOOR_GW3_WALKEVALUATOR_H
+
+#include "Structs.h"
+#include "Helper.h"
+
+#include "../../../math/Distributions.h"
+#include "../../../grid/Grid.h"
+
+namespace GW3 {
+
+	/** interface for all evaluators that return a probability for a given walk */
+	template <typename Node> class WalkEvaluator {
+
+	public:
+
+		/** 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;
+
+	};
+
+
+	/** evaluate the grid-node-importance importance(end) */
+	template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {
+
+		Grid<Node>* grid;
+
+	public:
+
+		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			(void) params;
+			(void) pStart;
+
+			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
+			const Node& node = grid->getNodeFor(gp);
+			const double p = node.getWalkImportance();
+			return std::pow(p,10);
+
+		}
+
+	};
+
+
+
+	/** 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;
+
+			if (pStart == 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 float diff = Heading::getSignedDiff(params.heading, head);
+			return Distribution::Normal<double>::getProbability(0, sigma, diff);
+
+		}
+
+	};
+
+	/** evaluate the difference between distance(start, end) and the requested distance */
+	template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
+
+		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);
+			return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+
+		}
+
+	};
+
+}
+
+#endif // INDOOR_GW3_WALKEVALUATOR_H

grid/walk/v3/WalkGenerator.h

@@ -0,0 +1,20 @@
+#ifndef INDOOR_GRIDWALKER3GENERATOR_H
+#define INDOOR_GRIDWALKER3GENERATOR_H
+
+#include "Structs.h"
+
+
+namespace GW3 {
+
+	class GridWalker3Generator {
+
+		virtual Walk getPossibleWalks() = 0;
+
+	};
+
+
+
+
+}
+
+#endif // INDOOR_GRIDWALKER3GENERATOR_H

grid/walk/v3/Walker.h

@@ -0,0 +1,422 @@
+#ifndef INDOOR_GW3_WALKER_H
+#define INDOOR_GW3_WALKER_H
+
+#include "../../../data/Timestamp.h"
+#include "../../../math/DrawList.h"
+#include "../../../math/Distributions.h"
+#include "../../../math/Stats.h"
+#include "../../../geo/Heading.h"
+#include "../../../math/stats/Variance.h"
+#include "../../../geo/BBox2.h"
+
+#include "../../Grid.h"
+
+#include "Helper.h"
+#include "Structs.h"
+#include "WalkEvaluator.h"
+
+namespace GW3 {
+
+	/**
+	 * modular grid-walker that takes various sub-components to determine
+	 * p(e) and thus randomly pick edges
+	 */
+	template <typename Node> class WalkerBase {
+
+	public:
+
+		/** get a new destination for the given start */
+		virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
+
+	};
+
+	template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
+
+		//RandomGenerator rnd;
+
+		std::vector<WalkEvaluator<Node>*> evals;
+
+	public:
+
+		/** make the code a little more readable */
+		using Helper = GW3::Helper<Node>;
+		using Walk = typename GW3::Walk<Node>;
+		using Walks = typename GW3::Walks<Node>;
+		using Nodes = typename GW3::Nodes<Node>;
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(WalkEvaluator<Node>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		/** 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) + (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);
+
+			WalkResult res;
+			res.heading = params.heading;
+			res.position = params.start;
+
+
+			const Point2 dir = res.heading.asVector();
+			const Point2 dst = params.start.xy() + (dir * params.distance_m);
+
+			// is dst reachable?
+			const Node* n = Helper::contains(grid, reachableNodes, dst);
+			if (n) {
+
+				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
+				const GridPoint gp = Helper::p3ToGp(p3);
+
+				if (grid.hasNodeFor(gp)) {
+					res.position = p3;					// update position
+					//res.heading;						// keep as-is
+					//res.probability;					// keep as-is
+					return res;							// done
+
+				} else {
+					std::cout << "WARN dst not found" << std::endl;
+					//throw "should not happen";
+
+				}
+
+			}
+
+			// 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);
+
+			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);
+			const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
+
+			const Point3 start = params.start;
+			const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
+
+			double p = 1;
+			for (const WalkEvaluator<Node>* eval : evals) {
+				const double p1 = eval->getProbability(start, end, params);
+				p *= p1;
+			}
+
+			if (start == end) {
+				res.probability = 0;
+				return res;
+			}
+
+			res.heading = Heading(start.xy(), end.xy());
+			res.probability = p;
+			res.position = end;
+			return res;
+
+		}
+
+	};
+
+
+	/**
+	 * 1) get all reachable nodes from "start" (within a certain distance)
+	 * 2) randomly pick one of em
+	 * 3) scatter positon within the node's square -> end position
+	 * 4) evaluate the probability of walking from start -> end
+	 */
+	template <typename Node> class WalkerWeightedRandom : public WalkerBase<Node> {
+
+		std::vector<WalkEvaluator<Node>*> evals;
+
+	public:
+
+		/** make the code a little more readable */
+		using Helper = GW3::Helper<Node>;
+		using Walk = typename GW3::Walk<Node>;
+		using Walks = typename GW3::Walks<Node>;
+		using Nodes = typename GW3::Nodes<Node>;
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(WalkEvaluator<Node>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		/** 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)
+//			const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
+
+//			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;
+
+			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;
+			};
+
+			//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);
+
+				}
+
+			}
+
+			//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;
+			}
+			res.position = end;
+			return res;
+
+		}
+
+	};
+
+
+
+
+		//const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
+
+
+
+
+	//		Distribution::Normal<float> dDist(1, 0.02);
+	//		Distribution::Normal<float> dHead(0, 0.01);
+
+//			int cnt = 0;
+//			while(true) {
+
+//				const Point2 dir = res.heading.asVector();
+//				const Point2 dst = params.start.xy() + (dir * realDist_m);
+
+//				// is dst reachable?
+//				const Node* n = Helper::contains(grid, reachableNodes, dst);
+//				if (n) {
+
+//					const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
+//					const GridPoint gp = Helper::p3ToGp(p3);
+
+//					if (grid.hasNodeFor(gp)) {
+//						res.position = p3;					// update position
+//						res.heading;						// keep as-is
+//						res.probability;					// keep as-is
+//						return res;							// done
+//					} else {
+//						std::cout << "WARN dst not found" << std::endl;
+//						//throw "should not happen";
+//					}
+
+//				}
+
+//				// reduce probability with every new run
+//				++cnt;
+//				res.probability /= 2;
+
+//				// before trying again, modify distance and angle
+//				//if (1 == 1) {
+//					res.heading = params.heading + dHead.draw() * cnt;
+//					realDist_m = params.distance_m * dDist.draw();// * cnt;
+//				//}
+
+//				// reached max retries?
+//				if (cnt > 8) {
+//					res.position = params.start;	// reset
+//					res.heading = params.heading;	// reset
+//					res.probability = 1e-50;			// unlikely
+//					return res;
+//				}	// did not work out....
+
+//			}
+
+		//	throw "error";
+
+		//}
+
+
+	//	const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
+
+	//		const Node* startNode = grid.getNodePtrFor(start);
+
+	//		Heading desiredHeading = ctrl.heading;
+
+	//		DrawList<Walk> weightedWalks;
+
+	//		const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
+	//		for (const Walk& walk : walks) {
+	//			const double prob = eval(walk, desiredHeading, dist_m);
+	//			weightedWalks.add(walk, prob);
+	//		}
+
+
+	//		Walk res = weightedWalks.get();
+	//		const Node* dst = res.back();
+	//		return dst;
+
+	//	}
+
+
+
+
+
+//		double evalDistance(const Walk& w, const float desiredDist) const {
+//			const Node* nStart = w.front();
+//			const Node* nEnd = w.back();
+//			const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
+//			return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
+//		}
+
+//		double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
+//			const Node* nStart = w.front();
+//			const Node* nEnd = w.back();
+//			if (nStart == nEnd) {
+//				std::cout << "warn! start == end" << std::endl;
+//				return 0;
+//			}
+//			Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
+//			const float diff = head.getDiffHalfRAD(desiredHeading);
+//			return Distribution::Normal<double>::getProbability(0, 0.3, diff);
+//		}
+
+//		double evalHeadingChanges(const Walk& w) const {
+//			Stats::Variance<float> var;
+//			for (int i = 0; i < w.size()-2; ++i) {
+//				const Node* n0 = w[i+0];
+//				const Node* n1 = w[i+1];
+//				const Node* n2 = w[i+2];
+//				Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
+//				Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
+//				const float diff = h1.getDiffHalfRAD(h2);
+//				var.add(diff);
+//			}
+//			const float totalVar = var.get();
+//			return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
+//		}
+
+//		double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
+
+//			return 1.0
+//					* evalHeadingStartEnd(w, desiredHeading)
+//					* evalDistance(w, desiredDistance)
+//					// * evalHeadingChanges(w);
+//			;
+
+//		}
+
+
+
+
+	//	Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
+
+	//		Walk walk;
+
+	//		float dist = 0;
+
+	//		const Node* cur = start;
+	//		while(true) {
+
+	//			walk.push_back(cur);
+	//			if (dist > dist_m) {break;}
+
+	//			const int numNeighbors = cur->getNumNeighbors();
+	//			//std::cout << "neighbors: " << numNeighbors << std::endl;
+	//			int idx = rand() % numNeighbors;
+	//			const Node* next = &grid.getNeighbor(*cur, idx);
+	//			dist += next->inMeter().getDistance(cur->inMeter());
+	//			cur = next;
+
+	//		}
+
+	//		return walk;
+
+	//	}
+
+}
+
+#endif // INDOOR_GW3_WALKER_H