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

some refactorings/fixes

Browse Source 
worked on nav-mesh stuff
new tests
This commit is contained in:
k-a-z-u
2018-01-24 11:27:11 +01:00
parent c9d02d440a
commit 1a1f249e9b
10 changed files with 70 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

247
floorplan/v2/FloorplanReader.h
View File

@@ -37,252 +37,7 @@ namespace Floorplan {
throw Exception( throw Exception(
std::string() + "error while loading XML " + file + "\n" + std::string() + "error while loading XML " + file + "\n" +
((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")));
);
} }
IndoorMap* map = parse(doc); IndoorMap* map = parse(doc);
return map; return map;

6
math/random/RandomGenerator.h
View File

@@ -1,5 +1,5 @@
#ifndef RANDOM_Random::RandomGenerator_H #ifndef RANDOM_Random_RandomGenerator_H
#define RANDOM_Random::RandomGenerator_H #define RANDOM_Random_RandomGenerator_H
#include <random> #include <random>
#include <chrono> #include <chrono>
@@ -26,4 +26,4 @@ namespace Random {
} }
#endif // K_MATH_RANDOM_Random::RandomGenerator_H #endif // RANDOM_Random_RandomGenerator_H

18
navMesh/NavMesh.h
View File

@@ -45,7 +45,7 @@ namespace NM {
} }
/** get the triangle this point belongs to (if any) */ /** get the triangle this point belongs to (if any) */
NavMeshLocation<Tria> getLocation(const Point3 pos) { NavMeshLocation<Tria> getLocation(const Point3 pos) const {
for (const Tria* tria : triangles) { for (const Tria* tria : triangles) {
if (tria->contains(pos)) { if (tria->contains(pos)) {
return NavMeshLocation<Tria>(pos, tria); return NavMeshLocation<Tria>(pos, tria);
@@ -54,6 +54,19 @@ namespace NM {
throw Exception("location not found within NavMesh: " + pos.asString()); throw Exception("location not found within NavMesh: " + pos.asString());
} }
/** get the triangle/point on the mesh that is nearest to the given location */
NavMeshLocation<Tria> getLocationNearestTo(const Point3 pos) const {
auto comp = [pos] (const Tria* t1, const Tria* t2) {
//return t1->getCenter().getDistance(pos) < t2->getCenter().getDistance(pos);
return t1->getDistanceApx(pos) < t2->getDistanceApx(pos);
};
auto it = std::min_element(triangles.begin(), triangles.end(), comp);
const Tria* best = *it;
Point3 rPos = best->toPoint3Near(pos.xy());
return NavMeshLocation<Tria>(rPos, best);
}
/** connect both triangles */ /** connect both triangles */
void connectBiDir(int idx1, int idx2) { void connectBiDir(int idx1, int idx2) {
connectUniDir(idx1,idx2); connectUniDir(idx1,idx2);
@@ -86,7 +99,8 @@ namespace NM {
/** ---------------- MISC ---------------- */ /** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandom() { /** get a random-generator for several mesh-actions */
NavMeshRandom<Tria> getRandom() const {
return NavMeshRandom<Tria>(triangles); return NavMeshRandom<Tria>(triangles);
} }

6
navMesh/NavMeshDebug.h
View File

@@ -50,7 +50,7 @@ namespace NM {
plot.add(&border); plot.add(&border);
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2); plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00"); plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");
plot.add(&distances); distances.setPointSize(2); distances.setPointType(7); plot.add(&distances); distances.setPointSize(0.75); distances.setPointType(7);
plot.add(&shortestPath); shortestPath.getStroke().setWidth(3); plot.add(&shortestPath); shortestPath.getStroke().setWidth(3);
} }
@@ -72,6 +72,8 @@ namespace NM {
} }
plot.getAxisCB().setRange(min, max + 0.000001); plot.getAxisCB().setRange(min, max + 0.000001);
} }
template <typename Tria> void addMesh(NavMesh<Tria>& nm) { template <typename Tria> void addMesh(NavMesh<Tria>& nm) {
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
@@ -127,7 +129,7 @@ namespace NM {
NavMeshRandom<Tria> rnd = mesh.getRandom(); NavMeshRandom<Tria> rnd = mesh.getRandom();
for (int i = 0; i < 900; ++i) { for (int i = 0; i < 5000; ++i) {
NavMeshLocation<Tria> loc = rnd.draw(); NavMeshLocation<Tria> loc = rnd.draw();
float v = loc.tria->interpolate(loc.pos, loc.tria->spFromP1.distance, loc.tria->spFromP2.distance, loc.tria->spFromP3.distance); float v = loc.tria->interpolate(loc.pos, loc.tria->spFromP1.distance, loc.tria->spFromP2.distance, loc.tria->spFromP3.distance);
distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v); distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v);

2
navMesh/NavMeshFactory.h
View File

@@ -12,7 +12,7 @@
#include "../lib/gpc/gpc.cpp.h" #include "../lib/gpc/gpc.cpp.h"
#include "../lib/Recast/Recast.h" #include "../lib/Recast/Recast.h"
#include <string.h> // memset
namespace NM { namespace NM {

2
navMesh/NavMeshRandom.h
View File

@@ -36,6 +36,8 @@ namespace NM {
/** ctor (const/non-const using T) */ /** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) { template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
Assert::isFalse(srcTriangles.empty(), "no triangles given. mesh is empty");
// 1st = almost always the same number?! // 1st = almost always the same number?!
gen(); gen(); gen(); gen();

19
navMesh/NavMeshTriangle.h
View File

@@ -185,6 +185,7 @@ namespace NM {
w = 1-u-v; w = 1-u-v;
} }
/** barycentric interpolation at Point p for val1@p1, val2@p2, val3@p3 */
template <typename T> T interpolate(const Point3 p, const T val1, const T val2, const T val3) const { template <typename T> T interpolate(const Point3 p, const T val1, const T val2, const T val3) const {
float u, v, w; float u, v, w;
@@ -194,6 +195,9 @@ namespace NM {
} }
/** does the triangle contain the given 3D point? */ /** does the triangle contain the given 3D point? */
bool contains(const Point3 p) const { bool contains(const Point3 p) const {
return (minZ <= p.z) && (maxZ >= p.z) && contains(p.xy()); return (minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
@@ -245,6 +249,21 @@ namespace NM {
} }
/** nearest point on the triangle */
Point3 toPoint3Near(const Point2 p) const {
float u, v;
getUV(p, u, v);
if (u < 0) {u = 0;}
if (u > 1) {u = 1;}
if (v < 0) {v = 0;}
if (v > 1) {v = 1;}
return getPoint(u,v);
}
/** get the triangle's size */ /** get the triangle's size */

12
navMesh/meta/NavMeshDijkstra.h
View File

@@ -11,7 +11,9 @@
namespace NM { namespace NM {
/** distance/neighbor-to-the target for each of the 3 triangle edge points */ /** distance/neighbor-to-the target for each of the 3 triangle edge points */
struct NavMeshTriangleDijkstra { class NavMeshTriangleDijkstra {
public:
/** next hop towards the pedestrian's target */ /** next hop towards the pedestrian's target */
struct ToTarget { struct ToTarget {
@@ -47,8 +49,12 @@ namespace NM {
ToTarget spFromP3; ToTarget spFromP3;
/** interpolate the distance towards the garget for the given point */ /** interpolate the distance towards the garget for the given point */
template <typename T> float getDistanceToDestination(const Point3 p) const { template <typename UserTriangleClass> float getDistanceToDestination(const Point3 p) const {
return T::interpolate(p, spFromP1.distance, spFromP2.distance, spFromP3.distance); // this one is a little bit awkward.. normally NavMeshTriangleDijkstra should extend NavMeshTriangle..
// however, this often yields issues for user-classes, extending NavMeshTriangle more than once
StaticAssert::AinheritsB<UserTriangleClass, NavMeshTriangle>();
const UserTriangleClass* userClass = static_cast<const UserTriangleClass*>(this); // must inherit NavMeshTriangle
return userClass->interpolate(p, spFromP1.distance, spFromP2.distance, spFromP3.distance);
} }
/** get the next neighbor-point/triangle for the given point */ /** get the next neighbor-point/triangle for the given point */

18
navMesh/walk/NavMeshWalkEval.h
View File

@@ -47,7 +47,7 @@ namespace NM {
/** /**
* evaluate the difference between head(start,end) and the requested heading * evaluate the difference between head(start,end) and the requested heading
*/ */
template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> { template <typename Tria> class WalkEvalHeadingStartEndVonMises : public NavMeshWalkEval<Tria> {
const double sigma_rad; const double sigma_rad;
const double kappa; const double kappa;
@@ -58,7 +58,7 @@ namespace NM {
// kappa = 1/var = 1/sigma^2 // kappa = 1/var = 1/sigma^2
// https://en.wikipedia.org/wiki/Von_Mises_distribution // https://en.wikipedia.org/wiki/Von_Mises_distribution
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) : WalkEvalHeadingStartEndVonMises(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) { sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
; ;
} }
@@ -144,10 +144,14 @@ namespace NM {
*/ */
template <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> { template <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {
const double p = 0.65; const double p;
public: public:
WalkEvalApproachesTarget(const double pApproaching = 0.65) : p(pApproaching) {
;
}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override { virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
// sanity check // sanity check
@@ -156,10 +160,12 @@ namespace NM {
const NavMeshLocation<Tria> locStart = walk.requested.start; const NavMeshLocation<Tria> locStart = walk.requested.start;
const NavMeshLocation<Tria> locEnd = walk.end; const NavMeshLocation<Tria> locEnd = walk.end;
const float distFromNew = locEnd.tria->getDistanceToDestination<Tria>(locEnd.pos); const float distFromNew = locEnd.tria-> template getDistanceToDestination<Tria>(locEnd.pos);
const float distFromOld = locStart.tria->getDistanceToDestination<Tria>(locStart.pos); const float distFromOld = locStart.tria-> template getDistanceToDestination<Tria>(locStart.pos);
return (distFromNew <= distFromOld) ? (p) : (1.0 - p); const double pRemain = 1.0 / (2+distFromNew);
return ((distFromNew <= distFromOld) ? (p) : (1.0 - p)) + std::pow(pRemain, 0.2);
} }

4
tests/navMesh/TestNavMeshDijkstra.cpp
View File

@@ -143,7 +143,8 @@ TEST(NavMeshDijkstra, path) {
// remove2.poly.points.push_back(Point2(-2,+2)); // remove2.poly.points.push_back(Point2(-2,+2));
// remove2.poly.points.push_back(Point2(-11,+2)); // remove2.poly.points.push_back(Point2(-11,+2));
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/mnt/vm/paper/diss/data/maps/map_stair1.xml"); //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/mnt/vm/paper/diss/data/maps/map_stair1.xml");
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");
NavMeshFactory<MyNMT1231902345> fac(&nm, set); NavMeshFactory<MyNMT1231902345> fac(&nm, set);
@@ -161,6 +162,7 @@ TEST(NavMeshDijkstra, path) {
NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw(); NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw();
start.tria->getDistanceToDestination<MyNMT1231902345>(start.pos); // just a compiler-test
NM::NavMeshDijkstra::stamp(nm, dst); NM::NavMeshDijkstra::stamp(nm, dst);