111 lines
3.0 KiB
C++
111 lines
3.0 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef GRIDWALKRANDOMHEADINGUPDATE_H
|
||
#define GRIDWALKRANDOMHEADINGUPDATE_H
|
||
|
||
#include "../../geo/Heading.h"
|
||
#include "../Grid.h"
|
||
|
||
|
||
#include "../../math/Random.h"
|
||
#include "../../nav/dijkstra/Dijkstra.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
|
||
* - 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 : 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 */
|
||
GridWalkRandomHeadingUpdate() {
|
||
gen.seed(1234);
|
||
}
|
||
|
||
|
||
|
||
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) override {
|
||
|
||
|
||
return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, -1);
|
||
|
||
}
|
||
|
||
private:
|
||
|
||
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& cur, float distRest_m, float headChange = -1) {
|
||
|
||
// 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;
|
||
|
||
// 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
|
||
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 // GRIDWALKRANDOMHEADINGUPDATE_H
|