133 lines
3.7 KiB
C++
133 lines
3.7 KiB
C++
#ifndef GRIDWALKRANDOMHEADINGUPDATEADV_H
|
|
#define GRIDWALKRANDOMHEADINGUPDATEADV_H
|
|
|
|
|
|
#include "../../geo/Heading.h"
|
|
#include "../Grid.h"
|
|
|
|
#include "../../math/DrawList.h"
|
|
#include "../../math/Distributions.h"
|
|
#include "../../nav/dijkstra/Dijkstra.h"
|
|
#include "../../math/Random.h"
|
|
|
|
#include "GridWalk.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 : public GridWalk<T> {
|
|
|
|
friend class GridWalkHelper;
|
|
|
|
private:
|
|
|
|
/** per-edge: change heading with this sigma */
|
|
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
|
|
|
|
/** fast random-number-generator */
|
|
Random::RandomGenerator 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) override {
|
|
|
|
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, float headChange = -1) {
|
|
|
|
drawer.reset();
|
|
|
|
// create a copy (to keep heading and walked distance)
|
|
GridWalkState<T> next = cur;
|
|
|
|
// change the heading at the beginning of each walk
|
|
if (headChange == -1) {headChange = headingChangeDist(gen);}
|
|
|
|
// get a new random heading
|
|
next.heading = cur.heading + headChange;
|
|
|
|
// 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 = Distribution::Normal<float>::getProbability(0, Angle::degToRad(40), diffRad);
|
|
|
|
// add the node's importance factor into the calculation
|
|
//const float prob2 = Distribution::Logistic<float>::getCDF(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
|
|
const float walked_m = next.node->getDistanceInMeter(*cur.node);
|
|
distRest_m -= walked_m;
|
|
|
|
// update the heading-change and walked-distance
|
|
next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD();
|
|
next.distanceWalked_m += walked_m;
|
|
|
|
// done?
|
|
if (distRest_m <= 0) {return next;}
|
|
|
|
// another round..
|
|
return walk(grid, next, distRest_m, headChange);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // GRIDWALKRANDOMHEADINGUPDATEADV_H
|