152 lines
4.0 KiB
C++
152 lines
4.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 GRIDWALKWEIGHTED3_H
|
||
#define GRIDWALKWEIGHTED3_H
|
||
|
||
#include "../../geo/Heading.h"
|
||
#include "../Grid.h"
|
||
|
||
#include "../../math/DrawList.h"
|
||
#include "../../math/Random.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(3);
|
||
|
||
/** per-edge: allowed heading difference */
|
||
static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);
|
||
|
||
|
||
/** allows drawing elements according to their probability */
|
||
DrawList<T&> drawer;
|
||
|
||
/** 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:
|
||
|
||
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) {
|
||
|
||
drawer.reset();;
|
||
|
||
// calculate the weight for all possible destinations from "cur"
|
||
for (T& neighbor : grid.neighbors(*cur.node)) {
|
||
|
||
// heading when walking from cur to neighbor
|
||
const Heading potentialHeading = getHeading(*cur.node, neighbor);
|
||
|
||
// angular difference
|
||
const float diff = cur.heading.getDiffHalfRAD(potentialHeading);
|
||
|
||
// probability for this change?
|
||
double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);
|
||
|
||
prob *= std::pow(neighbor.impPath, 5);
|
||
|
||
drawer.add(neighbor, prob);
|
||
|
||
}
|
||
|
||
State next(nullptr, cur.heading);
|
||
|
||
// pick a random destination
|
||
T& nDir = drawer.get();
|
||
const Heading hDir = getHeading(*cur.node, nDir);
|
||
//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.95;
|
||
next.heading = hDir;
|
||
next.heading += headingChangeDist(gen);
|
||
|
||
// compare two neighbors according to their implied heading change
|
||
auto compp = [&] (const T& n1, const T& n2) {
|
||
Heading h1 = getHeading(*cur.node, n1);
|
||
Heading h2 = getHeading(*cur.node, n2);
|
||
const float d1 = next.heading.getDiffHalfRAD(h1);
|
||
const float d2 = next.heading.getDiffHalfRAD(h2);
|
||
return d1 < d2;
|
||
};
|
||
|
||
// pick the neighbor best matching the new heading
|
||
auto it = grid.neighbors(*cur.node);
|
||
T& nn = *std::min_element(it.begin(), it.end(), compp);
|
||
next.node = &nn;
|
||
|
||
// // pervent dramatic heading changes. instead: try again
|
||
// if (cur.heading.getDiffHalfRAD(getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
|
||
// return State(nullptr, 0);
|
||
// }
|
||
|
||
// 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 // GRIDWALKWEIGHTED3_H
|