88 lines
2.7 KiB
C++
88 lines
2.7 KiB
C++
#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
|