98 lines
2.5 KiB
C++
98 lines
2.5 KiB
C++
#ifndef INDOOR_GW3_WALKEVALUATOR_H
|
|
#define INDOOR_GW3_WALKEVALUATOR_H
|
|
|
|
#include "Structs.h"
|
|
#include "Helper.h"
|
|
|
|
#include "../../../math/Distributions.h"
|
|
#include "../../../grid/Grid.h"
|
|
|
|
namespace GW3 {
|
|
|
|
/** interface for all evaluators that return a probability for a given walk */
|
|
template <typename Node> class WalkEvaluator {
|
|
|
|
public:
|
|
|
|
/** get the probability for the given walk */
|
|
//virtual double getProbability(const Walk<Node>& walk) const = 0;
|
|
|
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0;
|
|
|
|
};
|
|
|
|
|
|
/** evaluate the grid-node-importance importance(end) */
|
|
template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {
|
|
|
|
Grid<Node>* grid;
|
|
|
|
public:
|
|
|
|
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
|
|
|
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
|
|
|
|
(void) params;
|
|
(void) pStart;
|
|
|
|
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
|
|
const Node& node = grid->getNodeFor(gp);
|
|
const double p = node.getWalkImportance();
|
|
return std::pow(p,10);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
/** evaluate the difference between head(start,end) and the requested heading */
|
|
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
|
|
|
|
const double sigma;
|
|
|
|
public:
|
|
|
|
WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
|
|
|
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
|
|
|
|
(void) params;
|
|
|
|
if (pStart == pEnd) {
|
|
std::cout << "warn! start-position == end-positon" << std::endl;
|
|
return 0;
|
|
}
|
|
|
|
const Heading head(pStart.xy(), pEnd.xy());
|
|
const float diff = head.getDiffHalfRAD(params.heading);
|
|
//const float diff = Heading::getSignedDiff(params.heading, head);
|
|
return Distribution::Normal<double>::getProbability(0, sigma, diff);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
/** evaluate the difference between distance(start, end) and the requested distance */
|
|
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
|
|
|
|
const double sigma;
|
|
|
|
public:
|
|
|
|
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
|
|
|
|
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
|
|
|
|
const float walkedDistance_m = pStart.getDistance(pEnd);
|
|
return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // INDOOR_GW3_WALKEVALUATOR_H
|