From fee6cd349642cef4e4b8b50f9ed4b1aff913dba6 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 10 Jan 2018 16:57:19 +0100 Subject: [PATCH] worked on navMesh stuff - creation - walking - helper --- navMesh/NavMesh.h | 160 ++-- navMesh/NavMeshDebug.h | 105 ++- navMesh/NavMeshFactory.h | 1116 +++++++++++++------------ navMesh/NavMeshLocation.h | 34 +- navMesh/NavMeshPoly.h | 123 --- navMesh/NavMeshRandom.h | 88 +- navMesh/NavMeshTriangle.h | 222 +++-- navMesh/walk/NavMeshSub.h | 80 +- navMesh/walk/NavMeshWalkEval.h | 103 +++ navMesh/walk/NavMeshWalkHelper.h | 10 - navMesh/walk/NavMeshWalkParams.h | 64 ++ navMesh/walk/NavMeshWalkSimple.h | 121 ++- tests/navMesh/TestNavMeshFactory.cpp | 5 +- tests/navMesh/TestNavMeshSub.cpp | 7 +- tests/navMesh/TestNavMeshTriangle.cpp | 1 + 15 files changed, 1282 insertions(+), 957 deletions(-) delete mode 100644 navMesh/NavMeshPoly.h create mode 100644 navMesh/walk/NavMeshWalkEval.h delete mode 100644 navMesh/walk/NavMeshWalkHelper.h create mode 100644 navMesh/walk/NavMeshWalkParams.h diff --git a/navMesh/NavMesh.h b/navMesh/NavMesh.h index 362ee39..6077049 100644 --- a/navMesh/NavMesh.h +++ b/navMesh/NavMesh.h @@ -9,101 +9,113 @@ #include "NavMeshRandom.h" #include "NavMeshLocation.h" -template class NavMesh { +namespace NM { - /** all triangles within the mesh */ - std::vector triangles; + template class NavMesh { - BBox3 bbox; + /** all triangles within the mesh */ + std::vector 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 getLocation(const Point3 pos) { - for (const Tria* tria : triangles) { - if (tria->contains(pos)) { - return NavMeshLocation(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 getLocation(const Point3 pos) { + for (const Tria* tria : triangles) { + if (tria->contains(pos)) { + return NavMeshLocation(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 getRandomizer() { - return NavMeshRandom(triangles); - } + NavMeshRandom getRandom() { + return NavMeshRandom(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 diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h index 85a4578..682add0 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -9,67 +9,80 @@ #include #include -class NavMeshDebug { +namespace NM { -public: + /** + * debug plot NavMeshes + */ + class NavMeshDebug { - template static void show(NavMesh& 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 static void show(NavMesh& 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 diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 1899a33..7a5e282 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -5,569 +5,647 @@ #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" -enum SamplePartitionType { - SAMPLE_PARTITION_WATERSHED, - SAMPLE_PARTITION_MONOTONE, - SAMPLE_PARTITION_LAYERS, -}; +namespace NM { -struct TriangleIn { - Point3 p1; - Point3 p2; - Point3 p3; - 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) {;} -}; -struct TriangleOut { + class NavMeshPoly { - Point3 p1; - Point3 p2; - Point3 p3; - - int numNeighbors = 0; - int neighbors[3]; // each triangle has max 3 neighbors - - TriangleOut(const Point3 p1, const Point3 p2, const Point3 p3) : p1(p1), p2(p2), p3(p3), neighbors() {;} - - Point3 center() const { - return (p1+p2+p3) / 3; - } - -}; - -template class NavMeshFactory { - -private: - - NavMesh* dst = nullptr; - - std::vector triangles; - -public: - - NavMeshFactory(NavMesh* dst) : dst(dst) { - - } - - void build(Floorplan::IndoorMap* map) { - const BBox3 bbox = FloorplanHelper::getBBox(map); - for (const Floorplan::Floor* floor : map->floors) { - add(floor); - } - fire(bbox); - } - -private: - - /** add one floor */ - void add(const Floorplan::Floor* floor) { - - NavMeshPoly nmPoly(floor->atHeight); - - for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { - if (poly->method == Floorplan::OutlineMethod::ADD) { - nmPoly.add(poly->poly); + 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) { + ; } - for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { - if (poly->method == Floorplan::OutlineMethod::REMOVE) { - nmPoly.remove(poly->poly); - } + void add(const Floorplan::Polygon2& poly) { + GPCPolygon cur = toGPC(poly); + gpc_polygon_clip(GPC_UNION, &state, &cur, &state); } - for (Floorplan::FloorObstacle* obs : floor->obstacles) { - Floorplan::FloorObstacleLine* line = dynamic_cast(obs); - if (line != nullptr) { - nmPoly.remove(getPolygon(line)); - } + void remove(const Floorplan::Polygon2& poly) { + GPCPolygon cur = toGPC(poly); + gpc_polygon_clip(GPC_DIFF, &state, &cur, &state); } - std::vector> tmp = nmPoly.get(); - for (const std::vector& tria : tmp) { - const TriangleIn t(tria[0], tria[1], tria[2], 1); // TODO outdoor - triangles.push_back(t); - } + std::vector> get() { - // add all stairs - for (const Floorplan::Stair* stair : floor->stairs) { - const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); - for (const Floorplan::Quad3& quad : quads) { - const TriangleIn t1(quad.p1, quad.p2, quad.p3, 2); // TODO type - const TriangleIn t2(quad.p1, quad.p3, quad.p4, 2); - triangles.push_back(t1); - triangles.push_back(t2); - } - } + gpc_tristrip res; + res.num_strips = 0; + res.strip = nullptr; - } + //res.strip = (gpc_vertex_list*) malloc(1024); + gpc_polygon_to_tristrip(&state, &res); - bool fire(BBox3 bbox) { + std::vector> trias; - std::vector tData; - std::vector vData; - std::vector typeData; - - // floor outlines - for (const TriangleIn& t : triangles) { - - // swap YZ and polygon order - int startVert = vData.size() / 3; - - // invert triangle ? (CW vs CCW) - // ensure normal points UP - const Point3 norm = cross((t.p2-t.p1), (t.p3-t.p1)); - if (norm.z > 0) { - tData.push_back(startVert + 0); - tData.push_back(startVert + 2); - tData.push_back(startVert + 1); - } else { - tData.push_back(startVert + 0); - tData.push_back(startVert + 1); - tData.push_back(startVert + 2); - } - - typeData.push_back(t.type); - - vData.push_back(t.p1.x); - vData.push_back(t.p1.z); - vData.push_back(t.p1.y); - - vData.push_back(t.p2.x); - vData.push_back(t.p2.z); - vData.push_back(t.p2.y); - - vData.push_back(t.p3.x); - vData.push_back(t.p3.z); - vData.push_back(t.p3.y); - - } - - unsigned char* m_triareas = typeData.data(); - const float* verts = vData.data(); - const int* tris = tData.data(); - - int ntris = tData.size() / 3; - int nverts = vData.size() / 3; - - - //unsigned char* m_triareas; - rcHeightfield* m_solid; - rcCompactHeightfield* m_chf; - rcContourSet* m_cset; - rcPolyMesh* m_pmesh; - rcConfig m_cfg; - 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_agentHeight = 2.0f; - float m_agentRadius = 0.1f;//0.6f; - float m_agentMaxClimb = 0.5f; // 0.9f; - float m_agentMaxSlope = 45.0f; - 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_vertsPerPoly = 3;//6.0f; - float m_detailSampleDist = 6.0f; - float m_detailSampleMaxError = 1.0f;//1.0f; - int m_partitionType = SAMPLE_PARTITION_WATERSHED; - - - // Init build configuration from GUI - memset(&m_cfg, 0, sizeof(m_cfg)); - m_cfg.cs = m_cellSize; - m_cfg.ch = m_cellHeight; - m_cfg.walkableSlopeAngle = m_agentMaxSlope; - m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch); - m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch); - m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs); - m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize); - m_cfg.maxSimplificationError = m_edgeMaxError; - m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize); // Note: area = size*size - m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize); // Note: area = size*size - m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly; - m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist; - m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError; - - float bmin[3] = {bbox.getMin().x, bbox.getMin().z, bbox.getMin().y}; - float bmax[3] = {bbox.getMax().x, bbox.getMax().z, bbox.getMax().y};// x/z swapped? - - // Set the area where the navigation will be build. - // Here the bounds of the input mesh are used, but the - // area could be specified by an user defined box, etc. - rcVcopy(m_cfg.bmin, bmin); - rcVcopy(m_cfg.bmax, bmax); - rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height); - - // Reset build times gathering. - m_ctx->resetTimers(); - - // Start the build process. - m_ctx->startTimer(RC_TIMER_TOTAL); - - m_ctx->log(RC_LOG_PROGRESS, "Building navigation:"); - m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height); - m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f); - - // - // Step 2. Rasterize input polygon soup. - // - - // Allocate voxel heightfield where we rasterize our input data to. - m_solid = rcAllocHeightfield(); - if (!m_solid) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'."); - return false; - } - if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield."); - return false; - } - - // Allocate array that can hold triangle area types. - // If you have multiple meshes you need to process, allocate - // and array which can hold the max number of triangles you need to process. -// m_triareas = new unsigned char[ntris]; -// if (!m_triareas) -// { -// m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris); -// return false; -// } - - // 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 - // the are type for each of the meshes and rasterize them. - //memset(m_triareas, 0, ntris*sizeof(unsigned char)); - //rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas); - if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles."); - return false; - } - - bool m_keepInterResults = false; - bool m_filterLowHangingObstacles = false; - bool m_filterLedgeSpans = false; - bool m_filterWalkableLowHeightSpans = false; - - // std::vector! -// if (!m_keepInterResults) -// { -// delete [] m_triareas; -// m_triareas = 0; -// } - - // - // Step 3. Filter walkables surfaces. - // - - - - // Once all geoemtry is rasterized, we do initial pass of filtering to - // remove unwanted overhangs caused by the conservative rasterization - // as well as filter spans where the character cannot possibly stand. - if (m_filterLowHangingObstacles) - rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid); - if (m_filterLedgeSpans) - rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid); - if (m_filterWalkableLowHeightSpans) - rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid); - - - // - // Step 4. Partition walkable surface to simple regions. - // - - // Compact the heightfield so that it is faster to handle from now on. - // This will result more cache coherent data as well as the neighbours - // between walkable cells will be calculated. - m_chf = rcAllocCompactHeightfield(); - if (!m_chf) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'."); - return false; - } - if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data."); - return false; - } - - if (!m_keepInterResults) - { - rcFreeHeightField(m_solid); - m_solid = 0; - } - - // Erode the walkable area by agent radius. - if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode."); - return false; - } - - // (Optional) Mark areas. - // const ConvexVolume* vols = m_geom->getConvexVolumes(); - // for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i) - // rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf); - - - // Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas. - // There are 3 martitioning methods, each with some pros and cons: - // 1) Watershed partitioning - // - the classic Recast partitioning - // - creates the nicest tessellation - // - usually slowest - // - partitions the heightfield into nice regions without holes or overlaps - // - the are some corner cases where this method creates produces holes and overlaps - // - holes may appear when a small obstacles is close to large open area (triangulation can handle this) - // - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail - // * generally the best choice if you precompute the nacmesh, use this if you have large open areas - // 2) Monotone partioning - // - fastest - // - partitions the heightfield into regions without holes and overlaps (guaranteed) - // - creates long thin polygons, which sometimes causes paths with detours - // * use this if you want fast navmesh generation - // 3) Layer partitoining - // - quite fast - // - partitions the heighfield into non-overlapping regions - // - relies on the triangulation code to cope with holes (thus slower than monotone partitioning) - // - produces better triangles than monotone partitioning - // - does not have the corner cases of watershed partitioning - // - can be slow and create a bit ugly tessellation (still better than monotone) - // if you have large open areas with small obstacles (not a problem if you use tiles) - // * good choice to use for tiled navmesh with medium and small sized tiles - - if (m_partitionType == SAMPLE_PARTITION_WATERSHED) - { - // Prepare for region partitioning, by calculating distance field along the walkable surface. - if (!rcBuildDistanceField(m_ctx, *m_chf)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field."); - return false; + 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 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); } - // Partition the walkable surface into simple regions without holes. - if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions."); - return false; - } } - else if (m_partitionType == SAMPLE_PARTITION_MONOTONE) - { - // Partition the walkable surface into simple regions without holes. - // Monotone partitioning does not need distancefield. - if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions."); - return false; - } + + gpc_free_tristrip(&res); + + return std::move(trias); + + } + + private: + + GPCPolygon toGPC(Floorplan::Polygon2 poly) { + + std::vector verts; + for (Point2 p2 : poly.points) { + gpc_vertex vert; vert.x = p2.x; vert.y = p2.y; + verts.push_back(vert); } - else // SAMPLE_PARTITION_LAYERS - { - // Partition the walkable surface into simple regions without holes. - if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions."); - return false; + + 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, + SAMPLE_PARTITION_LAYERS, + }; + + struct TriangleIn { + Point3 p1; + Point3 p2; + Point3 p3; + 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) {;} + }; + + struct TriangleOut { + + Point3 p1; + Point3 p2; + Point3 p3; + + int numNeighbors = 0; + int neighbors[3]; // each triangle has max 3 neighbors + + TriangleOut(const Point3 p1, const Point3 p2, const Point3 p3) : p1(p1), p2(p2), p3(p3), neighbors() {;} + + Point3 center() const { + return (p1+p2+p3) / 3; + } + + }; + + template class NavMeshFactory { + + private: + + float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected + + NavMesh* dst = nullptr; + + std::vector triangles; + + public: + + NavMeshFactory(NavMesh* dst) : dst(dst) { + + } + + void build(Floorplan::IndoorMap* map) { + const BBox3 bbox = FloorplanHelper::getBBox(map); + for (const Floorplan::Floor* floor : map->floors) { + add(floor); + } + fire(bbox); + } + + /** get the smallest obstacle size that can be detected */ + float getMaxQuality_m() const { + return maxQuality_m; + } + + private: + + /** add one floor */ + void add(const Floorplan::Floor* floor) { + + NavMeshPoly nmPoly(floor->atHeight); + + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + if (poly->method == Floorplan::OutlineMethod::ADD) { + nmPoly.add(poly->poly); } } - // - // Step 5. Trace and simplify region contours. - // - - // Create contours. - m_cset = rcAllocContourSet(); - if (!m_cset) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'."); - return false; - } - if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours."); - return false; + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + if (poly->method == Floorplan::OutlineMethod::REMOVE) { + nmPoly.remove(poly->poly); + } } - // - // Step 6. Build polygons mesh from contours. - // - - // Build polygon navmesh from the contours. - m_pmesh = rcAllocPolyMesh(); - if (!m_pmesh) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'."); - return false; - } - if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours."); - return false; + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line != nullptr) { + nmPoly.remove(getPolygon(line)); + } } - // - // Step 7. Create detail mesh which allows to access approximate height on each polygon. - // - - m_dmesh = rcAllocPolyMeshDetail(); - if (!m_dmesh) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'."); - return false; + std::vector> tmp = nmPoly.get(); + for (const std::vector& tria : tmp) { + const TriangleIn t(tria[0], tria[1], tria[2], 1); // TODO outdoor + triangles.push_back(t); } - if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh)) - { - m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh."); - return false; - } - - if (!m_keepInterResults) - { - rcFreeCompactHeightfield(m_chf); - m_chf = 0; - rcFreeContourSet(m_cset); - m_cset = 0; - } - - - std::vector res; - - const float* orig = m_pmesh->bmin; - - // https://github.com/recastnavigation/recastnavigation/blob/master/Docs/Extern/Recast_api.txt - for (int i = 0; i < m_pmesh->npolys; ++i) { - - const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; - - const uint8_t type = m_pmesh->areas[i]; - -// Each entry is 2 * #nvp in length. The first half of the entry -// 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 -// 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.) - -// we only use exactly 3 vertices per polygon, no iteration needed - -// for (int j = 0; j < m_pmesh->nvp; ++j) { -// if (p[j] == RC_MESH_NULL_IDX) {break;} - -// const unsigned short* v = &m_pmesh->verts[p[j]*3]; -// const float x = orig[0] + v[0]*m_pmesh->cs; -// const float z = orig[1] + v[1]*m_pmesh->ch; -// const float y = orig[2] + v[2]*m_pmesh->cs; - -// pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST)); - -// } - - // un-swap Y/Z - const unsigned short* v0 = &m_pmesh->verts[p[0]*3]; - const Point3 p0(orig[0] + v0[0]*m_pmesh->cs, orig[2] + v0[2]*m_pmesh->cs, orig[1] + v0[1]*m_pmesh->ch); - - const unsigned short* v1 = &m_pmesh->verts[p[1]*3]; - const Point3 p1(orig[0] + v1[0]*m_pmesh->cs, orig[2] + v1[2]*m_pmesh->cs, orig[1] + v1[1]*m_pmesh->ch); - - const unsigned short* v2 = &m_pmesh->verts[p[2]*3]; - const Point3 p2(orig[0] + v2[0]*m_pmesh->cs, orig[2] + v2[2]*m_pmesh->cs, orig[1] + v2[1]*m_pmesh->ch); - - dst->add(p0,p1,p2,type); - - } - - // now, connect neighbors - for (int i = 0; i < m_pmesh->npolys; ++i) { - - const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; - - // find all neighbor polygons using their index - for (int j = 0; j < m_pmesh->nvp; ++j) { - int jj = j + m_pmesh->nvp; // offset, 2nd half of the array [size: 2*nvp] - if (p[jj] == RC_MESH_NULL_IDX) {continue;} // no neighbor for the current edge! - const int idx = p[jj]; - dst->connectUniDir(i, idx); + // add all stairs + for (const Floorplan::Stair* stair : floor->stairs) { + const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); + for (const Floorplan::Quad3& quad : quads) { + const TriangleIn t1(quad.p1, quad.p2, quad.p3, 2); // TODO type + const TriangleIn t2(quad.p1, quad.p3, quad.p4, 2); + triangles.push_back(t1); + triangles.push_back(t2); + } } } - return true; + bool fire(BBox3 bbox) { - } + std::vector tData; + std::vector vData; + std::vector typeData; + + // floor outlines + for (const TriangleIn& t : triangles) { + + // swap YZ and polygon order + int startVert = vData.size() / 3; + + // invert triangle ? (CW vs CCW) + // ensure normal points UP + const Point3 norm = cross((t.p2-t.p1), (t.p3-t.p1)); + if (norm.z > 0) { + tData.push_back(startVert + 0); + tData.push_back(startVert + 2); + tData.push_back(startVert + 1); + } else { + tData.push_back(startVert + 0); + tData.push_back(startVert + 1); + tData.push_back(startVert + 2); + } + + typeData.push_back(t.type); + + vData.push_back(t.p1.x); + vData.push_back(t.p1.z); + vData.push_back(t.p1.y); + + vData.push_back(t.p2.x); + vData.push_back(t.p2.z); + vData.push_back(t.p2.y); + + vData.push_back(t.p3.x); + vData.push_back(t.p3.z); + vData.push_back(t.p3.y); + + } + + unsigned char* m_triareas = typeData.data(); + const float* verts = vData.data(); + const int* tris = tData.data(); + + int ntris = tData.size() / 3; + int nverts = vData.size() / 3; -// void dump() { + //unsigned char* m_triareas; + rcHeightfield* m_solid; + rcCompactHeightfield* m_chf; + rcContourSet* m_cset; + rcPolyMesh* m_pmesh; + rcConfig m_cfg; + rcPolyMeshDetail* m_dmesh; + rcContext* m_ctx = new rcContext(); -// std::ofstream out("/tmp/1.dat"); -// for (const std::vector 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)); + 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.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.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; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS -// for (const std::vector 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); -// } + // Init build configuration from GUI + memset(&m_cfg, 0, sizeof(m_cfg)); + m_cfg.cs = m_cellSize; + m_cfg.ch = m_cellHeight; + m_cfg.walkableSlopeAngle = m_agentMaxSlope; + m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch); + m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch); + m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs); + m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize); + m_cfg.maxSimplificationError = m_edgeMaxError; + m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize); // Note: area = size*size + m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize); // Note: area = size*size + m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly; + m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist; + m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError; -// gp.draw(plot); -// gp.flush(); -// sleep(1000); + float bmin[3] = {bbox.getMin().x, bbox.getMin().z, bbox.getMin().y}; + float bmax[3] = {bbox.getMax().x, bbox.getMax().z, bbox.getMax().y};// x/z swapped? -// } + // Set the area where the navigation will be build. + // Here the bounds of the input mesh are used, but the + // area could be specified by an user defined box, etc. + rcVcopy(m_cfg.bmin, bmin); + rcVcopy(m_cfg.bmax, bmax); + rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height); - /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ - static Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) { - //const Line2 base(line->from*100, line->to*100); - const float thickness_m = line->thickness_m; - 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 - const Point2 p2 = line->from - perp * thickness_m/2; // start-down - const Point2 p3 = line->to + perp * thickness_m/2; // end-up - const Point2 p4 = line->to - perp * thickness_m/2; // end-down - Floorplan::Polygon2 res; - res.points.push_back(p1); - res.points.push_back(p2); - res.points.push_back(p4); - res.points.push_back(p3); - return res; - } + // Reset build times gathering. + m_ctx->resetTimers(); -}; + // Start the build process. + m_ctx->startTimer(RC_TIMER_TOTAL); + + m_ctx->log(RC_LOG_PROGRESS, "Building navigation:"); + m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height); + m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f); + + // + // Step 2. Rasterize input polygon soup. + // + + // Allocate voxel heightfield where we rasterize our input data to. + m_solid = rcAllocHeightfield(); + if (!m_solid) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'."); + return false; + } + if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield."); + return false; + } + + // Allocate array that can hold triangle area types. + // If you have multiple meshes you need to process, allocate + // and array which can hold the max number of triangles you need to process. + // m_triareas = new unsigned char[ntris]; + // if (!m_triareas) + // { + // m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris); + // return false; + // } + + // 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 + // the are type for each of the meshes and rasterize them. + //memset(m_triareas, 0, ntris*sizeof(unsigned char)); + //rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas); + if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles."); + return false; + } + + bool m_keepInterResults = false; + bool m_filterLowHangingObstacles = false; + bool m_filterLedgeSpans = false; + bool m_filterWalkableLowHeightSpans = false; + + // std::vector! + // if (!m_keepInterResults) + // { + // delete [] m_triareas; + // m_triareas = 0; + // } + + // + // Step 3. Filter walkables surfaces. + // + + + + // Once all geoemtry is rasterized, we do initial pass of filtering to + // remove unwanted overhangs caused by the conservative rasterization + // as well as filter spans where the character cannot possibly stand. + if (m_filterLowHangingObstacles) + rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid); + if (m_filterLedgeSpans) + rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid); + if (m_filterWalkableLowHeightSpans) + rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid); + + + // + // Step 4. Partition walkable surface to simple regions. + // + + // Compact the heightfield so that it is faster to handle from now on. + // This will result more cache coherent data as well as the neighbours + // between walkable cells will be calculated. + m_chf = rcAllocCompactHeightfield(); + if (!m_chf) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'."); + return false; + } + if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data."); + return false; + } + + if (!m_keepInterResults) + { + rcFreeHeightField(m_solid); + m_solid = 0; + } + + // Erode the walkable area by agent radius. + if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode."); + return false; + } + + // (Optional) Mark areas. + // const ConvexVolume* vols = m_geom->getConvexVolumes(); + // for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i) + // rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf); + + + // Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas. + // There are 3 martitioning methods, each with some pros and cons: + // 1) Watershed partitioning + // - the classic Recast partitioning + // - creates the nicest tessellation + // - usually slowest + // - partitions the heightfield into nice regions without holes or overlaps + // - the are some corner cases where this method creates produces holes and overlaps + // - holes may appear when a small obstacles is close to large open area (triangulation can handle this) + // - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail + // * generally the best choice if you precompute the nacmesh, use this if you have large open areas + // 2) Monotone partioning + // - fastest + // - partitions the heightfield into regions without holes and overlaps (guaranteed) + // - creates long thin polygons, which sometimes causes paths with detours + // * use this if you want fast navmesh generation + // 3) Layer partitoining + // - quite fast + // - partitions the heighfield into non-overlapping regions + // - relies on the triangulation code to cope with holes (thus slower than monotone partitioning) + // - produces better triangles than monotone partitioning + // - does not have the corner cases of watershed partitioning + // - can be slow and create a bit ugly tessellation (still better than monotone) + // if you have large open areas with small obstacles (not a problem if you use tiles) + // * good choice to use for tiled navmesh with medium and small sized tiles + + if (m_partitionType == SAMPLE_PARTITION_WATERSHED) + { + // Prepare for region partitioning, by calculating distance field along the walkable surface. + if (!rcBuildDistanceField(m_ctx, *m_chf)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field."); + return false; + } + + // Partition the walkable surface into simple regions without holes. + if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions."); + return false; + } + } + else if (m_partitionType == SAMPLE_PARTITION_MONOTONE) + { + // Partition the walkable surface into simple regions without holes. + // Monotone partitioning does not need distancefield. + if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions."); + return false; + } + } + else // SAMPLE_PARTITION_LAYERS + { + // Partition the walkable surface into simple regions without holes. + if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions."); + return false; + } + } + + // + // Step 5. Trace and simplify region contours. + // + + // Create contours. + m_cset = rcAllocContourSet(); + if (!m_cset) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'."); + return false; + } + if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours."); + return false; + } + + // + // Step 6. Build polygons mesh from contours. + // + + // Build polygon navmesh from the contours. + m_pmesh = rcAllocPolyMesh(); + if (!m_pmesh) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'."); + return false; + } + if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours."); + return false; + } + + // + // Step 7. Create detail mesh which allows to access approximate height on each polygon. + // + + m_dmesh = rcAllocPolyMeshDetail(); + if (!m_dmesh) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'."); + return false; + } + + if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh."); + return false; + } + + if (!m_keepInterResults) + { + rcFreeCompactHeightfield(m_chf); + m_chf = 0; + rcFreeContourSet(m_cset); + m_cset = 0; + } + + + std::vector res; + + const float* orig = m_pmesh->bmin; + + // https://github.com/recastnavigation/recastnavigation/blob/master/Docs/Extern/Recast_api.txt + for (int i = 0; i < m_pmesh->npolys; ++i) { + + const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; + + const uint8_t type = m_pmesh->areas[i]; + + // Each entry is 2 * #nvp in length. The first half of the entry + // 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 + // 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.) + + // we only use exactly 3 vertices per polygon, no iteration needed + + // for (int j = 0; j < m_pmesh->nvp; ++j) { + // if (p[j] == RC_MESH_NULL_IDX) {break;} + + // const unsigned short* v = &m_pmesh->verts[p[j]*3]; + // const float x = orig[0] + v[0]*m_pmesh->cs; + // const float z = orig[1] + v[1]*m_pmesh->ch; + // const float y = orig[2] + v[2]*m_pmesh->cs; + + // pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST)); + + // } + + // un-swap Y/Z + const unsigned short* v0 = &m_pmesh->verts[p[0]*3]; + const Point3 p0(orig[0] + v0[0]*m_pmesh->cs, orig[2] + v0[2]*m_pmesh->cs, orig[1] + v0[1]*m_pmesh->ch); + + const unsigned short* v1 = &m_pmesh->verts[p[1]*3]; + const Point3 p1(orig[0] + v1[0]*m_pmesh->cs, orig[2] + v1[2]*m_pmesh->cs, orig[1] + v1[1]*m_pmesh->ch); + + const unsigned short* v2 = &m_pmesh->verts[p[2]*3]; + const Point3 p2(orig[0] + v2[0]*m_pmesh->cs, orig[2] + v2[2]*m_pmesh->cs, orig[1] + v2[1]*m_pmesh->ch); + + dst->add(p0,p1,p2,type); + + } + + // now, connect neighbors + for (int i = 0; i < m_pmesh->npolys; ++i) { + + const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; + + // find all neighbor polygons using their index + for (int j = 0; j < m_pmesh->nvp; ++j) { + int jj = j + m_pmesh->nvp; // offset, 2nd half of the array [size: 2*nvp] + if (p[jj] == RC_MESH_NULL_IDX) {continue;} // no neighbor for the current edge! + const int idx = p[jj]; + dst->connectUniDir(i, idx); + } + + } + + return true; + + } + + + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const { + //const Line2 base(line->from*100, line->to*100); + 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 + const Point2 p2 = line->from - perp * thickness_m/2; // start-down + const Point2 p3 = line->to + perp * thickness_m/2; // end-up + const Point2 p4 = line->to - perp * thickness_m/2; // end-down + Floorplan::Polygon2 res; + res.points.push_back(p1); + res.points.push_back(p2); + res.points.push_back(p4); + res.points.push_back(p3); + return res; + } + + }; + +} #endif diff --git a/navMesh/NavMeshLocation.h b/navMesh/NavMeshLocation.h index 51d8fb9..1ed7c60 100644 --- a/navMesh/NavMeshLocation.h +++ b/navMesh/NavMeshLocation.h @@ -3,17 +3,35 @@ #include "../geo/Point3.h" -template 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 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 diff --git a/navMesh/NavMeshPoly.h b/navMesh/NavMeshPoly.h deleted file mode 100644 index 014338a..0000000 --- a/navMesh/NavMeshPoly.h +++ /dev/null @@ -1,123 +0,0 @@ -#ifndef POLYGON_H -#define POLYGON_H - -#include -#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> 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> 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 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 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 diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h index 32cc38e..b448cf2 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -5,46 +5,66 @@ #include #include "../math/DrawList.h" #include "../geo/Point3.h" + #include "NavMeshLocation.h" -template class NavMeshRandom { +namespace NM { - std::minstd_rand gen; - std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); - const std::vector& triangles; - DrawList lst; + /** + * randomly pick points within the area of the nav-mesh. + * points are picked evenly: + * bigger triangles are used more often + * + */ + template class NavMeshRandom { -public: + DrawList lst; + std::minstd_rand gen; + std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); + std::vector triangles; + + + uint32_t nextSeed() { + static uint32_t seed = 0; + return ++seed; + } + + public: + + /** ctor (const/non-const using T) */ + template NavMeshRandom(const std::vector& 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 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(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& 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 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(pos, tria); - } - - } - -}; +} #endif // NAVMESHRANDOM_H diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index a91ccb0..ce5d129 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -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 friend class NavMesh; - template 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 diff --git a/navMesh/walk/NavMeshSub.h b/navMesh/walk/NavMeshSub.h index 90fc789..57aa0e5 100644 --- a/navMesh/walk/NavMeshSub.h +++ b/navMesh/walk/NavMeshSub.h @@ -3,54 +3,80 @@ #include "../NavMesh.h" #include "../NavMeshLocation.h" +#include "../NavMeshRandom.h" #include #include +namespace NM { -template class NavMeshSub { + template class NavMeshSub { - std::vector toVisit; + std::vector toVisit; -public: + public: - NavMeshSub(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { - build(nm,loc,radius_m); - } + NavMeshSub(const NavMeshLocation& 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& nm, const NavMeshLocation& 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 getRandom() { + return NavMeshRandom(toVisit); + } - toVisit.push_back(loc.tria); + private: - std::unordered_set visited; + void build(const NavMeshLocation& loc, float radius_m) { - size_t next = 0; - while (next < toVisit.size()) { + std::unordered_set 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 diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h new file mode 100644 index 0000000..e9e79ef --- /dev/null +++ b/navMesh/walk/NavMeshWalkEval.h @@ -0,0 +1,103 @@ +#ifndef NAVMESHWALKEVAL_H +#define NAVMESHWALKEVAL_H + +#include "NavMeshWalkParams.h" +#include "../NavMeshLocation.h" +#include "../../math/Distributions.h" + +namespace NM { + + template struct NavMeshPotentialWalk { + + NavMeshWalkParams requested; + + NavMeshLocation end; + + NavMeshPotentialWalk(const NavMeshWalkParams& requested, const NavMeshLocation& end) : requested(requested), end(end) { + ; + } + + }; + + /** + * evaluate a NavMeshWalk from -> to = probability + */ + template class NavMeshWalkEval { + + public: + + virtual double getProbability(const NavMeshPotentialWalk& walk) const = 0; + + }; + + + + + + + + /** + * evaluate the difference between head(start,end) and the requested heading + */ + template class WalkEvalHeadingStartEnd : public NavMeshWalkEval { + + const double sigma_rad; + const double kappa; + Distribution::VonMises _dist; + Distribution::LUT 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& 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::getProbability(0, sigma, diff); + return dist.getProbability(diff); + + } + + }; + + /** + * evaluate the difference between distance(start, end) and the requested distance + */ + template class WalkEvalDistance : public NavMeshWalkEval { + + const double sigma; + + const Distribution::Normal dist; + + public: + + WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} + + virtual double getProbability(const NavMeshPotentialWalk& 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::getProbability(params.distance_m, sigma, walkedDistance_m); + + } + + }; + + +} + +#endif // NAVMESHWALKEVAL_H diff --git a/navMesh/walk/NavMeshWalkHelper.h b/navMesh/walk/NavMeshWalkHelper.h deleted file mode 100644 index 65cc4fb..0000000 --- a/navMesh/walk/NavMeshWalkHelper.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef NAVMESHWALKHELPER_H -#define NAVMESHWALKHELPER_H - -template class NavMeshWalkHelper { - - - -} - -#endif // NAVMESHWALKHELPER_H diff --git a/navMesh/walk/NavMeshWalkParams.h b/navMesh/walk/NavMeshWalkParams.h new file mode 100644 index 0000000..1e8d8f0 --- /dev/null +++ b/navMesh/walk/NavMeshWalkParams.h @@ -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 float inMeter(const int steps, const NavMeshLocation& 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 struct NavMeshWalkParams { + + /** walk starts here (pos/tria) */ + NavMeshLocation 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 diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index 2f197ec..86f49a9 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -2,42 +2,105 @@ #define NAVMESHWALKSIMPLE_H #include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" -template class NavMeshWalkSimpel { +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" -private: +namespace NM { - const NavMesh& mesh; + template class NavMeshWalkSimple { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + int hits = 0; + int misses = 0; + + public: + + + struct Result { + + NavMeshLocation location; + Heading heading; + double probability; + + Result() : heading(0) {;} + + }; + + public: + + /** ctor */ + NavMeshWalkSimple(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + Result getDestination(const NavMeshWalkParams& 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 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 rnd = reachable.getRandom(); + NavMeshLocation 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 pwalk(params, res.location); + res.probability = 1.0; + for (const NavMeshWalkEval* 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& mesh) : mesh(mesh) { - - } - - Result walk(const Params& params) { - - } - - } #endif // NAVMESHWALKSIMPLE_H diff --git a/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp index cf8928a..4362f60 100644 --- a/tests/navMesh/TestNavMeshFactory.cpp +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -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 nm; - NavMeshFactory fac(&nm); + NavMesh nm; + NavMeshFactory fac(&nm); fac.build(&map); ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5); diff --git a/tests/navMesh/TestNavMeshSub.cpp b/tests/navMesh/TestNavMeshSub.cpp index 9b3f4ab..2309cb5 100644 --- a/tests/navMesh/TestNavMeshSub.cpp +++ b/tests/navMesh/TestNavMeshSub.cpp @@ -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 nm; - NavMeshFactory fac(&nm); + NavMesh nm; + NavMeshFactory fac(&nm); fac.build(&map); - NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); + NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); } diff --git a/tests/navMesh/TestNavMeshTriangle.cpp b/tests/navMesh/TestNavMeshTriangle.cpp index 10027c3..2051bfe 100644 --- a/tests/navMesh/TestNavMeshTriangle.cpp +++ b/tests/navMesh/TestNavMeshTriangle.cpp @@ -3,6 +3,7 @@ #include "../Tests.h" #include "../../navMesh/NavMeshTriangle.h" +using namespace NM; TEST(NavMeshTriangle, contains) {