142 lines
3.7 KiB
C++
142 lines
3.7 KiB
C++
#ifndef NAVMESHWALKEVAL_H
|
|
#define NAVMESHWALKEVAL_H
|
|
|
|
#include "NavMeshWalkParams.h"
|
|
#include "../NavMeshLocation.h"
|
|
#include "../../math/Distributions.h"
|
|
#include "../../misc/PerfCheck.h"
|
|
|
|
namespace NM {
|
|
|
|
template <typename Tria> struct NavMeshPotentialWalk {
|
|
|
|
NavMeshWalkParams<Tria> requested;
|
|
|
|
NavMeshLocation<Tria> end;
|
|
|
|
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
|
|
;
|
|
}
|
|
|
|
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
|
|
;
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* evaluate a NavMeshWalk from -> to = probability
|
|
*/
|
|
template <typename Tria> class NavMeshWalkEval {
|
|
|
|
public:
|
|
|
|
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
* evaluate the difference between head(start,end) and the requested heading
|
|
*/
|
|
template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {
|
|
|
|
const double sigma_rad;
|
|
const double kappa;
|
|
Distribution::VonMises<double> _dist;
|
|
Distribution::LUT<double> dist;
|
|
|
|
public:
|
|
|
|
// kappa = 1/var = 1/sigma^2
|
|
// https://en.wikipedia.org/wiki/Von_Mises_distribution
|
|
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
|
|
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
|
|
;
|
|
}
|
|
|
|
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
|
|
|
|
PERF_REGION(4, "WalkEvalHeadingStartEnd");
|
|
|
|
Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");
|
|
|
|
const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
|
|
const float diff = head.getDiffHalfRAD(walk.requested.heading);
|
|
//const float diff = Heading::getSignedDiff(params.heading, head);
|
|
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
|
|
return dist.getProbability(diff);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* evaluate the difference between head(start,end) and the requested heading
|
|
*/
|
|
template <typename Tria> class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval<Tria> {
|
|
|
|
const double sigma_rad;
|
|
Distribution::Normal<double> dist;
|
|
|
|
public:
|
|
|
|
WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) :
|
|
sigma_rad(sigma_rad), dist(0, sigma_rad) {
|
|
;
|
|
}
|
|
|
|
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
|
|
|
|
PERF_REGION(4, "WalkEvalHeadingStartEnd");
|
|
|
|
Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");
|
|
|
|
const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
|
|
const float diff = head.getDiffHalfRAD(walk.requested.heading);
|
|
//const float diff = Heading::getSignedDiff(params.heading, head);
|
|
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
|
|
return dist.getProbability(diff);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
/**
|
|
* evaluate the difference between distance(start, end) and the requested distance
|
|
*/
|
|
template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {
|
|
|
|
const double sigma;
|
|
|
|
const Distribution::Normal<double> dist;
|
|
|
|
public:
|
|
|
|
WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
|
|
|
|
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
|
|
|
|
PERF_REGION(5, "WalkEvalDistance");
|
|
|
|
const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
|
|
const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
|
|
const float diff = walkedDistance_m - requestedDistance_m;
|
|
return dist.getProbability(diff);
|
|
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
}
|
|
|
|
#endif // NAVMESHWALKEVAL_H
|