kazu
9947dced15
added new helper methods/classes (e.g. for heading) new test cases optimize the dijkstra cleanups/refactoring added timed-benchmarks to the log many more...
161 lines
4.8 KiB
C++
161 lines
4.8 KiB
C++
#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
|
|
#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
|
|
|
|
#include "../../geo/Heading.h"
|
|
#include "../Grid.h"
|
|
|
|
#include "../../math/DrawList.h"
|
|
#include <KLib/math/distribution/Normal.h>
|
|
|
|
#include "../../nav/dijkstra/Dijkstra.h"
|
|
|
|
#include "GridWalkState.h"
|
|
#include "GridWalkHelper.h"
|
|
|
|
/**
|
|
* perform walks on the grid based on some sort of weighting
|
|
* and drawing from the weighted elements
|
|
*/
|
|
template <typename T> class GridWalkLightAtTheEndOfTheTunnel {
|
|
|
|
private:
|
|
|
|
/** per-edge: change heading with this sigma */
|
|
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
|
|
|
|
/** per-edge: allowed heading difference */
|
|
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
|
|
|
|
|
|
/** allows drawing elements according to their probability */
|
|
DrawList<T&> drawer;
|
|
|
|
/** 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);
|
|
|
|
Dijkstra<T> dijkstra;
|
|
|
|
public:
|
|
|
|
/** ctor with the target you want to reach */
|
|
template <int gridSize_cm, typename Access> GridWalkLightAtTheEndOfTheTunnel(Grid<gridSize_cm, T>& grid, const Access& acc, const T& target) {
|
|
|
|
// build all shortest path to reach th target
|
|
dijkstra.build(target, target, acc);
|
|
|
|
// attach a corresponding weight-information to each user-grid-node
|
|
for (T& node : grid) {
|
|
|
|
const DijkstraNode<T>* dn = dijkstra.getNode(node);
|
|
|
|
// should never be null as all nodes were evaluated
|
|
if (dn != nullptr) {
|
|
node.distToTarget = dn->cumWeight/2000;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
template <int gridSize_cm> GridWalkState<T> getDestination(Grid<gridSize_cm, 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);
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
template <int gridSize_cm> GridWalkState<T> walk(Grid<gridSize_cm, T>& grid, GridWalkState<T> cur, float distRest_m) {
|
|
|
|
drawer.reset();;
|
|
|
|
// calculate the weight for all possible destinations from "cur"
|
|
for (T& neighbor : grid.neighbors(*cur.node)) {
|
|
|
|
// heading when walking from cur to neighbor
|
|
const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);
|
|
|
|
// angular difference
|
|
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
|
|
|
|
// probability for this direction change?
|
|
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
|
|
|
|
// perfer locations reaching the target
|
|
const double shortening = cur.node->distToTarget - neighbor.distToTarget;
|
|
if (shortening > 0) {prob *= 30;} // << importance factor!!
|
|
|
|
drawer.add(neighbor, prob);
|
|
|
|
}
|
|
|
|
GridWalkState<T> next(nullptr, cur.heading);
|
|
|
|
// pick a random destination
|
|
T& nDir = drawer.get();
|
|
const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
|
|
//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
|
|
next.heading = hDir;
|
|
next.heading += headingChangeDist(gen);
|
|
|
|
// 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);
|
|
};
|
|
|
|
// 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);
|
|
// }
|
|
|
|
// 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 // GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
|