From 1a1f249e9bd06e276e4630852c153099470b13e9 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 24 Jan 2018 11:27:11 +0100 Subject: [PATCH] some refactorings/fixes worked on nav-mesh stuff new tests --- floorplan/v2/FloorplanReader.h | 247 +------------------------- math/random/RandomGenerator.h | 6 +- navMesh/NavMesh.h | 18 +- navMesh/NavMeshDebug.h | 6 +- navMesh/NavMeshFactory.h | 2 +- navMesh/NavMeshRandom.h | 2 + navMesh/NavMeshTriangle.h | 19 ++ navMesh/meta/NavMeshDijkstra.h | 12 +- navMesh/walk/NavMeshWalkEval.h | 18 +- tests/navMesh/TestNavMeshDijkstra.cpp | 4 +- 10 files changed, 70 insertions(+), 264 deletions(-) diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index b264e63..5d14315 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -37,252 +37,7 @@ namespace Floorplan { throw Exception( std::string() + "error while loading XML " + file + "\n" + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + - ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ); + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))); } IndoorMap* map = parse(doc); return map; diff --git a/math/random/RandomGenerator.h b/math/random/RandomGenerator.h index 4c41f36..c0dcd82 100644 --- a/math/random/RandomGenerator.h +++ b/math/random/RandomGenerator.h @@ -1,5 +1,5 @@ -#ifndef RANDOM_Random::RandomGenerator_H -#define RANDOM_Random::RandomGenerator_H +#ifndef RANDOM_Random_RandomGenerator_H +#define RANDOM_Random_RandomGenerator_H #include #include @@ -26,4 +26,4 @@ namespace Random { } -#endif // K_MATH_RANDOM_Random::RandomGenerator_H +#endif // RANDOM_Random_RandomGenerator_H diff --git a/navMesh/NavMesh.h b/navMesh/NavMesh.h index 6077049..10f1c3f 100644 --- a/navMesh/NavMesh.h +++ b/navMesh/NavMesh.h @@ -45,7 +45,7 @@ namespace NM { } /** get the triangle this point belongs to (if any) */ - NavMeshLocation getLocation(const Point3 pos) { + NavMeshLocation getLocation(const Point3 pos) const { for (const Tria* tria : triangles) { if (tria->contains(pos)) { return NavMeshLocation(pos, tria); @@ -54,6 +54,19 @@ namespace NM { throw Exception("location not found within NavMesh: " + pos.asString()); } + + /** get the triangle/point on the mesh that is nearest to the given location */ + NavMeshLocation 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(rPos, best); + } + /** connect both triangles */ void connectBiDir(int idx1, int idx2) { connectUniDir(idx1,idx2); @@ -86,7 +99,8 @@ namespace NM { /** ---------------- MISC ---------------- */ - NavMeshRandom getRandom() { + /** get a random-generator for several mesh-actions */ + NavMeshRandom getRandom() const { return NavMeshRandom(triangles); } diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h index a2dbbe3..b6d957c 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -50,7 +50,7 @@ namespace NM { plot.add(&border); 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(&distances); distances.setPointSize(2); distances.setPointType(7); + plot.add(&distances); distances.setPointSize(0.75); distances.setPointType(7); plot.add(&shortestPath); shortestPath.getStroke().setWidth(3); } @@ -72,6 +72,8 @@ namespace NM { } plot.getAxisCB().setRange(min, max + 0.000001); } + + template void addMesh(NavMesh& nm) { K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); @@ -127,7 +129,7 @@ namespace NM { NavMeshRandom rnd = mesh.getRandom(); - for (int i = 0; i < 900; ++i) { + for (int i = 0; i < 5000; ++i) { NavMeshLocation loc = rnd.draw(); 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); diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 3a03ee3..b87056a 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -12,7 +12,7 @@ #include "../lib/gpc/gpc.cpp.h" #include "../lib/Recast/Recast.h" - +#include // memset namespace NM { diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h index 391ed5c..01da8c9 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -36,6 +36,8 @@ namespace NM { /** ctor (const/non-const using T) */ template NavMeshRandom(const std::vector& srcTriangles) : lst(nextSeed()), gen(nextSeed()) { + Assert::isFalse(srcTriangles.empty(), "no triangles given. mesh is empty"); + // 1st = almost always the same number?! gen(); gen(); diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index 182b3b5..f7c98fb 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -185,6 +185,7 @@ namespace NM { w = 1-u-v; } + /** barycentric interpolation at Point p for val1@p1, val2@p2, val3@p3 */ template T interpolate(const Point3 p, const T val1, const T val2, const T val3) const { float u, v, w; @@ -194,6 +195,9 @@ namespace NM { } + + + /** does the triangle contain the given 3D point? */ bool contains(const Point3 p) const { 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 */ diff --git a/navMesh/meta/NavMeshDijkstra.h b/navMesh/meta/NavMeshDijkstra.h index 335f771..ca5ae24 100644 --- a/navMesh/meta/NavMeshDijkstra.h +++ b/navMesh/meta/NavMeshDijkstra.h @@ -11,7 +11,9 @@ namespace NM { /** 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 */ struct ToTarget { @@ -47,8 +49,12 @@ namespace NM { ToTarget spFromP3; /** interpolate the distance towards the garget for the given point */ - template float getDistanceToDestination(const Point3 p) const { - return T::interpolate(p, spFromP1.distance, spFromP2.distance, spFromP3.distance); + template float getDistanceToDestination(const Point3 p) const { + // 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(); + const UserTriangleClass* userClass = static_cast(this); // must inherit NavMeshTriangle + return userClass->interpolate(p, spFromP1.distance, spFromP2.distance, spFromP3.distance); } /** get the next neighbor-point/triangle for the given point */ diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index ca50610..cc3068e 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -47,7 +47,7 @@ namespace NM { /** * evaluate the difference between head(start,end) and the requested heading */ - template class WalkEvalHeadingStartEnd : public NavMeshWalkEval { + template class WalkEvalHeadingStartEndVonMises : public NavMeshWalkEval { const double sigma_rad; const double kappa; @@ -58,7 +58,7 @@ namespace NM { // kappa = 1/var = 1/sigma^2 // 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()) { ; } @@ -144,10 +144,14 @@ namespace NM { */ template class WalkEvalApproachesTarget : public NavMeshWalkEval { - const double p = 0.65; + const double p; public: + WalkEvalApproachesTarget(const double pApproaching = 0.65) : p(pApproaching) { + ; + } + virtual double getProbability(const NavMeshPotentialWalk& walk) const override { // sanity check @@ -156,10 +160,12 @@ namespace NM { const NavMeshLocation locStart = walk.requested.start; const NavMeshLocation locEnd = walk.end; - const float distFromNew = locEnd.tria->getDistanceToDestination(locEnd.pos); - const float distFromOld = locStart.tria->getDistanceToDestination(locStart.pos); + const float distFromNew = locEnd.tria-> template getDistanceToDestination(locEnd.pos); + const float distFromOld = locStart.tria-> template getDistanceToDestination(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); } diff --git a/tests/navMesh/TestNavMeshDijkstra.cpp b/tests/navMesh/TestNavMeshDijkstra.cpp index 934d3ee..625841e 100644 --- a/tests/navMesh/TestNavMeshDijkstra.cpp +++ b/tests/navMesh/TestNavMeshDijkstra.cpp @@ -143,7 +143,8 @@ TEST(NavMeshDijkstra, path) { // remove2.poly.points.push_back(Point2(-2,+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 fac(&nm, set); @@ -161,6 +162,7 @@ TEST(NavMeshDijkstra, path) { NM::NavMeshLocation start = rnd.draw(); + start.tria->getDistanceToDestination(start.pos); // just a compiler-test NM::NavMeshDijkstra::stamp(nm, dst);