FrankE
a2c9e575a2
- worked on about everything - grid walker using plugable modules - wifi models - new distributions - worked on geometric data-structures - added typesafe timestamps - worked on grid-building - added sensor-classes - added sensor analysis (step-detection, turn-detection) - offline data reader - many test-cases
69 lines
1.8 KiB
C++
69 lines
1.8 KiB
C++
#ifndef WALKMODULEFOLLOWDESTINATION_H
|
|
#define WALKMODULEFOLLOWDESTINATION_H
|
|
|
|
#include "WalkModule.h"
|
|
#include "../../../../nav/dijkstra/Dijkstra.h"
|
|
|
|
/**
|
|
* favour edges p(e) that approach the destination
|
|
*/
|
|
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
|
|
|
|
private:
|
|
|
|
Dijkstra<Node> dijkstra;
|
|
|
|
struct DijkstraMapper {
|
|
const Grid<Node>& grid;
|
|
DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
|
|
int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
|
|
const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
|
|
float getWeightBetween(const Node& n1, const Node& n2) const {
|
|
return n1.getDistanceInCM(n2) * n2.navImportance;
|
|
}
|
|
};
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
|
|
|
|
// shortest path calculation
|
|
dijkstra.build(&destination, DijkstraMapper(grid));
|
|
|
|
}
|
|
|
|
virtual void updateBefore(WalkState& state) override {
|
|
(void) state;
|
|
}
|
|
|
|
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
|
(void) state;
|
|
(void) curNode;
|
|
(void) nextNode;
|
|
}
|
|
|
|
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
|
|
|
(void) state;
|
|
(void) startNode;
|
|
|
|
const float kappa = 0.8;
|
|
const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
|
|
const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
|
|
|
|
// probability
|
|
return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
|
|
|
|
}
|
|
|
|
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
|
(void) state;
|
|
(void) startNode;
|
|
(void) endNode;
|
|
}
|
|
|
|
};
|
|
|
|
#endif // WALKMODULEFOLLOWDESTINATION_H
|