diff --git a/navMesh/NavMesh.h b/navMesh/NavMesh.h index 1740bc0..362ee39 100644 --- a/navMesh/NavMesh.h +++ b/navMesh/NavMesh.h @@ -7,11 +7,12 @@ #include #include "../math/DrawList.h" #include "NavMeshRandom.h" +#include "NavMeshLocation.h" template class NavMesh { /** all triangles within the mesh */ - std::vector triangles; + std::vector triangles; BBox3 bbox; @@ -28,12 +29,21 @@ public: /** add a new triangle */ void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) { - triangles.push_back(Tria(p1,p2,p3,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); @@ -42,8 +52,8 @@ public: /** connect both triangles */ void connectUniDir(int idxFrom, int idxTo) { - NavMeshTriangle& tria = triangles[idxFrom]; - tria._neighbors[tria._numNeighbors] = idxTo; + NavMeshTriangle* tria = triangles[idxFrom]; + tria->_neighbors[tria->_numNeighbors] = triangles[idxTo]; } /** allows for-each iteration over all included triangles */ @@ -53,7 +63,7 @@ public: decltype(triangles.end()) end() {return triangles.end();} /** array access */ - Tria& operator [] (const size_t idx) { + Tria* operator [] (const size_t idx) { Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds"); return triangles[idx]; } @@ -70,29 +80,29 @@ public: 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; +// } }; diff --git a/navMesh/NavMeshLocation.h b/navMesh/NavMeshLocation.h new file mode 100644 index 0000000..51d8fb9 --- /dev/null +++ b/navMesh/NavMeshLocation.h @@ -0,0 +1,19 @@ +#ifndef NAVMESHLOCATION_H +#define NAVMESHLOCATION_H + +#include "../geo/Point3.h" + +template struct NavMeshLocation { + + const Tria* tria; + + Point3 pos; + + /** ctor */ + NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), tria(tria) { + ; + } + +}; + +#endif // NAVMESHLOCATION_H diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h index 2fcd98a..32cc38e 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -5,12 +5,13 @@ #include #include "../math/DrawList.h" #include "../geo/Point3.h" +#include "NavMeshLocation.h" template class NavMeshRandom { std::minstd_rand gen; std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); - const std::vector& triangles; + const std::vector& triangles; DrawList lst; public: @@ -22,24 +23,24 @@ public: }; /** ctor */ - NavMeshRandom(const std::vector& triangles) : triangles(triangles) { + NavMeshRandom(const std::vector& triangles) : triangles(triangles) { for (size_t idx = 0; idx < triangles.size(); ++idx) { - lst.add(idx, triangles[idx].getArea()); + lst.add(idx, triangles[idx]->getArea()); } } /** draw a random point within the map */ - Result draw() { + NavMeshLocation draw() { const size_t idx = lst.get(); - const Tria& tria = triangles[idx]; + 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 Result(pos, idx); + return NavMeshLocation(pos, tria); } } diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index 6397ac3..a91ccb0 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -17,7 +17,7 @@ private: template friend class NavMesh; - int _neighbors[3]; + NavMeshTriangle* _neighbors[3]; int _numNeighbors; /** precalculated stuff */ @@ -51,6 +51,10 @@ public: } + decltype(std::begin(_neighbors)) begin() {return std::begin(_neighbors);} + + decltype(std::end(_neighbors)) end() {return std::end(_neighbors);} + Point3 getA() const { return p1; } diff --git a/navMesh/walk/NavMeshSub.h b/navMesh/walk/NavMeshSub.h new file mode 100644 index 0000000..90fc789 --- /dev/null +++ b/navMesh/walk/NavMeshSub.h @@ -0,0 +1,56 @@ +#ifndef NAVMESHSUB_H +#define NAVMESHSUB_H + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" + +#include +#include + + +template class NavMeshSub { + + std::vector toVisit; + +public: + + NavMeshSub(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { + build(nm,loc,radius_m); + } + +private: + + void build(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { + + // center to start searching + const Point3 center = loc.pos; + + toVisit.push_back(loc.tria); + + std::unordered_set visited; + + size_t next = 0; + while (next < toVisit.size()) { + + // next triangle + const Tria* cur = toVisit[next]; ++next; + + // 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/NavMeshWalkHelper.h b/navMesh/walk/NavMeshWalkHelper.h new file mode 100644 index 0000000..65cc4fb --- /dev/null +++ b/navMesh/walk/NavMeshWalkHelper.h @@ -0,0 +1,10 @@ +#ifndef NAVMESHWALKHELPER_H +#define NAVMESHWALKHELPER_H + +template class NavMeshWalkHelper { + + + +} + +#endif // NAVMESHWALKHELPER_H diff --git a/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp index 805bba1..cf8928a 100644 --- a/tests/navMesh/TestNavMeshFactory.cpp +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -30,8 +30,8 @@ TEST(NavMeshFactory, build1) { ASSERT_EQ(2, nm.getNumTriangles()); - ASSERT_EQ(nm.getNeighbor(0,0), nm[1]); - ASSERT_EQ(nm.getNeighbor(1,0), nm[0]); +// ASSERT_EQ(nm.getNeighbor(0,0), nm[1]); +// ASSERT_EQ(nm.getNeighbor(1,0), nm[0]); } diff --git a/tests/navMesh/TestNavMeshSub.cpp b/tests/navMesh/TestNavMeshSub.cpp new file mode 100644 index 0000000..9b3f4ab --- /dev/null +++ b/tests/navMesh/TestNavMeshSub.cpp @@ -0,0 +1,29 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../navMesh/NavMeshFactory.h" +#include "../../navMesh/walk/NavMeshSub.h" + +TEST(NavMeshSub, build1) { + + 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.poly.points.push_back(Point2(0,0)); + outline.poly.points.push_back(Point2(10,0)); + outline.poly.points.push_back(Point2(10,10)); + outline.poly.points.push_back(Point2(0,10)); + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + NavMesh nm; + NavMeshFactory fac(&nm); + fac.build(&map); + + NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); + + +} + +#endif diff --git a/tests/navMesh/TestNavMeshTriangle.cpp b/tests/navMesh/TestNavMeshTriangle.cpp index 1234161..10027c3 100644 --- a/tests/navMesh/TestNavMeshTriangle.cpp +++ b/tests/navMesh/TestNavMeshTriangle.cpp @@ -6,7 +6,7 @@ TEST(NavMeshTriangle, contains) { - NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0)); + NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1); ASSERT_TRUE(t1.contains(Point3(0,0,0))); ASSERT_TRUE(t1.contains(Point3(1,0,0))); @@ -21,10 +21,10 @@ TEST(NavMeshTriangle, contains) { TEST(NavMeshTriangle, area) { - NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0)); + NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1); ASSERT_NEAR(0.5, t1.getArea(), 0.0001); - NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9)); + NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9), 1); ASSERT_NEAR(0.5, t2.getArea(), 0.0001); }