added new helper methods

worked on gridWalker v3

geo/Circle2.h

@@ -5,6 +5,7 @@
 
 #include "Point2.h"
 #include "Ray2.h"
+#include "Line2.h"
 
 #include "../Assertions.h"
 
@@ -65,6 +66,24 @@ public:
 
 	}
 
+	/** does this circle intersect with the given ray? */
+	bool intersects(const Line2 line) const {
+
+		// https://math.stackexchange.com/questions/311921/get-location-of-vector-circle-intersection
+		Point2 dir = line.p2 - line.p1;
+		Point2 start = line.p1;
+		const float a = dir.x*dir.x + dir.y*dir.y;
+		const float b = 2 * dir.x * (start.x-center.x) + 2 * dir.y * (start.y - center.y);
+		const float c = (start.x-center.x) * (start.x-center.x) + (start.y - center.y)*(start.y - center.y) - radius*radius;
+		const float discr = b*b - 4*a*c;
+
+		if (discr < 0) {return false;}
+
+		const float t = (2*c) / (-b + std::sqrt(discr));
+		return (t <= 1) && (t >= 0);
+
+	}
+
 
 	/** configure this sphere to contain the given point-set */
 	void adjustToPointSet(const std::vector<Point2>& lst) {

geo/Heading.h

@@ -77,10 +77,10 @@ public:
 	}
 
 
-        Heading& operator = (const float _rad) {
+		Heading& operator = (const float _rad) {
-            rad = _rad;
+			rad = _rad;
-            return *this;
+			return *this;
-        }
+		}
 
 	/** compare two headings */
 	bool operator == (const Heading other) const {return rad == other.rad;}
@@ -95,6 +95,11 @@ public:
 
 	float getRAD() const {return rad;}
 
+	/** convert heading into a direction-vector */
+	Point2 asVector() const {
+		return Point2(std::cos(rad), std::sin(rad));
+	}
+
 //	/** get a random heading */
 //	static Heading rnd() {
 //		static std::minstd_rand gen(1234);

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/v3/GridWalk3Helper.h

@@ -0,0 +1,113 @@
+#ifndef GRIDWALK3HELPER_H
+#define GRIDWALK3HELPER_H
+
+#include "../../../nav/dijkstra/Dijkstra.h"
+#include <set>
+
+
+
+template <typename Node> class GridWalk3Helper {
+
+public:
+
+	/** one walk along several nodes */
+	struct Walk3 : public std::vector<const Node*> {
+
+	};
+
+	struct Walks3 : std::vector<Walk3> {
+
+	};
+
+	struct Nodes3 : std::vector<const Node*> {
+
+	};
+
+	/** get all possible walks from start within a given region */
+	static Walks3 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();
+
+		Walks3 walks;
+
+		for (const auto& it : nodes) {
+			Walk3 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 Nodes3 getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {
+
+		Nodes3 res;
+		std::unordered_map<uint32_t, float> distances;
+		std::vector<uint32_t> toVisit;
+
+		toVisit.push_back(start->getIdx());
+		distances[start->getIdx()] = 0.0f;
+
+		while (!toVisit.empty()) {
+
+			int curIdx = toVisit.front();
+			toVisit.erase(toVisit.begin());
+
+			const Node& curNode = grid[curIdx];
+			const float curDistance = distances[curIdx];
+			res.push_back(&curNode);				// remember for output
+
+			if (curDistance <= dist_m) {
+				for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+					const int neighborIdx = curNode.getNeighborIdx(i);
+					const Node& neighbor = grid[neighborIdx];
+					const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
+					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()) {
+						toVisit.push_back(neighborIdx);
+						distances[neighborIdx] = totalDist;
+
+					// reached earlier but found shorter way
+					} else {
+						if (distances[neighborIdx] > totalDist) {
+							distances[neighborIdx] = totalDist;
+						}
+					}
+
+				}
+			}
+
+		}
+
+		return res;
+
+	}
+
+};
+
+#endif // GRIDWALK3HELPER_H

grid/walk/v3/GridWalker3.h

@@ -8,12 +8,14 @@
 #include "../../../math/Stats.h"
 #include "../../../geo/Heading.h"
 #include "../../../math/stats/Variance.h"
+#include "GridWalk3Helper.h"
+#include "../../../geo/BBox2.h"
 
 /**
  * modular grid-walker that takes various sub-components to determine
  * p(e) and thus randomly pick edges
  */
-template <typename Node, typename WalkState, typename Control> class GridWalker3 {
+template <typename Node> class GridWalker3 {
 
 private:
 
@@ -24,38 +26,135 @@ private:
 
 public:
 
-
+	struct WalkParams {
-	/** one walk along several nodes */
+		Point3 start;
-	struct Walk : public std::vector<const Node*> {
+		float distance_m;
-
+		Heading heading = Heading(0);
 	};
 
-//	struct Walks : std::vector<Walk> {
 
-//	};
+	using Helper = GridWalk3Helper<Node>;
+	using Walk = typename GridWalk3Helper<Node>::Walk3;
+	using Walks = typename GridWalk3Helper<Node>::Walks3;
+	using Nodes = typename GridWalk3Helper<Node>::Nodes3;
 
+	GridPoint p3ToGp(const Point3 p) const {
+		const Point3 p100 = p*100;
+		return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
+	}
+
+	Point3 gpToP3(const GridPoint gp) const {
+		return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
+	}
 
 	/** perform the walk based on the configured setup */
-	const Node* getDestination(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
+	const Point3 getDestination(Grid<Node>& grid, const WalkParams& params) {
+		//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
+		return _drawThenCheck(grid, params);
 
-		const Node* startNode = grid.getNodePtrFor(start);
+	}
 
-		Heading desiredHeading = ctrl.heading;
+//	/** perform the walk based on the configured setup */
+//	const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {
 
-		DrawList<Walk> walks;
+//		//return _getFromPossibleWalks(grid, start, ctrl, dist_m);
+//		//return _drawThenCheck(grid, start, ctrl, dist_m);
+//		throw "error";
+//	}
+
+	const 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);
+	}
+
+	const Point3 _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
+
+		const GridPoint gpStart = p3ToGp(params.start);
+		const Node* startNode = grid.getNodePtrFor(gpStart);
+
+		static Distribution::Normal<float> dDist(1, 0.02);
+		static Distribution::Normal<float> dHead(0, 0.02);
+
+		// include one additional grid-cell (increased distance)
+		const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
+		const float range_m = params.distance_m + secBuffer_m;
+		const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
+
+		float realDist_m = params.distance_m;
+		Heading realHead = params.heading;// + dHead.draw();
+
+		int cnt = 0;
+		while(true) {
+
+			const Point2 dir = realHead.asVector();
+			const Point2 dst = params.start.xy() + (dir * realDist_m);
+
+			// is dst reachable?
+			for (const Node* n : nodes) {
+				//const float distToNode = n->inMeter().xy().getDistance(dst);
+				//if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) {
+				if (contains(grid, n, dst)) {
+					const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
+					const GridPoint gp = p3ToGp(p3);
+					if (grid.hasNodeFor(gp)) {
+						return p3;
+					} else {
+						std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
+					}
+				}
+			}
+
+			// before trying again, modify distance and angle
+			if (1 == 0) {
+				realDist_m *= dDist.draw();
+				realHead += dHead.draw();
+			}
+
+			// reached max retries?
+			if (++cnt > 10) {return params.start;}	// did not work out....
 
-		for (int i = 0; i < 100; ++i) {
-			const Walk walk = getRandomWalk(grid, startNode, dist_m);
-			const double prob = eval(walk, desiredHeading);
-			walks.add(walk, prob);
 		}
 
-		Walk res = walks.get();
+		throw "error";
-		const Node* dst = res.back();
-		return dst;
 
 	}
 
+//	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();
@@ -83,39 +182,43 @@ public:
 		return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
 	}
 
-	double eval(const Walk& w, const Heading desiredHeading) const {
+	double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
 
 		return 1.0
 				* evalHeadingStartEnd(w, desiredHeading)
+				* evalDistance(w, desiredDistance)
 				// * evalHeadingChanges(w);
 		;
 
 	}
 
-	Walk getRandomWalk(Grid<Node>& grid, const Node* start, const float dist_m) const {
 
-		Walk walk;
 
-		float dist = 0;
 
-		const Node* cur = start;
+//	Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
-		while(true) {
 
-			walk.push_back(cur);
+//		Walk walk;
-			if (dist > dist_m) {break;}
 
-			const int numNeighbors = cur->getNumNeighbors();
+//		float dist = 0;
-			//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;
 
-		}
+//		const Node* cur = start;
+//		while(true) {
 
-		return walk;
+//			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;
+
+//	}
 
 };
 

math/Interpolator.h

@@ -56,6 +56,11 @@ public:
 
 	}
 
+	/** get a list of all raw entries */
+	const std::vector<InterpolatorEntry>& getEntries() const {
+		return entries;
+	}
+
 protected:
 
 	/** special interpolation for the timestamp class */

nav/dijkstra/Dijkstra.h

@@ -30,11 +30,16 @@ public:
 	}
 
 	/** get the dijkstra-pendant for the given user-node. null if none matches */
-	const inline DijkstraNode<T>* getNode(const T& userNode) const {
+	inline const DijkstraNode<T>* getNode(const T& userNode) const {
 		auto it = nodes.find(&userNode);
 		return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
 	}
 
+	/** get all constructed dijkstra-nodes and their original pendant */
+	inline const std::unordered_map<const T*, DijkstraNode<T>*>& getNodes() const {
+		return nodes;
+	}
+
 	/** calculate all shortest paths from ANY node to the given destination */
 	template <typename Access> void build(const T* end, const Access& acc) {
 		build(end, nullptr, acc, NAN);
@@ -80,7 +85,7 @@ public:
 			if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;}
 
 			// when a maximum weight is given, stop when current cum-dist > maxWeight
-			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;}
+			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached weight limit: " + std::to_string(maxWeight)); break;}
 
 			// visit (and maybe update) each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {