341 lines
8.5 KiB
C++
341 lines
8.5 KiB
C++
#ifndef INDOOR_GW3_REACHABLE_H
|
|
#define INDOOR_GW3_REACHABLE_H
|
|
|
|
#include <vector>
|
|
#include <set>
|
|
#include "../../Grid.h"
|
|
|
|
namespace GW3 {
|
|
|
|
#define likely(x) __builtin_expect((x),1)
|
|
#define unlikely(x) __builtin_expect((x),0)
|
|
|
|
|
|
|
|
/**
|
|
* get all grid nodes that are reachable within x-edges (depth)
|
|
*/
|
|
template <typename Node> class ReachableByDepthUnsorted {
|
|
|
|
struct VisitEntry {
|
|
const Node* gn;
|
|
int depth;
|
|
VisitEntry() {;}
|
|
VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;}
|
|
};
|
|
|
|
struct Visits {
|
|
VisitEntry visits[512];// __attribute__((aligned(16)));
|
|
size_t head = 0;
|
|
size_t tail = 0;
|
|
VisitEntry& getNext() {
|
|
return visits[tail++];
|
|
}
|
|
void add(const VisitEntry& e) {
|
|
visits[head++] = e;
|
|
assert(head < 512);
|
|
//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
|
|
}
|
|
bool hasMore() const {
|
|
return head > tail;
|
|
}
|
|
};
|
|
|
|
const Grid<Node>& grid;
|
|
|
|
public:
|
|
|
|
ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
|
|
;
|
|
}
|
|
|
|
/** get all nodes reachable from start using maxDepth steps */
|
|
std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
|
|
|
|
std::unordered_set<const Node*> checked;
|
|
|
|
// assuming max 8 neighbors per node, we need
|
|
// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
|
|
// which is 1 + (1+2+3+4+5)*neighbors
|
|
// which is 1 + (n*n + n)/2*neighbors
|
|
// however this seems to be slow?!
|
|
//const int n = maxDepth + 1;
|
|
//const int maxEntries = (n * n + n) / 2 * 10 + 1;
|
|
//const int toAlloc = 4096 / sizeof(VisitEntry);
|
|
//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
|
|
//if (maxDepth > 9) {throw Exception("will not fit!");}
|
|
|
|
Visits toVisit;
|
|
|
|
// directly start with the node itself and all its neighbors
|
|
checked.insert(&start);
|
|
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
|
|
const int nIdx = start.getNeighborIdx(i);
|
|
const Node& gnNext = grid[nIdx];
|
|
checked.insert(&gnNext);
|
|
toVisit.add(VisitEntry(&gnNext, 1));
|
|
}
|
|
|
|
// check all to-be-visited nodes
|
|
while ( likely(toVisit.hasMore()) ) {
|
|
|
|
const VisitEntry& e = toVisit.getNext();
|
|
|
|
if ( likely(e.depth <= maxDepth) ) {
|
|
|
|
const Node* gnCur = e.gn;
|
|
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
|
|
const int nIdx = gnCur->getNeighborIdx(i);
|
|
const Node& gnNext = grid[nIdx];
|
|
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
|
|
toVisit.add(VisitEntry(&gnNext, e.depth+1));
|
|
checked.insert(&gnNext);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return checked;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
/**
|
|
* get all grid nodes that are reachable within x-edges (depth)
|
|
* additionally returns the needed walking distance in meter
|
|
*/
|
|
template <typename Node> class ReachableByDepthWithDistanceSorted {
|
|
|
|
struct VisitEntry {
|
|
const Node* gn;
|
|
int depth;
|
|
float dist_m;
|
|
int myIdx;
|
|
VisitEntry() {;}
|
|
VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
|
|
gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
|
|
};
|
|
|
|
struct Visits {
|
|
VisitEntry visits[1024];// __attribute__((aligned(16)));
|
|
size_t head = 0;
|
|
size_t tail = 0;
|
|
VisitEntry& getNext() {
|
|
return visits[tail++];
|
|
}
|
|
void add(const VisitEntry& e) {
|
|
visits[head++] = e;
|
|
assert(head < 1024);
|
|
//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
|
|
}
|
|
bool hasMore() const {
|
|
return head > tail;
|
|
}
|
|
void sort() {
|
|
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
|
|
return e1.dist_m < e2.dist_m;
|
|
};
|
|
std::sort(&visits[tail], &visits[head], comp);
|
|
}
|
|
};
|
|
|
|
const Grid<Node>& grid;
|
|
|
|
public:
|
|
|
|
/** result */
|
|
struct Entry {
|
|
|
|
const Node* node;
|
|
const float walkDistToStart_m;
|
|
const int prevIdx;
|
|
|
|
Entry(const Node* node, const float dist, const size_t prevIdx) :
|
|
node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}
|
|
|
|
bool hasPrev() const {
|
|
return prevIdx >= 0;
|
|
}
|
|
|
|
};
|
|
|
|
ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
|
|
;
|
|
}
|
|
|
|
/** get all nodes reachable from start using maxDepth steps */
|
|
std::vector<Entry> get(const Node& start, const int maxDepth) {
|
|
|
|
std::unordered_set<const Node*> checked;
|
|
std::vector<Entry> res;
|
|
|
|
Visits toVisit;
|
|
|
|
// directly start with the node itself and all its neighbors
|
|
checked.insert(&start);
|
|
res.push_back(Entry(&start, 0, -1));
|
|
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
|
|
const int nIdx = start.getNeighborIdx(i);
|
|
const Node& gnNext = grid[nIdx];
|
|
const float dist_m = gnNext.getDistanceInMeter(start);
|
|
toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size()));
|
|
res.push_back(Entry(&gnNext, dist_m, 0));
|
|
checked.insert(&gnNext);
|
|
}
|
|
toVisit.sort();
|
|
|
|
// check all to-be-visited nodes
|
|
while ( likely(toVisit.hasMore()) ) {
|
|
|
|
const VisitEntry& e = toVisit.getNext();
|
|
|
|
if ( likely(e.depth <= maxDepth) ) {
|
|
|
|
const Node* gnCur = e.gn;
|
|
|
|
// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
|
|
// const int nIdx = gnCur->getNeighborIdx(i);
|
|
// const Node& gnNext = grid[nIdx];
|
|
// if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
|
|
// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
|
|
// const float dist_m = e.dist_m + nodeNodeDist_m;
|
|
// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
|
|
// res.push_back(Entry(&gnNext, dist_m, e.myIdx));
|
|
// checked.insert(&gnNext);
|
|
// }
|
|
// }
|
|
|
|
// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
|
|
|
|
std::vector<VisitEntry> sub;
|
|
|
|
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
|
|
const int nIdx = gnCur->getNeighborIdx(i);
|
|
const Node& gnNext = grid[nIdx];
|
|
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
|
|
const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
|
|
const float dist_m = e.dist_m + nodeNodeDist_m;
|
|
//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
|
|
sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
|
|
res.push_back(Entry(&gnNext, dist_m, e.myIdx));
|
|
checked.insert(&gnNext);
|
|
|
|
}
|
|
}
|
|
|
|
// dijkstra.. sort the new nodes by destination to start
|
|
// only sorting the 8 new nodes seems enough due to the graph's layout
|
|
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
|
|
return e1.dist_m < e2.dist_m;
|
|
};
|
|
|
|
std::sort(sub.begin(), sub.end(), comp);
|
|
|
|
for (const VisitEntry& e : sub) {
|
|
toVisit.add(e);
|
|
}
|
|
|
|
}
|
|
|
|
// slower with same result ;)
|
|
//toVisit.sort();
|
|
|
|
}
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
/**
|
|
* 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 a list of all nodes that are reachable after checking several conditions */
|
|
template <typename Node, typename Conditions> class ReachableByConditionUnsorted {
|
|
|
|
|
|
public:
|
|
|
|
|
|
static std::vector<const Node*> get(const Grid<Node>& grid, const Node& start, const Conditions cond) {
|
|
|
|
//Node* curNode = nullptr;
|
|
std::unordered_set<uint32_t> scheduled;
|
|
_ToVisit toVisit;
|
|
toVisit.add(start.getIdx());
|
|
|
|
std::vector<const Node*> res;
|
|
|
|
while(!toVisit.empty()) {
|
|
|
|
// 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];
|
|
|
|
// process current node
|
|
res.push_back(&curNode);
|
|
scheduled.insert(curIdx);
|
|
|
|
// get all neighbors
|
|
const int numNeighbors = curNode.getNumNeighbors();
|
|
for (int i = 0; i < numNeighbors; ++i) {
|
|
|
|
const uint32_t neighborIdx = curNode.getNeighborIdx(i);
|
|
|
|
// already visited?
|
|
if (scheduled.find(neighborIdx) != scheduled.end()) {continue;}
|
|
scheduled.insert(neighborIdx);
|
|
|
|
// matches the used condition?
|
|
const Node& neighbor = grid[neighborIdx];
|
|
if (!cond.visit(neighbor)) {continue;}
|
|
|
|
// OK!
|
|
toVisit.add(neighborIdx);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// done
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
//const Node& next(const std::function<bool(const Node&)>& skip) {
|
|
//template <typename Skip> const Node& next(const Skip skip) {
|
|
const Node& next() {
|
|
|
|
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
}
|
|
|
|
#endif // REACHABLE_H
|