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

worked on navMesh stuff

Browse Source 
- creation
- walking
- helper
This commit is contained in:
k-a-z-u
2018-01-10 16:57:19 +01:00
parent 3fc9688825
commit fee6cd3496
15 changed files with 1282 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
navMesh/NavMesh.h
View File

@@ -9,19 +9,28 @@
#include "NavMeshRandom.h" #include "NavMeshRandom.h"
#include "NavMeshLocation.h" #include "NavMeshLocation.h"
template <typename Tria> class NavMesh { namespace NM {
template <typename Tria> class NavMesh {
/** all triangles within the mesh */ /** all triangles within the mesh */
std::vector<Tria*> triangles; std::vector<Tria*> triangles;
BBox3 bbox; BBox3 bbox;
public: public:
/** ctor */
NavMesh() { NavMesh() {
} }
/** dtor */
~NavMesh() {
for (const Tria* t : triangles) {delete t;}
triangles.clear();
}
/** the overall bounding-box */ /** the overall bounding-box */
const BBox3 getBBox() const { const BBox3 getBBox() const {
return bbox; return bbox;
@@ -35,13 +44,14 @@ public:
bbox.add(p3); bbox.add(p3);
} }
/** get the triangle this point belongs to (if any) */
NavMeshLocation<Tria> getLocation(const Point3 pos) { NavMeshLocation<Tria> getLocation(const Point3 pos) {
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);
} }
} }
throw Exception("location not found"); throw Exception("location not found within NavMesh: " + pos.asString());
} }
/** connect both triangles */ /** connect both triangles */
@@ -52,8 +62,8 @@ public:
/** connect both triangles */ /** connect both triangles */
void connectUniDir(int idxFrom, int idxTo) { void connectUniDir(int idxFrom, int idxTo) {
NavMeshTriangle* tria = triangles[idxFrom]; Tria* tria = triangles[idxFrom];
tria->_neighbors[tria->_numNeighbors] = triangles[idxTo]; tria->addNeighbor(triangles[idxTo]);
} }
/** allows for-each iteration over all included triangles */ /** allows for-each iteration over all included triangles */
@@ -76,34 +86,36 @@ public:
/** ---------------- MISC ---------------- */ /** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandomizer() { NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(triangles); return NavMeshRandom<Tria>(triangles);
} }
// /** ---------------- NEIGHBORS ---------------- */ // /** ---------------- NEIGHBORS ---------------- */
// /** get the number of neighbors for the given element */ // /** get the number of neighbors for the given element */
// int getNumNeighbors(const size_t idx) const { // int getNumNeighbors(const size_t idx) const {
// return getNumNeighbors(triangles[idx]); // return getNumNeighbors(triangles[idx]);
// } // }
// /** get the number of neighbors for the given element */ // /** get the number of neighbors for the given element */
// int getNumNeighbors(const Tria& e) const { // int getNumNeighbors(const Tria& e) const {
// return e._numNeighbors; // return e._numNeighbors;
// } // }
// /** get the n-th neighbor for the given node */ // /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const { // Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
// const Tria& node = triangles[nodeIdx]; // const Tria& node = triangles[nodeIdx];
// return getNeighbor(node, nth); // return getNeighbor(node, nth);
// } // }
// /** get the n-th neighbor for the given node */ // /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const Tria& tria, const size_t nth) const { // Tria& getNeighbor(const Tria& tria, const size_t nth) const {
// const Tria& neighbor = triangles[tria._neighbors[nth]]; // const Tria& neighbor = triangles[tria._neighbors[nth]];
// return (Tria&) neighbor; // return (Tria&) neighbor;
// } // }
}; };
}
#endif #endif

47
navMesh/NavMeshDebug.h
View File

@@ -9,9 +9,14 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h> #include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h> #include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
class NavMeshDebug { namespace NM {
public: /**
* debug plot NavMeshes
*/
class NavMeshDebug {
public:
template <typename Tria> static void show(NavMesh<Tria>& nm) { template <typename Tria> static void show(NavMesh<Tria>& nm) {
@@ -34,30 +39,36 @@ public:
points.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z)); points.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
points.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z)); points.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0)); // lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0)); // lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0));
// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z)); // lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
//stairs in eigene group? vlt gehen dann auch die dellen weg? //stairs in eigene group? vlt gehen dann auch die dellen weg?
for (const Tria& tria : nm) { for (const Tria* tria : nm) {
uint8_t type = tria.type; const uint8_t type = tria->getType();
if (type < 0 || type > 2) { if (type < 0 || type > 2) {
throw std::runtime_error("out of type-bounds"); throw std::runtime_error("out of type-bounds");
} }
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke); K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
pol->add(K::GnuplotCoordinate3(tria.p1.x, tria.p1.y, tria.p1.z, K::GnuplotCoordinateSystem::FIRST)); pol->add(K::GnuplotCoordinate3(tria->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria.p2.x, tria.p2.y, tria.p2.z, K::GnuplotCoordinateSystem::FIRST)); pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria.p3.x, tria.p3.y, tria.p3.z, K::GnuplotCoordinateSystem::FIRST)); pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST));
pol->close(); pol->close();
pol->setZIndex(tria.p3.z); pol->setZIndex(tria->getP3().z);
plot.getObjects().add(pol); plot.getObjects().add(pol);
for (int i = 0; i < nm.getNumNeighbors(tria); ++i) { //for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
const Tria& o = nm.getNeighbor(tria, i); // const Tria* o = nm.getNeighbor(tria, i);
const Point3 p1 = tria.getCenter(); // const Point3 p1 = tria->getCenter();
const Point3 p2 = o.getCenter(); // const Point3 p2 = o.getCenter();
//lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1)); // //lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
//}
for (const NavMeshTriangle* o : *tria) {
const Point3 p1 = tria->getCenter();
const Point3 p2 = o->getCenter();
// lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
} }
} }
@@ -70,6 +81,8 @@ public:
} }
}; };
}
#endif // NAVMESHDEBUG_H #endif // NAVMESHDEBUG_H

250
navMesh/NavMeshFactory.h
View File

@@ -5,26 +5,135 @@
#include "../floorplan/v2/FloorplanHelper.h" #include "../floorplan/v2/FloorplanHelper.h"
#include "NavMesh.h" #include "NavMesh.h"
#include "NavMeshPoly.h"
#include "NavMeshTriangle.h" #include "NavMeshTriangle.h"
#include "../lib/gpc/gpc.cpp.h"
#include "../lib/Recast/Recast.h" #include "../lib/Recast/Recast.h"
enum SamplePartitionType { namespace NM {
class NavMeshPoly {
struct GPCPolygon : gpc_polygon {
GPCPolygon() {
num_contours = 0;
contour = nullptr;
hole = nullptr;
}
~GPCPolygon() {
if (contour) {
gpc_free_polygon(this);
//free(contour->vertex); contour->vertex = nullptr;
}
free(contour); contour = nullptr;
free(hole); hole = nullptr;
}
GPCPolygon& operator = (const GPCPolygon& o) = delete;
GPCPolygon& operator = (GPCPolygon& o) {
this->contour = o.contour;
this->hole = o.hole;
this->num_contours = o.num_contours;
o.contour = nullptr;
o.hole = nullptr;
return *this;
}
};
private:
GPCPolygon state;
float z;
public:
NavMeshPoly(float z) : z(z) {
;
}
void add(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
}
std::vector<std::vector<Point3>> get() {
gpc_tristrip res;
res.num_strips = 0;
res.strip = nullptr;
//res.strip = (gpc_vertex_list*) malloc(1024);
gpc_polygon_to_tristrip(&state, &res);
std::vector<std::vector<Point3>> trias;
for (int i = 0; i < res.num_strips; ++i) {
gpc_vertex_list lst = res.strip[i];
for (int j = 2; j < lst.num_vertices; ++j) {
std::vector<Point3> tria;
gpc_vertex& v1 = lst.vertex[j-2];
gpc_vertex& v2 = lst.vertex[j-1];
gpc_vertex& v3 = lst.vertex[j];
tria.push_back(Point3(v1.x, v1.y, z));
tria.push_back(Point3(v2.x, v2.y, z));
tria.push_back(Point3(v3.x, v3.y, z));
trias.push_back(tria);
}
}
gpc_free_tristrip(&res);
return std::move(trias);
}
private:
GPCPolygon toGPC(Floorplan::Polygon2 poly) {
std::vector<gpc_vertex> verts;
for (Point2 p2 : poly.points) {
gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
verts.push_back(vert);
}
GPCPolygon gpol;
gpc_vertex_list list;
list.num_vertices = verts.size();
list.vertex = verts.data();
gpc_add_contour(&gpol, &list, 0);
return gpol;
}
};
enum SamplePartitionType {
SAMPLE_PARTITION_WATERSHED, SAMPLE_PARTITION_WATERSHED,
SAMPLE_PARTITION_MONOTONE, SAMPLE_PARTITION_MONOTONE,
SAMPLE_PARTITION_LAYERS, SAMPLE_PARTITION_LAYERS,
}; };
struct TriangleIn { struct TriangleIn {
Point3 p1; Point3 p1;
Point3 p2; Point3 p2;
Point3 p3; Point3 p3;
uint8_t type; uint8_t type;
TriangleIn(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : p1(p1), p2(p2), p3(p3), type(type) {;} TriangleIn(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : p1(p1), p2(p2), p3(p3), type(type) {;}
}; };
struct TriangleOut { struct TriangleOut {
Point3 p1; Point3 p1;
Point3 p2; Point3 p2;
@@ -39,17 +148,19 @@ struct TriangleOut {
return (p1+p2+p3) / 3; return (p1+p2+p3) / 3;
} }
}; };
template <typename Tria> class NavMeshFactory { template <typename Tria> class NavMeshFactory {
private: private:
float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected
NavMesh<Tria>* dst = nullptr; NavMesh<Tria>* dst = nullptr;
std::vector<TriangleIn> triangles; std::vector<TriangleIn> triangles;
public: public:
NavMeshFactory(NavMesh<Tria>* dst) : dst(dst) { NavMeshFactory(NavMesh<Tria>* dst) : dst(dst) {
@@ -63,7 +174,12 @@ public:
fire(bbox); fire(bbox);
} }
private: /** get the smallest obstacle size that can be detected */
float getMaxQuality_m() const {
return maxQuality_m;
}
private:
/** add one floor */ /** add one floor */
void add(const Floorplan::Floor* floor) { void add(const Floorplan::Floor* floor) {
@@ -166,20 +282,20 @@ private:
rcPolyMeshDetail* m_dmesh; rcPolyMeshDetail* m_dmesh;
rcContext* m_ctx = new rcContext(); rcContext* m_ctx = new rcContext();
float m_cellSize = 0.1f; //0.3f; // needed for 20cm walls to work! float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m
float m_cellHeight = 0.1f; //0.2f; float m_cellHeight = maxQuality_m/2.0f; //0.2f;
float m_agentHeight = 2.0f; float m_agentHeight = 2.0f;
float m_agentRadius = 0.1f;//0.6f; float m_agentRadius = 0.2f;//0.6f;
float m_agentMaxClimb = 0.5f; // 0.9f; float m_agentMaxClimb = maxQuality_m; // 0.9f; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail!
float m_agentMaxSlope = 45.0f; float m_agentMaxSlope = 45.0f; // elevator???
float m_regionMinSize = 2;//8; float m_regionMinSize = 2;//8;
float m_regionMergeSize = 20; float m_regionMergeSize = 20;
float m_edgeMaxLen = 10.0f; // maximal size for one triangle. too high = too many samples when walking! float m_edgeMaxLen = 10.0f; // maximal size for one triangle. too high = too many samples when walking!
float m_edgeMaxError = 1.0f; //1.3f; float m_edgeMaxError = 1.1f; //1.3f; // higher values allow joining some small triangles
float m_vertsPerPoly = 3;//6.0f; float m_vertsPerPoly = 3;//6.0f;
float m_detailSampleDist = 6.0f; float m_detailSampleDist = 6.0f;
float m_detailSampleMaxError = 1.0f;//1.0f; float m_detailSampleMaxError = 1.0f;//1.0f;
int m_partitionType = SAMPLE_PARTITION_WATERSHED; int m_partitionType = SAMPLE_PARTITION_WATERSHED; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS
// Init build configuration from GUI // Init build configuration from GUI
@@ -238,12 +354,12 @@ private:
// Allocate array that can hold triangle area types. // Allocate array that can hold triangle area types.
// If you have multiple meshes you need to process, allocate // If you have multiple meshes you need to process, allocate
// and array which can hold the max number of triangles you need to process. // and array which can hold the max number of triangles you need to process.
// m_triareas = new unsigned char[ntris]; // m_triareas = new unsigned char[ntris];
// if (!m_triareas) // if (!m_triareas)
// { // {
// m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris); // m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris);
// return false; // return false;
// } // }
// Find triangles which are walkable based on their slope and rasterize them. // Find triangles which are walkable based on their slope and rasterize them.
// If your input data is multiple meshes, you can transform them here, calculate // If your input data is multiple meshes, you can transform them here, calculate
@@ -262,11 +378,11 @@ private:
bool m_filterWalkableLowHeightSpans = false; bool m_filterWalkableLowHeightSpans = false;
// std::vector! // std::vector!
// if (!m_keepInterResults) // if (!m_keepInterResults)
// { // {
// delete [] m_triareas; // delete [] m_triareas;
// m_triareas = 0; // m_triareas = 0;
// } // }
// //
// Step 3. Filter walkables surfaces. // Step 3. Filter walkables surfaces.
@@ -456,25 +572,25 @@ private:
const uint8_t type = m_pmesh->areas[i]; const uint8_t type = m_pmesh->areas[i];
// Each entry is <tt>2 * #nvp</tt> in length. The first half of the entry // Each entry is <tt>2 * #nvp</tt> in length. The first half of the entry
// contains the indices of the polygon. The first instance of #RC_MESH_NULL_IDX // contains the indices of the polygon. The first instance of #RC_MESH_NULL_IDX
// indicates the end of the indices for the entry. The second half contains // indicates the end of the indices for the entry. The second half contains
// indices to neighbor polygons. A value of #RC_MESH_NULL_IDX indicates no // indices to neighbor polygons. A value of #RC_MESH_NULL_IDX indicates no
// connection for the associated edge. (I.e. The edge is a solid border.) // connection for the associated edge. (I.e. The edge is a solid border.)
// we only use exactly 3 vertices per polygon, no iteration needed // we only use exactly 3 vertices per polygon, no iteration needed
// for (int j = 0; j < m_pmesh->nvp; ++j) { // for (int j = 0; j < m_pmesh->nvp; ++j) {
// if (p[j] == RC_MESH_NULL_IDX) {break;} // if (p[j] == RC_MESH_NULL_IDX) {break;}
// const unsigned short* v = &m_pmesh->verts[p[j]*3]; // const unsigned short* v = &m_pmesh->verts[p[j]*3];
// const float x = orig[0] + v[0]*m_pmesh->cs; // const float x = orig[0] + v[0]*m_pmesh->cs;
// const float z = orig[1] + v[1]*m_pmesh->ch; // const float z = orig[1] + v[1]*m_pmesh->ch;
// const float y = orig[2] + v[2]*m_pmesh->cs; // const float y = orig[2] + v[2]*m_pmesh->cs;
// pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST)); // pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST));
// } // }
// un-swap Y/Z // un-swap Y/Z
const unsigned short* v0 = &m_pmesh->verts[p[0]*3]; const unsigned short* v0 = &m_pmesh->verts[p[0]*3];
@@ -510,50 +626,10 @@ private:
} }
// void dump() {
// std::ofstream out("/tmp/1.dat");
// for (const std::vector<Point3> tria : mesh.get(0)) {
// for (int i = 0; i < 4; ++i) {
// const Point3 p = tria[i%3];
// out << p.x << " " << p.y << " " << p.z << "\r\n";
// }
// out << "\r\n";
// out << "\r\n";
// }
// out.close();
// K::Gnuplot gp;
// gp << "set view equal xyz\n";
// K::GnuplotSplot plot;
// K::GnuplotSplotElementLines lines; plot.add(&lines);
// lines.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(20,0,0));
// lines.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(0,20,0));
// for (const std::vector<Point3> tria : mesh.get(0)) {
// K::GnuplotFill gFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#888888"), 1);
// K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000"));
// K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill, gStroke);
// for (const Point3 p : tria) {
// K::GnuplotCoordinate3 coord(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST);
// pol->add(coord);
// }
// pol->close();
// plot.getObjects().add(pol);
// }
// gp.draw(plot);
// gp.flush();
// sleep(1000);
// }
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
static Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) { Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const {
//const Line2 base(line->from*100, line->to*100); //const Line2 base(line->from*100, line->to*100);
const float thickness_m = line->thickness_m; const float thickness_m = std::max(line->thickness_m, maxQuality_m); // wall's thickness (make thin walls big enough to be detected)
const Point2 dir = (line->to - line->from); // obstacle's direction const Point2 dir = (line->to - line->from); // obstacle's direction
const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree)
const Point2 p1 = line->from + perp * thickness_m/2; // start-up const Point2 p1 = line->from + perp * thickness_m/2; // start-up
@@ -568,6 +644,8 @@ private:
return res; return res;
} }
}; };
}
#endif #endif

28
navMesh/NavMeshLocation.h
View File

@@ -3,17 +3,35 @@
#include "../geo/Point3.h" #include "../geo/Point3.h"
template <typename Tria> struct NavMeshLocation { class NavMeshTriangle;
const Tria* tria; namespace NM {
/**
* as Point3 -> Triangle (on Mesh) lookups are expensive,
* we try to combine both information (point -> triangle)
* most of the time using this structure
*/
template <typename Tria> struct NavMeshLocation {
/** point within the world (in meter) */
Point3 pos; Point3 pos;
/** ctor */ /** NavMeshTriangle the point belongs to */
NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), tria(tria) { const Tria* tria;
/** empty ctor */
NavMeshLocation() : pos(0,0,0), tria(nullptr) {
; ;
} }
}; /** ctor */
NavMeshLocation(const Point3 pos, const Tria* tria) : pos(pos), tria(tria) {
;
}
};
}
#endif // NAVMESHLOCATION_H #endif // NAVMESHLOCATION_H

123
navMesh/NavMeshPoly.h
View File

@@ -1,123 +0,0 @@
#ifndef POLYGON_H
#define POLYGON_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include "../lib/gpc/gpc.cpp.h"
class NavMeshPoly {
struct GPCPolygon : gpc_polygon {
GPCPolygon() {
// contour = (gpc_vertex_list*) calloc(0, 1024);
// contour->num_vertices = 0;
// contour->vertex = (gpc_vertex*) calloc(0, 1024);
// hole = (int*) calloc(0, 1024);
num_contours = 0;
contour = nullptr;
hole = nullptr;
}
~GPCPolygon() {
if (contour) {
gpc_free_polygon(this);
//free(contour->vertex); contour->vertex = nullptr;
}
free(contour); contour = nullptr;
free(hole); hole = nullptr;
}
GPCPolygon& operator = (const GPCPolygon& o) = delete;
GPCPolygon& operator = (GPCPolygon& o) {
this->contour = o.contour;
this->hole = o.hole;
this->num_contours = o.num_contours;
o.contour = nullptr;
o.hole = nullptr;
return *this;
}
};
private:
GPCPolygon state;
float z;
public:
NavMeshPoly(float z) : z(z) {
;
}
void add(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
//state = out;
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
//state = out;
}
std::vector<std::vector<Point3>> get() {
gpc_tristrip res;
res.num_strips = 0;
res.strip = nullptr;
//res.strip = (gpc_vertex_list*) malloc(1024);
gpc_polygon_to_tristrip(&state, &res);
std::vector<std::vector<Point3>> trias;
for (int i = 0; i < res.num_strips; ++i) {
gpc_vertex_list lst = res.strip[i];
// for (int j = 0; j < lst.num_vertices; ++j) {
// gpc_vertex& vert = lst.vertex[j];
// Point3 p3(vert.x, vert.y, z);
// tria.push_back(p3);
// }
for (int j = 2; j < lst.num_vertices; ++j) {
std::vector<Point3> tria;
gpc_vertex& v1 = lst.vertex[j-2];
gpc_vertex& v2 = lst.vertex[j-1];
gpc_vertex& v3 = lst.vertex[j];
tria.push_back(Point3(v1.x, v1.y, z));
tria.push_back(Point3(v2.x, v2.y, z));
tria.push_back(Point3(v3.x, v3.y, z));
trias.push_back(tria);
}
}
gpc_free_tristrip(&res);
return std::move(trias);
}
private:
GPCPolygon toGPC(Floorplan::Polygon2 poly) {
std::vector<gpc_vertex> verts;
for (Point2 p2 : poly.points) {
gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
verts.push_back(vert);
}
GPCPolygon gpol;
gpc_vertex_list list;
list.num_vertices = verts.size();
list.vertex = verts.data();
gpc_add_contour(&gpol, &list, 0);
return gpol;
}
};
#endif // POLYGON_H

58
navMesh/NavMeshRandom.h
View File

@@ -5,46 +5,66 @@
#include <vector> #include <vector>
#include "../math/DrawList.h" #include "../math/DrawList.h"
#include "../geo/Point3.h" #include "../geo/Point3.h"
#include "NavMeshLocation.h" #include "NavMeshLocation.h"
template <typename Tria> class NavMeshRandom { namespace NM {
/**
* randomly pick points within the area of the nav-mesh.
* points are picked evenly:
* bigger triangles are used more often
*
*/
template <typename Tria> class NavMeshRandom {
DrawList<size_t> lst;
std::minstd_rand gen; std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f); std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
const std::vector<Tria*>& triangles; std::vector<const Tria*> triangles;
DrawList<size_t> lst;
public:
struct Result { uint32_t nextSeed() {
Point3 pos; static uint32_t seed = 0;
size_t triaIdx; return ++seed;
Result(const Point3 pos, const size_t triaIdx) : pos(pos), triaIdx(triaIdx) {;}
};
/** ctor */
NavMeshRandom(const std::vector<Tria*>& triangles) : triangles(triangles) {
for (size_t idx = 0; idx < triangles.size(); ++idx) {
lst.add(idx, triangles[idx]->getArea());
}
} }
/** draw a random point within the map */ public:
/** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
// almost always the same number?!
gen();
// construct a DrawList (probability = size[area] of the triangle
// bigger triangles must be choosen more often
for (size_t idx = 0; idx < srcTriangles.size(); ++idx) {
this->triangles.push_back(srcTriangles[idx]);
this->lst.add(idx, srcTriangles[idx]->getArea());
}
}
/** draw a random point */
NavMeshLocation<Tria> draw() { NavMeshLocation<Tria> draw() {
// pick a random triangle to draw from
const size_t idx = lst.get(); const size_t idx = lst.get();
const Tria* tria = triangles[idx]; const Tria* tria = triangles[idx];
while (true) { while (true) {
const float u = dOnTriangle(gen); const float u = dOnTriangle(gen);
const float v = dOnTriangle(gen); const float v = dOnTriangle(gen);
if (u+v > 1) {continue;} if ((u+v) > 1) {continue;}
const Point3 pos = tria.getA() + (tria.getAB() * u) + (tria.getAC() * v); const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
return NavMeshLocation<Tria>(pos, tria); return NavMeshLocation<Tria>(pos, tria);
} }
} }
}; };
}
#endif // NAVMESHRANDOM_H #endif // NAVMESHRANDOM_H

114
navMesh/NavMeshTriangle.h
View File

@@ -4,25 +4,30 @@
#include "../geo/Point3.h" #include "../geo/Point3.h"
#include "../geo/Point2.h" #include "../geo/Point2.h"
class NavMeshTriangle { namespace NM {
public: /**
* represents one triangle within the NavMesh
* each Triangle has up to 3 neighbors (one per edge)
*
* for performance enhancements,
* some memeber attributes are pre-calculated once
*/
class NavMeshTriangle {
Point3 p1; private:
Point3 p2;
Point3 p3;
uint8_t type;
private:
template<typename> friend class NavMesh; template<typename> friend class NavMesh;
const Point3 p1;
const Point3 p2;
const Point3 p3;
const uint8_t type;
NavMeshTriangle* _neighbors[3]; NavMeshTriangle* _neighbors[3];
int _numNeighbors; int _numNeighbors;
/** precalculated stuff */ private: // precalculated stuff
private:
Point2 v0; Point2 v0;
Point2 v1; Point2 v1;
@@ -32,44 +37,63 @@ private:
float invDenom; float invDenom;
float area; float area;
float minZ;
float maxZ;
const Point3 center; const Point3 center;
const Point3 v12; const Point3 v12;
const Point3 v13; const Point3 v13;
public: public:
/** ctor */ /** ctor */
NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
p1(p1), p2(p2), p3(p3), type(type), p1(p1), p2(p2), p3(p3), type(type),
_neighbors(), _numNeighbors(0), _neighbors(), _numNeighbors(0),
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) { center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
precompute(); precompute();
} }
/** get the triangle's type */
uint8_t getType() const {return type;}
Point3 getP1() const {return p1;}
Point3 getP2() const {return p2;}
Point3 getP3() const {return p3;}
bool operator == (const NavMeshTriangle& o) const { bool operator == (const NavMeshTriangle& o) const {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3); return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
} }
/** is the triangle plain? (same Z for all points) */
decltype(std::begin(_neighbors)) begin() {return std::begin(_neighbors);} bool isPlain() const {
const float d1 = std::abs(p1.z - p2.z);
decltype(std::end(_neighbors)) end() {return std::end(_neighbors);} const float d2 = std::abs(p2.z - p3.z);
return (d1 < 0.1) && (d2 < 0.1);
Point3 getA() const {
return p1;
} }
Point3 getAB() const { const NavMeshTriangle* const* begin() const {return &_neighbors[0];}
return v12;
} const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}
Point3 getAC() const { Point3 getPoint(const float u, const float v) const {
return v13; return p1 + (v12*u) + (v13*v);
} }
/** 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());
}
const Point2 v2 = p.xy() - p1.xy(); /** does the triangle contain the given 2D point? */
bool contains(const Point2 p) const {
const Point2 v2 = p - p1.xy();
// Compute dot products // Compute dot products
float dot02 = dot(v0, v2); float dot02 = dot(v0, v2);
@@ -84,6 +108,26 @@ public:
} }
/** estimate the correct z-value for the given 2D point */
Point3 toPoint3(const Point2 p) const {
const Point2 v2 = p - p1.xy();
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// Compute barycentric coordinates
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
const Point3 res = getPoint(v,u);
return res;
}
/** get the triangle's size */ /** get the triangle's size */
float getArea() const { float getArea() const {
return area; return area;
@@ -96,11 +140,15 @@ public:
private: private:
/** perform some pre-calculations to speed things up */ /** perform some pre-calculations to speed things up */
void precompute() { void precompute() {
#warning "TODO, z buffer"
minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15; // TODO the builder does not align on the same height as we did
maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;
// Compute vectors // Compute vectors
v0 = p3.xy() - p1.xy(); v0 = p3.xy() - p1.xy();
v1 = p2.xy() - p1.xy(); v1 = p2.xy() - p1.xy();
@@ -124,7 +172,17 @@ private:
} }
protected:
}; void addNeighbor(NavMeshTriangle* o) {
Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
_neighbors[_numNeighbors] = o;
++_numNeighbors;
}
};
}
#endif // NAVMESHTRIANGLE_H #endif // NAVMESHTRIANGLE_H

64
navMesh/walk/NavMeshSub.h
View File

@@ -3,54 +3,80 @@
#include "../NavMesh.h" #include "../NavMesh.h"
#include "../NavMeshLocation.h" #include "../NavMeshLocation.h"
#include "../NavMeshRandom.h"
#include <vector> #include <vector>
#include <unordered_set> #include <unordered_set>
namespace NM {
template <typename Tria> class NavMeshSub { template <typename Tria> class NavMeshSub {
std::vector<const Tria*> toVisit; std::vector<const Tria*> toVisit;
public: public:
NavMeshSub(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) { NavMeshSub(const NavMeshLocation<Tria>& loc, float radius_m) {
build(nm,loc,radius_m); build(loc,radius_m);
} }
private: /** does this submesh contain the given point? */
bool contains(const Point2 p2) const {
for (const Tria* t : toVisit) {
if (t->contains(p2)) {return true;}
}
return false;
}
void build(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) { /** get the triangle that contains the given point (if any) */
const Tria* getContainingTriangle(const Point2 p2) const {
for (const Tria* t : toVisit) {
if (t->contains(p2)) {return t;}
}
return nullptr;
}
// center to start searching /** perform random operations on the submesh */
const Point3 center = loc.pos; NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(toVisit);
}
toVisit.push_back(loc.tria); private:
void build(const NavMeshLocation<Tria>& loc, float radius_m) {
std::unordered_set<const Tria*> visited; std::unordered_set<const Tria*> visited;
size_t next = 0; // 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 );
}
size_t next = 1; // start with the first neighbor (skip starting triangle itself)
while (next < toVisit.size()) { while (next < toVisit.size()) {
// next triangle // next triangle
const Tria* cur = toVisit[next]; ++next; const NavMeshTriangle* cur = toVisit[next]; ++next;
// neighbors // neighbors
for (const Tria* n : cur) { for (const auto* n : *cur) {
const float dist = loc.pos.getDistance(n.getCenter()); const Tria* t = (const Tria*) n;
const float dist = loc.pos.getDistance(n->getCenter());
if (dist > radius_m) {continue;} if (dist > radius_m) {continue;}
if (visited.find(n) != visited.end()) {continue;} if (visited.find(t) != visited.end()) {continue;}
toVisit.push_back(n); toVisit.push_back(t);
visited.push_back(n); visited.insert(t);
} }
} }
return toVisit;
} }
}; };
}
#endif // NAVMESHSUB_H #endif // NAVMESHSUB_H

103
navMesh/walk/NavMeshWalkEval.h Normal file
View File

@@ -0,0 +1,103 @@
#ifndef NAVMESHWALKEVAL_H
#define NAVMESHWALKEVAL_H
#include "NavMeshWalkParams.h"
#include "../NavMeshLocation.h"
#include "../../math/Distributions.h"
namespace NM {
template <typename Tria> struct NavMeshPotentialWalk {
NavMeshWalkParams<Tria> requested;
NavMeshLocation<Tria> end;
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
;
}
};
/**
* evaluate a NavMeshWalk from -> to = probability
*/
template <typename Tria> class NavMeshWalkEval {
public:
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;
};
/**
* evaluate the difference between head(start,end) and the requested heading
*/
template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {
const double sigma_rad;
const double kappa;
Distribution::VonMises<double> _dist;
Distribution::LUT<double> dist;
public:
// kappa = 1/var = 1/sigma^2
// https://en.wikipedia.org/wiki/Von_Mises_distribution
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
;
}
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;
}
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
*/
template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {
const double sigma;
const Distribution::Normal<double> dist;
public:
WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
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;
return dist.getProbability(diff);
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
}
};
}
#endif // NAVMESHWALKEVAL_H

10
navMesh/walk/NavMeshWalkHelper.h
View File

@@ -1,10 +0,0 @@
#ifndef NAVMESHWALKHELPER_H
#define NAVMESHWALKHELPER_H
template <typename Tria> class NavMeshWalkHelper {
}
#endif // NAVMESHWALKHELPER_H

64
navMesh/walk/NavMeshWalkParams.h Normal file
View File

@@ -0,0 +1,64 @@
#ifndef NAVMESHWALKPARAMS_H
#define NAVMESHWALKPARAMS_H
#include "../../geo/Heading.h"
#include "../NavMeshLocation.h"
namespace NM {
/** configure pedestrian StepSizes */
struct StepSizes {
float stepSizeFloor_m = NAN;
float stepSizeStair_m = NAN;
bool isValid() const {
return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
}
template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const {
Assert::isTrue(isValid(), "invalid step-sizes given");
if (start.tria->isPlain()) {
return stepSizeFloor_m * steps;
} else {
return stepSizeStair_m * steps;
}
}
};
/** configure walking from -> to */
template <typename Tria> struct NavMeshWalkParams {
/** walk starts here (pos/tria) */
NavMeshLocation<Tria> start;
// /** to-be-walked distance */
// float distance_m;
/** direction to walk to */
Heading heading;
/** number of steps to walk */
int numSteps;
/** configuration for pedestrian's step-sizes */
StepSizes stepSizes;
/** empty ctor */
NavMeshWalkParams() : heading(0) {;}
/** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */
float getToBeWalkedDistance() const {
return stepSizes.inMeter(numSteps, start);
}
};
}
#endif // NAVMESHWALKPARAMS_H

97
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -2,41 +2,104 @@
#define NAVMESHWALKSIMPLE_H #define NAVMESHWALKSIMPLE_H
#include "../NavMesh.h" #include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
template <typename Tria> class NavMeshWalkSimpel { #include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
private: namespace NM {
template <typename Tria> class NavMeshWalkSimple {
private:
const NavMesh<Tria>& mesh; const NavMesh<Tria>& mesh;
public: std::vector<NavMeshWalkEval<Tria>*> evals;
int hits = 0;
int misses = 0;
public:
struct Location {
size_t idx;
Point3 pos;
};
struct Result { struct Result {
Location loc;
NavMeshLocation<Tria> location;
Heading heading;
double probability;
Result() : heading(0) {;}
}; };
struct Params { public:
Location loc;
float distance_m;
float heading_rad;
};
public:
/** ctor */ /** ctor */
NavMeshWalkSimpel(const NavMesh<Tria>& mesh) : mesh(mesh) { NavMeshWalkSimple(const NavMesh<Tria>& mesh) : mesh(mesh) {
} }
Result walk(const Params& params) { /** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
Result getDestination(const NavMeshWalkParams<Tria>& params) {
Result res;
res.heading = params.heading;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
// construct reachable region
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 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
res.location.pos = dstTria->toPoint3(dst);
res.location.tria = dstTria;
++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
++misses;
} }
const int total = (hits + misses);
if (total % 10000 == 0) {
std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
}
const NavMeshPotentialWalk<Tria> pwalk(params, res.location);
res.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
res.probability *= p1;
}
return res;
}
};
} }

5
tests/navMesh/TestNavMeshFactory.cpp
View File

@@ -3,6 +3,7 @@
#include "../Tests.h" #include "../Tests.h"
#include "../../navMesh/NavMeshFactory.h" #include "../../navMesh/NavMeshFactory.h"
using namespace NM;
TEST(NavMeshFactory, build1) { TEST(NavMeshFactory, build1) {
@@ -16,8 +17,8 @@ TEST(NavMeshFactory, build1) {
outline.outdoor = false; outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD; outline.method = Floorplan::OutlineMethod::ADD;
NavMesh<NavMeshTriangle> nm; NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NavMeshTriangle> fac(&nm); NavMeshFactory<NM::NavMeshTriangle> fac(&nm);
fac.build(&map); fac.build(&map);
ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5); ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5);

7
tests/navMesh/TestNavMeshSub.cpp
View File

@@ -4,6 +4,7 @@
#include "../../navMesh/NavMeshFactory.h" #include "../../navMesh/NavMeshFactory.h"
#include "../../navMesh/walk/NavMeshSub.h" #include "../../navMesh/walk/NavMeshSub.h"
using namespace NM;
TEST(NavMeshSub, build1) { TEST(NavMeshSub, build1) {
@@ -17,11 +18,11 @@ TEST(NavMeshSub, build1) {
outline.outdoor = false; outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD; outline.method = Floorplan::OutlineMethod::ADD;
NavMesh<NavMeshTriangle> nm; NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NavMeshTriangle> fac(&nm); NavMeshFactory<NM::NavMeshTriangle> fac(&nm);
fac.build(&map); fac.build(&map);
NavMeshLocation<NavMeshTriangle> loc = nm.getLocation(Point3(1,1,1)); NavMeshLocation<NM::NavMeshTriangle> loc = nm.getLocation(Point3(1,1,1));
} }

1
tests/navMesh/TestNavMeshTriangle.cpp
View File

@@ -3,6 +3,7 @@
#include "../Tests.h" #include "../Tests.h"
#include "../../navMesh/NavMeshTriangle.h" #include "../../navMesh/NavMeshTriangle.h"
using namespace NM;
TEST(NavMeshTriangle, contains) { TEST(NavMeshTriangle, contains) {