167 lines
3.9 KiB
C++
167 lines
3.9 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#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 <typename Tria> class NavMeshWalkSinkOrSwim {
|
||
|
||
public:
|
||
|
||
struct Config {
|
||
Distribution::Uniform<float>* distanceVariation = nullptr;
|
||
Distribution::Uniform<float>* headingVariation = nullptr;
|
||
void check() {
|
||
Assert::isNotNull(distanceVariation, "distanceVariation must not be null");
|
||
Assert::isNotNull(headingVariation, "headingVariation must not be null");
|
||
}
|
||
};
|
||
|
||
private:
|
||
|
||
const NavMesh<Tria>& mesh;
|
||
|
||
std::vector<NavMeshWalkEval<Tria>*> evals;
|
||
|
||
Config cfg;
|
||
|
||
int hits = 0;
|
||
int misses = 0;
|
||
|
||
public:
|
||
|
||
|
||
/** single result */
|
||
struct ResultEntry {
|
||
NavMeshLocation<Tria> location;
|
||
Heading heading;
|
||
double probability;
|
||
ResultEntry() : heading(0) {;}
|
||
};
|
||
|
||
ResultEntry lastRes;
|
||
|
||
/** list of results */
|
||
using ResultList = std::vector<ResultEntry>;
|
||
|
||
public:
|
||
|
||
/** ctor without config */
|
||
NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh) : mesh(mesh), cfg() {
|
||
|
||
}
|
||
|
||
/** ctor with config */
|
||
NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh, Config cfg) : mesh(mesh), cfg(cfg) {
|
||
cfg.check();
|
||
}
|
||
|
||
/** add a new evaluator to the walker */
|
||
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
|
||
this->evals.push_back(eval);
|
||
}
|
||
|
||
ResultEntry getOne(const NavMeshWalkParams<Tria>& 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<Tria> 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<Tria> pwalk(params, re.location);
|
||
re.probability = 1.0;
|
||
for (const NavMeshWalkEval<Tria>* 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<Tria>& params) {
|
||
|
||
// sanity checks
|
||
params.check();
|
||
|
||
return {getOne(params)};
|
||
|
||
}
|
||
|
||
|
||
};
|
||
|
||
}
|
||
|
||
#endif // NAVMESHWALKSINKORSWIM_H
|