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

160
navMesh/NavMesh.h
View File

@@ -9,101 +9,113 @@
#include "NavMeshRandom.h"
#include "NavMeshLocation.h"
template <typename Tria> class NavMesh {
namespace NM {
/** all triangles within the mesh */
std::vector<Tria*> triangles;
template <typename Tria> class NavMesh {
BBox3 bbox;
/** all triangles within the mesh */
std::vector<Tria*> triangles;
public:
BBox3 bbox;
NavMesh() {
public:
}
/** ctor */
NavMesh() {
/** the overall bounding-box */
const BBox3 getBBox() const {
return bbox;
}
/** add a new triangle */
void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) {
triangles.push_back(new Tria(p1,p2,p3,type));
bbox.add(p1);
bbox.add(p2);
bbox.add(p3);
}
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");
}
/** connect both triangles */
void connectBiDir(int idx1, int idx2) {
connectUniDir(idx1,idx2);
connectUniDir(idx2,idx1);
}
/** dtor */
~NavMesh() {
for (const Tria* t : triangles) {delete t;}
triangles.clear();
}
/** connect both triangles */
void connectUniDir(int idxFrom, int idxTo) {
NavMeshTriangle* tria = triangles[idxFrom];
tria->_neighbors[tria->_numNeighbors] = triangles[idxTo];
}
/** the overall bounding-box */
const BBox3 getBBox() const {
return bbox;
}
/** allows for-each iteration over all included triangles */
decltype(triangles.begin()) begin() {return triangles.begin();}
/** add a new triangle */
void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) {
triangles.push_back(new Tria(p1,p2,p3,type));
bbox.add(p1);
bbox.add(p2);
bbox.add(p3);
}
/** allows for-each iteration over all included triangles */
decltype(triangles.end()) end() {return triangles.end();}
/** 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 within NavMesh: " + pos.asString());
}
/** array access */
Tria* operator [] (const size_t idx) {
Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds");
return triangles[idx];
}
/** connect both triangles */
void connectBiDir(int idx1, int idx2) {
connectUniDir(idx1,idx2);
connectUniDir(idx2,idx1);
}
/** get the number of triangles used */
size_t getNumTriangles() const {
return triangles.size();
}
/** connect both triangles */
void connectUniDir(int idxFrom, int idxTo) {
Tria* tria = triangles[idxFrom];
tria->addNeighbor(triangles[idxTo]);
}
/** ---------------- MISC ---------------- */
/** allows for-each iteration over all included triangles */
decltype(triangles.begin()) begin() {return triangles.begin();}
/** allows for-each iteration over all included triangles */
decltype(triangles.end()) end() {return triangles.end();}
/** array access */
Tria* operator [] (const size_t idx) {
Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds");
return triangles[idx];
}
/** get the number of triangles used */
size_t getNumTriangles() const {
return triangles.size();
}
/** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandomizer() {
return NavMeshRandom<Tria>(triangles);
}
NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(triangles);
}
// /** ---------------- NEIGHBORS ---------------- */
// /** ---------------- NEIGHBORS ---------------- */
// /** get the number of neighbors for the given element */
// int getNumNeighbors(const size_t idx) const {
// return getNumNeighbors(triangles[idx]);
// }
// /** get the number of neighbors for the given element */
// int getNumNeighbors(const size_t idx) const {
// return getNumNeighbors(triangles[idx]);
// }
// /** get the number of neighbors for the given element */
// int getNumNeighbors(const Tria& e) const {
// return e._numNeighbors;
// }
// /** get the number of neighbors for the given element */
// int getNumNeighbors(const Tria& e) const {
// return e._numNeighbors;
// }
// /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
// const Tria& node = triangles[nodeIdx];
// return getNeighbor(node, nth);
// }
// /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
// const Tria& node = triangles[nodeIdx];
// return getNeighbor(node, nth);
// }
// /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const Tria& tria, const size_t nth) const {
// const Tria& neighbor = triangles[tria._neighbors[nth]];
// return (Tria&) neighbor;
// }
// /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const Tria& tria, const size_t nth) const {
// const Tria& neighbor = triangles[tria._neighbors[nth]];
// return (Tria&) neighbor;
// }
};
};
}
#endif

105
navMesh/NavMeshDebug.h
View File

@@ -9,67 +9,80 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
class NavMeshDebug {
namespace NM {
public:
/**
* debug plot NavMeshes
*/
class NavMeshDebug {
template <typename Tria> static void show(NavMesh<Tria>& nm) {
public:
K::GnuplotFill gFill[3] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#111111"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaaa"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaff"), 1)
};
template <typename Tria> static void show(NavMesh<Tria>& nm) {
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
K::GnuplotFill gFill[3] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#111111"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaaa"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaff"), 1)
};
K::Gnuplot gp;
gp << "set view equal xy\n";
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true);
K::GnuplotSplotElementPoints points; plot.add(&points);
K::Gnuplot gp;
gp << "set view equal xy\n";
const BBox3 bbox = nm.getBBox();
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true);
K::GnuplotSplotElementPoints points; plot.add(&points);
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));
// 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.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
const BBox3 bbox = nm.getBBox();
//stairs in eigene group? vlt gehen dann auch die dellen weg?
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));
// 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.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?
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->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->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();
// //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));
}
for (const Tria& tria : nm) {
uint8_t type = tria.type;
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->close();
pol->setZIndex(tria.p3.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();
//lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
}
plot.getObjects().reOrderByZIndex();
gp.draw(plot);
gp.flush();
sleep(1);
}
plot.getObjects().reOrderByZIndex();
};
gp.draw(plot);
gp.flush();
sleep(1);
}
};
}
#endif // NAVMESHDEBUG_H

1116
navMesh/NavMeshFactory.h
View File

File diff suppressed because it is too large Load Diff

34
navMesh/NavMeshLocation.h
View File

@@ -3,17 +3,35 @@
#include "../geo/Point3.h"
template <typename Tria> struct NavMeshLocation {
class NavMeshTriangle;
const Tria* tria;
namespace NM {
Point3 pos;
/**
* 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 {
/** ctor */
NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), 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(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

88
navMesh/NavMeshRandom.h
View File

@@ -5,46 +5,66 @@
#include <vector>
#include "../math/DrawList.h"
#include "../geo/Point3.h"
#include "NavMeshLocation.h"
template <typename Tria> class NavMeshRandom {
namespace NM {
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;
/**
* 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 {
public:
DrawList<size_t> lst;
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
std::vector<const Tria*> triangles;
uint32_t nextSeed() {
static uint32_t seed = 0;
return ++seed;
}
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() {
// 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->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
return NavMeshLocation<Tria>(pos, tria);
}
}
struct Result {
Point3 pos;
size_t triaIdx;
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 */
NavMeshLocation<Tria> draw() {
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);
return NavMeshLocation<Tria>(pos, tria);
}
}
};
}
#endif // NAVMESHRANDOM_H

222
navMesh/NavMeshTriangle.h
View File

@@ -4,127 +4,185 @@
#include "../geo/Point3.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;
Point3 p2;
Point3 p3;
uint8_t type;
private:
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];
int _numNeighbors;
NavMeshTriangle* _neighbors[3];
int _numNeighbors;
/** precalculated stuff */
private: // precalculated stuff
private:
Point2 v0;
Point2 v1;
float dot00;
float dot01;
float dot11;
float invDenom;
float area;
Point2 v0;
Point2 v1;
float dot00;
float dot01;
float dot11;
float invDenom;
float area;
float minZ;
float maxZ;
const Point3 center;
const Point3 v12;
const Point3 v13;
const Point3 center;
const Point3 v12;
const Point3 v13;
public:
public:
/** ctor */
NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
p1(p1), p2(p2), p3(p3), type(type),
_neighbors(), _numNeighbors(0),
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
precompute();
}
/** ctor */
NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
p1(p1), p2(p2), p3(p3), type(type),
_neighbors(), _numNeighbors(0),
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
bool operator == (const NavMeshTriangle& o) const {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
}
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;}
decltype(std::begin(_neighbors)) begin() {return std::begin(_neighbors);}
bool operator == (const NavMeshTriangle& o) const {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
}
decltype(std::end(_neighbors)) end() {return std::end(_neighbors);}
/** 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 getA() const {
return p1;
}
const NavMeshTriangle* const* begin() const {return &_neighbors[0];}
Point3 getAB() const {
return v12;
}
const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}
Point3 getAC() const {
return v13;
}
Point3 getPoint(const float u, const float v) const {
return p1 + (v12*u) + (v13*v);
}
bool contains(const Point3 p) const {
/** 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 {
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
const Point2 v2 = p - p1.xy();
// Compute barycentric coordinates
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// Check if point is in triangle
return (u >= 0) && (v >= 0) && (u + v <= 1);
// Compute barycentric coordinates
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
}
// Check if point is in triangle
return (u >= 0) && (v >= 0) && (u + v <= 1);
/** get the triangle's size */
float getArea() const {
return area;
}
}
/** get the triangle's center-point */
Point3 getCenter() const {
return center;
}
/** 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;
}
private:
/** get the triangle's size */
float getArea() const {
return area;
}
/** perform some pre-calculations to speed things up */
void precompute() {
/** get the triangle's center-point */
Point3 getCenter() const {
return center;
}
// Compute vectors
v0 = p3.xy() - p1.xy();
v1 = p2.xy() - p1.xy();
// Compute dot products
dot00 = dot(v0, v0);
dot01 = dot(v0, v1);
dot11 = dot(v1, v1);
// Compute barycentric coordinates
invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
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();
// Compute dot products
dot00 = dot(v0, v0);
dot01 = dot(v0, v1);
dot11 = dot(v1, v1);
// Compute barycentric coordinates
invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float a = (p2-p1).length();
const float b = (p3-p1).length();
const float c = (p2-p3).length();
const float s = 0.5f * (a+b+c);
area = std::sqrt( s * (s-a) * (s-b) * (s-c) );
const float a = (p2-p1).length();
const float b = (p3-p1).length();
const float c = (p2-p3).length();
const float s = 0.5f * (a+b+c);
area = std::sqrt( s * (s-a) * (s-b) * (s-c) );
}
}
protected:
void addNeighbor(NavMeshTriangle* o) {
Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
_neighbors[_numNeighbors] = o;
++_numNeighbors;
}
};
};
}
#endif // NAVMESHTRIANGLE_H

80
navMesh/walk/NavMeshSub.h
View File

@@ -3,54 +3,80 @@
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../NavMeshRandom.h"
#include <vector>
#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) {
build(nm,loc,radius_m);
}
NavMeshSub(const NavMeshLocation<Tria>& loc, float 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
const Point3 center = loc.pos;
/** perform random operations on the submesh */
NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(toVisit);
}
toVisit.push_back(loc.tria);
private:
std::unordered_set<const Tria*> visited;
void build(const NavMeshLocation<Tria>& loc, float radius_m) {
size_t next = 0;
while (next < toVisit.size()) {
std::unordered_set<const Tria*> visited;
// next triangle
const Tria* cur = toVisit[next]; ++next;
// 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 NavMeshTriangle* cur = toVisit[next]; ++next;
// neighbors
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(t) != visited.end()) {continue;}
toVisit.push_back(t);
visited.insert(t);
}
// neighbors
for (const Tria* n : cur) {
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);
}
}
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

121
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -2,42 +2,105 @@
#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"
private:
namespace NM {
const NavMesh<Tria>& mesh;
template <typename Tria> class NavMeshWalkSimple {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
int hits = 0;
int misses = 0;
public:
struct Result {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
Result() : heading(0) {;}
};
public:
/** ctor */
NavMeshWalkSimple(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** 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;
}
public:
struct Location {
size_t idx;
Point3 pos;
};
struct Result {
Location loc;
};
struct Params {
Location loc;
float distance_m;
float heading_rad;
};
public:
/** ctor */
NavMeshWalkSimpel(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
Result walk(const Params& params) {
}
}
#endif // NAVMESHWALKSIMPLE_H

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) {