128 lines
3.4 KiB
C++
128 lines
3.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 WALKMODULEFOLLOWDESTINATION_H
|
||
#define WALKMODULEFOLLOWDESTINATION_H
|
||
|
||
#include "WalkModule.h"
|
||
#include "WalkStateHeading.h"
|
||
|
||
#include "../../../../nav/dijkstra/Dijkstra.h"
|
||
#include "../../../../nav/dijkstra/DijkstraPath.h"
|
||
|
||
#include "../../../../math/distribution/Normal.h"
|
||
#include "../../../../Assertions.h"
|
||
|
||
|
||
/**
|
||
* favor nodes that approach a known destination
|
||
*/
|
||
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
|
||
|
||
private:
|
||
|
||
const Grid<Node>& grid;
|
||
Dijkstra<Node> dijkstra;
|
||
const DijkstraNode<Node>* dnDest = nullptr;
|
||
|
||
struct DijkstraAccess {
|
||
const Grid<Node>& grid;
|
||
DijkstraAccess(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.getDistanceInMeter(n2) / n2.getNavImportance();
|
||
}
|
||
};
|
||
|
||
public:
|
||
|
||
/** ctor WITHOUT known destination*/
|
||
WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {
|
||
|
||
// ensure the template WalkState inherits from 'WalkStateFavorZ'
|
||
//StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
|
||
|
||
}
|
||
|
||
const Dijkstra<Node>& getDijkstra() const {
|
||
return dijkstra;
|
||
}
|
||
|
||
/** ctor WITH known destination*/
|
||
WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
|
||
setDestination(destination);
|
||
}
|
||
|
||
|
||
/** set the desired destination node */
|
||
void setDestination(const Node& dest) {
|
||
DijkstraAccess acc(grid);
|
||
dijkstra.build(&dest, acc);
|
||
dnDest = dijkstra.getNode(dest);
|
||
}
|
||
|
||
|
||
/** get the shortest path from the given start to the configured destination */
|
||
DijkstraPath<Node> getShortestPath(const Node& start) {
|
||
|
||
// destination unknown? -> empty path
|
||
if (!dnDest) {return DijkstraPath<Node>();}
|
||
|
||
const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
|
||
const DijkstraPath<Node> path(dnStart, dnDest);
|
||
return path;
|
||
|
||
}
|
||
|
||
virtual void updateBefore(WalkState& state, const Node& startNode) override {
|
||
(void) state;
|
||
(void) startNode;
|
||
}
|
||
|
||
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
||
(void) state;
|
||
(void) startNode;
|
||
(void) endNode;
|
||
|
||
}
|
||
|
||
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
||
(void) state;
|
||
(void) curNode;
|
||
(void) nextNode;
|
||
}
|
||
|
||
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
||
|
||
(void) state;
|
||
(void) startNode;
|
||
|
||
const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
|
||
const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);
|
||
|
||
if (dnCur == nullptr) {return 1.0;}
|
||
if (dnPot == nullptr) {return 1.0;}
|
||
|
||
constexpr double kappa = 0.70;
|
||
|
||
const float curDistToTarget = dnCur->cumWeight;
|
||
const float potDistToTarget = dnPot->cumWeight;
|
||
|
||
return (potDistToTarget < curDistToTarget) ? (kappa) : (1.0-kappa);
|
||
|
||
}
|
||
|
||
|
||
};
|
||
|
||
|
||
#endif // WALKMODULEFOLLOWDESTINATION_H
|