diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index 5c2ca68..9d51178 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -6,10 +6,146 @@ #include "Structs.h" +#include #include +#include + +//#define SHOW_DEBUG_PLOT + +#ifdef SHOW_DEBUG_PLOT + #include + #include + #include +#endif + namespace GW3 { + + /** get an iterator over all nodes reachable from the given start */ + template class ReachableIteratorSorted { + + const Grid& grid; + const Node& start; + + struct Next { + + uint32_t idx; + float distToStart; + + Next(uint32_t idx, float distToStart) : idx(idx), distToStart(distToStart) {;} + + /** compare by weight. same weight? : compare by pointer */ + bool operator < (const Next& o) const { + return (distToStart != o.distToStart) ? (distToStart < o.distToStart) : (idx < o.idx); + } + + }; + + Node* curNode = nullptr; + std::unordered_set visited; + std::set toVisit; + + public: + + ReachableIteratorSorted(const Grid& grid, const Node& start) : grid(grid), start(start) { + toVisit.insert(Next(start.getIdx(),0)); + } + + bool hasNext() const { + return !toVisit.empty(); + } + + const Node& next() { + + const Next cur = *(toVisit.begin()); // visit from inside out (needed for correct distance) + toVisit.erase(toVisit.begin()); + visited.insert(cur.idx); + + const Node& curNode = grid[cur.idx]; + + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + const float newDist = cur.distToStart + curNode.getDistanceInMeter(neighbor); + + // not yet reached -> store distance + if (visited.find(neighborIdx) == visited.end()) { + toVisit.insert(Next(neighborIdx, newDist)); + } + + } + + // done + return curNode; + + } + + }; + + /** get an iterator over all nodes reachable from the given start */ + template class ReachableIteratorUnsorted { + + const Grid& grid; + const Node& start; + + Node* curNode = nullptr; + std::unordered_set visited; + std::vector toVisit; + + public: + + ReachableIteratorUnsorted(const Grid& grid, const Node& start) : grid(grid), start(start) { + toVisit.push_back(start.getIdx()); + } + + bool hasNext() const { + return !toVisit.empty(); + } + + const Node& next(const std::function& skip) { + + const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance) + toVisit.erase(toVisit.begin()); + visited.insert(curIdx); + + const Node& curNode = grid[curIdx]; + + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + + // not yet reached -> store distance + if (!skip(neighbor)) { + if (visited.find(neighborIdx) == visited.end()) { + toVisit.push_back(neighborIdx); + } + } + + } + + // done + return curNode; + + } + + }; + + + struct ReachableSettings { + + float dist_m; + bool limitDistance = true; + + Heading heading = Heading(0); + float maxHeadingDiff_rad; + bool limitHeading = false; + + }; + + template class Helper { public: @@ -74,54 +210,80 @@ namespace GW3 { return walks; - } + } /** get all reachable nodes that are within a given range */ - static Nodes getAllReachableNodes(Grid& grid, const Node* start, const float dist_m) { + static Nodes getAllReachableNodes(Grid& grid, const Node* start, const ReachableSettings& set ) { + + //auto tStart = std::chrono::system_clock::now(); Nodes res; std::unordered_map distances; - std::vector toVisit; + std::vector toVisit; // std::queue was only barely faster: 900 vs 880 microseconds toVisit.push_back(start->getIdx()); distances[start->getIdx()] = 0.0f; + #ifdef SHOW_DEBUG_PLOT + static K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementColorPoints pts1; pts1.setPointType(7); pts1.setPointSize(1); + plot.add(&pts1); + #endif + while (!toVisit.empty()) { - int curIdx = toVisit.front(); + const int curIdx = toVisit.front(); // visit from inside out (needed for correct distance) toVisit.erase(toVisit.begin()); const Node& curNode = grid[curIdx]; const float curDistance = distances[curIdx]; - res.push_back(&curNode); // remember for output + res.push_back(&curNode); // remember for output - if (curDistance <= dist_m) { - for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + #ifdef SHOW_DEBUG_PLOT + pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance); + gp.draw(plot); + gp.flush(); + #endif - const int neighborIdx = curNode.getNeighborIdx(i); - const Node& neighbor = grid[neighborIdx]; - const float addDist = neighbor.inMeter().getDistance(curNode.inMeter()); - const float totalDist = curDistance + addDist; + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { - // this is like in dijkstra. keep the smallest distance to reach a node: + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + const float addDist = neighbor.getDistanceInMeter(curNode); + const float totalDist = curDistance + addDist; - // not yet reached -> store distance - if (distances.find(neighborIdx) == distances.end()) { - toVisit.push_back(neighborIdx); - distances[neighborIdx] = totalDist; + // this is like in dijkstra. keep the smallest distance to reach a node: - // reached earlier but found shorter way - } else { - if (distances[neighborIdx] > totalDist) { - distances[neighborIdx] = totalDist; - } + // not yet reached -> store distance + if (distances.find(neighborIdx) == distances.end()) { + distances[neighborIdx] = totalDist; + + if (set.limitDistance) { + if (totalDist > set.dist_m) {continue;} } + if (set.limitHeading) { + const Heading head(start->x_cm, start->y_cm, neighbor.x_cm, neighbor.y_cm); // angle between start and current node + const float diff = head.getDiffHalfRAD(set.heading); // difference between above angle and requested angle + if (diff > set.maxHeadingDiff_rad) {continue;} // more than 90 degree difference? -> ignore + } + toVisit.push_back(neighborIdx); // needs a visit? (still some distance left) + + + // reached earlier but found shorter way + } else if (distances[neighborIdx] > totalDist) { + distances[neighborIdx] = totalDist; } + } } + //auto tEnd = std::chrono::system_clock::now(); + //auto elapsed = std::chrono::duration_cast(tEnd - tStart); + //std::cout << elapsed.count() << std::endl; + return res; } diff --git a/grid/walk/v3/Structs.h b/grid/walk/v3/Structs.h index 335f0e3..8d46389 100644 --- a/grid/walk/v3/Structs.h +++ b/grid/walk/v3/Structs.h @@ -9,9 +9,13 @@ namespace GW3 { /** paremters for the walk */ struct WalkParams { + Point3 start; float distance_m; Heading heading = Heading(0); + + float lookFurther_m = 1.5; + }; /** result of the random walk */ diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index c740e72..8b73dbe 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -50,8 +50,12 @@ namespace GW3 { /** evaluate the difference between head(start,end) and the requested heading */ template class WalkEvalHeadingStartEnd : public WalkEvaluator { + const double sigma; + public: + WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;} + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { (void) params; @@ -64,7 +68,7 @@ namespace GW3 { const Heading head(pStart.xy(), pEnd.xy()); const float diff = head.getDiffHalfRAD(params.heading); //const float diff = Heading::getSignedDiff(params.heading, head); - return Distribution::Normal::getProbability(0, 0.04, diff); + return Distribution::Normal::getProbability(0, sigma, diff); } @@ -73,10 +77,12 @@ namespace GW3 { /** evaluate the difference between distance(start, end) and the requested distance */ template class WalkEvalDistance : public WalkEvaluator { - const double sigma = 0.1f; + const double sigma; public: + WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;} + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { const float walkedDistance_m = pStart.getDistance(pEnd); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 894b631..543059b 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -52,24 +52,30 @@ namespace GW3 { /** perform the walk based on the configured setup */ const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + Assert::isNot0(params.distance_m, "walking distance must be > 0"); + static std::mt19937 rndGen; const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); // include one additional grid-cell (increased distance) - const float secBuffer_m = grid.getGridSize_cm() / 100.0f; - const float range_m = params.distance_m + secBuffer_m; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); + const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1); + ReachableSettings set; + set.limitDistance = true; + set.dist_m = params.distance_m + secBuffer_m; + set.limitHeading = false; + set.heading = params.heading; + set.maxHeadingDiff_rad = M_PI/2; + const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); WalkResult res; res.heading = params.heading; res.position = params.start; - float realDist_m = params.distance_m; const Point2 dir = res.heading.asVector(); - const Point2 dst = params.start.xy() + (dir * realDist_m); + const Point2 dst = params.start.xy() + (dir * params.distance_m); // is dst reachable? const Node* n = Helper::contains(grid, reachableNodes, dst); @@ -80,12 +86,14 @@ namespace GW3 { if (grid.hasNodeFor(gp)) { res.position = p3; // update position - res.heading; // keep as-is - res.probability; // keep as-is + //res.heading; // keep as-is + //res.probability; // keep as-is return res; // done + } else { std::cout << "WARN dst not found" << std::endl; //throw "should not happen"; + } } @@ -150,61 +158,99 @@ namespace GW3 { /** perform the walk based on the configured setup */ const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + Assert::isNot0(params.distance_m, "walking distance must be > 0"); + static std::minstd_rand rndGen; const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); - // include one additional grid-cell (increased distance) - const float secBuffer_m = grid.getGridSize_cm() / 100.0f; - const float range_m = params.distance_m + secBuffer_m; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); +// // include one additional grid-cell (increased distance) +// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05); + +// ReachableSettings set; +// set.limitDistance = true; +// set.limitHeading = true; +// set.dist_m = params.distance_m + secBuffer_m; +// set.heading = params.heading; +// set.maxHeadingDiff_rad = M_PI/2; +// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); - // not found -> try random pick among all reachable nodes const float gridSize_m = grid.getGridSize_cm() / 100.0f; - std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); + //std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); Point3 best; double bestP = 0; +// DrawList drawer; const Point3 start = params.start; - // try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles") - for (int i = 0; i < 25; ++i) { + // 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)]; + // const Node* dstNode = reachableNodes[dNode(rndGen)]; - // random position within destination-node - std::uniform_real_distribution dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f); - const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + std::uniform_real_distribution dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f); - // destination = node-center + offset (within the node's bbox) - const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; - // sanity check - Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); - if (start == end) {continue;} + ReachableIteratorUnsorted ri(grid, *startNode); + const float maxDist = params.distance_m * 1.25 + gridSize_m; + + auto skip = [&] (const Node& n) { + const float dist_m = n.getDistanceInMeter(gpStart); + return dist_m > maxDist; + }; + + //for (const Node* dstNode : reachableNodes) { + while(ri.hasNext()) { + + const Node* dstNode = &ri.next(skip); +// const float dist_m = dstNode->getDistanceInMeter(gpStart); + +// if (dist_m > maxDist) { +// break; +// } + + for (int i = 0; i < 25; ++i) { + + // random position within destination-node + const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + + // destination = node-center + offset (within the node's bbox) + const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; + + // sanity check + if (start == end) {continue;} + if (!grid.hasNodeFor(Helper::p3ToGp(end))) { + std::cout << "random destination is not part of the grid" << std::endl; + continue; + } + + //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = end;} + //drawer.add(end, p); - double p = 1; - for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); - p *= p1; } - if (p > bestP) {bestP = p; best = end;} - } - //const Point3 end = lst.get(); + //const Point3 end = drawer.get(); const Point3 end = best; WalkResult res; if (start == end) { res.probability = 0; } else { res.heading = Heading(start.xy(), end.xy()); - res.probability = bestP; // 1 + res.probability = bestP; } res.position = end; return res;