163 lines
4.4 KiB
C++
163 lines
4.4 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 GRIDWALKSIMPLECONTROL_H
|
||
#define GRIDWALKSIMPLECONTROL_H
|
||
|
||
|
||
|
||
#include "../../geo/Heading.h"
|
||
#include "../Grid.h"
|
||
|
||
#include "../../math/Distributions.h"
|
||
#include "../../math/DrawList.h"
|
||
#include "../../math/Random.h"
|
||
|
||
#include "GridWalkState.h"
|
||
#include "GridWalkHelper.h"
|
||
#include "GridWalk.h"
|
||
|
||
template <typename T> class GridWalkSimpleControl : public GridWalk<T> {
|
||
|
||
friend class GridWalkHelper;
|
||
|
||
private:
|
||
|
||
/** 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);
|
||
|
||
|
||
|
||
DrawList<T&> drawer;
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
GridWalkSimpleControl() {
|
||
gen.seed(1234);
|
||
}
|
||
|
||
Distribution::Normal<float> dHead = Distribution::Normal<float>(1, 0.01);
|
||
Distribution::Normal<float> dWalk = Distribution::Normal<float>(1, 0.10);
|
||
Distribution::Normal<float> sWalk = Distribution::Normal<float>(0, 0.10);
|
||
|
||
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
|
||
|
||
// proportional change of the heading
|
||
|
||
|
||
// proportional change of the to-be-walked distance
|
||
//static Distribution::Normal<float> dWalk(1, 0.10);
|
||
|
||
distance_m = distance_m*dWalk.draw()*1.4; // TODO: why * X?
|
||
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(const T& start, 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(15), diff);
|
||
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
|
||
|
||
// nodes own importance
|
||
const double nodeProb = Distribution::Logistic<float>::getCDF(possible.imp, 1, 0.9);
|
||
//const double nodeProb = std::pow(possible.imp, 2);
|
||
//const double nodeProb = 1.0;
|
||
|
||
// bring it together
|
||
return angleProb * nodeProb;
|
||
|
||
}
|
||
|
||
Distribution::Uniform<float> dChange = Distribution::Uniform<float>(Angle::degToRad(0), +Angle::degToRad(359));
|
||
|
||
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(*start.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;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // GRIDWALKSIMPLECONTROL_H
|