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

128
grid/Grid.h
View File

@@ -250,61 +250,123 @@ public:
}
/**
* remove all nodes, marked for deletion.
* BEWARE: this will invalidate all indices used externally!
*/
void cleanup() {
Log::add(name, "running grid cleanup");
// check every single node
for (size_t i = 0; i < nodes.size(); ++i) {
// is this node marked as "deleted"? (idx == -1)
if (nodes[i]._idx == -1) {
// remove this node
deleteNode(i);
--i;
Log::add(name, "running grid cleanup", false);
Log::tick();
// generate a look-up-table for oldIndex (before deletion) -> newIndex (after deletion)
std::vector<int> oldToNew; oldToNew.resize(nodes.size());
int newIdx = 0;
for (size_t oldIdx = 0; oldIdx < nodes.size(); ++oldIdx) {
if (nodes[oldIdx].getIdx() != -1) {
oldToNew[oldIdx] = newIdx;
++newIdx;
}
}
// rebuild hashes
Log::add(name, "rebuilding UID hashes");
// adjust all indices from the old to the new mapping
for (size_t i = 0; i < nodes.size(); ++i) {
// skip the nodes actually marked for deletion
if (nodes[i]._idx == -1) {continue;}
// adjust the node's index
nodes[i]._idx = oldToNew[nodes[i]._idx];
// adjust its neighbor's indices
for (int j = 0; j < nodes[i]._numNeighbors; ++j) {
nodes[i]._neighbors[j] = oldToNew[nodes[i]._neighbors[j]];
}
}
// MUCH(!!!) faster than deleting nodes from the existing node-vector
// is to build a new one and swap those two
std::vector<T> newNodes;
for (size_t i = 0; i < nodes.size(); ++i) {
if (nodes[i]._idx != -1) {newNodes.push_back(nodes[i]);}
}
std::swap(nodes, newNodes);
Log::tock();
rebuildHashes();
}
/** rebuild the UID-hash-list */
void rebuildHashes() {
Log::add(name, "rebuilding UID hashes", false);
Log::tick();
hashes.clear();
for (size_t idx = 0; idx < nodes.size(); ++idx) {
hashes[getUID(nodes[idx])] = idx;
}
Log::tock();
}
// /**
// * remove all nodes, marked for deletion.
// * BEWARE: this will invalidate all indices used externally!
// */
// void cleanupOld() {
// Log::add(name, "running grid cleanup");
// // check every single node
// for (size_t i = 0; i < nodes.size(); ++i) {
// // is this node marked as "deleted"? (idx == -1)
// if (nodes[i]._idx == -1) {
// // remove this node
// deleteNode(i);
// --i;
// }
// }
// // rebuild hashes
// Log::add(name, "rebuilding UID hashes", false);
// Log::tick();
// hashes.clear();
// for (size_t idx = 0; idx < nodes.size(); ++idx) {
// hashes[getUID(nodes[idx])] = idx;
// }
// Log::tock();
// }
private:
/** hard-delete the given node */
void deleteNode(const int idx) {
// /** hard-delete the given node */
// void deleteNode(const int idx) {
_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
// _assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
// // COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
// remove him from the node list (reclaim its memory and its index)
nodes.erase(nodes.begin()+idx);
// // remove him from the node list (reclaim its memory and its index)
// nodes.erase(nodes.begin()+idx);
// decrement the index for all of the following nodes and adjust neighbor references
for (size_t i = 0; i < nodes.size(); ++i) {
// // decrement the index for all of the following nodes and adjust neighbor references
// for (size_t i = 0; i < nodes.size(); ++i) {
// decrement the higher indices (reclaim the free one)
if (nodes[i]._idx >= idx) { --nodes[i]._idx;}
// // decrement the higher indices (reclaim the free one)
// if (nodes[i]._idx >= idx) { --nodes[i]._idx;}
// adjust the neighbor references (decrement by one)
for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
}
// // adjust the neighbor references (decrement by one)
// for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
// if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
// }
}
}
// }
// }
public:

120
grid/factory/GridFactory.h
View File

@@ -42,7 +42,7 @@ public:
for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {
// check intersection with the floorplan
GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
const GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
if (intersects(bbox, floor)) {continue;}
// add to the grid
@@ -101,20 +101,19 @@ public:
void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm));
Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm), false);
Log::tick();
for (const Stair& s : stairs) {
for (int i = 0; i < grid.getNumNodes(); ++i) {
// potential starting-point for the stair
for (T& n : grid) {
// potential starting-point for the stair
T& n = (T&) grid[i];
// real starting point for the stair?
if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {
// node lies on the stair's starting edge?
if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {
// construct end-point by using the stair's direction
const Point3 end = Point3(n.x_cm, n.y_cm, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
GridPoint gp(end.x, end.y, end.z);
// does such and end-point exist within the grap? -> construct stair
@@ -130,6 +129,8 @@ public:
}
Log::tock();
}
/** build a stair (z-transition) from n1 to n2 */
@@ -147,14 +148,15 @@ public:
const int gridSize_cm = grid.getGridSize_cm();
// move upards in gridSize steps
for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) {
// calculate the percentage of reached upwards-distance
const float percent = z/zDiff;
const float percent = _z/zDiff;
// adjust (x,y) accordingly (interpolate)
int x = n1.x_cm + xDiff * percent;
int y = n1.y_cm + yDiff * percent;
int z = n1.z_cm + _z;
// snap (x,y) to the grid???
x = std::round(x / gridSize_cm) * gridSize_cm;
@@ -218,7 +220,7 @@ public:
const int idxStart = rand() % grid.getNumNodes();
set.clear();
Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
getConnected(idxStart, set);
getConnected(grid[idxStart], set);
Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
} while (set.size() < 0.5 * grid.getNumNodes());
@@ -234,30 +236,94 @@ public:
}
private:
/** remove all nodes not connected to n1 */
void removeIsolated(T& n1) {
/** recursively get all connected nodes and add them to the set */
void getConnected(const int idx, std::unordered_set<int>& set) {
// get the connected region around n1
Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
Log::tick();
std::unordered_set<int> set;
getConnected(n1, set);
Log::tock();
// get the node behind idx
const T& n1 = (T&) grid[idx];
// add him to the current region
set.insert(n1.getIdx());
// get all his (unprocessed) neighbors and add them to the region
for (const T& n2 : grid.neighbors(n1)) {
if (set.find(n2.getIdx()) == set.end()) {
getConnected(n2.getIdx(), set);
}
// remove all other
Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
Log::tick();
for (T& n2 : grid) {
if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
}
Log::tock();
// clean the grid (physically delete the removed nodes)
grid.cleanup();
}
private:
/** recursively get all connected nodes and add them to the set */
void getConnected(T& n1, std::unordered_set<int>& visited) {
std::unordered_set<int> toVisit;
toVisit.insert(n1.getIdx());
// run while there are new nodes to visit
while(!toVisit.empty()) {
// get the next node
int nextIdx = *toVisit.begin();
toVisit.erase(nextIdx);
visited.insert(nextIdx);
T& next = grid[nextIdx];
// get all his (unprocessed) neighbors and add them to the region
for (const T& n2 : grid.neighbors(next)) {
if (visited.find(n2.getIdx()) == visited.end()) {
toVisit.insert(n2.getIdx());
}
}
}
}
// /** recursively get all connected nodes and add them to the set */
// void getConnected(const int idx, std::unordered_set<int>& set) {
// // get the node behind idx
// const T& n1 = (T&) grid[idx];
// // add him to the current region
// set.insert(n1.getIdx());
// // get all his (unprocessed) neighbors and add them to the region
// for (const T& n2 : grid.neighbors(n1)) {
// if (set.find(n2.getIdx()) == set.end()) {
// getConnected(n2.getIdx(), set);
// }
// }
// }
// /** recursively get all connected nodes and add them to the set */
// void getConnected(const T& n1, std::unordered_set<int>& set) {
// // add him to the current region
// set.insert(n1.getIdx());
// // get all his (unprocessed) neighbors and add them to the region
// for (const T& n2 : grid.neighbors(n1)) {
// if (set.find(n2.getIdx()) == set.end()) {
// getConnected(n2, set);
// }
// }
// }
private:
/** does the bbox intersect with any of the floor's walls? */
bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
static inline bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
for (const Line2& l : floor.getObstacles()) {
if (bbox.intersects(l)) {return true;}
}

24
grid/factory/GridImportance.h
View File

@@ -52,6 +52,9 @@ public:
// process each node
for (T& n1 : g) {
// skip nodes on other than the requested floor-level
if (n1.z_cm != z_cm) {continue;}
// get the 10 nearest neighbors and their distance
size_t indices[numNeighbors];
float squaredDist[numNeighbors];
@@ -64,7 +67,9 @@ public:
neighbors.push_back(&inv[indices[i]]);
}
addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
n1.imp = 1.0f;
n1.imp += getWallImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
//addDoor(n1, neighbors);
// is the current node a door?
@@ -81,20 +86,16 @@ public:
// process each node again
for (T& n1 : g) {
static K::NormalDistribution favorDoors(0.0, 0.6);
static K::NormalDistribution favorDoors(0.0, 1.0);
// get the distance to the nearest door
const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
// importance for this node (based on the distance from the next door)
const float imp = 1.0 + favorDoors.getProbability(dist_m) * 0.35;
// adjust
n1.imp *= imp;
n1.imp += favorDoors.getProbability(dist_m) * 0.30;
}
}
/** is the given node connected to a staircase? */
@@ -190,10 +191,10 @@ public:
}
/** get the importance of the given node depending on its nearest wall */
template <typename T> void addImportance(T& nSrc, float dist_m) {
template <typename T> float getWallImportance(T& nSrc, float dist_m) {
// avoid sticking too close to walls (unlikely)
static K::NormalDistribution avoidWalls(0.0, 0.3);
static K::NormalDistribution avoidWalls(0.0, 0.4);
// favour walking near walls (likely)
static K::NormalDistribution sticToWalls(0.9, 0.5);
@@ -203,10 +204,9 @@ public:
if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance
nSrc.imp *= 1.0
- avoidWalls.getProbability(dist_m) * 0.35 // avoid walls
return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
+ sticToWalls.getProbability(dist_m) * 0.15 // walk near walls
+ farAway.getProbability(dist_m) * 0.20 // walk in the middle
+ farAway.getProbability(dist_m) * 0.15 // walk in the middle
;

61
grid/walk/GridWalkHelper.h
View File

@@ -12,6 +12,67 @@ public:
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
}
/** get the neighbor of "from" best matching the given heading "h" */
template <typename T> static T& getBestNeighbor(Grid<T>& grid, const T& from, const Heading h) {
auto comp = [&] (const T& n1, const T& n2) {
const Heading h1 = getHeading(from, n1);
const Heading h2 = getHeading(from, n2);
const float d1 = h.getDiffHalfRAD(h1);
const float d2 = h.getDiffHalfRAD(h2);
//return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2));
return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); // same heading? > random decision
};
auto neighbors = grid.neighbors(from);
return *std::min_element(neighbors.begin(), neighbors.end(), comp);
}
/**
* try to walk the given distance from the provided node.
* if this fails (algorithm cancel walk e.g. due to detected wall collissions)
* - try again from the start
* if this also fails several times
* - try to walk in the opposite direction instead (bounce-back)
* if this also fails
* - add some randomness and try again
*/
template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m) {
_assertTrue(distance_m >= 0, "distance must not be negative!");
GridWalkState<T> res;
//again:
int retries = numRetries;
// 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 = w.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 = w.walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
}
// still nothing found? -> modify and try again
if (res.node == nullptr) {
// start.node = &(*grid.neighbors(*start.node).begin());
// start.heading += 0.25;
// goto again;
res.node = &(*grid.neighbors(*start.node).begin());
res.heading = start.heading;
}
return res;
}
};
#endif // GRIDWALKHELPER_H

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);

120
grid/walk/GridWalkPushForward.h Normal file
View File

@@ -0,0 +1,120 @@
#ifndef GRIDWALKPUSHFORWARD_H
#define GRIDWALKPUSHFORWARD_H
/**
* todo
*/
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Exponential.h>
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
/**
* keeps something like an "average position within the last X steps"
* and tries to move away from this point as fast as possible
*
*/
template <typename T> class GridWalkPushForward {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
public:
/** ctor */
GridWalkPushForward() {
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
}
private:
// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
DrawList<T*> drawer;
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
drawer.reset();
// weight all neighbors based on this heading
for (T& neighbor : grid.neighbors(*cur.node)) {
// get the heading between the current node and its neighbor
const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
// calculate the difference from the requested heading
const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
// weight this change
const float prob1 = K::NormalDistribution::getProbability(0, HEADING_ALLOWED_SIGMA, diffRad);
// distance from average? and previous distance from average
const float distToAvg = Point3(neighbor.x_cm, neighbor.y_cm, neighbor.z_cm).getDistance(cur.avg);
const float prevDistToAvg = Point3(cur.node->x_cm, cur.node->y_cm, cur.node->z_cm).getDistance(cur.avg);
const float increase = distToAvg - prevDistToAvg;
// the distance from the average MUST increase
const float prob2 = (increase > 0) ? (1) : (0.1);
// add floorplan importance information
const float prob3 = std::pow(neighbor.imp, 1);
const float prob = prob1*prob2*prob3;
// add for drawing
drawer.add(&neighbor, prob);
}
// all neighbors are unlikely? -> start over
if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
GridWalkState<T> next;
// pick the neighbor best matching this new heading
next.node = drawer.get();
next.heading = GridWalkHelper::getHeading(*cur.node, *next.node) + headingChangeDist(gen);
// weighted average.. moves over time
next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1);
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKPUSHFORWARD_H

87
grid/walk/GridWalkRandomHeadingUpdate.h Normal file
View File

@@ -0,0 +1,87 @@
#ifndef GRIDWALKRANDOMHEADINGUPDATE_H
#define GRIDWALKRANDOMHEADINGUPDATE_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include <KLib/math/distribution/Normal.h>
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
/**
* for every walked edge: slightly update (scatter) the current heading
* pick the edge (neighbor) best matching the current heading
* if this neighbor's heading highly differs from the requested heading: start over
*
* PROs
* - very simple
*
* CONs
* - particles are bad at walking out of rooms for small grid sizes as there are too many options
* to stay within the room..
*
*/
template <typename T> class GridWalkRandomHeadingUpdate {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(4);
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
public:
/** ctor */
GridWalkRandomHeadingUpdate() {
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
}
private:
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
GridWalkState<T> next;
// get a new random heading
next.heading = cur.heading + headingChangeDist(gen);
// pick the neighbor best matching this new heading
next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);
// if the best matching neighbor is far of this requested heading
// (e.g. no good neighbor due to walls) cancel the walk. to force a retry
const float diff = GridWalkHelper::getHeading(*cur.node, *next.node).getDiffHalfRAD(next.heading);
if (diff > Angle::degToRad(45)) {
return GridWalkState<T>();
}
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKRANDOMHEADINGUPDATE_H

126
grid/walk/GridWalkRandomHeadingUpdateAdv.h Normal file
View File

@@ -0,0 +1,126 @@
#ifndef GRIDWALKRANDOMHEADINGUPDATEADV_H
#define GRIDWALKRANDOMHEADINGUPDATEADV_H
#include "../../geo/Heading.h"
#include "../Grid.h"
#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Exponential.h>
#include "../../nav/dijkstra/Dijkstra.h"
#include "GridWalkState.h"
#include "GridWalkHelper.h"
/**
* for every walked edge: slightly update (scatter) the current heading
* pick the edge (neighbor) best matching the current heading
* if this neighbor's heading highly differs from the requested heading: start over
*
* PROs
* - simple
* - fixes the issues of GridWalkRandomHeadingUpdate by incorporating floor information
* - adds additional randomness which should be more stable
*
*/
template <typename T> class GridWalkRandomHeadingUpdateAdv {
friend class GridWalkHelper;
private:
/** per-edge: change heading with this sigma */
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
/** fast random-number-generator */
std::minstd_rand gen;
/** 0-mean normal distribution */
std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);
public:
/** ctor */
GridWalkRandomHeadingUpdateAdv() {
;
}
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T> start, const float distance_m) {
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m);
}
private:
// https://de.wikipedia.org/wiki/Logistische_Verteilung
/** alpha = move the center, beta = slope */
const float logisticDist(const float x, const float alpha, const float beta) {
return 1 / (1 + std::exp( -((x-alpha)/beta) ) );
}
// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
DrawList<T*> drawer;
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T> cur, float distRest_m) {
drawer.reset();
GridWalkState<T> next;
// get a new random heading
next.heading = cur.heading + headingChangeDist(gen);
// weight all neighbors based on this heading
for (T& neighbor : grid.neighbors(*cur.node)) {
// get the heading between the current node and its neighbor
const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
// calculate the difference from the requested heading
const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);
// weight this change
const float prob1 = K::NormalDistribution::getProbability(0, Angle::degToRad(40), diffRad);
// add the node's importance factor into the calculation
const float prob2 = logisticDist(neighbor.imp, 1.0, 0.05);
//const float prob2 = std::pow(neighbor.imp, 10);
// final importance
const float prob = prob1 * prob2;
// add for drawing
drawer.add(&neighbor, prob);
}
// all neighbors are unlikely? -> start over
if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
// pick the neighbor best matching this new heading
next.node = drawer.get();
// // if the best matching neighbor is far of this requested heading
// // (e.g. no good neighbor due to walls) cancel the walk. to force a retry
// const float diff = GridWalkHelper::getHeading(*cur.node, *next.node).getDiffHalfRAD(next.heading);
// if (diff > Angle::degToRad(45)) {
// return GridWalkState<T>();
// }
// get the distance up to this neighbor
distRest_m -= next.node->getDistanceInMeter(*cur.node);
// done?
if (distRest_m <= 0) {return next;}
// another round..
return walk(grid, next, distRest_m);
}
};
#endif // GRIDWALKRANDOMHEADINGUPDATEADV_H

3
grid/walk/GridWalkState.h
View File

@@ -2,6 +2,7 @@
#define GRIDWALKSTATE_H
#include "../../geo/Heading.h"
#include "../../geo/Point3.h"
template <typename T> struct GridWalkState {
@@ -14,6 +15,8 @@ template <typename T> struct GridWalkState {
/** empty ctor */
GridWalkState() : node(nullptr), heading(0) {;}
Point3 avg = Point3(0,0,0);
/** ctor with user-node and heading */
GridWalkState(const T* node, const Heading heading) : node(node), heading(heading) {;}