diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index c0fec12..5d75082 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -131,7 +131,8 @@ namespace NM { 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); + const double res = dist.getProbability(diff); + return res; //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); } diff --git a/navMesh/walk/NavMeshWalkSinkOrSwim.h b/navMesh/walk/NavMeshWalkSinkOrSwim.h new file mode 100644 index 0000000..6860134 --- /dev/null +++ b/navMesh/walk/NavMeshWalkSinkOrSwim.h @@ -0,0 +1,156 @@ +#ifndef NAVMESHWALKSINKORSWIM_H +#define NAVMESHWALKSINKORSWIM_H + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" +#include "../../math/distribution/Normal.h" +#include "../../math/distribution/Uniform.h" + +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" + +namespace NM { + + /** + * try to move to the requested location + * and, if not, return null + */ + template class NavMeshWalkSinkOrSwim { + + public: + + struct Config { + Distribution::Uniform* distanceVariation = nullptr; + Distribution::Uniform* headingVariation = nullptr; + void check() { + Assert::isNotNull(distanceVariation, "distanceVariation must not be null"); + Assert::isNotNull(headingVariation, "headingVariation must not be null"); + } + }; + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + Config cfg; + + int hits = 0; + int misses = 0; + + public: + + + /** single result */ + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + ResultEntry lastRes; + + /** list of results */ + using ResultList = std::vector; + + public: + + /** ctor without config */ + NavMeshWalkSinkOrSwim(const NavMesh& mesh) : mesh(mesh), cfg() { + + } + + /** ctor with config */ + NavMeshWalkSinkOrSwim(const NavMesh& mesh, Config cfg) : mesh(mesh), cfg(cfg) { + cfg.check(); + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + ResultEntry getOne(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + ResultEntry re; + + // variation? + const float distVar = (cfg.distanceVariation) ? (cfg.distanceVariation->draw()) : (0); + const float headingVar = (cfg.headingVariation) ? (cfg.headingVariation->draw()) : (0); + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance() + distVar; + const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; + + // construct reachable region + NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + // get the to-be-reached destination's position (using start+distance+heading) + const Heading heading = params.heading + headingVar; + const Point2 dir = heading.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + re.heading = params.heading; // heading was OK -> keep + re.location.pos = dstTria->toPoint3(dst); // new destination position + re.location.tria = dstTria; // new destination triangle + re.probability = 1; + ++hits; + + // calculate probability + const NavMeshPotentialWalk pwalk(params, re.location); + re.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + re.probability *= p1; + } + + lastRes = re; + + } else { + +// re.heading = params.heading; // keep +// re.location = params.start; // keep +// re.probability = 0; // kill + re = lastRes; + //re.probability *= 0.1; + ++misses; + + } + + const int total = (hits + misses); + if (total % 10000 == 0) { + //std::cout << "hits: " << (hits*100/total) << "%" << std::endl; + } + + // done + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + return {getOne(params)}; + + } + + + }; + +} + +#endif // NAVMESHWALKSINKORSWIM_H