minor changes to floorplan

fixed some compile issues
worked on nav-meshes
added some tests

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

geo/Angle.h

@@ -4,6 +4,7 @@
 #include <cmath>
 #include "../Assertions.h"
 #include "Point2.h"
+#include "../math/speed.h"
 
 #define PI		((float) M_PI)
 

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;

math/speed.h

@@ -0,0 +1,63 @@
+#ifndef SPEED_H
+#define SPEED_H
+
+#include <cmath>
+
+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

misc/PerfCheck.h

@@ -0,0 +1,130 @@
+#ifndef PERFCHECK_H
+#define PERFCHECK_H
+
+
+//#define WITH_PERF_CHECK
+
+#ifdef WITH_PERF_CHECK
+
+	#include <string>
+	#include <time.h>
+	#include <unordered_map>
+
+	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<std::string, Stats>& map() {
+		static std::unordered_map<std::string, Stats> stats;
+		return stats;
+	}
+
+	void add(const std::string& name, const clock_t diff) {
+		std::unordered_map<std::string, Stats>& m = map();
+		m[name].calls += 1;
+		m[name].time += diff;
+	}
+
+};
+*/
+
+#endif // PERFCHECK_H

navMesh/NavMeshDebug.h

@@ -7,6 +7,7 @@
 #include <KLib/misc/gnuplot/GnuplotSplot.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
 #include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
 
 namespace NM {
@@ -18,27 +19,61 @@ namespace NM {
 
 	public:
 
-		template <typename Tria> static void show(NavMesh<Tria>& nm) {
+		K::Gnuplot gp;
+		K::GnuplotSplot plot;
+		K::GnuplotSplotElementLines lines;
+		K::GnuplotSplotElementPoints border;
+		K::GnuplotSplotElementColorPoints particles;
+		K::GnuplotSplotElementLines pathEstimated;
 
-			K::GnuplotFill gFill[3] = {
+	private:
-				K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#111111"), 1),
+
-				K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaaa"), 1),
+		K::GnuplotFill gFill[6] = {
-				K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaff"), 1)
+			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 <typename T> void showParticles(const std::vector<T>& 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 <typename Tria> void addMesh(NavMesh<Tria>& 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));
+			border.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.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";
 		}
 
 	};

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 <typename Tria> class NavMeshFactory {
 
 	private:
 
-		float maxQuality_m = 0.20f;	// 25cm elements are the smallest to-be-detected
-
 		NavMesh<Tria>* dst = nullptr;
 
+		const NavMeshSettings& settings;
+
 		std::vector<TriangleIn> triangles;
 
 	public:
 
-		NavMeshFactory(NavMesh<Tria>* dst) : dst(dst) {
+		NavMeshFactory(NavMesh<Tria>* 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 */
+//		/** get the smallest obstacle size that can be detected */
-		float getMaxQuality_m() const {
+//		float getMaxQuality_m() const {
-			return maxQuality_m;
+//			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<Floorplan::FloorObstacleLine*>(obs);
+//				if (line != nullptr) {
+//					nmPoly.remove(getPolygon(line));
+//				}
+//			}
+
+//			std::vector<std::vector<Point3>> tmp = nmPoly.get();
+//			for (const std::vector<Point3>& 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) {
+					// get all other polygons of this floor, that are tagged as "remove" and remove them (many will be outside of the added polygon)
-				if (poly->method == Floorplan::OutlineMethod::REMOVE) {
+					for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
-					nmPoly.remove(poly->poly);
+						if (poly->method == Floorplan::OutlineMethod::REMOVE) {
-				}
+							nmPoly.remove(poly->poly);
-			}
+						}
+					}
 
-			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
+					// get all obstacles of this floor and remove them from the polygon as well (many will be outside of the added polygon)
-				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
+					for (Floorplan::FloorObstacle* obs : floor->obstacles) {
-				if (line != nullptr) {
+						Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
-					nmPoly.remove(getPolygon(line));
+						if (line != nullptr) {
-				}
+							nmPoly.remove(getPolygon(line));
-			}
+						}
+					}
+
+					// construct and add
+					std::vector<std::vector<Point3>> tmp = nmPoly.get();
+					int type = poly->outdoor ? (int) NavMeshType::FLOOR_OUTDOOR : (int) NavMeshType::FLOOR_INDOOR;
+					for (const std::vector<Point3>& tria : tmp) {
+						const TriangleIn t(tria[0], tria[1], tria[2], type);
+						triangles.push_back(t);
+					}
+
+				}
 
-			std::vector<std::vector<Point3>> tmp = nmPoly.get();
-			for (const std::vector<Point3>& 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<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
+				const std::vector<Floorplan::Quad3> 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<Floorplan::FloorObstacleDoor*>(obs);
+					if (door != nullptr) {
+						nmDoors.add(getPolygon(door));
+					}
+				}
+
+				// construct and add triangles
+				std::vector<std::vector<Point3>> tmp = nmDoors.get();
+				for (const std::vector<Point3>& 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<int> tData;
 			std::vector<float> vData;
 			std::vector<uint8_t> 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_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_cellHeight = maxQuality_m/2.0f;	//0.2f;
-			float m_agentHeight = 2.0f;
+//			float m_agentHeight = 1.8f;
-			float m_agentRadius = 0.2f;//0.6f;
+//			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_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_agentMaxSlope = 45.0f;							// elevator???
-			float m_regionMinSize = 2;//8;
+//			float m_regionMinSize = 2;//8;
-			float m_regionMergeSize = 20;
+//			float m_regionMergeSize = 20;
-			float m_edgeMaxLen = 10.0f;								// maximal size for one triangle. too high = too many samples when walking!
+//			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_edgeMaxError = 1.1f; //1.3f;					// higher values allow joining some small triangles
-			float m_vertsPerPoly = 3;//6.0f;
+//			float m_vertsPerPoly = 3;//6.0f;
-			float m_detailSampleDist = 6.0f;
+//			float m_detailSampleDist = 6.0f;
-			float m_detailSampleMaxError = 1.0f;//1.0f;
+//			float m_detailSampleMaxError = 1.0f;//1.0f;
-			int m_partitionType = SAMPLE_PARTITION_WATERSHED;		// SAMPLE_PARTITION_WATERSHED		SAMPLE_PARTITION_MONOTONE		SAMPLE_PARTITION_LAYERS
+//			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.cs = settings.getCellSizeXY();
-				m_cfg.ch = m_cellHeight;
+				m_cfg.ch = settings.getCellSizeZ();
-				m_cfg.walkableSlopeAngle = m_agentMaxSlope;
+				m_cfg.walkableSlopeAngle = settings.agentMaxSlope;
-				m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch);
+				m_cfg.walkableHeight = (int)ceilf(settings.agentHeight / m_cfg.ch);
-				m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch);
+				m_cfg.walkableClimb = (int)floorf(settings.getMaxClimb() / m_cfg.ch);
-				m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs);
+				m_cfg.walkableRadius = (int)ceilf(settings.agentRadius / m_cfg.cs);
-				m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize);
+				m_cfg.maxEdgeLen = (int)(settings.edgeMaxLen / settings.getCellSizeXY());
-				m_cfg.maxSimplificationError = m_edgeMaxError;
+				m_cfg.maxSimplificationError = settings.edgeMaxError;
-				m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize);		// Note: area = size*size
+				m_cfg.minRegionArea = (int)rcSqr(settings.regionMinSize);		// Note: area = size*size
-				m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize);	// Note: area = size*size
+				m_cfg.mergeRegionArea = (int)rcSqr(settings.regionMergeSize);	// Note: area = size*size
-				m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly;
+				m_cfg.maxVertsPerPoly = settings.vertsPerPoly;
-				m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist;
+				m_cfg.detailSampleDist = settings.detailSampleDist < 0.9f ? 0 : settings.getCellSizeXY() * settings.detailSampleDist;
-				m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError;
+				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;
 					}
-				}
+					break;
-				else if (m_partitionType == SAMPLE_PARTITION_MONOTONE)
+
-				{
+				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<TriangleOut> res;
+		//	std::vector<TriangleOut> 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, settings.maxQuality_m);	// wall's thickness (make thin walls big enough to be detected)
-			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
@@ -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;
+		}
+
 	};
 
 }

navMesh/NavMeshFactoryListener.h

@@ -0,0 +1,20 @@
+#ifndef NAVMESHFACTORYLISTENER_H
+#define NAVMESHFACTORYLISTENER_H
+
+#include <string>
+
+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

navMesh/NavMeshRandom.h

@@ -5,6 +5,7 @@
 #include <vector>
 #include "../math/DrawList.h"
 #include "../geo/Point3.h"
+#include "../misc/PerfCheck.h"
 
 #include "NavMeshLocation.h"
 
@@ -21,6 +22,7 @@ namespace NM {
 		DrawList<size_t> lst;
 		std::minstd_rand gen;
 		std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
+		std::uniform_real_distribution<float> dHeading = std::uniform_real_distribution<float>(0, M_PI*2);
 		std::vector<const Tria*> triangles;
 
 
@@ -34,8 +36,8 @@ namespace NM {
 		/** ctor (const/non-const using T) */
 		template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
 
-			// almost always the same number?!
+			// 1st = almost always the same number?!
-			gen();
+			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<Tria> 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) {
+			// get random (u,v) on triangle
-				const float u = dOnTriangle(gen);
+			float u = dOnTriangle(gen);
-				const float v = dOnTriangle(gen);
+			float v = dOnTriangle(gen);
-				if ((u+v) > 1) {continue;}
+
-				const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
+			// if the (u,v) is outside of the triangle, mirror it so its inside the triangle again
-				return NavMeshLocation<Tria>(pos, tria);
+			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<Tria>(pos, tria);
+
+		}
+
+		/** draw a random location within the given radius */
+		NavMeshLocation<Tria> drawWithin(const Point3 center, const float radius) {
+
+			std::uniform_real_distribution<float> 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<Tria> loc(p3, t);
+						return loc;
+
+					}
+				}
+
 			}
 
 		}

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

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

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

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<Tria> getRandom() {
+		NavMeshRandom<Tria> getRandom() const {
 			return NavMeshRandom<Tria>(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<Tria>& loc, float radius_m) {
 
+			PERF_REGION(6, "NavMeshSub::build()");
+
 			std::unordered_set<const Tria*> visited;
 
 			// starting-triangle + all its (max 3) neighbors
 			toVisit.push_back(loc.tria);
 			visited.insert(loc.tria);
-			for (const auto* n : *loc.tria) {
+//			for (const auto* n : *loc.tria) {
-				toVisit.push_back( (const Tria*)n );
+//				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);

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<Tria> end;
 
+		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
+			;
+		}
+
 		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
 			;
 		}
@@ -57,10 +62,9 @@ namespace NM {
 
 		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
 
-			if (walk.requested.start.pos == walk.end.pos) {
+			PERF_REGION(4, "WalkEvalHeadingStartEnd");
-				std::cout << "warn! start-position == end-positon" << std::endl;
+
-				return 0;
+			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 <typename Tria> class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval<Tria> {
+
+		const double sigma_rad;
+		Distribution::Normal<double> dist;
+
+	public:
+
+		WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) :
+			sigma_rad(sigma_rad), dist(0, sigma_rad) {
+			;
+		}
+
+		virtual double getProbability(const NavMeshPotentialWalk<Tria>& 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<double>::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<Tria>& 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;

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()) {
+			if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {
-				return stepSizeFloor_m * steps;
-			} else {
 				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<Tria> 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;
+
 	};
 
 }

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 <typename Tria> class NavMeshWalkRandom {
+
+	private:
+
+		const NavMesh<Tria>& mesh;
+
+		std::vector<NavMeshWalkEval<Tria>*> evals;
+
+	public:
+
+		struct ResultEntry {
+			NavMeshLocation<Tria> location;
+			Heading heading;
+			double probability;
+			ResultEntry() : heading(0) {;}
+		};
+
+		struct ResultList : std::vector<ResultEntry> {};
+
+	public:
+
+		/** ctor */
+		NavMeshWalkRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
+
+		}
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		ResultEntry getOne(const NavMeshWalkParams<Tria>& 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<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+			NavMeshRandom<Tria> rnd = reachable.getRandom();
+			NavMeshPotentialWalk<Tria> 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<Tria>& 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<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+			NavMeshRandom<Tria> rnd = reachable.getRandom();
+			NavMeshPotentialWalk<Tria> 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<Tria>& pwalk) const {
+			PERF_REGION(2, "NavMeshWalkRandom::EvalLoop");
+			double p = 1.0;
+			for (const NavMeshWalkEval<Tria>* eval : evals) {
+				const double p1 = eval->getProbability(pwalk);
+				p *= p1;
+			}
+			return p;
+		}
+
+
+	};
+
+}
+
+#endif // NAVMESHWALKRANDOM_H

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 <typename Tria> class NavMeshWalkSemiRandom {
+
+	private:
+
+		const NavMesh<Tria>& mesh;
+
+		std::vector<NavMeshWalkEval<Tria>*> evals;
+
+	public:
+
+
+		struct ResultEntry {
+			NavMeshLocation<Tria> location;
+			Heading heading;
+			double probability;
+			ResultEntry() : heading(0) {;}
+		};
+
+		struct ResultList : public std::vector<ResultEntry> {};
+
+	public:
+
+		/** ctor */
+		NavMeshWalkSemiRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
+
+		}
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
+
+			static Distribution::Normal<float> dDist(1.0, 0.4);
+			static Distribution::Normal<float> dHead(0.0, 1.0);
+
+			// construct reachable region
+			const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
+			const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+			ResultEntry re;
+
+			NavMeshPotentialWalk<Tria> 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<Tria>& params) const {
+
+			static Distribution::Normal<float> dDist(1.0, 0.4);
+			static Distribution::Normal<float> dHead(0.0, 1.0);
+
+			ResultList res;
+
+			// construct reachable region
+			const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
+			const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+			NavMeshPotentialWalk<Tria> 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<Tria>& pwalk) const {
+			double p = 1.0;
+			for (const NavMeshWalkEval<Tria>* eval : evals) {
+				const double p1 = eval->getProbability(pwalk);
+				p *= p1;
+			}
+			return p;
+		}
+
+
+	};
+
+}
+
+#endif // NAVMESHWALKSEMIRANDOM_H

navMesh/walk/NavMeshWalkSimple.h

@@ -25,16 +25,17 @@ namespace NM {
 	public:
 
 
-		struct Result {
+		/** single result */
-
+		struct ResultEntry {
 			NavMeshLocation<Tria> location;
 			Heading heading;
 			double probability;
-
+			ResultEntry() : heading(0) {;}
-			Result() : heading(0) {;}
-
 		};
 
+		/** list of results */
+		using ResultList = std::vector<ResultEntry>;
+
 	public:
 
 		/** ctor */
@@ -47,10 +48,11 @@ namespace NM {
 			this->evals.push_back(eval);
 		}
 
-		Result getDestination(const NavMeshWalkParams<Tria>& params) {
 
-			Result res;
+
-			res.heading = params.heading;
+		ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
+
+			ResultEntry re;
 
 			// to-be-walked distance;
 			const float toBeWalkedDist = params.getToBeWalkedDistance();
@@ -60,7 +62,7 @@ namespace NM {
 			NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
 
 			// get the to-be-reached destination's position (using start+distance+heading)
-			const Point2 dir = res.heading.asVector();
+			const Point2 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);
+				re.heading = params.heading;										// heading was OK -> keep
-				res.location.tria = dstTria;
+				re.location.pos = dstTria->toPoint3(dst);							// new destination position
+				re.location.tria = dstTria;											// new destination triangle
 				++hits;
 
 			} else {
 
-				NavMeshRandom<Tria> rnd = reachable.getRandom();
+				NavMeshRandom<Tria> rnd = reachable.getRandom();					// random-helper
-				NavMeshLocation<Tria> rndLoc = rnd.draw();
+				re.location = rnd.draw();											// get a random destianation
-				res.location = rndLoc;
+				re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading
-				res.heading = Heading(params.start.pos.xy(), rndLoc.pos.xy());	// update the heading
 				++misses;
 
 			}
@@ -87,17 +89,23 @@ namespace NM {
 				std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
 			}
 
-			const NavMeshPotentialWalk<Tria> pwalk(params, res.location);
+			// calculate probability
-			res.probability = 1.0;
+			const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
+			re.probability = 1.0;
 			for (const NavMeshWalkEval<Tria>* eval : evals) {
 				const double p1 = eval->getProbability(pwalk);
-				res.probability *= p1;
+				re.probability *= p1;
 			}
 
-			return res;
+			// done
+			return re;
 
 		}
 
+		ResultList getMany(const NavMeshWalkParams<Tria>& params) {
+			return {getOne(params)};
+		}
+
 
 	};
 

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

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

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

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::NavMeshTriangle> nm;
+	NavMeshFactory<NM::NavMeshTriangle> 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<NM::NavMeshTriangle> rnd = nm.getRandom();
+
+	for (int i = 0; i < 5000*1000; ++i) {
+		NavMeshLocation<NM::NavMeshTriangle> 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::NavMeshTriangle> nm;
+	NavMeshFactory<NM::NavMeshTriangle> 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<float> 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<NM::NavMeshTriangle> loc = nm.getLocation(Point3(x,y,0));
+		NavMeshSub<NM::NavMeshTriangle>(loc, 5);
+	}
+
+}
+
+
+
+#endif

tests/navMesh/TestNavMeshFactory.cpp

@@ -17,8 +17,9 @@ TEST(NavMeshFactory, build1) {
 	outline.outdoor = false;
 	outline.method = Floorplan::OutlineMethod::ADD;
 
+	NavMeshSettings set;
 	NavMesh<NM::NavMeshTriangle> nm;
-	NavMeshFactory<NM::NavMeshTriangle> fac(&nm);
+	NavMeshFactory<NM::NavMeshTriangle> fac(&nm,set);
 	fac.build(&map);
 
 	ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5);

tests/navMesh/TestNavMeshSub.cpp

@@ -18,11 +18,68 @@ TEST(NavMeshSub, build1) {
 	outline.outdoor = false;
 	outline.method = Floorplan::OutlineMethod::ADD;
 
+	NavMeshSettings set;
 	NavMesh<NM::NavMeshTriangle> nm;
-	NavMeshFactory<NM::NavMeshTriangle> fac(&nm);
+	NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
 	fac.build(&map);
 
-	NavMeshLocation<NM::NavMeshTriangle> 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::NavMeshTriangle> nm;
+	NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
+	fac.build(&map);
+
+	NavMeshRandom<NM::NavMeshTriangle> rnd = nm.getRandom();
+
+	for (int i = 0; i < 1000; ++i) {
+		NavMeshLocation<NM::NavMeshTriangle> loc = rnd.draw();
+		ASSERT_TRUE(loc.tria->contains(loc.pos));
+
+		NavMeshSub<NM::NavMeshTriangle> sub2(loc, 5);
+		NavMeshRandom<NM::NavMeshTriangle> rnd2 = sub2.getRandom();
+		for (int j = 0; j < 100; ++j) {
+			NavMeshLocation<NM::NavMeshTriangle> loc2 = rnd2.draw();
+			ASSERT_TRUE(loc2.tria->contains(loc2.pos));
+			ASSERT_TRUE(sub2.contains(loc2.pos));
+		}
+
+	}
+
 
 
 }