351 lines
8.8 KiB
C++
351 lines
8.8 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_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
|