343 lines
9.4 KiB
C++
343 lines
9.4 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef INDOOR_GW3_HELPER_H
|
||
#define INDOOR_GW3_HELPER_H
|
||
|
||
#include "../../../nav/dijkstra/Dijkstra.h"
|
||
#include "../../Grid.h"
|
||
|
||
#include "Structs.h"
|
||
|
||
#include <vector>
|
||
#include <set>
|
||
|
||
#include <KLib/math/random/RandomIterator.h>
|
||
|
||
//#define SHOW_DEBUG_PLOT
|
||
|
||
#ifdef SHOW_DEBUG_PLOT
|
||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
||
#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
|
||
#endif
|
||
|
||
namespace GW3 {
|
||
|
||
|
||
/** get an iterator over all nodes reachable from the given start */
|
||
template <typename Node> class ReachableIteratorSorted {
|
||
|
||
const Grid<Node>& 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<uint32_t> visited;
|
||
std::set<Next> toVisit;
|
||
|
||
public:
|
||
|
||
ReachableIteratorSorted(const Grid<Node>& 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;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
/**
|
||
* data-structure to track to-be-visited nodes
|
||
* push_back, pop_front
|
||
* as pop_front is costly, we omit the pop and use a head-index instead
|
||
* memory-consumption vs speed
|
||
*/
|
||
struct ToVisit {
|
||
size_t nextIdx = 0;
|
||
std::vector<uint32_t> vec;
|
||
ToVisit() {vec.reserve(256);}
|
||
void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
|
||
uint32_t next() {return vec[nextIdx++];}
|
||
bool empty() const {return nextIdx >= vec.size();}
|
||
};
|
||
|
||
/** get an iterator over all nodes reachable from the given start */
|
||
template <typename Node, typename Conditions> class ReachableIteratorUnsorted {
|
||
|
||
const Grid<Node>& grid;
|
||
const Node& start;
|
||
|
||
Node* curNode = nullptr;
|
||
std::unordered_set<uint32_t> visited;
|
||
ToVisit toVisit;
|
||
|
||
Conditions cond;
|
||
|
||
public:
|
||
|
||
ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) {
|
||
toVisit.add(start.getIdx());
|
||
}
|
||
|
||
bool hasNext() const {
|
||
return !toVisit.empty();
|
||
}
|
||
|
||
//const Node& next(const std::function<bool(const Node&)>& skip) {
|
||
//template <typename Skip> const Node& next(const Skip skip) {
|
||
const Node& next() {
|
||
|
||
// get the next to-be-visited node
|
||
const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance)
|
||
const Node& curNode = grid[curIdx];
|
||
|
||
// mark as "visited"
|
||
visited.insert(curIdx);
|
||
|
||
// get all neighbors
|
||
const int numNeighbors = curNode.getNumNeighbors();
|
||
for (int i = 0; i < numNeighbors; ++i) {
|
||
|
||
const uint32_t neighborIdx = curNode.getNeighborIdx(i);
|
||
const Node& neighbor = grid[neighborIdx];
|
||
|
||
const bool visit = cond.visit(neighbor) ;
|
||
|
||
// not yet reached -> store distance
|
||
if (visit) {
|
||
if (visited.find(neighborIdx) == visited.end()) {
|
||
toVisit.add(neighborIdx);
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
// done
|
||
return curNode;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
|
||
struct ReachableSettings {
|
||
|
||
float dist_m;
|
||
bool limitDistance = true;
|
||
|
||
Heading heading = Heading(0);
|
||
float maxHeadingDiff_rad;
|
||
bool limitHeading = false;
|
||
|
||
};
|
||
|
||
|
||
template <typename Node> class Helper {
|
||
|
||
public:
|
||
|
||
// static GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
|
||
// const Point3 p100 = p*100;
|
||
// //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);
|
||
}
|
||
|
||
/** does the given grid-node contain the provided point-in-question? */
|
||
static 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);
|
||
}
|
||
|
||
// /** does one of the given grid-nodes contains the provided point-in-question? */
|
||
// static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
|
||
// for (const Node* n : nodes) {
|
||
// if (contains(grid, n, pt)) {
|
||
// return n;
|
||
// }
|
||
// }
|
||
// return nullptr;
|
||
// }
|
||
|
||
/** does one of the given grid-nodes contains the provided point-in-question? */
|
||
static const Node* contains(const Grid<Node>& grid, const std::vector<const Node*>& 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<Node> 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();
|
||
|
||
Walks<Node> walks;
|
||
|
||
for (const auto& it : nodes) {
|
||
Walk<Node> 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 Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const ReachableSettings& set ) {
|
||
|
||
//auto tStart = std::chrono::system_clock::now();
|
||
|
||
Nodes<Node> res;
|
||
std::unordered_map<uint32_t, float> distances;
|
||
std::vector<uint32_t> 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()) {
|
||
|
||
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
|
||
|
||
#ifdef SHOW_DEBUG_PLOT
|
||
pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance);
|
||
gp.draw(plot);
|
||
gp.flush();
|
||
#endif
|
||
|
||
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
|
||
|
||
const int neighborIdx = curNode.getNeighborIdx(i);
|
||
const Node& neighbor = grid[neighborIdx];
|
||
const float addDist = neighbor.getDistanceInMeter(curNode);
|
||
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()) {
|
||
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<std::chrono::milliseconds>(tEnd - tStart);
|
||
//std::cout << elapsed.count() << std::endl;
|
||
|
||
return res;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
}
|
||
|
||
#endif // INDOOR_GW3_HELPER_H
|