326 lines
8.5 KiB
C++
326 lines
8.5 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 GRIDWALKSHORTESTPATHCONTROL_H
|
||
#define GRIDWALKSHORTESTPATHCONTROL_H
|
||
|
||
|
||
#include "../../geo/Heading.h"
|
||
#include "../Grid.h"
|
||
|
||
#include "../../math/Distributions.h"
|
||
#include "../../math/DrawList.h"
|
||
#include "../../math/Random.h"
|
||
|
||
#include "../../nav/dijkstra/Dijkstra.h"
|
||
#include "../../nav/dijkstra/DijkstraPath.h"
|
||
#include "../../misc/KNN.h"
|
||
#include "../../misc/KNNArray.h"
|
||
|
||
#include "GridWalkState.h"
|
||
#include "GridWalkHelper.h"
|
||
#include "GridWalk.h"
|
||
|
||
|
||
|
||
#include <KLib/math/statistics/Statistics.h>
|
||
|
||
template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {
|
||
|
||
|
||
protected:
|
||
|
||
/** put dijkstra-nodes into the KNN */
|
||
class Wrapper {
|
||
|
||
private:
|
||
|
||
DijkstraPath<T>* path;
|
||
|
||
public:
|
||
|
||
Wrapper() : path(nullptr) {;}
|
||
|
||
/** ctor with the underlying data structure */
|
||
Wrapper(DijkstraPath<T>* path) : path(path) {
|
||
;
|
||
}
|
||
|
||
/** get the number of elements to search throrugh */
|
||
inline int kdtree_get_point_count() const {
|
||
return path->size();
|
||
}
|
||
|
||
/** use nanoflanns default bbox */
|
||
template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
|
||
(void) bb; return false;
|
||
}
|
||
|
||
/** get the idx-th element's dim-th coordinate */
|
||
inline float kdtree_get_pt(const size_t idx, const int dim) const {
|
||
return (*((*path)[idx]).element)[dim];
|
||
}
|
||
|
||
/** get the SQUARED distance between the given coordinates and the provided element */
|
||
inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
|
||
const float d0 = p1[0] - (*((*path)[idx_p2]).element)[0];
|
||
const float d1 = p1[1] - (*((*path)[idx_p2]).element)[1];
|
||
const float d2 = p1[2] - (*((*path)[idx_p2]).element)[2];
|
||
return (d0*d0) + (d1*d1) + (d2*d2);
|
||
}
|
||
|
||
|
||
};
|
||
|
||
|
||
|
||
friend class GridWalkHelper;
|
||
|
||
protected:
|
||
|
||
/** 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);
|
||
|
||
Dijkstra<T> dijkstra;
|
||
const T& target;
|
||
|
||
//Point3 centerOfMass = Point3(0,0,0);
|
||
Wrapper wrapper;
|
||
DijkstraPath<T>* path = nullptr;
|
||
//KNN<Wrapper,3>* knn = nullptr;
|
||
|
||
|
||
DrawList<T&> drawer;
|
||
|
||
std::vector<Point3> points;
|
||
Point3 centerOfMass;
|
||
float stdDevDist;
|
||
|
||
public:
|
||
|
||
|
||
|
||
/** ctor with the target you want to reach */
|
||
template <typename Access> GridWalkShortestPathControl(Grid<T>& grid, const Access& acc, const T& target) : target(target) {
|
||
|
||
(void) grid;
|
||
|
||
gen.seed(1234);
|
||
|
||
// build all shortest path to reach th target
|
||
dijkstra.build(&target, acc);
|
||
|
||
}
|
||
|
||
int recalc = 0;
|
||
|
||
int times = 3;
|
||
float pOther = 0.10;
|
||
|
||
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
|
||
|
||
|
||
// update the center-of-mass
|
||
points.push_back( (Point3)*start.node );
|
||
|
||
//centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
|
||
//if (path == nullptr) {rebuildPath(grid);}
|
||
|
||
|
||
|
||
if (++recalc >= 7500) {
|
||
|
||
// center of mass
|
||
Point3 sum(0,0,0);
|
||
for (const Point3& p : points) {sum += p;}
|
||
centerOfMass = sum/points.size();
|
||
|
||
// deviation from the center of mass
|
||
float dSum = 0;
|
||
float dSum2 = 0;
|
||
for (const Point3& p : points) {
|
||
const float d = p.getDistance(centerOfMass);
|
||
dSum += d;
|
||
dSum2 += d*d;
|
||
}
|
||
dSum /= points.size();
|
||
dSum2 /= points.size();
|
||
stdDevDist = std::sqrt( (dSum2) - (dSum * dSum) ) * times;
|
||
|
||
// restart
|
||
points.clear();
|
||
recalc = 0;
|
||
|
||
// update
|
||
rebuildPath(grid, centerOfMass);
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
// if (knn != nullptr) {
|
||
// const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} );
|
||
// if (dist > 10000) {
|
||
// rebuildPath(grid);
|
||
// }
|
||
// }
|
||
|
||
// proportional change of the heading
|
||
static Distribution::Normal<float> dHead(1, 0.01);
|
||
|
||
// proportional change of the to-be-walked distance
|
||
static Distribution::Normal<float> dWalk(1, 0.10);
|
||
|
||
distance_m = distance_m*dWalk.draw()*1.5; // TODO: why *2?
|
||
headChange_rad = headChange_rad*dHead.draw();
|
||
|
||
static Distribution::Normal<float> sWalk(0, 0.10);
|
||
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
|
||
|
||
return walk(grid, start, distance_m, headChange_rad);
|
||
|
||
}
|
||
|
||
private:
|
||
|
||
double getProbability(Grid<T>& grid, const T& start, const T& prev, const T& possible, const Heading head) const {
|
||
|
||
// TODO: WHY?! not only when going back to the start?
|
||
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
|
||
if (start.z_cm == possible.z_cm) {return 0;} // back to the start
|
||
throw 1;
|
||
return 0.5;// stair start/end TODO: fix
|
||
}
|
||
|
||
// get the angle between START and the possible next node
|
||
const Heading possibleHead = GridWalkHelper::getHeading(start, possible);
|
||
|
||
// calculate the difference
|
||
const float diff = possibleHead.getDiffHalfRAD(head);
|
||
|
||
// // compare this heading with the requested one
|
||
const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff);
|
||
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
|
||
|
||
// nodes own importance
|
||
double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025;
|
||
|
||
if (path != nullptr) {
|
||
//const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
|
||
int steps = stdDevDist / grid.getGridSize_cm();
|
||
const float pToTarget = possible.getDistanceInMeter(*path->getFromStart(steps).element);
|
||
const float sToTarget = prev.getDistanceInMeter(*path->getFromStart(steps).element);
|
||
nodeProb = (pToTarget < sToTarget) ? (1.0) : (pOther);
|
||
//const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} );
|
||
//nodeProb = (pd < sd) ? 1 : 0.0;
|
||
//nodeProb = Distribution::Exponential<float>::getProbability(0.9, pToTarget);
|
||
//nodeProb = Distribution::Exponential<float>::getProbability(1.0, tDist_m);
|
||
}
|
||
|
||
// bring it together
|
||
return angleProb * nodeProb;
|
||
|
||
}
|
||
|
||
GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad) {
|
||
|
||
// try-again distribution
|
||
//static Distribution::Normal<float> dHead(0, Angle::degToRad(10));
|
||
//static Distribution::Normal<float> dUpdate(0, Angle::degToRad(3));
|
||
static Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));
|
||
|
||
int retries = 5;
|
||
float walked_m = 0;
|
||
GridWalkState<T> cur = start;
|
||
|
||
// the desired heading
|
||
Heading reqHeading = start.heading + (headChange_rad);
|
||
|
||
// walk until done
|
||
while(walked_m < distance_m) {
|
||
|
||
// evaluate all neighbors
|
||
drawer.reset();
|
||
for (T& neighbor : grid.neighbors(*cur.node)) {
|
||
|
||
const double prob = getProbability(grid, *start.node, *cur.node, neighbor, reqHeading);
|
||
drawer.add(neighbor, prob);
|
||
|
||
}
|
||
|
||
// too bad? start over!
|
||
if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) {
|
||
walked_m = 0;
|
||
cur = start;
|
||
//WHAT THE HELL
|
||
if (retries == 0) { reqHeading = dChange.draw(); }
|
||
continue;
|
||
}
|
||
|
||
// get the neighbor
|
||
const T& neighbor = drawer.get();
|
||
|
||
// update
|
||
walked_m += neighbor.getDistanceInMeter(*cur.node);
|
||
cur.node = &neighbor;
|
||
|
||
}
|
||
|
||
cur.distanceWalked_m = NAN;
|
||
cur.headingChange_rad = NAN;
|
||
cur.heading = reqHeading;
|
||
|
||
return cur;
|
||
|
||
}
|
||
|
||
/** rebuild the path for the given center point */
|
||
void rebuildPath(Grid<T>& grid, const Point3 centerOfMass) {
|
||
|
||
// find the grid node nearest to the current center-of-mass
|
||
auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); };
|
||
const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode);
|
||
|
||
delete path; path = nullptr;
|
||
//delete knn; knn = nullptr;
|
||
|
||
DijkstraNode<T>* dnTarget = dijkstra.getNode(target);
|
||
DijkstraNode<T>* dnStart = dijkstra.getNode(currentMass);
|
||
|
||
if (dnStart == nullptr) {
|
||
return;
|
||
}
|
||
|
||
// calculate the path from there to the target
|
||
try {
|
||
path = new DijkstraPath<T>(dnStart, dnTarget);
|
||
|
||
// create k-nn lookup
|
||
wrapper = Wrapper(path);
|
||
//knn = new KNN<Wrapper, 3>(wrapper);
|
||
} catch (...) {
|
||
//knn = nullptr;
|
||
path = nullptr;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
};
|
||
|
||
#endif // GRIDWALKSHORTESTPATHCONTROL_H
|