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

20
navMesh/NavMesh.h
View File

@@ -9,6 +9,8 @@
#include "NavMeshRandom.h"
#include "NavMeshLocation.h"
namespace NM {
template <typename Tria> class NavMesh {
/** all triangles within the mesh */
@@ -18,10 +20,17 @@ template <typename Tria> class NavMesh {
public:
/** ctor */
NavMesh() {
}
/** dtor */
~NavMesh() {
for (const Tria* t : triangles) {delete t;}
triangles.clear();
}
/** the overall bounding-box */
const BBox3 getBBox() const {
return bbox;
@@ -35,13 +44,14 @@ public:
bbox.add(p3);
}
/** get the triangle this point belongs to (if any) */
NavMeshLocation<Tria> getLocation(const Point3 pos) {
for (const Tria* tria : triangles) {
if (tria->contains(pos)) {
return NavMeshLocation<Tria>(pos, tria);
}
}
throw Exception("location not found");
throw Exception("location not found within NavMesh: " + pos.asString());
}
/** connect both triangles */
@@ -52,8 +62,8 @@ public:
/** connect both triangles */
void connectUniDir(int idxFrom, int idxTo) {
NavMeshTriangle* tria = triangles[idxFrom];
tria->_neighbors[tria->_numNeighbors] = triangles[idxTo];
Tria* tria = triangles[idxFrom];
tria->addNeighbor(triangles[idxTo]);
}
/** allows for-each iteration over all included triangles */
@@ -76,7 +86,7 @@ public:
/** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandomizer() {
NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(triangles);
}
@@ -106,4 +116,6 @@ public:
};
}
#endif

33
navMesh/NavMeshDebug.h
View File

@@ -9,6 +9,11 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
namespace NM {
/**
* debug plot NavMeshes
*/
class NavMeshDebug {
public:
@@ -40,23 +45,29 @@ public:
//stairs in eigene group? vlt gehen dann auch die dellen weg?
for (const Tria& tria : nm) {
uint8_t type = tria.type;
for (const Tria* tria : nm) {
const uint8_t type = tria->getType();
if (type < 0 || type > 2) {
throw std::runtime_error("out of type-bounds");
}
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.p2.x, tria.p2.y, tria.p2.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->getP1().x, tria->getP1().y, tria->getP1().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->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST));
pol->close();
pol->setZIndex(tria.p3.z);
pol->setZIndex(tria->getP3().z);
plot.getObjects().add(pol);
for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
const Tria& o = nm.getNeighbor(tria, i);
const Point3 p1 = tria.getCenter();
const Point3 p2 = o.getCenter();
//for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
// const Tria* o = nm.getNeighbor(tria, i);
// 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));
//}
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));
}
@@ -72,4 +83,6 @@ public:
};
}
#endif // NAVMESHDEBUG_H

178
navMesh/NavMeshFactory.h
View File

@@ -5,11 +5,120 @@
#include "../floorplan/v2/FloorplanHelper.h"
#include "NavMesh.h"
#include "NavMeshPoly.h"
#include "NavMeshTriangle.h"
#include "../lib/gpc/gpc.cpp.h"
#include "../lib/Recast/Recast.h"
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_MONOTONE,
@@ -45,6 +154,8 @@ template <typename Tria> class NavMeshFactory {
private:
float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected
NavMesh<Tria>* dst = nullptr;
std::vector<TriangleIn> triangles;
@@ -63,6 +174,11 @@ public:
fire(bbox);
}
/** get the smallest obstacle size that can be detected */
float getMaxQuality_m() const {
return maxQuality_m;
}
private:
/** add one floor */
@@ -166,20 +282,20 @@ private:
rcPolyMeshDetail* m_dmesh;
rcContext* m_ctx = new rcContext();
float m_cellSize = 0.1f; //0.3f; // needed for 20cm walls to work!
float m_cellHeight = 0.1f; //0.2f;
float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m
float m_cellHeight = maxQuality_m/2.0f; //0.2f;
float m_agentHeight = 2.0f;
float m_agentRadius = 0.1f;//0.6f;
float m_agentMaxClimb = 0.5f; // 0.9f;
float m_agentMaxSlope = 45.0f;
float m_agentRadius = 0.2f;//0.6f;
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; // elevator???
float m_regionMinSize = 2;//8;
float m_regionMergeSize = 20;
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_detailSampleDist = 6.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
@@ -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! */
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 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 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree)
const Point2 p1 = line->from + perp * thickness_m/2; // start-up
@@ -570,4 +646,6 @@ private:
};
}
#endif

24
navMesh/NavMeshLocation.h
View File

@@ -3,17 +3,35 @@
#include "../geo/Point3.h"
class NavMeshTriangle;
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 {
const Tria* tria;
/** point within the world (in meter) */
Point3 pos;
/** NavMeshTriangle the point belongs to */
const Tria* tria;
/** empty ctor */
NavMeshLocation() : pos(0,0,0), tria(nullptr) {
;
}
/** ctor */
NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), tria(tria) {
NavMeshLocation(const Point3 pos, const Tria* tria) : pos(pos), tria(tria) {
;
}
};
}
#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

50
navMesh/NavMeshRandom.h
View File

@@ -5,41 +5,59 @@
#include <vector>
#include "../math/DrawList.h"
#include "../geo/Point3.h"
#include "NavMeshLocation.h"
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::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
const std::vector<Tria*>& triangles;
DrawList<size_t> lst;
std::vector<const Tria*> triangles;
uint32_t nextSeed() {
static uint32_t seed = 0;
return ++seed;
}
public:
struct Result {
Point3 pos;
size_t triaIdx;
Result(const Point3 pos, const size_t triaIdx) : pos(pos), triaIdx(triaIdx) {;}
};
/** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
/** ctor */
NavMeshRandom(const std::vector<Tria*>& triangles) : triangles(triangles) {
for (size_t idx = 0; idx < triangles.size(); ++idx) {
lst.add(idx, triangles[idx]->getArea());
}
// 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 within the map */
}
/** draw a random point */
NavMeshLocation<Tria> draw() {
// pick a random triangle to draw from
const size_t idx = lst.get();
const Tria* tria = triangles[idx];
while (true) {
const float u = dOnTriangle(gen);
const float v = dOnTriangle(gen);
if (u+v > 1) {continue;}
const Point3 pos = tria.getA() + (tria.getAB() * u) + (tria.getAC() * v);
if ((u+v) > 1) {continue;}
const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
return NavMeshLocation<Tria>(pos, tria);
}
@@ -47,4 +65,6 @@ public:
};
}
#endif // NAVMESHRANDOM_H

106
navMesh/NavMeshTriangle.h
View File

@@ -4,25 +4,30 @@
#include "../geo/Point3.h"
#include "../geo/Point2.h"
namespace NM {
/**
* 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 {
public:
Point3 p1;
Point3 p2;
Point3 p3;
uint8_t type;
private:
template<typename> friend class NavMesh;
const Point3 p1;
const Point3 p2;
const Point3 p3;
const uint8_t type;
NavMeshTriangle* _neighbors[3];
int _numNeighbors;
/** precalculated stuff */
private:
private: // precalculated stuff
Point2 v0;
Point2 v1;
@@ -32,6 +37,9 @@ private:
float invDenom;
float area;
float minZ;
float maxZ;
const Point3 center;
const Point3 v12;
const Point3 v13;
@@ -43,33 +51,49 @@ public:
p1(p1), p2(p2), p3(p3), type(type),
_neighbors(), _numNeighbors(0),
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
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 {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
}
decltype(std::begin(_neighbors)) begin() {return std::begin(_neighbors);}
decltype(std::end(_neighbors)) end() {return std::end(_neighbors);}
Point3 getA() const {
return p1;
/** is the triangle plain? (same Z for all points) */
bool isPlain() const {
const float d1 = std::abs(p1.z - p2.z);
const float d2 = std::abs(p2.z - p3.z);
return (d1 < 0.1) && (d2 < 0.1);
}
Point3 getAB() const {
return v12;
}
Point3 getAC() const {
return v13;
const NavMeshTriangle* const* begin() const {return &_neighbors[0];}
const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}
Point3 getPoint(const float u, const float v) const {
return p1 + (v12*u) + (v13*v);
}
/** does the triangle contain the given 3D point? */
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
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 */
float getArea() const {
return area;
@@ -101,6 +145,10 @@ private:
/** perform some pre-calculations to speed things up */
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
v0 = p3.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

60
navMesh/walk/NavMeshSub.h
View File

@@ -3,10 +3,12 @@
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../NavMeshRandom.h"
#include <vector>
#include <unordered_set>
namespace NM {
template <typename Tria> class NavMeshSub {
@@ -14,43 +16,67 @@ template <typename Tria> class NavMeshSub {
public:
NavMeshSub(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) {
build(nm,loc,radius_m);
NavMeshSub(const NavMeshLocation<Tria>& loc, float radius_m) {
build(loc,radius_m);
}
/** 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;
}
/** 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;
}
/** perform random operations on the submesh */
NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(toVisit);
}
private:
void build(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) {
// center to start searching
const Point3 center = loc.pos;
toVisit.push_back(loc.tria);
void build(const NavMeshLocation<Tria>& loc, float radius_m) {
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()) {
// next triangle
const Tria* cur = toVisit[next]; ++next;
const NavMeshTriangle* cur = toVisit[next]; ++next;
// neighbors
for (const Tria* n : cur) {
const float dist = loc.pos.getDistance(n.getCenter());
for (const auto* n : *cur) {
const Tria* t = (const Tria*) n;
const float dist = loc.pos.getDistance(n->getCenter());
if (dist > radius_m) {continue;}
if (visited.find(n) != visited.end()) {continue;}
toVisit.push_back(n);
visited.push_back(n);
if (visited.find(t) != visited.end()) {continue;}
toVisit.push_back(t);
visited.insert(t);
}
}
return toVisit;
}
};
}
#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

89
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -2,41 +2,104 @@
#define NAVMESHWALKSIMPLE_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"
namespace NM {
template <typename Tria> class NavMeshWalkSimple {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
int hits = 0;
int misses = 0;
public:
struct Location {
size_t idx;
Point3 pos;
};
struct Result {
Location loc;
};
struct Params {
Location loc;
float distance_m;
float heading_rad;
NavMeshLocation<Tria> location;
Heading heading;
double probability;
Result() : heading(0) {;}
};
public:
/** 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 "../../navMesh/NavMeshFactory.h"
using namespace NM;
TEST(NavMeshFactory, build1) {
@@ -16,8 +17,8 @@ TEST(NavMeshFactory, build1) {
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
NavMesh<NavMeshTriangle> nm;
NavMeshFactory<NavMeshTriangle> fac(&nm);
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm);
fac.build(&map);
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/walk/NavMeshSub.h"
using namespace NM;
TEST(NavMeshSub, build1) {
@@ -17,11 +18,11 @@ TEST(NavMeshSub, build1) {
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
NavMesh<NavMeshTriangle> nm;
NavMeshFactory<NavMeshTriangle> fac(&nm);
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm);
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 "../../navMesh/NavMeshTriangle.h"
using namespace NM;
TEST(NavMeshTriangle, contains) {