114 lines
2.8 KiB
C++
114 lines
2.8 KiB
C++
#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
|