FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

minor changes to floorplan

Browse Source 
fixed some compile issues
worked on nav-meshes
added some tests
This commit is contained in:
FrankE
2018-01-16 12:41:05 +01:00
parent fee6cd3496
commit 55061ef0da
24 changed files with 1288 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
navMesh/walk/NavMeshSub.h
View File

@@ -28,6 +28,14 @@ namespace NM {
return false;
}
/** does this submesh contain the given point? */
bool contains(const Point3 p3) const {
for (const Tria* t : toVisit) {
if (t->contains(p3)) {return true;}
}
return false;
}
/** get the triangle that contains the given point (if any) */
const Tria* getContainingTriangle(const Point2 p2) const {
for (const Tria* t : toVisit) {
@@ -37,24 +45,36 @@ namespace NM {
}
/** perform random operations on the submesh */
NavMeshRandom<Tria> getRandom() {
NavMeshRandom<Tria> getRandom() const {
return NavMeshRandom<Tria>(toVisit);
}
/** allows for-each iteration over all included triangles */
decltype(toVisit.begin()) begin() {return toVisit.begin();}
/** allows for-each iteration over all included triangles */
decltype(toVisit.end()) end() {return toVisit.end();}
private:
void build(const NavMeshLocation<Tria>& loc, float radius_m) {
PERF_REGION(6, "NavMeshSub::build()");
std::unordered_set<const Tria*> visited;
// starting-triangle + all its (max 3) neighbors
toVisit.push_back(loc.tria);
visited.insert(loc.tria);
for (const auto* n : *loc.tria) {
toVisit.push_back( (const Tria*)n );
}
// for (const auto* n : *loc.tria) {
// toVisit.push_back( (const Tria*)n );
// }
// size_t next = 1; // start with the first neighbor (skip starting triangle itself)
size_t next = 1; // start with the first neighbor (skip starting triangle itself)
size_t next = 0;
while (next < toVisit.size()) {
// next triangle
@@ -63,7 +83,8 @@ namespace NM {
// neighbors
for (const auto* n : *cur) {
const Tria* t = (const Tria*) n;
const float dist = loc.pos.getDistance(n->getCenter());
//const float dist = loc.pos.getDistance(n->getCenter());
const float dist = n->getDistanceApx(loc.pos);
if (dist > radius_m) {continue;}
if (visited.find(t) != visited.end()) {continue;}
toVisit.push_back(t);

46
navMesh/walk/NavMeshWalkEval.h
View File

@@ -4,6 +4,7 @@
#include "NavMeshWalkParams.h"
#include "../NavMeshLocation.h"
#include "../../math/Distributions.h"
#include "../../misc/PerfCheck.h"
namespace NM {
@@ -13,6 +14,10 @@ namespace NM {
NavMeshLocation<Tria> end;
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
;
}
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
;
}
@@ -57,10 +62,9 @@ namespace NM {
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
if (walk.requested.start.pos == walk.end.pos) {
std::cout << "warn! start-position == end-positon" << std::endl;
return 0;
}
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);
@@ -72,6 +76,38 @@ namespace NM {
};
/**
* 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
*/
@@ -87,6 +123,8 @@ namespace NM {
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;

26
navMesh/walk/NavMeshWalkParams.h
View File

@@ -3,6 +3,7 @@
#include "../../geo/Heading.h"
#include "../NavMeshLocation.h"
#include "../NavMeshType.h"
namespace NM {
@@ -20,12 +21,18 @@ namespace NM {
Assert::isTrue(isValid(), "invalid step-sizes given");
if (start.tria->isPlain()) {
return stepSizeFloor_m * steps;
} else {
if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {
return stepSizeStair_m * steps;
} else {
return stepSizeFloor_m * steps;
}
// if (start.tria->isPlain()) {
// return stepSizeFloor_m * steps;
// } else {
// return stepSizeStair_m * steps;
// }
}
};
@@ -36,9 +43,6 @@ namespace NM {
/** walk starts here (pos/tria) */
NavMeshLocation<Tria> start;
// /** to-be-walked distance */
// float distance_m;
/** direction to walk to */
Heading heading;
@@ -54,9 +58,17 @@ namespace NM {
/** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */
float getToBeWalkedDistance() const {
return stepSizes.inMeter(numSteps, start);
if (_toBeWalkedDistance != _toBeWalkedDistance) {
_toBeWalkedDistance = stepSizes.inMeter(numSteps, start);
}
return _toBeWalkedDistance;
}
private:
// precalc
mutable float _toBeWalkedDistance = NAN;
};
}

146
navMesh/walk/NavMeshWalkRandom.h Normal file
View File

@@ -0,0 +1,146 @@
#ifndef NAVMESHWALKRANDOM_H
#define NAVMESHWALKRANDOM_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* pick a truely random destination within the reachable area
* weight this area (evaluators)
* repeat this several times to find a robus destination
*/
template <typename Tria> class NavMeshWalkRandom {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
struct ResultList : std::vector<ResultEntry> {};
public:
/** ctor */
NavMeshWalkRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
ResultEntry res;
res.probability = 0;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1;
// construct reachable region
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshPotentialWalk<Tria> pwalk(params);
// improve quality (the higher, the better)
for (int i = 0; i < 25; ++i) {
PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");
// draw a random destination
// is this destination within the reachable area? (triangles might be larger!)
pwalk.end = rnd.draw();
if (pwalk.end.pos.getDistance(params.start.pos) > toBeWalkedDistSafe) {
--i; continue;
}
// calculate the probability for this destination
const double p = eval(pwalk);
// better?
if (p > res.probability) {
res.location = pwalk.end;
res.probability = p;
}
}
// destination is known. update the heading
res.heading = Heading(params.start.pos.xy(), res.location.pos.xy());
return res;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) const {
ResultList res;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1;
// construct reachable region
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshPotentialWalk<Tria> pwalk(params);
// improve quality (the higher, the better)
for (int i = 0; i < 25; ++i) {
PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");
pwalk.end = rnd.drawWithin(params.start.pos, toBeWalkedDistSafe);
// calculate the probability for this destination
const double p = eval(pwalk);
ResultEntry re;
re.heading = Heading(params.start.pos.xy(), pwalk.end.pos.xy());
re.location = pwalk.end;
re.probability = p;
res.push_back(re);
}
return res;
}
double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
PERF_REGION(2, "NavMeshWalkRandom::EvalLoop");
double p = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
p *= p1;
}
return p;
}
};
}
#endif // NAVMESHWALKRANDOM_H

169
navMesh/walk/NavMeshWalkSemiRandom.h Normal file
View File

@@ -0,0 +1,169 @@
#ifndef NAVMESHWALKSEMIRANDOM_H
#define NAVMESHWALKSEMIRANDOM_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* similar to NavMeshWalkRandom but:
* pick a semi random destination within the reachable area (requested distance/heading + strong deviation)
* if this destination is reachable:
* weight this area (evaluators)
* repeat this some times to find a robus destination
*/
template <typename Tria> class NavMeshWalkSemiRandom {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
struct ResultList : public std::vector<ResultEntry> {};
public:
/** ctor */
NavMeshWalkSemiRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
static Distribution::Normal<float> dDist(1.0, 0.4);
static Distribution::Normal<float> dHead(0.0, 1.0);
// construct reachable region
const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
ResultEntry re;
NavMeshPotentialWalk<Tria> pwalk(params);
pwalk.end = reachable.getRandom().draw(); // to have at least a non-start solution
re.probability = eval(pwalk);
re.location = pwalk.end;
for (int i = 0; i < 25; ++i) {
const float distance = params.getToBeWalkedDistance() * dDist.draw();
const Heading head = params.heading + dHead.draw();
// only forward!
if (distance < 0.01) {continue;}
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = head.asVector();
const Point2 dst = params.start.pos.xy() + (dir * distance);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
pwalk.end.pos = dstTria->toPoint3(dst);
pwalk.end.tria = dstTria;
const double p = eval(pwalk);
// better?
if (p > re.probability) {
re.location = pwalk.end;
re.probability = p;
re.heading = head;
}
}
}
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) const {
static Distribution::Normal<float> dDist(1.0, 0.4);
static Distribution::Normal<float> dHead(0.0, 1.0);
ResultList res;
// construct reachable region
const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshPotentialWalk<Tria> pwalk(params);
for (int i = 0; i < 25; ++i) {
const float distance = params.getToBeWalkedDistance() * dDist.draw();
const Heading head = params.heading + dHead.draw();
// only forward!
if (distance < 0.01) {continue;}
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = head.asVector();
const Point2 dst = params.start.pos.xy() + (dir * distance);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
pwalk.end.pos = dstTria->toPoint3(dst);
pwalk.end.tria = dstTria;
const double p = eval(pwalk);
ResultEntry re;
re.location = pwalk.end;
re.probability = p;
re.heading = head;
res.push_back(re);
}
}
return res;
}
double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
double p = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
p *= p1;
}
return p;
}
};
}
#endif // NAVMESHWALKSEMIRANDOM_H

46
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -25,16 +25,17 @@ namespace NM {
public:
struct Result {
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
Result() : heading(0) {;}
ResultEntry() : heading(0) {;}
};
/** list of results */
using ResultList = std::vector<ResultEntry>;
public:
/** ctor */
@@ -47,10 +48,11 @@ namespace NM {
this->evals.push_back(eval);
}
Result getDestination(const NavMeshWalkParams<Tria>& params) {
Result res;
res.heading = params.heading;
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
ResultEntry re;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
@@ -60,7 +62,7 @@ namespace NM {
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = res.heading.asVector();
const Point2 dir = params.heading.asVector();
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
const Tria* dstTria = reachable.getContainingTriangle(dst);
@@ -68,16 +70,16 @@ namespace NM {
// is above destination reachable?
if (dstTria) {
res.location.pos = dstTria->toPoint3(dst);
res.location.tria = 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
++hits;
} else {
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshLocation<Tria> rndLoc = rnd.draw();
res.location = rndLoc;
res.heading = Heading(params.start.pos.xy(), rndLoc.pos.xy()); // update the heading
NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper
re.location = rnd.draw(); // get a random destianation
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading
++misses;
}
@@ -87,17 +89,23 @@ namespace NM {
std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
}
const NavMeshPotentialWalk<Tria> pwalk(params, res.location);
res.probability = 1.0;
// 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);
res.probability *= p1;
re.probability *= p1;
}
return res;
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
return {getOne(params)};
}
};