worked on grid-walker and synthetic steps/turns
This commit is contained in:
k-a-z-u
@@ -7,250 +7,333 @@
|
||||
|
||||
namespace GW3 {
|
||||
|
||||
#define likely(x) __builtin_expect((x),1)
|
||||
#define unlikely(x) __builtin_expect((x),0)
|
||||
#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 {
|
||||
/**
|
||||
* 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 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;
|
||||
}
|
||||
};
|
||||
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;
|
||||
const Grid<Node>& grid;
|
||||
|
||||
public:
|
||||
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));
|
||||
ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
|
||||
;
|
||||
}
|
||||
|
||||
// check all to-be-visited nodes
|
||||
while ( likely(toVisit.hasMore()) ) {
|
||||
/** get all nodes reachable from start using maxDepth steps */
|
||||
std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
|
||||
|
||||
const VisitEntry& e = toVisit.getNext();
|
||||
std::unordered_set<const Node*> checked;
|
||||
|
||||
if ( likely(e.depth <= maxDepth) ) {
|
||||
// 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!");}
|
||||
|
||||
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);
|
||||
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;
|
||||
|
||||
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) {
|
||||
/**
|
||||
* get all grid nodes that are reachable within x-edges (depth)
|
||||
* additionally returns the needed walking distance in meter
|
||||
*/
|
||||
template <typename Node> class ReachableByDepthWithDistanceSorted {
|
||||
|
||||
std::unordered_set<const Node*> checked;
|
||||
std::vector<Entry> res;
|
||||
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) {;}
|
||||
};
|
||||
|
||||
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
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
std::sort(sub.begin(), sub.end(), 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);
|
||||
|
||||
for (const VisitEntry& e : sub) {
|
||||
toVisit.add(e);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// slower with same result ;)
|
||||
//toVisit.sort();
|
||||
// done
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
//const Node& next(const std::function<bool(const Node&)>& skip) {
|
||||
//template <typename Skip> const Node& next(const Skip skip) {
|
||||
const Node& next() {
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user