FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

some fixes [multithreading,..]

Browse Source 
needed interface changes [new options]
logger for android
wifi-ap-optimization
new test-cases
This commit is contained in:
FrankE
2016-09-28 12:19:14 +02:00
parent 91e3367372
commit 4f511d907e
62 changed files with 1418 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
grid/walk/v2/modules/WalkModuleFollowDestination.h
View File

@@ -2,39 +2,77 @@
#define WALKMODULEFOLLOWDESTINATION_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../nav/dijkstra/Dijkstra.h"
#include "../../../../nav/dijkstra/DijkstraPath.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/**
* favour edges p(e) that approach the destination
* 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;
struct DijkstraMapper {
struct DijkstraAccess {
const Grid<Node>& grid;
DijkstraMapper(const Grid<Node>& grid) : grid(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.getDistanceInCM(n2) * n2.navImportance;
return n1.getDistanceInMeter(n2) / n2.getNavImportance();
}
};
public:
/** ctor */
WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
/** ctor WITHOUT known destination*/
WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {
// shortest path calculation
dijkstra.build(&destination, DijkstraMapper(grid));
// ensure the template WalkState inherits from 'WalkStateFavorZ'
//StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
/** 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) {
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 {
@@ -43,26 +81,28 @@ public:
(void) nextNode;
}
virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
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);
const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);
// probability
return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
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);
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
};
#endif // WALKMODULEFOLLOWDESTINATION_H