diff --git a/geo/Heading.h b/geo/Heading.h index a2952d4..8381f10 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -27,6 +27,11 @@ public: Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds"); } + /** ctor from(x,y) -> to(x,y) */ + Heading(const Point2 from, const Point2 to) : Heading(from.x, from.y, to.x, to.y) { + ; + } + /** POSITIVE angular difference [0:PI] */ float getDiffHalfRAD(const Heading other) const { return Angle::getDiffRAD_2PI_PI(rad, other.rad); diff --git a/grid/walk/v3/GridWalk3Helper.h b/grid/walk/v3/GridWalk3Helper.h deleted file mode 100644 index 5b79c75..0000000 --- a/grid/walk/v3/GridWalk3Helper.h +++ /dev/null @@ -1,113 +0,0 @@ -#ifndef GRIDWALK3HELPER_H -#define GRIDWALK3HELPER_H - -#include "../../../nav/dijkstra/Dijkstra.h" -#include - - - -template class GridWalk3Helper { - -public: - - /** one walk along several nodes */ - struct Walk3 : public std::vector { - - }; - - struct Walks3 : std::vector { - - }; - - struct Nodes3 : std::vector { - - }; - - /** get all possible walks from start within a given region */ - static Walks3 getAllPossibleWalks(Grid& grid, const Node* start, const float dist_m) { - - struct Access { - Grid& grid; - Access(Grid& 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 dijkstra; - dijkstra.build(start, nullptr, acc, maxDist_m); - - const std::unordered_map*>& nodes = dijkstra.getNodes(); - - Walks3 walks; - - for (const auto& it : nodes) { - Walk3 walk; - DijkstraNode* 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& grid, const Node* start, const float dist_m) { - - Nodes3 res; - std::unordered_map distances; - std::vector 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 diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h deleted file mode 100644 index ec59386..0000000 --- a/grid/walk/v3/GridWalker3.h +++ /dev/null @@ -1,257 +0,0 @@ -#ifndef GRIDWALKER3_H -#define GRIDWALKER3_H - -#include "../../../data/Timestamp.h" -#include "../../Grid.h" -#include "../../../math/DrawList.h" -#include "../../../math/Distributions.h" -#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 class GridWalker3 { - -private: - - /** all modules to evaluate */ -// std::vector*> modules; - - RandomGenerator rnd; - -public: - - /** paremters for the walk */ - struct WalkParams { - Point3 start; - float distance_m; - Heading heading = Heading(0); - }; - - /** result of the random walk */ - struct WalkResult { - Point3 position; - Heading heading = Heading(0); - double probability = 1.0; - }; - - - using Helper = GridWalk3Helper; - using Walk = typename GridWalk3Helper::Walk3; - using Walks = typename GridWalk3Helper::Walks3; - using Nodes = typename GridWalk3Helper::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 WalkResult getDestination(Grid& grid, const WalkParams& params) { - //return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m); - return _drawThenCheck(grid, params); - - } - -// /** perform the walk based on the configured setup */ -// const Point3 getDestination(Grid& grid, const GridPoint start, const params) { - -// //return _getFromPossibleWalks(grid, start, ctrl, dist_m); -// //return _drawThenCheck(grid, start, ctrl, dist_m); -// throw "error"; -// } - - /** does the given grid-node contain the provided point-in-question? */ - const bool contains(const Grid& grid, const Node* n, Point2 pt) { - const float gridSize_m = grid.getGridSize_cm() / 100.0f; - const float d = gridSize_m / 2.0f; - const Point2 pMin = n->inMeter().xy() - Point2(d, d); - const Point2 pMax = n->inMeter().xy() + Point2(d, d); - const BBox2 bbox(pMin, pMax); - return bbox.contains(pt); - } - - /** does one of the given grid-node scontains the provided point-in-question? */ - const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { - for (const Node* n : nodes) { - if (contains(grid, n, pt)) {return n;} - } - return nullptr; - } - - const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { - - const GridPoint gpStart = p3ToGp(params.start); - const Node* startNode = grid.getNodePtrFor(gpStart); - - static Distribution::Normal dDist(1, 0.02); - static Distribution::Normal dHead(0, 0.01); - - // 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); - - WalkResult res; - res.heading = params.heading; - res.position = params.start; - float realDist_m = params.distance_m; - - int cnt = 0; - while(true) { - - const Point2 dir = res.heading.asVector(); - const Point2 dst = params.start.xy() + (dir * realDist_m); - - // is dst reachable? - const Node* n = contains(grid, reachableNodes, dst); - if (n) { - - const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); - const GridPoint gp = p3ToGp(p3); - - if (grid.hasNodeFor(gp)) { - res.position = p3; // update position - res.heading; // keep as-is - res.probability; // keep as-is - return res; // done - } else { - std::cout << "WARN dst not found" << std::endl; - //throw "should not happen"; - } - - } - - // reduce probability with every new run - ++cnt; - res.probability /= 2; - - // before trying again, modify distance and angle - //if (1 == 1) { - res.heading = params.heading + dHead.draw() * cnt; - realDist_m = params.distance_m * dDist.draw();// * cnt; - //} - - // reached max retries? - if (cnt > 8) { - res.position = params.start; // reset - res.heading = params.heading; // reset - res.probability = 1e-50; // unlikely - return res; - } // did not work out.... - - } - - throw "error"; - - } - -// const Node* _getFromPossibleWalks(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { - -// const Node* startNode = grid.getNodePtrFor(start); - -// Heading desiredHeading = ctrl.heading; - -// DrawList 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::getProbability(desiredDist, 0.1, walkDist); - } - - double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { - const Node* nStart = w.front(); - const Node* nEnd = w.back(); - if (nStart == nEnd) { - std::cout << "warn! start == end" << std::endl; - return 0; - } - Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm); - const float diff = head.getDiffHalfRAD(desiredHeading); - return Distribution::Normal::getProbability(0, 0.3, diff); - } - - double evalHeadingChanges(const Walk& w) const { - Stats::Variance var; - for (int i = 0; i < w.size()-2; ++i) { - const Node* n0 = w[i+0]; - const Node* n1 = w[i+1]; - const Node* n2 = w[i+2]; - Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm); - Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm); - const float diff = h1.getDiffHalfRAD(h2); - var.add(diff); - } - const float totalVar = var.get(); - return Distribution::Normal::getProbability(0, 0.3, totalVar); - } - - double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const { - - return 1.0 - * evalHeadingStartEnd(w, desiredHeading) - * evalDistance(w, desiredDistance) - // * evalHeadingChanges(w); - ; - - } - - - - -// Walk getRandomWalk2(Grid& grid, const Node* start, const float dist_m) const { - -// Walk walk; - -// float dist = 0; - -// const Node* cur = start; -// while(true) { - -// walk.push_back(cur); -// if (dist > dist_m) {break;} - -// const int numNeighbors = cur->getNumNeighbors(); -// //std::cout << "neighbors: " << numNeighbors << std::endl; -// int idx = rand() % numNeighbors; -// const Node* next = &grid.getNeighbor(*cur, idx); -// dist += next->inMeter().getDistance(cur->inMeter()); -// cur = next; - -// } - -// return walk; - -// } - -}; - -#endif // GRIDWALKER3_H diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h new file mode 100644 index 0000000..5c2ca68 --- /dev/null +++ b/grid/walk/v3/Helper.h @@ -0,0 +1,133 @@ +#ifndef INDOOR_GW3_HELPER_H +#define INDOOR_GW3_HELPER_H + +#include "../../../nav/dijkstra/Dijkstra.h" +#include "../../Grid.h" + +#include "Structs.h" + +#include + +namespace GW3 { + + template class Helper { + + public: + + static GridPoint p3ToGp(const Point3 p) { + const Point3 p100 = p*100; + return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) ); + } + + static Point3 gpToP3(const GridPoint gp) { + return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f); + } + + /** does the given grid-node contain the provided point-in-question? */ + static bool contains(const Grid& grid, const Node* n, Point2 pt) { + const float gridSize_m = grid.getGridSize_cm() / 100.0f; + const float d = gridSize_m / 2.0f; + const Point2 pMin = n->inMeter().xy() - Point2(d, d); + const Point2 pMax = n->inMeter().xy() + Point2(d, d); + const BBox2 bbox(pMin, pMax); + return bbox.contains(pt); + } + + /** does one of the given grid-nodes contains the provided point-in-question? */ + static const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { + for (const Node* n : nodes) { + if (contains(grid, n, pt)) {return n;} + } + return nullptr; + } + + /** get all possible walks from start within a given region */ + static Walks getAllPossibleWalks(Grid& grid, const Node* start, const float dist_m) { + + struct Access { + Grid& grid; + Access(Grid& 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 dijkstra; + dijkstra.build(start, nullptr, acc, maxDist_m); + + const std::unordered_map*>& nodes = dijkstra.getNodes(); + + Walks walks; + + for (const auto& it : nodes) { + Walk walk; + DijkstraNode* node = it.second; + do { + const Node* gridNode = node->element; + walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted + node = node->previous; + } while (node); + walks.push_back(walk); + } + + return walks; + + } + + /** get all reachable nodes that are within a given range */ + static Nodes getAllReachableNodes(Grid& grid, const Node* start, const float dist_m) { + + Nodes res; + std::unordered_map distances; + std::vector 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 // INDOOR_GW3_HELPER_H diff --git a/grid/walk/v3/Structs.h b/grid/walk/v3/Structs.h new file mode 100644 index 0000000..335f0e3 --- /dev/null +++ b/grid/walk/v3/Structs.h @@ -0,0 +1,43 @@ +#ifndef INDOOR_GW3_STRUCTS_H +#define INDOOR_GW3_STRUCTS_H + +#include "../../../geo/Heading.h" +#include "../../../geo/Point3.h" +#include + +namespace GW3 { + + /** paremters for the walk */ + struct WalkParams { + Point3 start; + float distance_m; + Heading heading = Heading(0); + }; + + /** result of the random walk */ + struct WalkResult { + Point3 position; + Heading heading = Heading(0); + double probability = 1.0; + }; + + /** several nodes */ + template struct Nodes : public std::vector { + + }; + + /** one walk along several nodes */ + template struct Walk : public std::vector { + + }; + + /** several walks */ + template struct Walks : public std::vector> { + + }; + + + +} + +#endif // INDOOR_GW3_STRUCTS_H diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h new file mode 100644 index 0000000..c740e72 --- /dev/null +++ b/grid/walk/v3/WalkEvaluator.h @@ -0,0 +1,91 @@ +#ifndef INDOOR_GW3_WALKEVALUATOR_H +#define INDOOR_GW3_WALKEVALUATOR_H + +#include "Structs.h" +#include "Helper.h" + +#include "../../../math/Distributions.h" +#include "../../../grid/Grid.h" + +namespace GW3 { + + /** interface for all evaluators that return a probability for a given walk */ + template class WalkEvaluator { + + public: + + /** get the probability for the given walk */ + //virtual double getProbability(const Walk& walk) const = 0; + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0; + + }; + + + /** evaluate the grid-node-importance importance(end) */ + template class WalkEvalEndNodeProbability : public WalkEvaluator { + + Grid* grid; + + public: + + WalkEvalEndNodeProbability(Grid* grid) : grid(grid) {;} + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + + (void) params; + (void) pStart; + + const GridPoint gp = Helper::p3ToGp(pEnd); + const Node& node = grid->getNodeFor(gp); + const double p = node.getWalkImportance(); + return std::pow(p,10); + + } + + }; + + + + /** evaluate the difference between head(start,end) and the requested heading */ + template class WalkEvalHeadingStartEnd : public WalkEvaluator { + + public: + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + + (void) params; + + if (pStart == pEnd) { + std::cout << "warn! start-position == end-positon" << std::endl; + return 0; + } + + const Heading head(pStart.xy(), pEnd.xy()); + const float diff = head.getDiffHalfRAD(params.heading); + //const float diff = Heading::getSignedDiff(params.heading, head); + return Distribution::Normal::getProbability(0, 0.04, diff); + + } + + }; + + /** evaluate the difference between distance(start, end) and the requested distance */ + template class WalkEvalDistance : public WalkEvaluator { + + const double sigma = 0.1f; + + public: + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + + const float walkedDistance_m = pStart.getDistance(pEnd); + return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); + + } + + }; + +} + +#endif // INDOOR_GW3_WALKEVALUATOR_H diff --git a/grid/walk/v3/WalkGenerator.h b/grid/walk/v3/WalkGenerator.h new file mode 100644 index 0000000..28f9cc1 --- /dev/null +++ b/grid/walk/v3/WalkGenerator.h @@ -0,0 +1,20 @@ +#ifndef INDOOR_GRIDWALKER3GENERATOR_H +#define INDOOR_GRIDWALKER3GENERATOR_H + +#include "Structs.h" + + +namespace GW3 { + + class GridWalker3Generator { + + virtual Walk getPossibleWalks() = 0; + + }; + + + + +} + +#endif // INDOOR_GRIDWALKER3GENERATOR_H diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h new file mode 100644 index 0000000..894b631 --- /dev/null +++ b/grid/walk/v3/Walker.h @@ -0,0 +1,376 @@ +#ifndef INDOOR_GW3_WALKER_H +#define INDOOR_GW3_WALKER_H + +#include "../../../data/Timestamp.h" +#include "../../../math/DrawList.h" +#include "../../../math/Distributions.h" +#include "../../../math/Stats.h" +#include "../../../geo/Heading.h" +#include "../../../math/stats/Variance.h" +#include "../../../geo/BBox2.h" + +#include "../../Grid.h" + +#include "Helper.h" +#include "Structs.h" +#include "WalkEvaluator.h" + +namespace GW3 { + + /** + * modular grid-walker that takes various sub-components to determine + * p(e) and thus randomly pick edges + */ + template class WalkerBase { + + public: + + /** get a new destination for the given start */ + virtual const WalkResult getDestination(Grid& grid, const WalkParams& params) const = 0; + + }; + + template class WalkerDirectDestination : public WalkerBase { + + //RandomGenerator rnd; + + std::vector*> evals; + + public: + + /** make the code a little more readable */ + using Helper = GW3::Helper; + using Walk = typename GW3::Walk; + using Walks = typename GW3::Walks; + using Nodes = typename GW3::Nodes; + + /** add a new evaluator to the walker */ + void addEvaluator(WalkEvaluator* eval) { + this->evals.push_back(eval); + } + + /** perform the walk based on the configured setup */ + const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + + 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); + + 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); + + // is dst reachable? + const Node* n = Helper::contains(grid, reachableNodes, dst); + if (n) { + + const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); + const GridPoint gp = Helper::p3ToGp(p3); + + if (grid.hasNodeFor(gp)) { + res.position = p3; // update position + res.heading; // keep as-is + res.probability; // keep as-is + return res; // done + } else { + std::cout << "WARN dst not found" << std::endl; + //throw "should not happen"; + } + + } + + // not found -> try random pick among all reachable nodes + const float gridSize_m = grid.getGridSize_cm() / 100.0f; + std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); + + const Node* dstNode = reachableNodes[dNode(rndGen)]; + + // random position within destination-node + std::uniform_real_distribution dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f); + const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + + const Point3 start = params.start; + const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (start == end) { + res.probability = 0; + return res; + } + + res.heading = Heading(start.xy(), end.xy()); + res.probability = p; + res.position = end; + return res; + + } + + }; + + + /** + * 1) get all reachable nodes from "start" (within a certain distance) + * 2) randomly pick one of em + * 3) scatter positon within the node's square -> end position + * 4) evaluate the probability of walking from start -> end + */ + template class WalkerWeightedRandom : public WalkerBase { + + std::vector*> evals; + + public: + + /** make the code a little more readable */ + using Helper = GW3::Helper; + using Walk = typename GW3::Walk; + using Walks = typename GW3::Walks; + using Nodes = typename GW3::Nodes; + + /** add a new evaluator to the walker */ + void addEvaluator(WalkEvaluator* eval) { + this->evals.push_back(eval); + } + + /** perform the walk based on the configured setup */ + const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + + 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); + + // 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); + + + Point3 best; + double bestP = 0; + + 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) { + + 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); + + // 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;} + + 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 = best; + WalkResult res; + if (start == end) { + res.probability = 0; + } else { + res.heading = Heading(start.xy(), end.xy()); + res.probability = bestP; // 1 + } + res.position = end; + return res; + + } + + }; + + + + + //const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { + + + + + // Distribution::Normal dDist(1, 0.02); + // Distribution::Normal dHead(0, 0.01); + +// int cnt = 0; +// while(true) { + +// const Point2 dir = res.heading.asVector(); +// const Point2 dst = params.start.xy() + (dir * realDist_m); + +// // is dst reachable? +// const Node* n = Helper::contains(grid, reachableNodes, dst); +// if (n) { + +// const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); +// const GridPoint gp = Helper::p3ToGp(p3); + +// if (grid.hasNodeFor(gp)) { +// res.position = p3; // update position +// res.heading; // keep as-is +// res.probability; // keep as-is +// return res; // done +// } else { +// std::cout << "WARN dst not found" << std::endl; +// //throw "should not happen"; +// } + +// } + +// // reduce probability with every new run +// ++cnt; +// res.probability /= 2; + +// // before trying again, modify distance and angle +// //if (1 == 1) { +// res.heading = params.heading + dHead.draw() * cnt; +// realDist_m = params.distance_m * dDist.draw();// * cnt; +// //} + +// // reached max retries? +// if (cnt > 8) { +// res.position = params.start; // reset +// res.heading = params.heading; // reset +// res.probability = 1e-50; // unlikely +// return res; +// } // did not work out.... + +// } + + // throw "error"; + + //} + + + // const Node* _getFromPossibleWalks(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + + // const Node* startNode = grid.getNodePtrFor(start); + + // Heading desiredHeading = ctrl.heading; + + // DrawList 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::getProbability(desiredDist, 0.1, walkDist); +// } + +// double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { +// const Node* nStart = w.front(); +// const Node* nEnd = w.back(); +// if (nStart == nEnd) { +// std::cout << "warn! start == end" << std::endl; +// return 0; +// } +// Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm); +// const float diff = head.getDiffHalfRAD(desiredHeading); +// return Distribution::Normal::getProbability(0, 0.3, diff); +// } + +// double evalHeadingChanges(const Walk& w) const { +// Stats::Variance var; +// for (int i = 0; i < w.size()-2; ++i) { +// const Node* n0 = w[i+0]; +// const Node* n1 = w[i+1]; +// const Node* n2 = w[i+2]; +// Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm); +// Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm); +// const float diff = h1.getDiffHalfRAD(h2); +// var.add(diff); +// } +// const float totalVar = var.get(); +// return Distribution::Normal::getProbability(0, 0.3, totalVar); +// } + +// double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const { + +// return 1.0 +// * evalHeadingStartEnd(w, desiredHeading) +// * evalDistance(w, desiredDistance) +// // * evalHeadingChanges(w); +// ; + +// } + + + + + // Walk getRandomWalk2(Grid& grid, const Node* start, const float dist_m) const { + + // Walk walk; + + // float dist = 0; + + // const Node* cur = start; + // while(true) { + + // walk.push_back(cur); + // if (dist > dist_m) {break;} + + // const int numNeighbors = cur->getNumNeighbors(); + // //std::cout << "neighbors: " << numNeighbors << std::endl; + // int idx = rand() % numNeighbors; + // const Node* next = &grid.getNeighbor(*cur, idx); + // dist += next->inMeter().getDistance(cur->inMeter()); + // cur = next; + + // } + + // return walk; + + // } + +} + +#endif // INDOOR_GW3_WALKER_H