FrankE
e6329e1db4
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
88 lines
2.2 KiB
C++
88 lines
2.2 KiB
C++
#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
|