From 3807c621c739241b7dc3455dce43f3484c972f35 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 17 Oct 2017 13:01:26 +0200 Subject: [PATCH] added new helper methods worked on gridWalker v3 --- geo/Circle2.h | 19 ++++ geo/Heading.h | 13 ++- grid/GridNode.h | 3 + grid/factory/v2/GridFactory.h | 6 +- grid/walk/v3/GridWalk3Helper.h | 113 +++++++++++++++++++++ grid/walk/v3/GridWalker3.h | 173 ++++++++++++++++++++++++++------- math/Interpolator.h | 5 + nav/dijkstra/Dijkstra.h | 9 +- 8 files changed, 299 insertions(+), 42 deletions(-) create mode 100644 grid/walk/v3/GridWalk3Helper.h diff --git a/geo/Circle2.h b/geo/Circle2.h index 49eccc2..e335fb8 100644 --- a/geo/Circle2.h +++ b/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& lst) { diff --git a/geo/Heading.h b/geo/Heading.h index ca22b6c..a2952d4 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -77,10 +77,10 @@ public: } - Heading& operator = (const float _rad) { - rad = _rad; - return *this; - } + Heading& operator = (const float _rad) { + rad = _rad; + 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); diff --git a/grid/GridNode.h b/grid/GridNode.h index 6b21e87..306105c 100755 --- a/grid/GridNode.h +++ b/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;} diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index d79abc8..4a5809a 100755 --- a/grid/factory/v2/GridFactory.h +++ b/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(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(fo)) { // DOORS ARE NOT AN OBSTACLE diff --git a/grid/walk/v3/GridWalk3Helper.h b/grid/walk/v3/GridWalk3Helper.h new file mode 100644 index 0000000..5b79c75 --- /dev/null +++ b/grid/walk/v3/GridWalk3Helper.h @@ -0,0 +1,113 @@ +#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 index e42c91b..29d025c 100644 --- a/grid/walk/v3/GridWalker3.h +++ b/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 class GridWalker3 { +template class GridWalker3 { private: @@ -24,38 +26,135 @@ private: public: - - /** one walk along several nodes */ - struct Walk : public std::vector { - + struct WalkParams { + Point3 start; + float distance_m; + Heading heading = Heading(0); }; -// struct Walks : std::vector { -// }; + 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 Node* getDestination(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + const Point3 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); - const Node* startNode = grid.getNodePtrFor(start); + } - Heading desiredHeading = ctrl.heading; +// /** perform the walk based on the configured setup */ +// const Point3 getDestination(Grid& grid, const GridPoint start, const params) { - DrawList walks; +// //return _getFromPossibleWalks(grid, start, ctrl, dist_m); +// //return _drawThenCheck(grid, start, ctrl, dist_m); +// throw "error"; +// } + + 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); + } + + const Point3 _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.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(); - const Node* dst = res.back(); - return dst; + 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(); @@ -83,39 +182,43 @@ public: return Distribution::Normal::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& grid, const Node* start, const float dist_m) const { - Walk walk; - float dist = 0; - const Node* cur = start; - while(true) { +// Walk getRandomWalk2(Grid& grid, const Node* start, const float dist_m) const { - walk.push_back(cur); - if (dist > dist_m) {break;} +// Walk walk; - 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; +// float dist = 0; - } +// 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; + +// } }; diff --git a/math/Interpolator.h b/math/Interpolator.h index d362949..b1f0b57 100644 --- a/math/Interpolator.h +++ b/math/Interpolator.h @@ -56,6 +56,11 @@ public: } + /** get a list of all raw entries */ + const std::vector& getEntries() const { + return entries; + } + protected: /** special interpolation for the timestamp class */ diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index 8ca5ccb..f1ffbe3 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/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* getNode(const T& userNode) const { + inline const DijkstraNode* 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*>& getNodes() const { + return nodes; + } + /** calculate all shortest paths from ANY node to the given destination */ template 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) {