FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

dijkstra is now bleching fast

Browse Source 
deleting from the grid is now bleaching fast
added new helper methods
many new test-cases
many new methods for geo classes and others
added a bunch of new grid-walkers
This commit is contained in:
FrankE
2016-01-26 18:13:30 +01:00
parent b503fb9bdc
commit e6329e1db4
26 changed files with 824 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
grid/walk/GridWalkLightAtTheEndOfTheTunnel.h
View File

@@ -18,10 +18,12 @@
*/
template <typename T> class GridWalkLightAtTheEndOfTheTunnel {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
/** per-edge: allowed heading difference */
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
@@ -62,23 +64,7 @@ public:
GridWalkState<T> getDestination(Grid<T>& grid, GridWalkState<T> start, float distance_m) {
int retries = 2;
GridWalkState<T> res;
// try to walk the given distance from the start
// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
do {
res = walk(grid, start, distance_m);
} while (res.node == nullptr && --retries);
// still reaching a dead end?
// -> try a walk in the opposite direction instead
if (res.node == nullptr) {
res = walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> keep the start as-is
return (res.node == nullptr) ? (start) : (res);
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
}
@@ -102,7 +88,17 @@ private:
// perfer locations reaching the target
const double shortening = cur.node->distToTarget - neighbor.distToTarget;
if (shortening > 0) {prob *= 30;} // << importance factor!!
if (shortening >= 0) {prob *= 5;} // << importance factor!!
// prob = 0.1;
// if (diff < Angle::degToRad(40)) {prob += 0.2;}
// else if (diff < Angle::degToRad(20)) {prob += 0.5;}
// if (shortening >= 0) {prob += 0.5;}
//prob *= std::pow(neighbor.imp, 5);
//prob = (shortening >= 0) ? (2) : (0.75);
drawer.add(neighbor, prob);
@@ -114,34 +110,38 @@ private:
T& nDir = drawer.get();
const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
//next.heading = Heading( cur.heading.getRAD() * 0.2 + hDir.getRAD() * 0.8 );
next.heading = hDir;
next.heading += headingChangeDist(gen);
next.node = &nDir;
// compare two neighbors according to their implied heading change
auto compp = [&] (const T& n1, const T& n2) {
Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
const float d1 = next.heading.getDiffHalfRAD(h1);
const float d2 = next.heading.getDiffHalfRAD(h2);
// same heading -> prefer nodes nearer to the target. needed for stairs!!!
// BAD: leads to straight lines in some palces. see solution B (below)
//return (d1 < d2) && (n1.distToTarget < n2.distToTarget);
//// // compare two neighbors according to their implied heading change
//// auto compp = [&] (const T& n1, const T& n2) {
//// Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
//// Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
//// const float d1 = next.heading.getDiffHalfRAD(h1);
//// const float d2 = next.heading.getDiffHalfRAD(h2);
//// // same heading -> prefer nodes nearer to the target. needed for stairs!!!
//// // BAD: leads to straight lines in some palces. see solution B (below)
//// //return (d1 < d2) && (n1.distToTarget < n2.distToTarget);
// VERY IMPORTANT!
// pick the node with the smallest heading change.
// if the heading change is the same for two nodes, pick a random one!
return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);
};
//// // VERY IMPORTANT!
//// // pick the node with the smallest heading change.
//// // if the heading change is the same for two nodes, pick a random one!
//// return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);
//// };
// pick the neighbor best matching the new heading
auto it = grid.neighbors(*cur.node);
T& nn = *std::min_element(it.begin(), it.end(), compp);
next.node = &nn;
//// // pick the neighbor best matching the new heading
//// auto it = grid.neighbors(*cur.node);
//// T& nn = *std::min_element(it.begin(), it.end(), compp);
//// next.node = &nn;
// // pervent dramatic heading changes. instead: try again
// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
// return State(nullptr, 0);
// }
// next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
//// // pervent dramatic heading changes. instead: try again
//// if (cur.heading.getDiffHalfRAD(GridWalkHelper::getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
//// return GridWalkState<T>(nullptr, 0);
//// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);