98 lines
3.1 KiB
C++
98 lines
3.1 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 GRIDWALKHELPER_H
|
||
#define GRIDWALKHELPER_H
|
||
|
||
#include "../../geo/Heading.h"
|
||
#include "GridWalkState.h"
|
||
|
||
class GridWalkHelper {
|
||
|
||
public:
|
||
|
||
/** get the heading-change between the two given locations */
|
||
template <typename T> static Heading getHeading(const T& from, const T& to) {
|
||
return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm);
|
||
}
|
||
|
||
/** get the neighbor of "from" best matching the given heading "h" */
|
||
template <typename T> static T& getBestNeighbor(Grid<T>& grid, const T& from, const Heading h) {
|
||
|
||
auto comp = [&] (const T& n1, const T& n2) {
|
||
const Heading h1 = getHeading(from, n1);
|
||
const Heading h2 = getHeading(from, n2);
|
||
const float d1 = h.getDiffHalfRAD(h1);
|
||
const float d2 = h.getDiffHalfRAD(h2);
|
||
//return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2));
|
||
//return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2); // same heading? > random decision
|
||
//return ((from.z_cm != n1.z_cm) && (from.z_cm == n2.z_cm)) ||
|
||
// (d1 < d2) ||
|
||
//(rand() < RAND_MAX/2);
|
||
if (d1 < d2) {return true;}
|
||
else if (d1 == d2) {return n1.z_cm != n2.z_cm;}
|
||
return false;
|
||
};
|
||
|
||
auto neighbors = grid.neighbors(from);
|
||
return *std::min_element(neighbors.begin(), neighbors.end(), comp);
|
||
|
||
}
|
||
|
||
/**
|
||
* try to walk the given distance from the provided node.
|
||
* if this fails (algorithm cancel walk e.g. due to detected wall collissions)
|
||
* - try again from the start
|
||
* if this also fails several times
|
||
* - try to walk in the opposite direction instead (bounce-back)
|
||
* if this also fails
|
||
* - add some randomness and try again
|
||
*/
|
||
template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m, const float headChange_rad) {
|
||
|
||
Assert::isTrue(distance_m >= 0, "distance must not be negative!");
|
||
Assert::isNotNull(start.node, "starting node must not be null");
|
||
|
||
GridWalkState<T> res;
|
||
|
||
//again:
|
||
|
||
int retries = numRetries;
|
||
|
||
// 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 = w.walk(grid, start, distance_m, headChange_rad);
|
||
} while (res.node == nullptr && --retries);
|
||
|
||
// still reaching a dead end?
|
||
// -> try a walk in the opposite direction instead
|
||
if (res.node == nullptr) {
|
||
GridWalkState<T> newState(start.node, start.heading.getInverted());
|
||
res = w.walk(grid, newState, distance_m, headChange_rad);
|
||
}
|
||
|
||
// still nothing found? -> modify and try again
|
||
if (res.node == nullptr) {
|
||
// start.node = &(*grid.neighbors(*start.node).begin());
|
||
// start.heading += 0.25;
|
||
// goto again;
|
||
res.node = &(*grid.neighbors(*start.node).begin());
|
||
res.heading = start.heading;
|
||
}
|
||
|
||
return res;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // GRIDWALKHELPER_H
|