136 lines
3.8 KiB
C++
136 lines
3.8 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 GRIDWALKPUSHFORWARD_H
|
||
#define GRIDWALKPUSHFORWARD_H
|
||
|
||
/**
|
||
* todo
|
||
*/
|
||
#include "../../geo/Heading.h"
|
||
#include "../Grid.h"
|
||
|
||
#include "../../math/DrawList.h"
|
||
#include "../../math/Random.h"
|
||
#include "../../nav/dijkstra/Dijkstra.h"
|
||
|
||
#include "GridWalkState.h"
|
||
#include "GridWalkHelper.h"
|
||
#include "GridWalk.h"
|
||
|
||
/**
|
||
* keeps something like an "average position within the last X steps"
|
||
* and tries to move away from this point as fast as possible
|
||
*
|
||
*/
|
||
template <typename T> class GridWalkPushForward : public GridWalk<T> {
|
||
|
||
friend class GridWalkHelper;
|
||
|
||
private:
|
||
|
||
/** per-edge: change heading with this sigma */
|
||
static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(3);
|
||
|
||
static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);
|
||
|
||
/** 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 */
|
||
GridWalkPushForward() {
|
||
;
|
||
}
|
||
|
||
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) {
|
||
|
||
drawer.reset();
|
||
|
||
// 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, HEADING_ALLOWED_SIGMA, diffRad);
|
||
|
||
|
||
// distance from average? and previous distance from average
|
||
const float distToAvg = Point3(neighbor.x_cm, neighbor.y_cm, neighbor.z_cm).getDistance(cur.avg);
|
||
const float prevDistToAvg = Point3(cur.node->x_cm, cur.node->y_cm, cur.node->z_cm).getDistance(cur.avg);
|
||
const float increase = distToAvg - prevDistToAvg;
|
||
|
||
// the distance from the average MUST increase
|
||
const float prob2 = (increase > 0) ? (1) : (0.1);
|
||
|
||
// add floorplan importance information
|
||
const float prob3 = std::pow(neighbor.imp, 1);
|
||
|
||
const float prob = prob1*prob2*prob3;
|
||
|
||
// add for drawing
|
||
drawer.add(&neighbor, prob);
|
||
|
||
}
|
||
|
||
|
||
// all neighbors are unlikely? -> start over
|
||
if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}
|
||
|
||
// create a copy (to keep heading and walked distance)
|
||
GridWalkState<T> next = cur;
|
||
|
||
// pick the neighbor best matching this new heading
|
||
next.node = drawer.get();
|
||
next.heading = GridWalkHelper::getHeading(*cur.node, *next.node) + headingChangeDist(gen);
|
||
|
||
// weighted average.. moves over time
|
||
next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1);
|
||
|
||
// 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);
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // GRIDWALKPUSHFORWARD_H
|