worked on nav-meshes

navMesh/NavMesh.h

@@ -7,11 +7,12 @@
 #include <random>
 #include "../math/DrawList.h"
 #include "NavMeshRandom.h"
+#include "NavMeshLocation.h"
 
 template <typename Tria> class NavMesh {
 
 	/** all triangles within the mesh */
-	std::vector<Tria> triangles;
+	std::vector<Tria*> 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<Tria> getLocation(const Point3 pos) {
+		for (const Tria* tria : triangles) {
+			if (tria->contains(pos)) {
+				return NavMeshLocation<Tria>(pos, tria);
+			}
+		}
+		throw Exception("location not found");
+	}
+
 	/** connect both triangles */
 	void connectBiDir(int idx1, int idx2) {
 		connectUniDir(idx1,idx2);
@@ -42,8 +52,8 @@ public:
 
 	/** connect both triangles */
 	void connectUniDir(int idxFrom, int idxTo) {
-		NavMeshTriangle& tria = triangles[idxFrom];
+		NavMeshTriangle* tria = triangles[idxFrom];
-		tria._neighbors[tria._numNeighbors] = idxTo;
+		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<Tria>(triangles);
 	}
 
-	/** ---------------- NEIGHBORS ---------------- */
+//	/** ---------------- NEIGHBORS ---------------- */
 
-	/** get the number of neighbors for the given element */
+//	/** get the number of neighbors for the given element */
-	int getNumNeighbors(const size_t idx) const {
+//	int getNumNeighbors(const size_t idx) const {
-		return getNumNeighbors(triangles[idx]);
+//		return getNumNeighbors(triangles[idx]);
-	}
+//	}
 
-	/** get the number of neighbors for the given element */
+//	/** get the number of neighbors for the given element */
-	int getNumNeighbors(const Tria& e) const {
+//	int getNumNeighbors(const Tria& e) const {
-		return e._numNeighbors;
+//		return e._numNeighbors;
-	}
+//	}
 
-	/** get the n-th neighbor for the given node */
+//	/** get the n-th neighbor for the given node */
-	Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
+//	Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
-		const Tria& node = triangles[nodeIdx];
+//		const Tria& node = triangles[nodeIdx];
-		return getNeighbor(node, nth);
+//		return getNeighbor(node, nth);
-	}
+//	}
 
-	/** get the n-th neighbor for the given node */
+//	/** get the n-th neighbor for the given node */
-	Tria& getNeighbor(const Tria& tria, const size_t nth) const {
+//	Tria& getNeighbor(const Tria& tria, const size_t nth) const {
-		const Tria& neighbor = triangles[tria._neighbors[nth]];
+//		const Tria& neighbor = triangles[tria._neighbors[nth]];
-		return (Tria&) neighbor;
+//		return (Tria&) neighbor;
-	}
+//	}
 
 };
 

navMesh/NavMeshLocation.h

@@ -0,0 +1,19 @@
+#ifndef NAVMESHLOCATION_H
+#define NAVMESHLOCATION_H
+
+#include "../geo/Point3.h"
+
+template <typename Tria> struct NavMeshLocation {
+
+	const Tria* tria;
+
+	Point3 pos;
+
+	/** ctor */
+	NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), tria(tria) {
+		;
+	}
+
+};
+
+#endif // NAVMESHLOCATION_H

navMesh/NavMeshRandom.h

@@ -5,12 +5,13 @@
 #include <vector>
 #include "../math/DrawList.h"
 #include "../geo/Point3.h"
+#include "NavMeshLocation.h"
 
 template <typename Tria> class NavMeshRandom {
 
 	std::minstd_rand gen;
 	std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
-	const std::vector<Tria>& triangles;
+	const std::vector<Tria*>& triangles;
 	DrawList<size_t> lst;
 
 public:
@@ -22,24 +23,24 @@ public:
 	};
 
 	/** ctor */
-	NavMeshRandom(const std::vector<Tria>& triangles) : triangles(triangles) {
+	NavMeshRandom(const std::vector<Tria*>& 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<Tria> 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<Tria>(pos, tria);
 		}
 
 	}

navMesh/NavMeshTriangle.h

@@ -17,7 +17,7 @@ private:
 
 	template<typename> 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;
 	}

navMesh/walk/NavMeshSub.h

@@ -0,0 +1,56 @@
+#ifndef NAVMESHSUB_H
+#define NAVMESHSUB_H
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+
+#include <vector>
+#include <unordered_set>
+
+
+template <typename Tria> class NavMeshSub {
+
+	std::vector<const Tria*> toVisit;
+
+public:
+
+	NavMeshSub(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) {
+		build(nm,loc,radius_m);
+	}
+
+private:
+
+	void build(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) {
+
+		// center to start searching
+		const Point3 center = loc.pos;
+
+		toVisit.push_back(loc.tria);
+
+		std::unordered_set<const Tria*> 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

navMesh/walk/NavMeshWalkHelper.h

@@ -0,0 +1,10 @@
+#ifndef NAVMESHWALKHELPER_H
+#define NAVMESHWALKHELPER_H
+
+template <typename Tria> class NavMeshWalkHelper {
+
+
+
+}
+
+#endif // NAVMESHWALKHELPER_H

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(0,0), nm[1]);
-	ASSERT_EQ(nm.getNeighbor(1,0), nm[0]);
+//	ASSERT_EQ(nm.getNeighbor(1,0), nm[0]);
 
 }
 

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<NavMeshTriangle> nm;
+	NavMeshFactory<NavMeshTriangle> fac(&nm);
+	fac.build(&map);
+
+	NavMeshLocation<NavMeshTriangle> loc = nm.getLocation(Point3(1,1,1));
+
+
+}
+
+#endif

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);
 
 }