started testing a new grid-builder

minor fixes
worked on walkers

grid/Grid.h

@@ -177,10 +177,11 @@ public:
 	}
 
 	/** get the center-node the given Point belongs to */
-	const T& getNodeFor(const GridPoint& p) {
+	const T& getNodeFor(const GridPoint& p) const {
-		const UID uid = getUID(p);
+		//const UID uid = getUID(p);
-		Assert::isTrue(hashes.find(uid) != hashes.end(), "element not found!");
+		auto it = hashes.find(getUID(p));
-		return nodes[hashes[uid]];
+		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 */
@@ -249,11 +250,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 */
@@ -295,6 +296,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) {

grid/factory/v3/GridFactory3.h

@@ -0,0 +1,473 @@
+#ifndef GRIDFACTORY3_H
+#define GRIDFACTORY3_H
+
+#include "../../Grid.h"
+#include "../../../floorplan/v2/Floorplan.h"
+#include "HelperPoly3.h"
+
+
+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;}
+
+
+				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.5 / 100.0f && distz_cm < gs_cm) {
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
+					if (n1.getDistanceInCM(n2) < gs_cm * 1.45  && !isBlocked(map, n1, n2)) {
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+				} 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.45 / 100.0f && distz_cm <= gs_cm) {
+						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));
+		}
+
+		for (int y = y1; y <= y2; y += gs_cm) {
+			for (int x = x1; x <= x2; x += gs_cm) {
+
+				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) {
+				for (int x = x1; x <= x2; x += gs_cm) {
+
+					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

@@ -174,10 +174,11 @@ namespace GW3 {
 
 	public:
 
-		static GridPoint p3ToGp(const Point3 p) {
+//		static GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
-			const Point3 p100 = p*100;
+//			const Point3 p100 = p*100;
-			return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
+//			//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);

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 float walkedDist_m, const WalkParams& params) const = 0;
+		virtual double getProbability(const PotentialWalk& walk) const = 0;
 
 	};
 
@@ -31,12 +54,9 @@ namespace GW3 {
 
 		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			(void) params;
+			const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
-			(void) pStart;
-
-			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
 			const Node& node = grid->getNodeFor(gp);
 			const double p = node.getWalkImportance();
 			return p;
@@ -65,17 +85,15 @@ namespace GW3 {
 			;
 		}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			(void) params;
+			if (walk.pStart == walk.pEnd) {
-
-			if (pStart == pEnd) {
 				std::cout << "warn! start-position == end-positon" << std::endl;
 				return 0;
 			}
 
-			const Heading head(pStart.xy(), pEnd.xy());
+			const Heading head(walk.pStart.xy(), walk.pEnd.xy());
-			const float diff = head.getDiffHalfRAD(params.heading);
+			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 dist.getProbability(diff);
@@ -87,18 +105,20 @@ 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), dist(0, 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 float walkDist_m, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			const float requestedDistance_m = params.distance_m;
+			const float requestedDistance_m = walk.params.getDistanceInMeter(grid);
-			const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd);
+			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

@@ -58,18 +58,22 @@ namespace GW3 {
 		/** perform the walk based on the configured setup */
 		const WalkResult getDestination(const WalkParams& params) const override {
 
-			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+			Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
+
+			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
+
 
 			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(params.start, dst, params.start.getDistance(dst), params);
+					const double p1 = eval->getProbability(pWalk);
 					p *= p1;
 				}
 				return p;
@@ -97,7 +101,7 @@ namespace GW3 {
 					return (startNode->getDistanceInMeter(n)) < maxDist_m;
 				}
 			};
-			Cond cond(params.distance_m+secBuffer_m, startNode);
+			Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
 			std::vector<const Node*>  reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
 
 			WalkResult res;
@@ -106,7 +110,7 @@ namespace GW3 {
 
 			// 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 above destination reachable?
 			const Node* n = Helper::contains(grid, reachableNodes, dst);
@@ -114,14 +118,14 @@ namespace GW3 {
 			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 *= 1;//getP(p3);		// keep as-is
+					res.probability *= getP(p3);		// keep as-is
 					return res;							// done
 
 				} else {
@@ -145,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, start.getDistance(end), params);
+				const double p1 = eval->getProbability(pWalk);
 				p *= p1;
 			}
 
@@ -157,8 +163,29 @@ namespace GW3 {
 			}
 
 			res.heading = Heading(start.xy(), end.xy());
-			res.probability *= 0.1;//getP(end);
+			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;
+
+			}
+
+			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
+
 			return res;
 
 		}
@@ -204,14 +231,16 @@ namespace GW3 {
 		/** perform the walk based on the configured setup */
 		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);
+
+			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!");}
 
-			const float maxDist = params.distance_m + gridSize_m;
+			const float maxDist = walkDist_m + gridSize_m;
-			const int depth = std::ceil(params.distance_m / gridSize_m) + 1;
+			const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
 
 			Point3 best; double bestP = 0;
 			//DrawList<Point3> drawer;
@@ -331,9 +360,11 @@ namespace GW3 {
 						//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(start, end, walkDist_m, params);
+							const double p1 = eval->getProbability(pWalk);
 							p *= p1;
 						}