worked on grid-walker and synthetic steps/turns
This commit is contained in:
k-a-z-u
@@ -193,10 +193,22 @@ namespace GW3 {
|
||||
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 Nodes<Node>& nodes, Point2 pt) {
|
||||
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;}
|
||||
if (contains(grid, n, pt)) {
|
||||
return n;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
@@ -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() {
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -65,35 +65,63 @@ namespace GW3 {
|
||||
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
||||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||||
|
||||
// calculate a walk's probability
|
||||
auto getP = [&] (const Point3 dst) {
|
||||
double p = 1;
|
||||
for (const WalkEvaluator<Node>* eval : evals) {
|
||||
const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params);
|
||||
p *= p1;
|
||||
}
|
||||
return p;
|
||||
};
|
||||
|
||||
// include one additional grid-cell (increased distance)
|
||||
const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
|
||||
ReachableSettings set;
|
||||
set.limitDistance = true;
|
||||
set.dist_m = params.distance_m + secBuffer_m;
|
||||
set.limitHeading = false;
|
||||
set.heading = params.heading;
|
||||
set.maxHeadingDiff_rad = M_PI/2;
|
||||
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
|
||||
//const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1);
|
||||
const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15);
|
||||
|
||||
// ReachableSettings set;
|
||||
// set.limitDistance = true;
|
||||
// set.dist_m = params.distance_m + secBuffer_m;
|
||||
// set.limitHeading = false;
|
||||
// set.heading = params.heading;
|
||||
// set.maxHeadingDiff_rad = M_PI/2;
|
||||
|
||||
// // get all nodes that satisfy above constraints
|
||||
// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
|
||||
|
||||
struct Cond {
|
||||
const float maxDist_m;
|
||||
const Node* startNode;
|
||||
Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
|
||||
bool visit(const Node& n) const {
|
||||
return (startNode->getDistanceInMeter(n)) < maxDist_m;
|
||||
}
|
||||
};
|
||||
Cond cond(params.distance_m+secBuffer_m, startNode);
|
||||
std::vector<const Node*> reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
|
||||
|
||||
WalkResult res;
|
||||
res.heading = params.heading;
|
||||
res.position = params.start;
|
||||
|
||||
|
||||
// get the to-be-reached destination's position (using start+distance+heading)
|
||||
const Point2 dir = res.heading.asVector();
|
||||
const Point2 dst = params.start.xy() + (dir * params.distance_m);
|
||||
|
||||
// is dst reachable?
|
||||
// is above destination reachable?
|
||||
const Node* n = Helper::contains(grid, reachableNodes, dst);
|
||||
//const Node* n = ri.contains(dst);
|
||||
if (n) {
|
||||
|
||||
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
||||
const GridPoint gp = Helper::p3ToGp(p3);
|
||||
|
||||
|
||||
|
||||
if (grid.hasNodeFor(gp)) {
|
||||
res.position = p3; // update position
|
||||
//res.heading; // keep as-is
|
||||
//res.probability; // keep as-is
|
||||
res.probability *= getP(p3); // keep as-is
|
||||
return res; // done
|
||||
|
||||
} else {
|
||||
@@ -129,7 +157,7 @@ namespace GW3 {
|
||||
}
|
||||
|
||||
res.heading = Heading(start.xy(), end.xy());
|
||||
res.probability = p;
|
||||
res.probability *= getP(end);
|
||||
res.position = end;
|
||||
return res;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user