170 lines
4.1 KiB
C++
170 lines
4.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 GRIDWALKERMULTI_H
|
||
#define GRIDWALKERMULTI_H
|
||
|
||
#include "../../Grid.h"
|
||
#include "../../../math/DrawList.h"
|
||
#include "modules/WalkModule.h"
|
||
|
||
// FOR TESTING
|
||
#include "../../../geo/Heading.h"
|
||
|
||
/**
|
||
* modular grid-walker that takes various sub-components to determine
|
||
* p(e) and thus randomly pick edges
|
||
*/
|
||
template <typename Node, typename WalkState> class GridWalkerMulti {
|
||
|
||
private:
|
||
|
||
struct Path {
|
||
std::vector<const Node*> nodes;
|
||
const Node* getSrc() const {return nodes.front();}
|
||
const Node* getDst() const {return nodes.back();}
|
||
};
|
||
|
||
|
||
private:
|
||
|
||
/** all modules to evaluate */
|
||
std::vector<WalkModule<Node, WalkState>*> modules;
|
||
|
||
public:
|
||
|
||
/** add the given module */
|
||
void addModule(WalkModule<Node, WalkState>* module) {
|
||
modules.push_back(module);
|
||
}
|
||
|
||
/** perform the walk based on the configured setup */
|
||
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float dist_m) {
|
||
|
||
// get the node that corresponds to start;
|
||
const Node* const startNode = grid.getNodePtrFor(_startState.startPos);
|
||
Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
|
||
|
||
const uint64_t seed = (uint64_t) startNode + rand();
|
||
DrawList<Path> drawPath;
|
||
drawPath.setSeed(seed);
|
||
|
||
for (int i = 0; i < 1; ++i) {
|
||
|
||
float dist = dist_m;
|
||
|
||
// currently examined node
|
||
const Node* curNode = startNode;
|
||
|
||
Path path;
|
||
path.nodes.push_back(curNode);
|
||
|
||
// until distance is reached
|
||
while (dist > 0) {
|
||
|
||
DrawList<const Node*> drawEdge;
|
||
drawEdge.setSeed(seed+1);
|
||
|
||
//if (curNode->getNumNeighbors() < 6) {break;}
|
||
|
||
// evaluate each neighbor
|
||
for (const Node& neighbor : grid.neighbors(*curNode)) {
|
||
const double prob = getProb(*startNode, *curNode, neighbor);
|
||
drawEdge.add(&neighbor, prob);
|
||
}
|
||
|
||
// pick a neighbor
|
||
const Node* nextNode = drawEdge.get();
|
||
|
||
dist -= nextNode->getDistanceInMeter(*curNode);
|
||
|
||
curNode = nextNode;
|
||
|
||
path.nodes.push_back(curNode);
|
||
|
||
}
|
||
|
||
const double prob = getProb(path);
|
||
drawPath.add(path, prob);
|
||
|
||
}
|
||
|
||
const Path& bestPath = drawPath.get();
|
||
|
||
WalkState nextState = _startState;
|
||
nextState.startPos = *(bestPath.getDst());
|
||
return nextState;
|
||
|
||
|
||
}
|
||
|
||
private:
|
||
|
||
inline double getProb(const Node& n0, const Node& n1, const Node& n2) {
|
||
static float angle = 0; angle += 0.05;
|
||
angle = std::fmod(angle, 6);
|
||
if (&n1 == &n2) {return 1;}
|
||
const Heading head1(angle);
|
||
const Heading head2(n1.x_cm, n1.y_cm, n2.x_cm, n2.y_cm);
|
||
const float diff = head1.getDiffHalfRAD(head2);
|
||
return rand() % 32;
|
||
}
|
||
|
||
inline double getProb(const Path& path) {
|
||
const Point3 p1 = path.getSrc()->inMeter();
|
||
const Point3 p2 = path.getDst()->inMeter();
|
||
const float dist = p1.getDistance(p2);
|
||
return 1/dist;
|
||
}
|
||
|
||
/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
|
||
inline void updateBefore(WalkState& state) {
|
||
|
||
for (WalkModule<Node, WalkState>* mdl : modules) {
|
||
mdl->updateBefore(state);
|
||
}
|
||
|
||
}
|
||
|
||
/** update the WalkState after the random walk (if needed) */
|
||
inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {
|
||
|
||
for (WalkModule<Node, WalkState>* mdl : modules) {
|
||
mdl->updateAfter(state, startNode, endNode);
|
||
}
|
||
|
||
}
|
||
|
||
/** one step taken. inform modules */
|
||
inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {
|
||
|
||
for (WalkModule<Node, WalkState>* mdl : modules) {
|
||
mdl->step(state, curNode, nextNode);
|
||
}
|
||
|
||
}
|
||
|
||
/** get the probability for the given random walk (one edge) */
|
||
inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {
|
||
|
||
double prob = 1.0;
|
||
|
||
for (const WalkModule<Node, WalkState>* mdl : modules) {
|
||
prob *= mdl->getProbability(state, start, cur, next);
|
||
}
|
||
|
||
return prob;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // GRIDWALKERMULTI_H
|