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...
128 lines
3.3 KiB
C++
128 lines
3.3 KiB
C++
#ifndef GRIDWALKWEIGHTED2_H
|
|
#define GRIDWALKWEIGHTED2_H
|
|
|
|
#include "../../geo/Heading.h"
|
|
#include "../Grid.h"
|
|
|
|
#include "../../math/DrawList.h"
|
|
#include <KLib/math/distribution/Normal.h>
|
|
|
|
/**
|
|
* perform walks on the grid based on some sort of weighting
|
|
* and drawing from the weighted elements
|
|
*/
|
|
template <typename T> class GridWalkWeighted {
|
|
|
|
public:
|
|
|
|
struct State {
|
|
const T* node;
|
|
Heading heading;
|
|
|
|
State() : node(nullptr), heading(0) {;}
|
|
State(const T* node, Heading heading) : node(node), heading(heading) {;}
|
|
|
|
};
|
|
|
|
|
|
private:
|
|
|
|
/** per-edge: change heading with this sigma */
|
|
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(5);
|
|
|
|
/** per-edge: allowed heading difference */
|
|
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(20);
|
|
|
|
|
|
/** 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);
|
|
|
|
public:
|
|
|
|
template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {
|
|
|
|
int retries = 2;
|
|
State 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, State(start.node, start.heading.getInverted()), distance_m);
|
|
}
|
|
|
|
// still nothing found? -> keep the start as-is
|
|
return (res.node == nullptr) ? (start) : (res);
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
static Heading getHeading(const T& from, const T& to) {
|
|
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
|
|
}
|
|
|
|
template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {
|
|
|
|
// make the draw-list empty (faster than allocating a new one every time?)
|
|
drawer.reset();
|
|
|
|
// calculate probabilites for the neighboring nodes
|
|
for (T& neighbor : grid.neighbors(*cur.node)) {
|
|
|
|
// heading when walking from cur to neighbor
|
|
Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, neighbor.x_cm, neighbor.y_cm);
|
|
|
|
// angular difference
|
|
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
|
|
|
|
// probability for this change?
|
|
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
|
|
|
|
// adjust by the neighbors importance factor
|
|
//prob += std::pow(neighbor.impPath, 2);
|
|
prob *= neighbor.imp;
|
|
//prob *= K::NormalDistribution::getProbability(1.4, 0.2, neighbor.imp);
|
|
|
|
drawer.add(neighbor, prob);
|
|
|
|
}
|
|
|
|
// if there is no likely neighbor at all, we reached a dead end
|
|
// -> start over!
|
|
if (drawer.getCumProbability() < 0.01) {return State(nullptr, 0);}
|
|
|
|
// get the next node
|
|
T* nn = &drawer.get();
|
|
|
|
//Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, nn->x_cm, nn->y_cm);
|
|
//State next(nn, potentialHeading);
|
|
State next(&nn, cur.heading);
|
|
next.heading += headingChangeDist(gen);
|
|
|
|
// 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 // GRIDWALKWEIGHTED2_H
|