diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index d03f30d..8844d85 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -130,6 +130,10 @@ namespace Floorplan { default: throw Exception("out of bounds"); } } + /** same z-value for all points? */ + bool isLeveled() const { + return (p1.z == p2.z) && (p2.z == p3.z) && (p3.z == p4.z); + } }; /** additional type-info for obstacles */ @@ -202,6 +206,7 @@ namespace Floorplan { /** describes one floor within the map, starting at a given height */ struct Floor { + bool enabled = true; float atHeight; // the floor's starting height float height; // the floor's total height (from start) std::string name; // the floor's name diff --git a/geo/Angle.h b/geo/Angle.h index eabe999..a568450 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -4,6 +4,7 @@ #include #include "../Assertions.h" #include "Point2.h" +#include "../math/speed.h" #define PI ((float) M_PI) diff --git a/lib/gpc/gpc.cpp.h b/lib/gpc/gpc.cpp.h index 1f8371f..1dc913e 100755 --- a/lib/gpc/gpc.cpp.h +++ b/lib/gpc/gpc.cpp.h @@ -1017,7 +1017,7 @@ static void minimax_test(gpc_polygon *subj, gpc_polygon *clip, gpc_op op) =========================================================================== */ -void gpc_free_polygon(gpc_polygon *p) +inline void gpc_free_polygon(gpc_polygon *p) { int c; @@ -1029,7 +1029,7 @@ void gpc_free_polygon(gpc_polygon *p) } -void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) +inline void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) { int c, v; @@ -1056,7 +1056,7 @@ void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) } -void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) +inline void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) { int c, v; @@ -1076,7 +1076,7 @@ void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) } -void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) +inline void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) { int *extended_hole, c, v; gpc_vertex_list *extended_contour; @@ -1116,7 +1116,7 @@ void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) } -void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, +inline void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, gpc_polygon *result) { sb_tree *sbtree= NULL; @@ -1754,7 +1754,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, } -void gpc_free_tristrip(gpc_tristrip *t) +inline void gpc_free_tristrip(gpc_tristrip *t) { int s; @@ -1765,7 +1765,7 @@ void gpc_free_tristrip(gpc_tristrip *t) } -void gpc_polygon_to_tristrip(gpc_polygon *s, gpc_tristrip *t) +inline void gpc_polygon_to_tristrip(gpc_polygon *s, gpc_tristrip *t) { gpc_polygon c; @@ -1776,7 +1776,7 @@ void gpc_polygon_to_tristrip(gpc_polygon *s, gpc_tristrip *t) } -void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, +inline void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, gpc_tristrip *result) { sb_tree *sbtree= NULL; diff --git a/math/speed.h b/math/speed.h new file mode 100644 index 0000000..9b32577 --- /dev/null +++ b/math/speed.h @@ -0,0 +1,63 @@ +#ifndef SPEED_H +#define SPEED_H + +#include + +class Speed { + +public: + +#define PI_FLOAT 3.14159265f +#define PIBY2_FLOAT 1.5707963f + + + static inline float atan2(float y, float x) { + + //http://pubs.opengroup.org/onlinepubs/009695399/functions/atan2.html + //Volkan SALMA + + const float ONEQTR_PI = M_PI / 4.0; + const float THRQTR_PI = 3.0 * M_PI / 4.0; + float r, angle; + float abs_y = fabs(y) + 1e-10f; // kludge to prevent 0/0 condition + if ( x < 0.0f ) { + r = (x + abs_y) / (abs_y - x); + angle = THRQTR_PI; + } else { + r = (x - abs_y) / (x + abs_y); + angle = ONEQTR_PI; + } + angle += (0.1963f * r * r - 0.9817f) * r; + if ( y < 0.0f ) + return( -angle ); // negate if in quad III or IV + else + return( angle ); + + } + +// // https://gist.github.com/volkansalma/2972237 +// static inline float atan2(const float y, const float x) { +// if ( x == 0.0f ) { +// if ( y > 0.0f ) return PIBY2_FLOAT; +// if ( y == 0.0f ) return 0.0f; +// return -PIBY2_FLOAT; +// } +// float atan; +// float z = y/x; +// if ( fabs( z ) < 1.0f ) { +// atan = z/(1.0f + 0.28f*z*z); +// if ( x < 0.0f ) { +// if ( y < 0.0f ) return atan - PI_FLOAT; +// return atan + PI_FLOAT; +// } +// } else { +// atan = PIBY2_FLOAT - z/(z*z + 0.28f); +// if ( y < 0.0f ) return atan - PI_FLOAT; +// } +// return atan; +// } + + +}; + +#endif // SPEED_H diff --git a/misc/PerfCheck.h b/misc/PerfCheck.h new file mode 100644 index 0000000..a72e455 --- /dev/null +++ b/misc/PerfCheck.h @@ -0,0 +1,130 @@ +#ifndef PERFCHECK_H +#define PERFCHECK_H + + +//#define WITH_PERF_CHECK + +#ifdef WITH_PERF_CHECK + + #include + #include + #include + + class PerfCheck { + + struct Stats { + size_t calls = 0; + clock_t time = 0; + }; + + uint32_t name; + clock_t in; + + static Stats stats[1024]; + + public: + + /** ctor */ + explicit PerfCheck(const uint32_t name) : name(name), in(clock()) { + ; + } + + /** dtor */ + ~PerfCheck() { + stats[name].calls += 1; + stats[name].time += clock() - in; + } + + PerfCheck(PerfCheck const&) = delete; + + PerfCheck& operator = (PerfCheck const&) = delete; + + static void dump() { + for (int i = 0; i < 1024; ++i) { + const Stats& s = stats[i]; + if (s.calls != 0) { + std::cout << i << ":\t"; + std::cout << "\tcalls: " << s.calls; + std::cout << "\ttime: " << s.time; + std::cout << "\ttime/call: " << ((double)s.time / (double)s.calls); + std::cout << std::endl; + } + } + } + + + + // static inline Stats* map() { + // static Stats stats[1024];// = new Stats[1024]; + // return stats; + // } + + }; + + PerfCheck::Stats PerfCheck::stats[1024]; + + #define PERF_REGION(idx, name) PerfCheck pcr_idx(idx) + #define PERF_DUMP() PerfCheck::dump(); + +#else + + #define PERF_REGION(idx, name) + #define PERF_DUMP() + +#endif + + + +/* +class PerfCheck { + + std::string name; + clock_t in; + +public: + + explicit PerfCheck(const std::string& name) : name(name), in(clock()) { + ; + } + + ~PerfCheck() { + const clock_t diff = (clock() - in); + add(name, diff); + } + + PerfCheck(PerfCheck const&) = delete; + + PerfCheck& operator = (PerfCheck const&) = delete; + + static void dump() { + for (const auto& it : map()) { + std::cout << it.first << ":\t"; + std::cout << "\tcalls: " << it.second.calls; + std::cout << "\ttime: " << it.second.time; + std::cout << "\ttime/call: " << ((double)it.second.time / (double)it.second.calls); + std::cout << std::endl; + } + } + +private: + + struct Stats { + size_t calls = 0; + clock_t time = 0; + }; + + static std::unordered_map& map() { + static std::unordered_map stats; + return stats; + } + + void add(const std::string& name, const clock_t diff) { + std::unordered_map& m = map(); + m[name].calls += 1; + m[name].time += diff; + } + +}; +*/ + +#endif // PERFCHECK_H diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h index 682add0..deedec0 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -7,6 +7,7 @@ #include #include #include +#include #include namespace NM { @@ -18,27 +19,61 @@ namespace NM { public: - template static void show(NavMesh& nm) { + K::Gnuplot gp; + K::GnuplotSplot plot; + K::GnuplotSplotElementLines lines; + K::GnuplotSplotElementPoints border; + K::GnuplotSplotElementColorPoints particles; + K::GnuplotSplotElementLines pathEstimated; - 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) - }; + private: + + K::GnuplotFill gFill[6] = { + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed + }; + + public: + + NavMeshDebug() { + gp << "set view equal xy\n"; + plot.add(&lines); lines.setShowPoints(true); + plot.add(&border); + plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2); + plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00"); + + } + + void draw() { + gp.draw(plot); + gp.flush(); + } + + template void showParticles(const std::vector& particles) { + this->particles.clear(); + double min = +999; + double max = -999; + for (const T& p : particles) { + const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z); + const double prob = std::pow(p.weight, 0.25); + this->particles.add(p3, prob); + if (prob > max) {max = prob;} + if (prob < min) {min = prob;} + } + plot.getAxisCB().setRange(min, max + 0.000001); + } + template void addMesh(NavMesh& nm) { K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); - K::Gnuplot gp; - gp << "set view equal xy\n"; - - K::GnuplotSplot plot; - K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true); - K::GnuplotSplotElementPoints points; plot.add(&points); - const BBox3 bbox = nm.getBBox(); - 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)); + border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z)); + border.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)); @@ -47,7 +82,7 @@ namespace NM { for (const Tria* tria : nm) { const uint8_t type = tria->getType(); - if (type < 0 || type > 2) { + if (type < 0 || type > 5) { throw std::runtime_error("out of type-bounds"); } K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke); @@ -75,10 +110,15 @@ namespace NM { plot.getObjects().reOrderByZIndex(); - gp.draw(plot); - gp.flush(); - sleep(1); + } + + void setGT(const Point3 pt) { + gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n"; + } + + void setCurPos(const Point3 pt) { + gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n"; } }; diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 7a5e282..3a03ee3 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -6,10 +6,14 @@ #include "NavMesh.h" #include "NavMeshTriangle.h" +#include "NavMeshFactoryListener.h" +#include "NavMeshType.h" +#include "NavMeshSettings.h" #include "../lib/gpc/gpc.cpp.h" #include "../lib/Recast/Recast.h" + namespace NM { @@ -119,11 +123,7 @@ namespace NM { - enum SamplePartitionType { - SAMPLE_PARTITION_WATERSHED, - SAMPLE_PARTITION_MONOTONE, - SAMPLE_PARTITION_LAYERS, - }; + struct TriangleIn { Point3 p1; @@ -150,86 +150,172 @@ namespace NM { }; +#define NMF_STEPS 8 + template class NavMeshFactory { private: - float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected - NavMesh* dst = nullptr; + const NavMeshSettings& settings; + std::vector triangles; public: - NavMeshFactory(NavMesh* dst) : dst(dst) { + NavMeshFactory(NavMesh* dst, const NavMeshSettings& settings) : dst(dst), settings(settings) { } - void build(Floorplan::IndoorMap* map) { + void build(Floorplan::IndoorMap* map, NavMeshFactoryListener* listener = nullptr) { + if (listener) {listener->onNavMeshBuildUpdateMajor("preparing");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 0);} const BBox3 bbox = FloorplanHelper::getBBox(map); for (const Floorplan::Floor* floor : map->floors) { add(floor); } - fire(bbox); + fire(bbox, listener); } - /** get the smallest obstacle size that can be detected */ - float getMaxQuality_m() const { - return maxQuality_m; - } +// /** 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); + if (!floor->enabled) {return;} +// NavMeshPoly nmPoly(floor->atHeight); + +// for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { +// if (poly->method == Floorplan::OutlineMethod::ADD) { +// nmPoly.add(poly->poly); +// } +// } + +// for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { +// if (poly->method == Floorplan::OutlineMethod::REMOVE) { +// nmPoly.remove(poly->poly); +// } +// } + +// for (Floorplan::FloorObstacle* obs : floor->obstacles) { +// Floorplan::FloorObstacleLine* line = dynamic_cast(obs); +// if (line != nullptr) { +// nmPoly.remove(getPolygon(line)); +// } +// } + +// 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); +// } + + + + // we need this strange loop, as we need to distinguish between indoor and outdoor regions/polygons + // adding all "add" polygons first and removing "remove" polygons / obstacles afterwards is more performant + // but does not allow for tagging the "add" polygons (indoor/outdoor/...) + // thats why we have to tread each "add" polygon on its own (and remove all potential elements from it) for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + + // if this is a to-be-added polygon, add it if (poly->method == Floorplan::OutlineMethod::ADD) { + + NavMeshPoly nmPoly(floor->atHeight); nmPoly.add(poly->poly); - } - } - for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { - if (poly->method == Floorplan::OutlineMethod::REMOVE) { - nmPoly.remove(poly->poly); - } - } + // get all other polygons of this floor, that are tagged as "remove" and remove them (many will be outside of the added polygon) + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + if (poly->method == Floorplan::OutlineMethod::REMOVE) { + nmPoly.remove(poly->poly); + } + } - for (Floorplan::FloorObstacle* obs : floor->obstacles) { - Floorplan::FloorObstacleLine* line = dynamic_cast(obs); - if (line != nullptr) { - nmPoly.remove(getPolygon(line)); - } - } + // get all obstacles of this floor and remove them from the polygon as well (many will be outside of the added polygon) + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line != nullptr) { + nmPoly.remove(getPolygon(line)); + } + } + + // construct and add + std::vector> tmp = nmPoly.get(); + int type = poly->outdoor ? (int) NavMeshType::FLOOR_OUTDOOR : (int) NavMeshType::FLOOR_INDOOR; + for (const std::vector& tria : tmp) { + const TriangleIn t(tria[0], tria[1], tria[2], type); + triangles.push_back(t); + } + + } - 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); } // add all stairs + // those must be DIRECTLY connected to the ending floor (stair's ending edge connected to an edge of the floor) + // otherwise the stair ends UNDER a floor polygon and is thus not added (higher polygons always win) for (const Floorplan::Stair* stair : floor->stairs) { - const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); + const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); // slightly grow to ensure connection?! 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); + + // stair has two options: either leveled parts (no steps) and skewed parts (steps) + // as those affect the pedestrian's step-length, we tag them differently + const int type = quad.isLeveled() ? (int) NavMeshType::STAIR_LEVELED : (int) NavMeshType::STAIR_SKEWED; + const TriangleIn t1(quad.p1, quad.p2, quad.p3, type); + const TriangleIn t2(quad.p1, quad.p3, quad.p4, type); triangles.push_back(t1); triangles.push_back(t2); + + // sanity check. should never happen. just to be ultra sure + const Point3 norm1 = cross((t1.p2-t1.p1), (t1.p3-t1.p1)); + const Point3 norm2 = cross((t2.p2-t2.p1), (t2.p3-t2.p1)); + Assert::isTrue(norm1.z > 0, "detected invalid culling for stair-quad. normal points downwards"); + Assert::isTrue(norm2.z > 0, "detected invalid culling for stair-quad. normal points downwards"); + } } + // finally create additional triangles for the doors to tag doors differently (tagging also seems to improve the triangulation result) + // note: door-regions are already walkable as doors are NOT removed from the outline + // however: adding them again here seems to work.. triangles at the end of the list seem to overwrite (tagging) previous ones -> fine + { + + // add (overlay) all doors for tagging them within the plan + NavMeshPoly nmDoors(floor->atHeight); + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleDoor* door = dynamic_cast(obs); + if (door != nullptr) { + nmDoors.add(getPolygon(door)); + } + } + + // construct and add triangles + std::vector> tmp = nmDoors.get(); + for (const std::vector& tria : tmp) { + const TriangleIn t(tria[0], tria[1], tria[2], (int) NavMeshType::DOOR); + triangles.push_back(t); + } + + } + } - bool fire(BBox3 bbox) { + bool fire(BBox3 bbox, NavMeshFactoryListener* listener) { std::vector tData; std::vector vData; std::vector typeData; + if (listener) {listener->onNavMeshBuildUpdateMajor("building polygons");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 1);} + // floor outlines for (const TriangleIn& t : triangles) { @@ -282,37 +368,37 @@ namespace NM { rcPolyMeshDetail* m_dmesh; rcContext* m_ctx = new rcContext(); - 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 +// 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 = 1.8f; +// 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 // 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; + m_cfg.cs = settings.getCellSizeXY(); + m_cfg.ch = settings.getCellSizeZ(); + m_cfg.walkableSlopeAngle = settings.agentMaxSlope; + m_cfg.walkableHeight = (int)ceilf(settings.agentHeight / m_cfg.ch); + m_cfg.walkableClimb = (int)floorf(settings.getMaxClimb() / m_cfg.ch); + m_cfg.walkableRadius = (int)ceilf(settings.agentRadius / m_cfg.cs); + m_cfg.maxEdgeLen = (int)(settings.edgeMaxLen / settings.getCellSizeXY()); + m_cfg.maxSimplificationError = settings.edgeMaxError; + m_cfg.minRegionArea = (int)rcSqr(settings.regionMinSize); // Note: area = size*size + m_cfg.mergeRegionArea = (int)rcSqr(settings.regionMergeSize); // Note: area = size*size + m_cfg.maxVertsPerPoly = settings.vertsPerPoly; + m_cfg.detailSampleDist = settings.detailSampleDist < 0.9f ? 0 : settings.getCellSizeXY() * settings.detailSampleDist; + m_cfg.detailSampleMaxError = settings.getCellSizeZ() * settings.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? @@ -338,15 +424,16 @@ namespace NM { // Step 2. Rasterize input polygon soup. // + if (listener) {listener->onNavMeshBuildUpdateMajor("rasterizing polygons");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 2);} + // Allocate voxel heightfield where we rasterize our input data to. m_solid = rcAllocHeightfield(); - if (!m_solid) - { + 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)) - { + 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; } @@ -366,8 +453,7 @@ namespace NM { // 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)) - { + 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; } @@ -377,18 +463,9 @@ namespace NM { bool m_filterLedgeSpans = false; bool m_filterWalkableLowHeightSpans = false; - // std::vector! - // if (!m_keepInterResults) - // { - // delete [] m_triareas; - // m_triareas = 0; - // } - - // // Step 3. Filter walkables surfaces. - // - - + if (listener) {listener->onNavMeshBuildUpdateMajor("filtering");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 3);} // Once all geoemtry is rasterized, we do initial pass of filtering to // remove unwanted overhangs caused by the conservative rasterization @@ -401,30 +478,27 @@ namespace NM { rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid); - // // Step 4. Partition walkable surface to simple regions. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("partitioning");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 4);} // 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) - { + 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)) - { + 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) - { + //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)) @@ -465,103 +539,107 @@ namespace NM { // 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) - { + + switch (settings.partitionType) { + + case SamplePartitionType::SAMPLE_PARTITION_WATERSHED: + // Prepare for region partitioning, by calculating distance field along the walkable surface. - if (!rcBuildDistanceField(m_ctx, *m_chf)) - { + 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)) - { + 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) - { + break; + + case SamplePartitionType::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)) - { + 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 - { + + break; + + case SamplePartitionType::SAMPLE_PARTITION_LAYERS: + // Partition the walkable surface into simple regions without holes. - if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) - { + if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions."); return false; } + + break; + + default: + + throw Exception("unsupported SamplePartitionType"); + } - // // Step 5. Trace and simplify region contours. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("tracing");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 5);} // Create contours. m_cset = rcAllocContourSet(); - if (!m_cset) - { + 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)) - { + 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. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("building triangles");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 6);} // Build polygon navmesh from the contours. m_pmesh = rcAllocPolyMesh(); - if (!m_pmesh) - { + 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)) - { + 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. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("building details");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 7);} m_dmesh = rcAllocPolyMeshDetail(); - if (!m_dmesh) - { + 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)) - { + 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) - { + //if (!m_keepInterResults) { rcFreeCompactHeightfield(m_chf); m_chf = 0; rcFreeContourSet(m_cset); m_cset = 0; - } + //} - std::vector res; + // std::vector res; const float* orig = m_pmesh->bmin; @@ -628,8 +706,7 @@ namespace NM { /** 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 float thickness_m = std::max(line->thickness_m, settings.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 @@ -644,6 +721,23 @@ namespace NM { return res; } + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleDoor* door) const { + const float thickness_m = std::max(0.3f, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected) + const Point2 dir = (door->to - door->from); // obstacle's direction + const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) + const Point2 p1 = door->from + perp * thickness_m/2; // start-up + const Point2 p2 = door->from - perp * thickness_m/2; // start-down + const Point2 p3 = door->to + perp * thickness_m/2; // end-up + const Point2 p4 = door->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; + } + }; } diff --git a/navMesh/NavMeshFactoryListener.h b/navMesh/NavMeshFactoryListener.h new file mode 100644 index 0000000..986ea45 --- /dev/null +++ b/navMesh/NavMeshFactoryListener.h @@ -0,0 +1,20 @@ +#ifndef NAVMESHFACTORYLISTENER_H +#define NAVMESHFACTORYLISTENER_H + +#include + +namespace NM { + + /** listen for events during the build process */ + class NavMeshFactoryListener { + + public: + + virtual void onNavMeshBuildUpdateMajor(const std::string& what) = 0; + virtual void onNavMeshBuildUpdateMajor(const int cnt, const int cur) = 0; + + }; + +} + +#endif // NAVMESHFACTORYLISTENER_H diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h index b448cf2..391ed5c 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -5,6 +5,7 @@ #include #include "../math/DrawList.h" #include "../geo/Point3.h" +#include "../misc/PerfCheck.h" #include "NavMeshLocation.h" @@ -21,6 +22,7 @@ namespace NM { DrawList lst; std::minstd_rand gen; std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); + std::uniform_real_distribution dHeading = std::uniform_real_distribution(0, M_PI*2); std::vector triangles; @@ -34,8 +36,8 @@ namespace NM { /** ctor (const/non-const using T) */ template NavMeshRandom(const std::vector& srcTriangles) : lst(nextSeed()), gen(nextSeed()) { - // almost always the same number?! - gen(); + // 1st = almost always the same number?! + gen(); gen(); // construct a DrawList (probability = size[area] of the triangle // bigger triangles must be choosen more often @@ -49,16 +51,56 @@ namespace NM { /** draw a random point */ NavMeshLocation draw() { + PERF_REGION(3, "NavMeshRandom::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); + // get random (u,v) on triangle + float u = dOnTriangle(gen); + float v = dOnTriangle(gen); + + // if the (u,v) is outside of the triangle, mirror it so its inside the triangle again + if ((u+v) > 1) { + u = 1.0f - u; + v = 1.0f - v; + } + + // done + const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v); + return NavMeshLocation(pos, tria); + + } + + /** draw a random location within the given radius */ + NavMeshLocation drawWithin(const Point3 center, const float radius) { + + std::uniform_real_distribution dDistance(0.001, radius); + + while(true) { + const float head = dHeading(gen); + const float dist = dDistance(gen); + + const float ox = std::cos(head) * dist; + const float oy = std::sin(head) * dist; + + // 2D destination (ignore z) + const Point2 dst(center.x + ox, center.y + oy); + + for (const Tria* t : triangles) { + + // if triangle contains 2D position + if (t->contains(dst)) { + + // convert it to a 3D position + const Point3 p3 = t->toPoint3(dst); + const NavMeshLocation loc(p3, t); + return loc; + + } + } + } } diff --git a/navMesh/NavMeshSettings.h b/navMesh/NavMeshSettings.h new file mode 100644 index 0000000..1ef3de7 --- /dev/null +++ b/navMesh/NavMeshSettings.h @@ -0,0 +1,63 @@ +#ifndef NAVMESHSETTINGS_H +#define NAVMESHSETTINGS_H + +namespace NM { + + enum class SamplePartitionType { + SAMPLE_PARTITION_WATERSHED, + SAMPLE_PARTITION_MONOTONE, + SAMPLE_PARTITION_LAYERS, + }; + + struct NavMeshSettings { + + + /** maximum resolution for outputs. nothing below this size will be detected (walls, doors, ..) */ + float maxQuality_m = 0.20f; + + + /** height of the walking person (used to delete regions below other regions) */ + float agentHeight = 1.8f; + + /** radius of the walking person (used to shrink the walkable area) */ + float agentRadius = 0.2f; + + /** the max angle (degree) the pedestrian is able to walk */ + float agentMaxSlope = 45.0f; // elevator??? + + + /** maximal size for one triangle. too high = too many samples when walking! */ + float edgeMaxLen = 10.0f; + + + /** higher values allow joining some small triangles */ + float edgeMaxError = 1.1f; //1.3f; + + /** algorithm choice */ + SamplePartitionType partitionType = SamplePartitionType::SAMPLE_PARTITION_WATERSHED; + + const float regionMinSize = 2;//8; // (isolated) regions smaller than this will not be rendered?! + const float regionMergeSize = 20; //?? + const int vertsPerPoly = 3;//6.0f; + const float detailSampleDist = 6.0f; + const float detailSampleMaxError = 1.0f;//1.0f; + + + float getCellSizeXY() const { + return maxQuality_m / 2.0f; + } + + float getCellSizeZ() const { + return maxQuality_m / 2.0f; + } + + /** allow jumping onto stairs from the side. usually we do not want this -> set it as low as possible */ + float getMaxClimb() const { + return maxQuality_m; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail! + } + + }; + +} + +#endif // NAVMESHSETTINGS_H diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index ce5d129..60f8358 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -34,7 +34,7 @@ namespace NM { float dot00; float dot01; float dot11; - float invDenom; + double invDenom; float area; float minZ; @@ -66,6 +66,44 @@ namespace NM { Point3 getP3() const {return p3;} + /** get the distance between the given point and the triangle using approximate tests */ + float getDistanceApx(const Point3 pt) const { + +// const float d1 = pt.getDistance(p1); +// const float d2 = pt.getDistance(p2); +// const float d3 = pt.getDistance(p3); +// const float d4 = pt.getDistance(center); +// const float d5 = pt.getDistance((p1-p2)/2); +// const float d6 = pt.getDistance((p2-p3)/2); +// const float d7 = pt.getDistance((p3-p1)/2); +// return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7)))))); + +// const float d1 = pt.getDistance(p1); +// const float d2 = pt.getDistance(p2); +// const float d3 = pt.getDistance(p3); +// const float d4 = pt.getDistance(center); +// return std::min(d1, std::min(d2, std::min(d3,d4))); + + float bestD = 99999; + Point3 bestP; + Point3 dir12 = p2-p1; + Point3 dir13 = p3-p1; + Point3 dir23 = p3-p2; + for (float f = 0; f < 1; f += 0.05f) { + const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt); + const Point3 pos2 = p1 + dir13 * f; const float dist2 = pos2.getDistance(pt); + const Point3 pos3 = p2 + dir23 * f; const float dist3 = pos3.getDistance(pt); + if (dist1 < bestD) {bestP = pos1; bestD = dist1;} + if (dist2 < bestD) {bestP = pos2; bestD = dist2;} + if (dist3 < bestD) {bestP = pos3; bestD = dist3;} + } + + return bestD; + + + } + + bool operator == (const NavMeshTriangle& o) const { return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3); } @@ -122,7 +160,11 @@ namespace NM { float v = (dot00 * dot12 - dot01 * dot02) * invDenom; const Point3 res = getPoint(v,u); - return res; + Assert::isNear(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D"); + Assert::isNear(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D"); + + //return res; + return Point3(p.x, p.y, res.z); // only use the new z, keep input as-is } @@ -159,7 +201,7 @@ namespace NM { dot11 = dot(v1, v1); // Compute barycentric coordinates - invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01); diff --git a/navMesh/NavMeshType.h b/navMesh/NavMeshType.h new file mode 100644 index 0000000..19f1352 --- /dev/null +++ b/navMesh/NavMeshType.h @@ -0,0 +1,22 @@ +#ifndef NAVMESHTYPE_H +#define NAVMESHTYPE_H + +namespace NM { + enum class NavMeshType { + + UNWALKABLE, // needed by Recast + + FLOOR_INDOOR, + FLOOR_OUTDOOR, + + DOOR, + + STAIR_LEVELED, // eben + STAIR_SKEWED, // schraeg + + ELEVATOR, + + }; +} + +#endif // NAVMESHTYPE_H diff --git a/navMesh/walk/NavMeshSub.h b/navMesh/walk/NavMeshSub.h index 57aa0e5..5b084f3 100644 --- a/navMesh/walk/NavMeshSub.h +++ b/navMesh/walk/NavMeshSub.h @@ -28,6 +28,14 @@ namespace NM { return false; } + /** does this submesh contain the given point? */ + bool contains(const Point3 p3) const { + for (const Tria* t : toVisit) { + if (t->contains(p3)) {return true;} + } + return false; + } + /** get the triangle that contains the given point (if any) */ const Tria* getContainingTriangle(const Point2 p2) const { for (const Tria* t : toVisit) { @@ -37,24 +45,36 @@ namespace NM { } /** perform random operations on the submesh */ - NavMeshRandom getRandom() { + NavMeshRandom getRandom() const { return NavMeshRandom(toVisit); } + + /** allows for-each iteration over all included triangles */ + decltype(toVisit.begin()) begin() {return toVisit.begin();} + + /** allows for-each iteration over all included triangles */ + decltype(toVisit.end()) end() {return toVisit.end();} + + + private: void build(const NavMeshLocation& loc, float radius_m) { + PERF_REGION(6, "NavMeshSub::build()"); + std::unordered_set visited; // 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 ); - } +// 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) - size_t next = 1; // start with the first neighbor (skip starting triangle itself) + size_t next = 0; while (next < toVisit.size()) { // next triangle @@ -63,7 +83,8 @@ namespace NM { // neighbors for (const auto* n : *cur) { const Tria* t = (const Tria*) n; - const float dist = loc.pos.getDistance(n->getCenter()); + //const float dist = loc.pos.getDistance(n->getCenter()); + const float dist = n->getDistanceApx(loc.pos); if (dist > radius_m) {continue;} if (visited.find(t) != visited.end()) {continue;} toVisit.push_back(t); diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index e9e79ef..bae5e10 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -4,6 +4,7 @@ #include "NavMeshWalkParams.h" #include "../NavMeshLocation.h" #include "../../math/Distributions.h" +#include "../../misc/PerfCheck.h" namespace NM { @@ -13,6 +14,10 @@ namespace NM { NavMeshLocation end; + NavMeshPotentialWalk(const NavMeshWalkParams& requested) : requested(requested) { + ; + } + NavMeshPotentialWalk(const NavMeshWalkParams& requested, const NavMeshLocation& end) : requested(requested), end(end) { ; } @@ -57,10 +62,9 @@ namespace NM { 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; - } + PERF_REGION(4, "WalkEvalHeadingStartEnd"); + + Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position"); const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy()); const float diff = head.getDiffHalfRAD(walk.requested.heading); @@ -72,6 +76,38 @@ namespace NM { }; + /** + * evaluate the difference between head(start,end) and the requested heading + */ + template class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval { + + const double sigma_rad; + Distribution::Normal dist; + + public: + + WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) : + sigma_rad(sigma_rad), dist(0, sigma_rad) { + ; + } + + virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + + PERF_REGION(4, "WalkEvalHeadingStartEnd"); + + Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position"); + + 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 */ @@ -87,6 +123,8 @@ namespace NM { virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + PERF_REGION(5, "WalkEvalDistance"); + 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; diff --git a/navMesh/walk/NavMeshWalkParams.h b/navMesh/walk/NavMeshWalkParams.h index 1e8d8f0..b569fbc 100644 --- a/navMesh/walk/NavMeshWalkParams.h +++ b/navMesh/walk/NavMeshWalkParams.h @@ -3,6 +3,7 @@ #include "../../geo/Heading.h" #include "../NavMeshLocation.h" +#include "../NavMeshType.h" namespace NM { @@ -20,12 +21,18 @@ namespace NM { Assert::isTrue(isValid(), "invalid step-sizes given"); - if (start.tria->isPlain()) { - return stepSizeFloor_m * steps; - } else { + if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) { return stepSizeStair_m * steps; + } else { + return stepSizeFloor_m * steps; } +// if (start.tria->isPlain()) { +// return stepSizeFloor_m * steps; +// } else { +// return stepSizeStair_m * steps; +// } + } }; @@ -36,9 +43,6 @@ namespace NM { /** walk starts here (pos/tria) */ NavMeshLocation start; -// /** to-be-walked distance */ -// float distance_m; - /** direction to walk to */ Heading heading; @@ -54,9 +58,17 @@ namespace NM { /** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */ float getToBeWalkedDistance() const { - return stepSizes.inMeter(numSteps, start); + if (_toBeWalkedDistance != _toBeWalkedDistance) { + _toBeWalkedDistance = stepSizes.inMeter(numSteps, start); + } + return _toBeWalkedDistance; } + private: + + // precalc + mutable float _toBeWalkedDistance = NAN; + }; } diff --git a/navMesh/walk/NavMeshWalkRandom.h b/navMesh/walk/NavMeshWalkRandom.h new file mode 100644 index 0000000..db625c4 --- /dev/null +++ b/navMesh/walk/NavMeshWalkRandom.h @@ -0,0 +1,146 @@ +#ifndef NAVMESHWALKRANDOM_H +#define NAVMESHWALKRANDOM_H + + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" + +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" + +namespace NM { + + /** + * pick a truely random destination within the reachable area + * weight this area (evaluators) + * repeat this several times to find a robus destination + */ + template class NavMeshWalkRandom { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + public: + + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + struct ResultList : std::vector {}; + + public: + + /** ctor */ + NavMeshWalkRandom(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + ResultEntry getOne(const NavMeshWalkParams& params) const { + + ResultEntry res; + res.probability = 0; + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance(); + const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1; + + // construct reachable region + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + NavMeshRandom rnd = reachable.getRandom(); + NavMeshPotentialWalk pwalk(params); + + // improve quality (the higher, the better) + for (int i = 0; i < 25; ++i) { + + PERF_REGION(1, "NavMeshWalkRandom::SampleLoop"); + + // draw a random destination + // is this destination within the reachable area? (triangles might be larger!) + pwalk.end = rnd.draw(); + if (pwalk.end.pos.getDistance(params.start.pos) > toBeWalkedDistSafe) { + --i; continue; + } + + // calculate the probability for this destination + const double p = eval(pwalk); + + // better? + if (p > res.probability) { + res.location = pwalk.end; + res.probability = p; + } + + } + + // destination is known. update the heading + res.heading = Heading(params.start.pos.xy(), res.location.pos.xy()); + return res; + + } + + ResultList getMany(const NavMeshWalkParams& params) const { + + ResultList res; + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance(); + const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1; + + // construct reachable region + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + NavMeshRandom rnd = reachable.getRandom(); + NavMeshPotentialWalk pwalk(params); + + // improve quality (the higher, the better) + for (int i = 0; i < 25; ++i) { + + PERF_REGION(1, "NavMeshWalkRandom::SampleLoop"); + + pwalk.end = rnd.drawWithin(params.start.pos, toBeWalkedDistSafe); + + // calculate the probability for this destination + const double p = eval(pwalk); + + ResultEntry re; + re.heading = Heading(params.start.pos.xy(), pwalk.end.pos.xy()); + re.location = pwalk.end; + re.probability = p; + res.push_back(re); + + } + + return res; + + } + + double eval(const NM::NavMeshPotentialWalk& pwalk) const { + PERF_REGION(2, "NavMeshWalkRandom::EvalLoop"); + double p = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + p *= p1; + } + return p; + } + + + }; + +} + +#endif // NAVMESHWALKRANDOM_H diff --git a/navMesh/walk/NavMeshWalkSemiRandom.h b/navMesh/walk/NavMeshWalkSemiRandom.h new file mode 100644 index 0000000..b3c4866 --- /dev/null +++ b/navMesh/walk/NavMeshWalkSemiRandom.h @@ -0,0 +1,169 @@ +#ifndef NAVMESHWALKSEMIRANDOM_H +#define NAVMESHWALKSEMIRANDOM_H + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" + +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" + + +namespace NM { + + /** + * similar to NavMeshWalkRandom but: + * pick a semi random destination within the reachable area (requested distance/heading + strong deviation) + * if this destination is reachable: + * weight this area (evaluators) + * repeat this some times to find a robus destination + */ + template class NavMeshWalkSemiRandom { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + public: + + + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + struct ResultList : public std::vector {}; + + public: + + /** ctor */ + NavMeshWalkSemiRandom(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + ResultEntry getOne(const NavMeshWalkParams& params) const { + + static Distribution::Normal dDist(1.0, 0.4); + static Distribution::Normal dHead(0.0, 1.0); + + // construct reachable region + const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1; + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + ResultEntry re; + + NavMeshPotentialWalk pwalk(params); + pwalk.end = reachable.getRandom().draw(); // to have at least a non-start solution + re.probability = eval(pwalk); + re.location = pwalk.end; + + + for (int i = 0; i < 25; ++i) { + + const float distance = params.getToBeWalkedDistance() * dDist.draw(); + const Heading head = params.heading + dHead.draw(); + + // only forward! + if (distance < 0.01) {continue;} + + // get the to-be-reached destination's position (using start+distance+heading) + const Point2 dir = head.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * distance); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + pwalk.end.pos = dstTria->toPoint3(dst); + pwalk.end.tria = dstTria; + const double p = eval(pwalk); + + // better? + if (p > re.probability) { + re.location = pwalk.end; + re.probability = p; + re.heading = head; + } + + } + + } + + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) const { + + static Distribution::Normal dDist(1.0, 0.4); + static Distribution::Normal dHead(0.0, 1.0); + + ResultList res; + + // construct reachable region + const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1; + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + NavMeshPotentialWalk pwalk(params); + + for (int i = 0; i < 25; ++i) { + + const float distance = params.getToBeWalkedDistance() * dDist.draw(); + const Heading head = params.heading + dHead.draw(); + + // only forward! + if (distance < 0.01) {continue;} + + // get the to-be-reached destination's position (using start+distance+heading) + const Point2 dir = head.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * distance); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + pwalk.end.pos = dstTria->toPoint3(dst); + pwalk.end.tria = dstTria; + const double p = eval(pwalk); + + ResultEntry re; + re.location = pwalk.end; + re.probability = p; + re.heading = head; + res.push_back(re); + + } + + } + + return res; + + } + + double eval(const NM::NavMeshPotentialWalk& pwalk) const { + double p = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + p *= p1; + } + return p; + } + + + }; + +} + +#endif // NAVMESHWALKSEMIRANDOM_H diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index 86f49a9..4184ba0 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -25,16 +25,17 @@ namespace NM { public: - struct Result { - + /** single result */ + struct ResultEntry { NavMeshLocation location; Heading heading; double probability; - - Result() : heading(0) {;} - + ResultEntry() : heading(0) {;} }; + /** list of results */ + using ResultList = std::vector; + public: /** ctor */ @@ -47,10 +48,11 @@ namespace NM { this->evals.push_back(eval); } - Result getDestination(const NavMeshWalkParams& params) { - Result res; - res.heading = params.heading; + + ResultEntry getOne(const NavMeshWalkParams& params) { + + ResultEntry re; // to-be-walked distance; const float toBeWalkedDist = params.getToBeWalkedDistance(); @@ -60,7 +62,7 @@ namespace NM { 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 dir = params.heading.asVector(); const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist); const Tria* dstTria = reachable.getContainingTriangle(dst); @@ -68,16 +70,16 @@ namespace NM { // is above destination reachable? if (dstTria) { - res.location.pos = dstTria->toPoint3(dst); - res.location.tria = dstTria; + re.heading = params.heading; // heading was OK -> keep + re.location.pos = dstTria->toPoint3(dst); // new destination position + re.location.tria = dstTria; // new destination triangle ++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 + NavMeshRandom rnd = reachable.getRandom(); // random-helper + re.location = rnd.draw(); // get a random destianation + re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading ++misses; } @@ -87,17 +89,23 @@ namespace NM { std::cout << "hits: " << (hits*100/total) << "%" << std::endl; } - const NavMeshPotentialWalk pwalk(params, res.location); - res.probability = 1.0; + // calculate probability + const NavMeshPotentialWalk pwalk(params, re.location); + re.probability = 1.0; for (const NavMeshWalkEval* eval : evals) { const double p1 = eval->getProbability(pwalk); - res.probability *= p1; + re.probability *= p1; } - return res; + // done + return re; } + ResultList getMany(const NavMeshWalkParams& params) { + return {getOne(params)}; + } + }; diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index f173852..0fbf933 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -111,6 +111,10 @@ public: return Base::get(distance); } + bool doneAtDistance(const float distance) const { + return Base::getMaxKey() < distance; + } + /** at the given distance: are we walking on a plain surface or up/down? */ bool isPlain(const float distance) const { const Point3 pos1 = getPosAfterDistance(distance); diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index a24efd7..b0adf50 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -55,7 +55,7 @@ private: public: /** ctor with the walker to follow */ - SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.3, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : + SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.35, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : //stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m), noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) { diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index b4fadfb..17dcc7a 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -49,6 +49,10 @@ public: this->listeners.push_back(l); } + bool done() { + return path.doneAtDistance(this->walkedDistance); + } + /** increment the walk */ Point3 tick(const Timestamp timePassed) { diff --git a/tests/navMesh/TestNavMeshBenchmark.cpp b/tests/navMesh/TestNavMeshBenchmark.cpp new file mode 100644 index 0000000..5ca8482 --- /dev/null +++ b/tests/navMesh/TestNavMeshBenchmark.cpp @@ -0,0 +1,101 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../navMesh/NavMeshFactory.h" +#include "../../navMesh/walk/NavMeshSub.h" +using namespace NM; + +TEST(NavMeshBenchmark, benchDraw) { + + Floorplan::IndoorMap map; + Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3; + Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline); + + // circle (many triangles) + int i = 0; + for (float f = 0; f < M_PI*2; f += 0.1) { + const float x = std::cos(f) * 10; + const float y = std::sin(f) * 10; + outline.poly.points.push_back(Point2(x,y)); + ++i; + } + + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + NavMeshSettings set; + NavMesh nm; + NavMeshFactory fac(&nm, set); + fac.build(&map); + + ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5); + ASSERT_NEAR(-10, nm.getBBox().getMin().y, 0.5); + ASSERT_NEAR( 0, nm.getBBox().getMin().z, 0.5); + + ASSERT_NEAR(+10, nm.getBBox().getMax().x, 0.5); + ASSERT_NEAR(+10, nm.getBBox().getMax().y, 0.5); + ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5); + + ASSERT_EQ(45, nm.getNumTriangles()); + + + NavMeshRandom rnd = nm.getRandom(); + + for (int i = 0; i < 5000*1000; ++i) { + NavMeshLocation loc = rnd.draw(); + } + +} + + + +TEST(NavMeshBenchmark, benchSubRegion) { + + Floorplan::IndoorMap map; + Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3; + Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline); + + // circle (many triangles) + int i = 0; + for (float f = 0; f < M_PI*2; f += 0.1) { + const float x = std::cos(f) * 10; + const float y = std::sin(f) * 10; + outline.poly.points.push_back(Point2(x,y)); + ++i; + } + + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + NavMeshSettings set; + NavMesh nm; + NavMeshFactory fac(&nm, set); + fac.build(&map); + + ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5); + ASSERT_NEAR(-10, nm.getBBox().getMin().y, 0.5); + ASSERT_NEAR( 0, nm.getBBox().getMin().z, 0.5); + + ASSERT_NEAR(+10, nm.getBBox().getMax().x, 0.5); + ASSERT_NEAR(+10, nm.getBBox().getMax().y, 0.5); + ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5); + + ASSERT_EQ(45, nm.getNumTriangles()); + + std::minstd_rand gen(1337); + std::uniform_real_distribution dist(0, M_PI*2); + + for (int i = 0; i < 50000; ++i) { + const float f = dist(gen); + const float x = std::cos(f) * 9; + const float y = std::sin(f) * 9; + NavMeshLocation loc = nm.getLocation(Point3(x,y,0)); + NavMeshSub(loc, 5); + } + +} + + + +#endif diff --git a/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp index 4362f60..c5dba81 100644 --- a/tests/navMesh/TestNavMeshFactory.cpp +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -17,8 +17,9 @@ TEST(NavMeshFactory, build1) { outline.outdoor = false; outline.method = Floorplan::OutlineMethod::ADD; + NavMeshSettings set; NavMesh nm; - NavMeshFactory fac(&nm); + NavMeshFactory fac(&nm,set); 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 2309cb5..27c7cbe 100644 --- a/tests/navMesh/TestNavMeshSub.cpp +++ b/tests/navMesh/TestNavMeshSub.cpp @@ -18,11 +18,68 @@ TEST(NavMeshSub, build1) { outline.outdoor = false; outline.method = Floorplan::OutlineMethod::ADD; + NavMeshSettings set; NavMesh nm; - NavMeshFactory fac(&nm); + NavMeshFactory fac(&nm, set); fac.build(&map); - NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); + nm.getLocation(Point3(1,1,0)); + nm.getLocation(Point3(8,0.2,0)); + nm.getLocation(Point3(0.2,8,0)); + nm.getLocation(Point3(4.5,4.5,0)); + + + + +} + +TEST(NavMeshSub, draw) { + + Floorplan::IndoorMap map; + Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3; + Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline); + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + // circle (many triangles) + int i = 0; + for (float f = 0; f < M_PI*2; f += 0.1) { + const float x = std::cos(f) * 10; + const float y = std::sin(f) * 10; + outline.poly.points.push_back(Point2(x,y)); + ++i; + } + + + Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove); + remove.outdoor = false; + remove.method = Floorplan::OutlineMethod::REMOVE; + remove.poly.points.push_back(Point2(-2,-2)); + remove.poly.points.push_back(Point2(+2,-2)); + remove.poly.points.push_back(Point2(+2,+2)); + remove.poly.points.push_back(Point2(-2,+2)); + + NavMeshSettings set; + NavMesh nm; + NavMeshFactory fac(&nm, set); + fac.build(&map); + + NavMeshRandom rnd = nm.getRandom(); + + for (int i = 0; i < 1000; ++i) { + NavMeshLocation loc = rnd.draw(); + ASSERT_TRUE(loc.tria->contains(loc.pos)); + + NavMeshSub sub2(loc, 5); + NavMeshRandom rnd2 = sub2.getRandom(); + for (int j = 0; j < 100; ++j) { + NavMeshLocation loc2 = rnd2.draw(); + ASSERT_TRUE(loc2.tria->contains(loc2.pos)); + ASSERT_TRUE(sub2.contains(loc2.pos)); + } + + } + }