merged

grid/Grid.h

@@ -16,6 +16,11 @@
 #include "../geo/BBox3.h"
 #include "../misc/Debug.h"
 
+#define GM_BOX			1
+#define GM_HOBEYCOMB	2
+#define GRID_MODE		GM_BOX
+
+
 /**
  * grid of a given-size, storing some user-data-value which
  * - extends GridPoint and GridNode
@@ -177,10 +182,11 @@ public:
 	}
 
 	/** get the center-node the given Point belongs to */
-	const T& getNodeFor(const GridPoint& p) {
-		const UID uid = getUID(p);
-		Assert::isTrue(hashes.find(uid) != hashes.end(), "element not found!");
-		return nodes[hashes[uid]];
+	const T& getNodeFor(const GridPoint& p) const {
+		//const UID uid = getUID(p);
+		auto it = hashes.find(getUID(p));
+		Assert::isTrue(it != hashes.end(), "element not found!");
+		return nodes[it->second];
 	}
 
 	/** get the center-node the given Point belongs to. or nullptr if not present */
@@ -213,6 +219,14 @@ public:
 		return GridNodeBBox(node, gridSize_cm);
 	}
 
+	/** is this node part of a non-plain stair/escalator */
+	bool isPlain(const T& n1) const {
+		for (const T& n2 : neighbors(n1)) {
+			if (n2.z_cm != n1.z_cm) {return false;}
+		}
+		return true;
+	}
+
 	/**
 	 * get an UID for the given point.
 	 * this works only for aligned points.
@@ -231,9 +245,18 @@ public:
 		const uint64_t center = 1 << 19;
 
 		// build
+#if (GRID_MODE == GM_HOBEYCOMB)
+		const int xx = ((int)std::round(p.y_cm / gridSize_cm) % 2 == 0) ? (0) : (gridSize_cm/2);
+		const uint64_t x = center + (int64_t) idxX(p.x_cm-xx);
+		const uint64_t y = center + (int64_t) idxY(p.y_cm);
+		const uint64_t z = center + (int64_t) idxZ(p.z_cm);
+#elif (GRID_MODE == GM_BOX)
 		const uint64_t x = center + (int64_t) idxX(p.x_cm);
 		const uint64_t y = center + (int64_t) idxY(p.y_cm);
 		const uint64_t z = center + (int64_t) idxZ(p.z_cm);
+#endif
+
+
 
 		return (z << 40) | (y << 20) | (x << 0);
 
@@ -241,11 +264,11 @@ public:
 
 	inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);}
 	inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);}
-	inline int idxZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm);}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
+	inline int idxZ(const int z_cm) const {return std::round(z_cm / 20.0f);}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
 
 	inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;}
 	inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;}
-	inline int snapZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm) * gridSize_cm;}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
+	inline int snapZ(const int z_cm) const {return std::round(z_cm / 20.0f) * 20;}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
 
 
 	/** array access */
@@ -287,6 +310,11 @@ public:
 		}
 	}
 
+	/** convert to a GridPoint coordinate (in cm) */
+	GridPoint toGridPoint(const Point3 pos_m) const {
+		return GridPoint( snapX(pos_m.x*100), snapY(pos_m.y*100), snapZ(pos_m.z*100) );
+	}
+
 	/** remove the given array-index by moving all follwing elements down by one */
 	template <typename X> void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) {
 		for (int i = idxToRemove+1; i < arrayLen; ++i) {
@@ -469,11 +497,11 @@ public:
 
 
 
-	NeighborForEach neighbors(const int idx) {
+	NeighborForEach neighbors(const int idx) const {
 		return neighbors(nodes[idx]);
 	}
 
-	NeighborForEach neighbors(const T& node) {
+	NeighborForEach neighbors(const T& node) const {
 		return NeighborForEach(*this, node._idx);
 	}
 
@@ -513,9 +541,13 @@ private:
 
 	/** asssert that the given element is aligned to the grid */
 	void assertAligned(const T& elem) {
+#if (GRID_MODE == GM_HOBEYCOMB)
+
+#elif (GRID_MODE == GM_BOX)
 		if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
 		if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
 		//if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
+#endif
 	}
 
 };

grid/GridNode.h

@@ -87,6 +87,7 @@ public:
 	/** set the node's semantic type */
 	void setType(const uint8_t type) {this->_type = type;}
 
+
 //	/** get the n-th neighbor for this node */
 //	template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
 //		return grid.getNeighbor(_idx, nth);

grid/factory/v3/GridFactory3.h

@@ -0,0 +1,492 @@
+#ifndef GRIDFACTORY3_H
+#define GRIDFACTORY3_H
+
+#include "../../Grid.h"
+#include "../../../floorplan/v2/Floorplan.h"
+#include "HelperPoly3.h"
+#include <unordered_set>
+
+#if (GRID_MODE == GM_BOX)
+
+#define GF3_ITER_XY for (int y = y1; y <= y2; y += gs_cm) {	for (int x = x1; x <= x2; x += gs_cm) {
+
+#elif (GRID_MODE == GM_HOBEYCOMB)
+
+#define GF3_ITER_XY\
+for (int y = y1; y <= y2; y += gs_cm) {\
+	const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);\
+	for (int x = x1-xx; x <= x2; x += gs_cm) {
+
+#endif
+
+template <typename Node> class GridFactory3 {
+
+private:
+
+
+	Grid<Node>& grid;
+	const int gs_cm;
+
+	struct NewNode {
+		GridPoint pos;
+		int type;
+		NewNode(const GridPoint pos, const int type) : pos(pos), type(type) {;}
+		bool operator == (const NewNode& o) const {return o.pos == pos;}
+	};
+
+public:
+
+	GridFactory3(Grid<Node>& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) {
+
+	}
+
+	void build(const Floorplan::IndoorMap* map) {
+
+		std::vector<NewNode> add;
+		std::vector<NewNode> rem;
+
+		for (const Floorplan::Floor* floor : map->floors) {
+
+//			for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+
+//				const std::vector<NewNode> pts = getPointsOn(floor, *poly);
+//				if (poly->method == Floorplan::OutlineMethod::ADD) {
+//					add.insert(add.end(), pts.begin(), pts.end());
+//				} else {
+//					rem.insert(rem.end(), pts.begin(), pts.end());
+//				}
+
+//			}
+
+			const std::vector<NewNode> pts = getPointsOn(floor);
+			add.insert(add.end(), pts.begin(), pts.end());
+
+			for (const Floorplan::Stair* stair : floor->stairs) {
+				std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
+				const std::vector<NewNode> pts = getPointsOn(floor, quads);
+				add.insert(add.end(), pts.begin(), pts.end());
+			}
+
+
+		}
+
+		for (const NewNode& nn : add) {
+			auto it = std::find(rem.begin(), rem.end(), nn);
+			if (it == rem.end()) {
+				if (!grid.hasNodeFor(nn.pos)) {
+					Node n(nn.pos.x_cm, nn.pos.y_cm, nn.pos.z_cm);
+					n.setType(nn.type);
+					grid.add(n);
+				}
+			}
+		}
+
+		connect(map);
+		removeIsolatedNodes();
+
+	}
+
+	bool isBlocked(const Floorplan::IndoorMap* map, const Node& n1, const Node& n2) {
+
+		Line2 lNodes(n1.inMeter().xy(), n2.inMeter().xy());
+
+		for (Floorplan::Floor* floor : map->floors) {
+
+			if (n1.inMeter().z != floor->atHeight) {continue;}
+			if (n2.inMeter().z != floor->atHeight) {continue;}
+
+			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
+				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
+				if (line) {
+					const std::vector<Line2> lines = getThickLines(line);
+					for (const Line2& lObs : lines) {
+						if (lObs.getSegmentIntersection(lNodes)) {
+							return true;
+						}
+					}
+				}
+			}
+		}
+
+		return false;
+
+	}
+
+	/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
+	static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
+		//const Line2 base(line->from*100, line->to*100);
+		const float thickness_m = line->thickness_m;
+		const Point2 dir = (line->to - line->from);				// obstacle's direction
+		const Point2 perp = dir.perpendicular().normalized();	// perpendicular direction (90 degree)
+		const Point2 p1 = line->from + perp * thickness_m/2;		// start-up
+		const Point2 p2 = line->from - perp * thickness_m/2;		// start-down
+		const Point2 p3 = line->to + perp * thickness_m/2;		// end-up
+		const Point2 p4 = line->to - perp * thickness_m/2;		// end-down
+		return {
+			Line2(p1, p2),
+			Line2(p3, p4),
+			Line2(p2, p4),
+			Line2(p1, p3),
+		};
+	}
+
+	void connect(const Floorplan::IndoorMap* map) {
+
+		for (Node& n1 : grid) {
+			for (Node& n2 : grid) {
+
+				if (n1 == n2) {continue;}
+
+
+				// stair with floor
+				if (
+						(n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) ||
+						(n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR)
+					) {
+
+					const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
+					const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
+					if (distxy > 0 && distxy < gs_cm * 1.2 / 100.0f && distz_cm < gs_cm) {		// [1.85]
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				// floor with floor
+				} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
+					if (n1.getDistanceInCM(n2) < gs_cm * 1.2  && !isBlocked(map, n1, n2)) {		// [1.2 | 1.845]
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				// stair with stair
+				} else if (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_STAIR) {
+
+					const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
+					const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
+
+//					if (n1.getDistanceInCM(n2) < gs_cm * 1.45  && !isBlocked(map, n1, n2)) {
+					if (distxy < gs_cm * 1.2 / 100.0f && distz_cm <= gs_cm) {		// [1.845]
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				}
+
+
+//				if (n1.getDistanceInCM(n2) < gs_cm * 1.7  && !isBlocked(map, n1, n2)) {
+//					if (n1.fullyConnected()) {continue;}
+//					if (n2.fullyConnected()) {continue;}
+//					grid.connectUniDir(n1, n2);
+//				}
+
+			}
+		}
+
+	}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+	/** recursively get all connected nodes and add them to the set */
+	void getConnected(Node& n1, std::unordered_set<int>& visited) {
+
+		std::unordered_set<int> toVisit;
+		toVisit.insert(n1.getIdx());
+
+		// run while there are new nodes to visit
+		while(!toVisit.empty()) {
+
+			// get the next node
+			int nextIdx = *toVisit.begin();
+			toVisit.erase(nextIdx);
+			visited.insert(nextIdx);
+			Node& next = grid[nextIdx];
+
+			// get all his (unprocessed) neighbors and add them to the region
+			for (const Node& n2 : grid.neighbors(next)) {
+				if (visited.find(n2.getIdx()) == visited.end()) {
+					toVisit.insert(n2.getIdx());
+				}
+			}
+
+		}
+
+	}
+
+	void removeIsolatedNodes() {
+
+		//std::cout << "todo: remove" << std::endl;
+		//return;
+
+		// try to start at the first stair
+		for (Node& n : grid) {
+			if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
+		}
+
+		// no stair found? try to start at the first node
+		removeIsolatedNodes(grid[0]);
+
+	}
+
+	/** remove all nodes not connected to n1 */
+	void removeIsolatedNodes(Node& n1) {
+
+		// get the connected region around n1
+		//Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
+		//Log::tick();
+		std::unordered_set<int> set;
+		getConnected(n1, set);
+		//Log::tock();
+
+		//const int numToRemove = grid.getNumNodes() - set.size();
+		//int numRemoved = 0;
+
+		// remove all other
+		//Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
+		//Log::tick();
+		for (Node& n2 : grid) {
+			if (set.find(n2.getIdx()) == set.end()) {
+
+				// sanity check
+				// wouldn't make sense that a stair-node is removed..
+				// maybe something went wrong elsewhere???
+				Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!");
+				Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!");
+				//Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!");
+
+				// proceed ;)
+				grid.remove(n2);
+
+				//++numRemoved;
+				//std::cout << numRemoved << ":" << numToRemove << std::endl;
+
+			}
+		}
+		//Log::tock();
+
+		// clean the grid (physically delete the removed nodes)
+		grid.cleanup();
+
+	}
+
+
+//	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) {
+
+//		std::vector<NewNode> res;
+
+//		BBox2 bbox;
+//		for (Point2 pt : poly.poly.points) {bbox.add(pt);}
+
+//		int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
+//		int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
+
+//		int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
+//		int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
+
+//		int z = floor->atHeight * 100;
+
+//		for (int y = y1; y <= y2; y += gs_cm) {
+//			for (int x = x1; x <= x2; x += gs_cm) {
+
+//				GridPoint gp(x, y, z);
+//				int type = poly.outdoor ? GridNode::TYPE_OUTDOOR : GridNode::TYPE_FLOOR;
+//				res.push_back(NewNode(gp, type));
+
+//			}
+//		}
+
+//		return res;
+
+//	}
+
+
+
+	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor) {
+
+		std::vector<NewNode> res;
+
+		BBox2 bbox;
+		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+			for (Point2 pt : poly->poly.points) {bbox.add(pt);}
+		}
+
+		int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
+		int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
+
+		int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
+		int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
+
+		int z = floor->atHeight * 100;
+
+		struct Combo {
+			HelperPoly3 poly;
+			const Floorplan::FloorOutlinePolygon* orig;
+			Combo(HelperPoly3 poly, const Floorplan::FloorOutlinePolygon* orig) : poly(poly), orig(orig) {;}
+		};
+
+		std::vector<Combo> polygons;
+		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+			HelperPoly3 pol(*poly);
+			polygons.push_back(Combo(pol, poly));
+		}
+
+		GF3_ITER_XY
+
+				int type = GridNode::TYPE_FLOOR;
+				bool remove = false;
+				bool add = false;
+
+				for (const Combo& c : polygons) {
+					if (c.poly.contains(Point2(x,y))) {
+						if (c.orig->method == Floorplan::OutlineMethod::ADD) {add = true;}
+						if (c.orig->method == Floorplan::OutlineMethod::REMOVE) {remove = true; break;}
+						if (c.orig->outdoor) {type = GridNode::TYPE_OUTDOOR;}
+					}
+				}
+
+				if (add && !remove) {
+					GridPoint gp(x, y, z);
+					res.push_back(NewNode(gp, type));
+				}
+
+			}
+		}
+
+		return res;
+
+	}
+
+
+
+	//
+
+	//				const std::vector<NewNode> pts = getPointsOn(floor, *poly);
+	//				if (poly->method == Floorplan::OutlineMethod::ADD) {
+	//					add.insert(add.end(), pts.begin(), pts.end());
+	//				} else {
+	//					rem.insert(rem.end(), pts.begin(), pts.end());
+	//				}
+
+	//			}
+
+	static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
+		const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
+		double d00 = dot(v0, v0);
+		double d01 = dot(v0, v1);
+		double d11 = dot(v1, v1);
+		double d20 = dot(v2, v0);
+		double d21 = dot(v2, v1);
+		double denom = d00 * d11 - d01 * d01;
+		v = (d11 * d20 - d01 * d21) / denom;
+		w = (d00 * d21 - d01 * d20) / denom;
+		u = 1.0f - v - w;
+		return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0);
+	}
+
+//	void isBlocked(const GridPoint& gp) {
+//		for (Floorplan::Floor* floor : map->floors) {
+//			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
+//				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
+//				if (line) {
+//					line->
+//				}
+//			}
+//		}
+//	}
+
+	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const std::vector<Floorplan::Quad3>& quads) {
+
+		std::vector<NewNode> res;
+
+		// whole stair
+		BBox3 bboxStair;
+		for (const Floorplan::Quad3& quad : quads) {
+			bboxStair.add(quad.p1);
+			bboxStair.add(quad.p2);
+			bboxStair.add(quad.p3);
+			bboxStair.add(quad.p4);
+		}
+
+		// stair's starting and ending z (must be connected to a floor)
+		//int z1 = grid.snapZ( (floor->atHeight) * 100 );
+		//
+		int z2 = grid.snapZ( (floor->atHeight + bboxStair.getMax().z) * 100 );
+
+		// one quad
+		for (const Floorplan::Quad3& quad : quads) {
+
+			BBox3 bbox;
+			bbox.add(quad.p1);
+			bbox.add(quad.p2);
+			bbox.add(quad.p3);
+			bbox.add(quad.p4);
+
+
+			int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
+			int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
+
+			int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
+			int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
+
+			//int zFloor = floor->atHeight * 100;
+
+//			for (int y = y1; y <= y2; y += gs_cm) {
+//				const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);
+//				for (int x = x1-xx; x <= x2; x += gs_cm) {
+
+			GF3_ITER_XY
+
+					int z = 0;
+					Point2 p(x/100.0f, y/100.0f);
+
+					float u,v,w;
+					if (bary(p, quad.p1.xy(), quad.p2.xy(), quad.p3.xy(), u, v, w)) {
+						z = (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) * 100;
+					} else if (bary(p, quad.p1.xy(), quad.p3.xy(), quad.p4.xy(), u, v, w)) {
+						z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
+					} else {
+						// outside of the quad -> skip
+						//z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
+						continue;
+
+						//z = zFloor + (
+						//		(quad.p1.z*u + quad.p2.z*v + quad.p3.z*w)
+						//	) * 100;
+
+					}
+
+					//z = grid.snapZ(z);
+
+					const GridPoint gp(x, y, z);
+					const int type = GridNode::TYPE_STAIR;
+					res.push_back(NewNode(gp, type));
+
+				}
+			}
+
+		}
+
+		// scale to ensure starting at floor, and ending at floor
+
+		return res;
+
+	}
+
+};
+
+#endif // GRIDFACTORY3_H

grid/factory/v3/HelperPoly3.h

@@ -0,0 +1,101 @@
+#ifndef HELPERPOLY3_H
+#define HELPERPOLY3_H
+
+#include "../../../geo/Point2.h"
+#include "../../../geo/Point3.h"
+#include "../../../geo/BBox2.h"
+#include "../../../geo/BBox3.h"
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../grid/Grid.h"
+
+/** helper class for polygon methods */
+struct HelperPoly3 {
+
+	BBox2 bbox_cm;
+	std::vector<Point2> points_cm;
+
+	/** empty ctor */
+	HelperPoly3() {
+		;
+	}
+
+	/** ctor from floorplan-polygon */
+	HelperPoly3(const Floorplan::FloorOutlinePolygon& poly) {
+		for (Point2 p : poly.poly.points) { add(p * 100); }
+	}
+
+	/** ctor from floorplan-quad */
+	HelperPoly3(const Floorplan::Quad3& quad) {
+		add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
+	}
+
+	/** ctor from floorplan-polygon */
+	HelperPoly3(const Floorplan::Polygon2& poly) {
+		for (Point2 p : poly.points) { add(p * 100); }
+	}
+
+	void add(const Point2 p) {
+		points_cm.push_back(p);
+		bbox_cm.add(p);
+	}
+
+	void add(const Point3& p) {
+		points_cm.push_back(p.xy());
+		bbox_cm.add(p.xy());
+	}
+
+	/** does the polygon contain the given point (in cm)? */
+	bool contains(const Point2 p_cm) const {
+
+		// not within bbox? -> not within polygon
+		if (!bbox_cm.contains(p_cm)) {return false;}
+
+		// ensure the point is at least a bit outside of the polygon
+		const float x1_cm = bbox_cm.getMin().x - 17.71920;
+		const float y1_cm = bbox_cm.getMin().y - 23.10923891;
+
+		// construct line between point outside of the polygon and the point in question
+		const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y);
+
+		// determine the number of intersections
+		int hits = 0;
+		const int cnt = points_cm.size();
+		for (int i = 0; i < cnt; ++i) {
+			const Point2 p1 = points_cm[(i+0)%cnt];
+			const Point2 p2 = points_cm[(i+1)%cnt];
+			const Line2 l12(p1, p2);
+			if (l12.getSegmentIntersection(l)) {++hits;}
+		}
+
+		// inside or outside?
+		return ((hits % 2) == 1);
+
+	}
+
+	/** call a user-function for each GRID-ALIGNED point within the polygon */
+	void forEachGridPoint(const int gridSize_cm, std::function<void(int x_cm, int y_cm)> callback) const {
+
+		int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm;
+		int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm;
+
+		int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm;
+		int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm;
+
+		// process each point within the (aligned) bbox
+		for (int y = y1; y <= y2; y += gridSize_cm) {
+			for (int x = x1; x <= x2; x += gridSize_cm) {
+
+				// does this point belong to the polygon?
+				if (!contains(Point2(x,y))) {continue;}
+
+				// call the callback
+				callback(x,y);
+
+			}
+		}
+
+	}
+
+};
+
+#endif // HELPERPOLY3_H

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;