From c9d02d440a19d01d6c84e8759740257bf4883b6d Mon Sep 17 00:00:00 2001 From: frank Date: Sun, 21 Jan 2018 13:41:17 +0100 Subject: [PATCH] added dijkstra support to navmesh --- floorplan/v2/FloorplanReader.h | 288 ++++++++++++++++++++++++-- navMesh/NavMeshDebug.h | 16 +- navMesh/NavMeshTriangle.h | 2 +- navMesh/meta/NavMeshDijkstra.h | 185 ++++++++++++++--- navMesh/walk/NavMeshWalkEval.h | 29 +++ tests/navMesh/TestNavMeshDijkstra.cpp | 72 ++++++- 6 files changed, 536 insertions(+), 56 deletions(-) diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 462b78e..b264e63 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -36,9 +36,253 @@ namespace Floorplan { if (res != tinyxml2::XMLError::XML_SUCCESS) { throw Exception( std::string() + "error while loading XML " + file + "\n" + - ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + - ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) - ); + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ); } IndoorMap* map = parse(doc); return map; @@ -53,8 +297,8 @@ namespace Floorplan { if (res != tinyxml2::XMLError::XML_SUCCESS) { throw Exception( std::string() + "error while parsing XML\n" + - ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + - ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) + ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" + + ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")) ); } IndoorMap* map = parse(doc); @@ -63,7 +307,7 @@ namespace Floorplan { private: - #define FOREACH_NODE(out, in) for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() ) + #define FOREACH_NODE(out, in) for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() ) static void assertNode(const std::string& node, const XMLElem* el) { std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'"; @@ -138,7 +382,7 @@ namespace Floorplan { if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);} if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);} if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);} - if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} + if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);} } return floor; } @@ -226,22 +470,22 @@ namespace Floorplan { } - /** parse the tag */ - static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { - std::vector vec; - FOREACH_NODE(n, el) { - if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } - } - return vec; - } + /** parse the tag */ + static std::vector parseFloorGroundTruthPoints(const XMLElem* el) { + std::vector vec; + FOREACH_NODE(n, el) { + if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); } + } + return vec; + } - /** parse a tag */ - static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { - GroundTruthPoint* gtp = new GroundTruthPoint(); - gtp->id = el->IntAttribute("id"); - gtp->pos = parsePoint3(el); - return gtp; - } + /** parse a tag */ + static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) { + GroundTruthPoint* gtp = new GroundTruthPoint(); + gtp->id = el->IntAttribute("id"); + gtp->pos = parsePoint3(el); + return gtp; + } /** parse the tag */ diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h index a5b7cdb..a2dbbe3 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -29,6 +29,7 @@ namespace NM { K::GnuplotSplotElementColorPoints particles; K::GnuplotSplotElementLines pathEstimated; K::GnuplotSplotElementColorPoints distances; + K::GnuplotSplotElementLines shortestPath; private: @@ -50,6 +51,7 @@ namespace NM { 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(&shortestPath); shortestPath.getStroke().setWidth(3); } void draw() { @@ -118,6 +120,8 @@ namespace NM { template void addDijkstra(NavMesh& mesh) { + distances.clear(); + // ensure Tria extends NavMeshTriangleDijkstra StaticAssert::AinheritsB(); @@ -125,7 +129,7 @@ namespace NM { for (int i = 0; i < 900; ++i) { NavMeshLocation loc = rnd.draw(); - float v = loc.tria->interpolate(loc.pos, loc.tria->distAtP1, loc.tria->distAtP2, loc.tria->distAtP3); + 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); } @@ -144,6 +148,16 @@ namespace NM { } + template void addDijkstra(std::vector>& path) { + shortestPath.clear(); + for (auto& e : path) { + K::GnuplotPoint3 gp(e.pos.x, e.pos.y, e.pos.z); + shortestPath.add(gp); + } + } + + + void setGT(const Point3 pt) { gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n"; diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index eb21bd1..182b3b5 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -32,7 +32,7 @@ namespace NM { NavMeshTriangle* _neighbors[3]; int _numNeighbors; - private: // precalculated stuff + protected: // precalculated stuff // Point2 v0; // Point2 v1; diff --git a/navMesh/meta/NavMeshDijkstra.h b/navMesh/meta/NavMeshDijkstra.h index bc0c101..335f771 100644 --- a/navMesh/meta/NavMeshDijkstra.h +++ b/navMesh/meta/NavMeshDijkstra.h @@ -6,15 +6,105 @@ #include #include #include +#include "../NavMeshDebug.h" namespace NM { - /** distance 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 { - float distAtCenter; - float distAtP1; - float distAtP2; - float distAtP3; + + /** next hop towards the pedestrian's target */ + struct ToTarget { + struct NextTarget { + const NavMeshTriangle* tria = nullptr; + int pointIndex; + Point3 point() const { + switch(pointIndex) { + case 0: return tria->getP1(); + case 1: return tria->getP2(); + case 2: return tria->getP3(); + case 3: return tria->getCenter(); + default: throw Exception("invalid point index"); + } + } + template ToTarget hop() const { + const Tria* t = (const Tria*)tria; + switch(pointIndex) { + case 0: return t->spFromP1; + case 1: return t->spFromP2; + case 2: return t->spFromP3; + case 3: return t->spFromCenter; + default: throw Exception("invalid point index"); + } + } + } next; + float distance = 0; + }; + + ToTarget spFromCenter; + ToTarget spFromP1; + ToTarget spFromP2; + 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); + } + + /** get the next neighbor-point/triangle for the given point */ + template NavMeshTriangleDijkstra::ToTarget nearestHop(const Point3 p) const { + const Tria* tria = static_cast(this); + const Point3 pc = tria->getCenter(); + const Point3 p1 = tria->getP1(); + const Point3 p2 = tria->getP2(); + const Point3 p3 = tria->getP3(); + const float dc = p.getDistance(pc); + const float d1 = p.getDistance(p1); + const float d2 = p.getDistance(p2); + const float d3 = p.getDistance(p3); +// if (dc < d1 && dc < d2 && dc < d3) {return NavMeshLocation(pc, static_cast(spFromCenter.next));} +// if (d1 < dc && d1 < d2 && d1 < d3) {return NavMeshLocation(p1, static_cast(spFromP1.next));} +// if (d2 < dc && d2 < d1 && d2 < d3) {return NavMeshLocation(p2, static_cast(spFromP2.next));} +// if (d3 < dc && d3 < d1 && d3 < d2) {return NavMeshLocation(p3, static_cast(spFromP3.next));} + if (dc < d1 && dc < d2 && dc < d3) {return spFromCenter;} + if (d1 < dc && d1 < d2 && d1 < d3) {return spFromP1;} + if (d2 < dc && d2 < d1 && d2 < d3) {return spFromP2;} + if (d3 < dc && d3 < d1 && d3 < d2) {return spFromP3;} + throw Exception("invalid code-path detected"); + } + + /** get the complete path from p towards to pedestrian's destination */ + template std::vector> getPathToDestination(const Point3 p) const { + std::vector> path; +// NavMeshLocation cur(p, static_cast(this)); +// path.push_back(cur); +// while(true) { +// //NavMeshLocation step = prev.tria->nextHop(prev.pos);//static_cast(prev.tria)->next(prev.pos); +// NavMeshLocation step = static_cast(prev.tria)->nextHop(prev.pos); + +// path.push_back(step); +// if (step.tria == nullptr) {break;} // reached end +// prev = step; +// } +// return path; + + // starting point + NavMeshLocation first(p, static_cast(this)); + path.push_back(first); + + // first hop towards the destination + NavMeshTriangleDijkstra::ToTarget cur = nearestHop(p); + + // iterate all hops towards the destination + while(cur.next.tria) { + const NavMeshLocation loc(cur.next.point(), (const Tria*)cur.next.tria); + path.push_back(loc); + cur = cur.next.hop(); + } + + return path; + } + }; // // mapper @@ -28,11 +118,13 @@ namespace NM { class NavMeshDijkstra { - struct Node { - Point3 pt; - std::vector neighbors; - Node(const Point3 pt) : pt(pt) {;} - bool operator == (const Node& o ) {return o.pt == pt;} + struct TemporalNode { + Point3 pt; // a point within the map + const NavMeshTriangle* triangle; // the triangle the point belongs to + int pointIndex; // [0-2] (3 edge points) or 3 [center] + std::vector neighbors; + TemporalNode(const Point3 pt, const NavMeshTriangle* triangle, int pointIndex) : pt(pt), triangle(triangle), pointIndex(pointIndex) {;} + bool operator == (const TemporalNode& o ) {return o.pt == pt;} bool operator == (const Point3& pos ) {return pt == pos;} operator std::string() const {return asString();} std::string asString() const { @@ -43,33 +135,46 @@ namespace NM { struct NodeComp { Point3 pos; //bool operator () (const Node* n) {return n->pt == pos;} - bool operator () (const Node* n) {return n->pt.getDistance(pos) < 0.0001;} + bool operator () (const TemporalNode* n) {return n->pt.getDistance(pos) < 0.0001;} NodeComp(const Point3 pos) : pos(pos) {;} }; struct NodeAccess { - int getNumNeighbors(const Node& n) const {return n.neighbors.size();} - const Node* getNeighbor(const Node& n, const int idx) const {return n.neighbors[idx];} - float getWeightBetween(const Node& n1, const Node& n2) const {return n1.pt.getDistance(n2.pt);} + int getNumNeighbors(const TemporalNode& n) const {return n.neighbors.size();} + const TemporalNode* getNeighbor(const TemporalNode& n, const int idx) const {return n.neighbors[idx];} + float getWeightBetween(const TemporalNode& n1, const TemporalNode& n2) const {return n1.pt.getDistance(n2.pt);} }; public: + /** attach distance/triangle-to-target to the ToTarget struct */ + static void set(NavMeshTriangleDijkstra::ToTarget& t, const DijkstraNode* n) { + t.distance = n->cumWeight; + t.next.tria = (n->previous) ? (n->previous->element->triangle) : (nullptr); + t.next.pointIndex = (n->previous) ? (n->previous->element->pointIndex) : (-1); + } + template static void stamp(NavMesh& mesh, const Point3 dst) { // ensure Tria extends NavMeshTriangleDijkstra StaticAssert::AinheritsB(); - // point3 to mesh location - //NavMeshLocation endLoc = mesh.getLocation(dst); + // build finer mesh for dijkstra + std::vector nodes = net(mesh); - std::vector nodes = net(mesh); - Node* end = nodes[0]; // TODO; + // point3 to mesh location + NavMeshLocation endLoc = mesh.getLocation(dst); + auto it = std::find_if(nodes.begin(), nodes.end(), NodeComp(endLoc.tria->getCenter())); + if (it == nodes.end()) {throw Exception("end node not found");} + TemporalNode* end = *it; + + +// Node* end = nodes[0]; // TODO; NodeAccess acc; // dijkstra - Dijkstra dijkstra; + Dijkstra dijkstra; dijkstra.build(end, acc); for (Tria* t : mesh) { @@ -80,43 +185,61 @@ namespace NM { auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3())); auto* dnCenter = dijkstra.getNode(*itCenter); - if (dnCenter != nullptr) {t->distAtCenter = dnCenter->cumWeight;} + if (dnCenter != nullptr) {set(t->spFromCenter, dnCenter);} auto* dn1 = dijkstra.getNode(*it1); - if (dn1 != nullptr) {t->distAtP1 = dn1->cumWeight;} + if (dn1 != nullptr) {set(t->spFromP1, dn1);} auto* dn2 = dijkstra.getNode(*it2); - if (dn2 != nullptr) {t->distAtP2 = dn2->cumWeight;} + if (dn2 != nullptr) {set(t->spFromP2, dn2);} auto* dn3 = dijkstra.getNode(*it3); - if (dn3 != nullptr) {t->distAtP3 = dn3->cumWeight;} + if (dn3 != nullptr) {set(t->spFromP3, dn3);} } + + + NavMeshDebug dbg; + dbg.addMesh(mesh); + dbg.addDijkstra(mesh); + dbg.draw(); + + + int zzz = 0; (void) zzz; + } - template static std::vector net(NavMesh& mesh) { + /** + * a normal navigation mesh only connects adjacent triangles (can be thought of as "from center to center") + * however, later on, we need a distance estimation for any point within the triangle. + * we thus need dijkstra to estimate the distance for every edge of the triangle (to allow for barycentric interpolation) + * we thus build a temporal graph which contains all triangle centers and edge-points. + * Note: many triangles share the same edge-points! + * likewise, all possible connections are drawn. + */ + template static std::vector net(NavMesh& mesh) { - std::vector nodes; + std::vector nodes; // 1) add all triangle nodes (center, p1, p2, p3) for (const Tria* t : mesh) { auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter())); - if (itCenter == nodes.end()) {nodes.push_back(new Node(t->getCenter()));} + if (itCenter == nodes.end()) {nodes.push_back(new TemporalNode(t->getCenter(), t, 3));} //Node n1(t->getP1()); auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1())); - if (it1 == nodes.end()) {nodes.push_back(new Node(t->getP1()));} + if (it1 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP1(), t, 0));} //Node n2(t->getP2()); auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2())); - if (it2 == nodes.end()) {nodes.push_back(new Node(t->getP2()));} + if (it2 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP2(), t, 1));} //Node n3(t->getP1()); auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3())); - if (it3 == nodes.end()) {nodes.push_back(new Node(t->getP3()));} + if (it3 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP3(), t, 2));} } @@ -159,8 +282,8 @@ namespace NM { K::GnuplotSplot plot; K::GnuplotSplotElementLines lines; plot.add(&lines); - for (const Node* n1 : nodes) { - for (const Node* n2 : n1->neighbors) { + for (const TemporalNode* n1 : nodes) { + for (const TemporalNode* n2 : n1->neighbors) { const K::GnuplotPoint3 gp1(n1->pt.x, n1->pt.y, n1->pt.z); const K::GnuplotPoint3 gp2(n2->pt.x, n2->pt.y, n2->pt.z); lines.addSegment(gp1, gp2); diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index bae5e10..ca50610 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -5,6 +5,9 @@ #include "../NavMeshLocation.h" #include "../../math/Distributions.h" #include "../../misc/PerfCheck.h" +#include "../../Assertions.h" +#include "../meta/NavMeshDijkstra.h" + namespace NM { @@ -136,6 +139,32 @@ namespace NM { }; + /** + * higher probability for potints that approach the target location + */ + template class WalkEvalApproachesTarget : public NavMeshWalkEval { + + const double p = 0.65; + + public: + + virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + + // sanity check + StaticAssert::AinheritsB(); + + 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); + + return (distFromNew <= distFromOld) ? (p) : (1.0 - p); + + } + + }; + } #endif // NAVMESHWALKEVAL_H diff --git a/tests/navMesh/TestNavMeshDijkstra.cpp b/tests/navMesh/TestNavMeshDijkstra.cpp index 2c8243b..934d3ee 100644 --- a/tests/navMesh/TestNavMeshDijkstra.cpp +++ b/tests/navMesh/TestNavMeshDijkstra.cpp @@ -90,7 +90,6 @@ TEST(NavMeshDijkstra, build2) { - NavMeshFactory fac(&nm, set); fac.build(map); @@ -107,5 +106,76 @@ TEST(NavMeshDijkstra, build2) { } +TEST(NavMeshDijkstra, path) { + + NavMeshSettings set; + NavMesh nm; + +// Floorplan::IndoorMap map; +// Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3; +// Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline); + +// // circle (many triangles) +// int i = 0; +// for (float f = 0; f < M_PI*2; f += 0.8) { +// const float x = std::cos(f) * 10; +// const float y = std::sin(f) * 10; +// outline.poly.points.push_back(Point2(x,y)); +// ++i; +// } + +// outline.outdoor = false; +// outline.method = Floorplan::OutlineMethod::ADD; + +// Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove); +// remove.outdoor = false; +// remove.method = Floorplan::OutlineMethod::REMOVE; +// remove.poly.points.push_back(Point2(+4,-3)); +// remove.poly.points.push_back(Point2(+11,-3)); +// remove.poly.points.push_back(Point2(+11,+1)); +// remove.poly.points.push_back(Point2(+4,+1)); + +// Floorplan::FloorOutlinePolygon remove2; floor.outline.push_back(&remove2); +// remove2.outdoor = false; +// remove2.method = Floorplan::OutlineMethod::REMOVE; +// remove2.poly.points.push_back(Point2(-11,-2)); +// remove2.poly.points.push_back(Point2(-2,-2)); +// 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"); + + + NavMeshFactory fac(&nm, set); + fac.build(map); + + + NavMeshDebug dbg; + dbg.addMesh(nm); + dbg.draw(); + + NM::NavMeshRandom rnd = nm.getRandom(); + const Point3 dst = rnd.draw().pos; + + for (int i = 0; i < 1000; ++i) { + + + NM::NavMeshLocation start = rnd.draw(); + + NM::NavMeshDijkstra::stamp(nm, dst); + + //NM::NavMeshLocation start = start;//nm.getLocation(Point3(0,-6,0)); + std::vector> path = start.tria->getPathToDestination(start.pos); + std::cout << path.size() << std::endl; + + dbg.addDijkstra(nm); + dbg.addDijkstra(path); + dbg.draw(); + + sleep(1); + + } + +} #endif