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

added dijkstra support to navmesh

Browse Source
This commit is contained in:
FrankE
2018-01-21 13:41:17 +01:00
parent 3c72bc814c
commit c9d02d440a
6 changed files with 536 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

288
floorplan/v2/FloorplanReader.h
View File

@@ -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 <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec;
FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
}
return vec;
}
/** parse the <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec;
FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
}
return vec;
}
/** parse a <gtpoint> tag */
static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
GroundTruthPoint* gtp = new GroundTruthPoint();
gtp->id = el->IntAttribute("id");
gtp->pos = parsePoint3(el);
return gtp;
}
/** parse a <gtpoint> tag */
static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
GroundTruthPoint* gtp = new GroundTruthPoint();
gtp->id = el->IntAttribute("id");
gtp->pos = parsePoint3(el);
return gtp;
}
/** parse the <accesspoints> tag */

16
navMesh/NavMeshDebug.h
View File

@@ -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 <typename Tria> void addDijkstra(NavMesh<Tria>& mesh) {
distances.clear();
// ensure Tria extends NavMeshTriangleDijkstra
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
@@ -125,7 +129,7 @@ namespace NM {
for (int i = 0; i < 900; ++i) {
NavMeshLocation<Tria> 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 <typename Tria> void addDijkstra(std::vector<NM::NavMeshLocation<Tria>>& 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";

2
navMesh/NavMeshTriangle.h
View File

@@ -32,7 +32,7 @@ namespace NM {
NavMeshTriangle* _neighbors[3];
int _numNeighbors;
private: // precalculated stuff
protected: // precalculated stuff
// Point2 v0;
// Point2 v1;

185
navMesh/meta/NavMeshDijkstra.h
View File

@@ -6,15 +6,105 @@
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#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 <typename Tria> 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 <typename T> 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 <typename Tria> NavMeshTriangleDijkstra::ToTarget nearestHop(const Point3 p) const {
const Tria* tria = static_cast<const Tria*>(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<Tria>(pc, static_cast<const Tria*>(spFromCenter.next));}
// if (d1 < dc && d1 < d2 && d1 < d3) {return NavMeshLocation<Tria>(p1, static_cast<const Tria*>(spFromP1.next));}
// if (d2 < dc && d2 < d1 && d2 < d3) {return NavMeshLocation<Tria>(p2, static_cast<const Tria*>(spFromP2.next));}
// if (d3 < dc && d3 < d1 && d3 < d2) {return NavMeshLocation<Tria>(p3, static_cast<const Tria*>(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 <typename Tria> std::vector<NavMeshLocation<Tria>> getPathToDestination(const Point3 p) const {
std::vector<NavMeshLocation<Tria>> path;
// NavMeshLocation<Tria> cur(p, static_cast<const Tria*>(this));
// path.push_back(cur);
// while(true) {
// //NavMeshLocation<Tria> step = prev.tria->nextHop(prev.pos);//static_cast<const NavMeshTriangleDijkstra*>(prev.tria)->next(prev.pos);
// NavMeshLocation<Tria> step = static_cast<const NavMeshTriangleDijkstra*>(prev.tria)->nextHop<Tria>(prev.pos);
// path.push_back(step);
// if (step.tria == nullptr) {break;} // reached end
// prev = step;
// }
// return path;
// starting point
NavMeshLocation<Tria> first(p, static_cast<const Tria*>(this));
path.push_back(first);
// first hop towards the destination
NavMeshTriangleDijkstra::ToTarget cur = nearestHop<Tria>(p);
// iterate all hops towards the destination
while(cur.next.tria) {
const NavMeshLocation<Tria> loc(cur.next.point(), (const Tria*)cur.next.tria);
path.push_back(loc);
cur = cur.next.hop<Tria>();
}
return path;
}
};
// // mapper
@@ -28,11 +118,13 @@ namespace NM {
class NavMeshDijkstra {
struct Node {
Point3 pt;
std::vector<Node*> 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<TemporalNode*> 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<TemporalNode>* 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 <typename Tria> static void stamp(NavMesh<Tria>& mesh, const Point3 dst) {
// ensure Tria extends NavMeshTriangleDijkstra
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
// point3 to mesh location
//NavMeshLocation<Tria> endLoc = mesh.getLocation(dst);
// build finer mesh for dijkstra
std::vector<TemporalNode*> nodes = net(mesh);
std::vector<Node*> nodes = net(mesh);
Node* end = nodes[0]; // TODO;
// point3 to mesh location
NavMeshLocation<Tria> 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<Node> dijkstra;
Dijkstra<TemporalNode> 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 <typename Tria> static std::vector<Node*> net(NavMesh<Tria>& 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 <typename Tria> static std::vector<TemporalNode*> net(NavMesh<Tria>& mesh) {
std::vector<Node*> nodes;
std::vector<TemporalNode*> 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);

29
navMesh/walk/NavMeshWalkEval.h
View File

@@ -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 <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {
const double p = 0.65;
public:
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
// sanity check
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
const NavMeshLocation<Tria> locStart = walk.requested.start;
const NavMeshLocation<Tria> locEnd = walk.end;
const float distFromNew = locEnd.tria->getDistanceToDestination<Tria>(locEnd.pos);
const float distFromOld = locStart.tria->getDistanceToDestination<Tria>(locStart.pos);
return (distFromNew <= distFromOld) ? (p) : (1.0 - p);
}
};
}
#endif // NAVMESHWALKEVAL_H

72
tests/navMesh/TestNavMeshDijkstra.cpp
View File

@@ -90,7 +90,6 @@ TEST(NavMeshDijkstra, build2) {
NavMeshFactory<MyNMT1231902345> fac(&nm, set);
fac.build(map);
@@ -107,5 +106,76 @@ TEST(NavMeshDijkstra, build2) {
}
TEST(NavMeshDijkstra, path) {
NavMeshSettings set;
NavMesh<MyNMT1231902345> 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<MyNMT1231902345> fac(&nm, set);
fac.build(map);
NavMeshDebug dbg;
dbg.addMesh(nm);
dbg.draw();
NM::NavMeshRandom<MyNMT1231902345> rnd = nm.getRandom();
const Point3 dst = rnd.draw().pos;
for (int i = 0; i < 1000; ++i) {
NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw();
NM::NavMeshDijkstra::stamp(nm, dst);
//NM::NavMeshLocation<MyNMT1231902345> start = start;//nm.getLocation(Point3(0,-6,0));
std::vector<NM::NavMeshLocation<MyNMT1231902345>> path = start.tria->getPathToDestination<MyNMT1231902345>(start.pos);
std::cout << path.size() << std::endl;
dbg.addDijkstra(nm);
dbg.addDijkstra(path);
dbg.draw();
sleep(1);
}
}
#endif