177 lines
4.7 KiB
C++
177 lines
4.7 KiB
C++
#ifndef NAVMESHWALKEVAL_H
|
|
#define NAVMESHWALKEVAL_H
|
|
|
|
#include "NavMeshWalkParams.h"
|
|
#include "../NavMeshLocation.h"
|
|
#include "../../math/distribution/Normal.h"
|
|
#include "../../misc/PerfCheck.h"
|
|
#include "../../Assertions.h"
|
|
#include "../meta/NavMeshDijkstra.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 WalkEvalHeadingStartEndVonMises : 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
|
|
// WalkEvalHeadingStartEndVonMises(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);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
/**
|
|
* higher probability for potints that approach the target location
|
|
*/
|
|
template <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {
|
|
|
|
const double p;
|
|
|
|
public:
|
|
|
|
WalkEvalApproachesTarget(const double pApproaching = 0.65) : p(pApproaching) {
|
|
;
|
|
}
|
|
|
|
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
|
|
|
|
// sanity check
|
|
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
|
|
|
|
const NavMeshLocation<Tria> locStart = walk.requested.start;
|
|
const NavMeshLocation<Tria> locEnd = walk.end;
|
|
|
|
const float distFromNew = locEnd.tria-> template getDistanceToDestination<Tria>(locEnd.pos);
|
|
const float distFromOld = locStart.tria-> template getDistanceToDestination<Tria>(locStart.pos);
|
|
|
|
const double pRemain = 1.0 / (2+distFromNew);
|
|
|
|
return ((distFromNew <= distFromOld) ? (p) : (1.0 - p)) + std::pow(pRemain, 0.2);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // NAVMESHWALKEVAL_H
|