merged

CMakeLists.txt

@@ -85,6 +85,7 @@ ADD_DEFINITIONS(
 	-DWITH_TESTS
 	-DWITH_ASSERTIONS
 	-DWITH_DEBUG_LOG
+        -D_GLIBCXX_DEBUG
 
 
 )

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)
 

grid/Grid.h

@@ -16,6 +16,11 @@
 #include "../geo/BBox3.h"
 #include "../misc/Debug.h"
 
+#define GM_BOX			1
+#define GM_HOBEYCOMB	2
+#define GRID_MODE		GM_BOX
+
+
 /**
  * grid of a given-size, storing some user-data-value which
  * - extends GridPoint and GridNode
@@ -177,10 +182,11 @@ public:
 	}
 
 	/** get the center-node the given Point belongs to */
-	const T& getNodeFor(const GridPoint& p) {
+	const T& getNodeFor(const GridPoint& p) const {
-		const UID uid = getUID(p);
+		//const UID uid = getUID(p);
-		Assert::isTrue(hashes.find(uid) != hashes.end(), "element not found!");
+		auto it = hashes.find(getUID(p));
-		return nodes[hashes[uid]];
+		Assert::isTrue(it != hashes.end(), "element not found!");
+		return nodes[it->second];
 	}
 
 	/** get the center-node the given Point belongs to. or nullptr if not present */
@@ -213,6 +219,14 @@ public:
 		return GridNodeBBox(node, gridSize_cm);
 	}
 
+	/** is this node part of a non-plain stair/escalator */
+	bool isPlain(const T& n1) const {
+		for (const T& n2 : neighbors(n1)) {
+			if (n2.z_cm != n1.z_cm) {return false;}
+		}
+		return true;
+	}
+
 	/**
 	 * get an UID for the given point.
 	 * this works only for aligned points.
@@ -231,9 +245,18 @@ public:
 		const uint64_t center = 1 << 19;
 
 		// build
+#if (GRID_MODE == GM_HOBEYCOMB)
+		const int xx = ((int)std::round(p.y_cm / gridSize_cm) % 2 == 0) ? (0) : (gridSize_cm/2);
+		const uint64_t x = center + (int64_t) idxX(p.x_cm-xx);
+		const uint64_t y = center + (int64_t) idxY(p.y_cm);
+		const uint64_t z = center + (int64_t) idxZ(p.z_cm);
+#elif (GRID_MODE == GM_BOX)
 		const uint64_t x = center + (int64_t) idxX(p.x_cm);
 		const uint64_t y = center + (int64_t) idxY(p.y_cm);
 		const uint64_t z = center + (int64_t) idxZ(p.z_cm);
+#endif
+
+
 
 		return (z << 40) | (y << 20) | (x << 0);
 
@@ -241,11 +264,11 @@ public:
 
 	inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);}
 	inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);}
-	inline int idxZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm);}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
+	inline int idxZ(const int z_cm) const {return std::round(z_cm / 20.0f);}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
 
 	inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;}
 	inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;}
-	inline int snapZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm) * gridSize_cm;}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
+	inline int snapZ(const int z_cm) const {return std::round(z_cm / 20.0f) * 20;}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
 
 
 	/** array access */
@@ -287,6 +310,11 @@ public:
 		}
 	}
 
+	/** convert to a GridPoint coordinate (in cm) */
+	GridPoint toGridPoint(const Point3 pos_m) const {
+		return GridPoint( snapX(pos_m.x*100), snapY(pos_m.y*100), snapZ(pos_m.z*100) );
+	}
+
 	/** remove the given array-index by moving all follwing elements down by one */
 	template <typename X> void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) {
 		for (int i = idxToRemove+1; i < arrayLen; ++i) {
@@ -469,11 +497,11 @@ public:
 
 
 
-	NeighborForEach neighbors(const int idx) {
+	NeighborForEach neighbors(const int idx) const {
 		return neighbors(nodes[idx]);
 	}
 
-	NeighborForEach neighbors(const T& node) {
+	NeighborForEach neighbors(const T& node) const {
 		return NeighborForEach(*this, node._idx);
 	}
 
@@ -513,9 +541,13 @@ private:
 
 	/** asssert that the given element is aligned to the grid */
 	void assertAligned(const T& elem) {
+#if (GRID_MODE == GM_HOBEYCOMB)
+
+#elif (GRID_MODE == GM_BOX)
 		if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
 		if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
 		//if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
+#endif
 	}
 
 };

grid/GridNode.h

@@ -87,6 +87,7 @@ public:
 	/** set the node's semantic type */
 	void setType(const uint8_t type) {this->_type = type;}
 
+
 //	/** get the n-th neighbor for this node */
 //	template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
 //		return grid.getNeighbor(_idx, nth);

grid/factory/v3/GridFactory3.h

@@ -0,0 +1,492 @@
+#ifndef GRIDFACTORY3_H
+#define GRIDFACTORY3_H
+
+#include "../../Grid.h"
+#include "../../../floorplan/v2/Floorplan.h"
+#include "HelperPoly3.h"
+#include <unordered_set>
+
+#if (GRID_MODE == GM_BOX)
+
+#define GF3_ITER_XY for (int y = y1; y <= y2; y += gs_cm) {	for (int x = x1; x <= x2; x += gs_cm) {
+
+#elif (GRID_MODE == GM_HOBEYCOMB)
+
+#define GF3_ITER_XY\
+for (int y = y1; y <= y2; y += gs_cm) {\
+	const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);\
+	for (int x = x1-xx; x <= x2; x += gs_cm) {
+
+#endif
+
+template <typename Node> class GridFactory3 {
+
+private:
+
+
+	Grid<Node>& grid;
+	const int gs_cm;
+
+	struct NewNode {
+		GridPoint pos;
+		int type;
+		NewNode(const GridPoint pos, const int type) : pos(pos), type(type) {;}
+		bool operator == (const NewNode& o) const {return o.pos == pos;}
+	};
+
+public:
+
+	GridFactory3(Grid<Node>& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) {
+
+	}
+
+	void build(const Floorplan::IndoorMap* map) {
+
+		std::vector<NewNode> add;
+		std::vector<NewNode> rem;
+
+		for (const Floorplan::Floor* floor : map->floors) {
+
+//			for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+
+//				const std::vector<NewNode> pts = getPointsOn(floor, *poly);
+//				if (poly->method == Floorplan::OutlineMethod::ADD) {
+//					add.insert(add.end(), pts.begin(), pts.end());
+//				} else {
+//					rem.insert(rem.end(), pts.begin(), pts.end());
+//				}
+
+//			}
+
+			const std::vector<NewNode> pts = getPointsOn(floor);
+			add.insert(add.end(), pts.begin(), pts.end());
+
+			for (const Floorplan::Stair* stair : floor->stairs) {
+				std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
+				const std::vector<NewNode> pts = getPointsOn(floor, quads);
+				add.insert(add.end(), pts.begin(), pts.end());
+			}
+
+
+		}
+
+		for (const NewNode& nn : add) {
+			auto it = std::find(rem.begin(), rem.end(), nn);
+			if (it == rem.end()) {
+				if (!grid.hasNodeFor(nn.pos)) {
+					Node n(nn.pos.x_cm, nn.pos.y_cm, nn.pos.z_cm);
+					n.setType(nn.type);
+					grid.add(n);
+				}
+			}
+		}
+
+		connect(map);
+		removeIsolatedNodes();
+
+	}
+
+	bool isBlocked(const Floorplan::IndoorMap* map, const Node& n1, const Node& n2) {
+
+		Line2 lNodes(n1.inMeter().xy(), n2.inMeter().xy());
+
+		for (Floorplan::Floor* floor : map->floors) {
+
+			if (n1.inMeter().z != floor->atHeight) {continue;}
+			if (n2.inMeter().z != floor->atHeight) {continue;}
+
+			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
+				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
+				if (line) {
+					const std::vector<Line2> lines = getThickLines(line);
+					for (const Line2& lObs : lines) {
+						if (lObs.getSegmentIntersection(lNodes)) {
+							return true;
+						}
+					}
+				}
+			}
+		}
+
+		return false;
+
+	}
+
+	/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
+	static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
+		//const Line2 base(line->from*100, line->to*100);
+		const float thickness_m = line->thickness_m;
+		const Point2 dir = (line->to - line->from);				// obstacle's direction
+		const Point2 perp = dir.perpendicular().normalized();	// perpendicular direction (90 degree)
+		const Point2 p1 = line->from + perp * thickness_m/2;		// start-up
+		const Point2 p2 = line->from - perp * thickness_m/2;		// start-down
+		const Point2 p3 = line->to + perp * thickness_m/2;		// end-up
+		const Point2 p4 = line->to - perp * thickness_m/2;		// end-down
+		return {
+			Line2(p1, p2),
+			Line2(p3, p4),
+			Line2(p2, p4),
+			Line2(p1, p3),
+		};
+	}
+
+	void connect(const Floorplan::IndoorMap* map) {
+
+		for (Node& n1 : grid) {
+			for (Node& n2 : grid) {
+
+				if (n1 == n2) {continue;}
+
+
+				// stair with floor
+				if (
+						(n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) ||
+						(n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR)
+					) {
+
+					const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
+					const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
+					if (distxy > 0 && distxy < gs_cm * 1.2 / 100.0f && distz_cm < gs_cm) {		// [1.85]
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				// floor with floor
+				} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
+					if (n1.getDistanceInCM(n2) < gs_cm * 1.2  && !isBlocked(map, n1, n2)) {		// [1.2 | 1.845]
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				// stair with stair
+				} else if (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_STAIR) {
+
+					const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
+					const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
+
+//					if (n1.getDistanceInCM(n2) < gs_cm * 1.45  && !isBlocked(map, n1, n2)) {
+					if (distxy < gs_cm * 1.2 / 100.0f && distz_cm <= gs_cm) {		// [1.845]
+						if (n1.fullyConnected()) {continue;}
+						if (n2.fullyConnected()) {continue;}
+						grid.connectUniDir(n1, n2);
+					}
+
+				}
+
+
+//				if (n1.getDistanceInCM(n2) < gs_cm * 1.7  && !isBlocked(map, n1, n2)) {
+//					if (n1.fullyConnected()) {continue;}
+//					if (n2.fullyConnected()) {continue;}
+//					grid.connectUniDir(n1, n2);
+//				}
+
+			}
+		}
+
+	}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+	/** recursively get all connected nodes and add them to the set */
+	void getConnected(Node& n1, std::unordered_set<int>& visited) {
+
+		std::unordered_set<int> toVisit;
+		toVisit.insert(n1.getIdx());
+
+		// run while there are new nodes to visit
+		while(!toVisit.empty()) {
+
+			// get the next node
+			int nextIdx = *toVisit.begin();
+			toVisit.erase(nextIdx);
+			visited.insert(nextIdx);
+			Node& next = grid[nextIdx];
+
+			// get all his (unprocessed) neighbors and add them to the region
+			for (const Node& n2 : grid.neighbors(next)) {
+				if (visited.find(n2.getIdx()) == visited.end()) {
+					toVisit.insert(n2.getIdx());
+				}
+			}
+
+		}
+
+	}
+
+	void removeIsolatedNodes() {
+
+		//std::cout << "todo: remove" << std::endl;
+		//return;
+
+		// try to start at the first stair
+		for (Node& n : grid) {
+			if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
+		}
+
+		// no stair found? try to start at the first node
+		removeIsolatedNodes(grid[0]);
+
+	}
+
+	/** remove all nodes not connected to n1 */
+	void removeIsolatedNodes(Node& n1) {
+
+		// get the connected region around n1
+		//Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
+		//Log::tick();
+		std::unordered_set<int> set;
+		getConnected(n1, set);
+		//Log::tock();
+
+		//const int numToRemove = grid.getNumNodes() - set.size();
+		//int numRemoved = 0;
+
+		// remove all other
+		//Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
+		//Log::tick();
+		for (Node& n2 : grid) {
+			if (set.find(n2.getIdx()) == set.end()) {
+
+				// sanity check
+				// wouldn't make sense that a stair-node is removed..
+				// maybe something went wrong elsewhere???
+				Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!");
+				Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!");
+				//Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!");
+
+				// proceed ;)
+				grid.remove(n2);
+
+				//++numRemoved;
+				//std::cout << numRemoved << ":" << numToRemove << std::endl;
+
+			}
+		}
+		//Log::tock();
+
+		// clean the grid (physically delete the removed nodes)
+		grid.cleanup();
+
+	}
+
+
+//	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) {
+
+//		std::vector<NewNode> res;
+
+//		BBox2 bbox;
+//		for (Point2 pt : poly.poly.points) {bbox.add(pt);}
+
+//		int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
+//		int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
+
+//		int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
+//		int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
+
+//		int z = floor->atHeight * 100;
+
+//		for (int y = y1; y <= y2; y += gs_cm) {
+//			for (int x = x1; x <= x2; x += gs_cm) {
+
+//				GridPoint gp(x, y, z);
+//				int type = poly.outdoor ? GridNode::TYPE_OUTDOOR : GridNode::TYPE_FLOOR;
+//				res.push_back(NewNode(gp, type));
+
+//			}
+//		}
+
+//		return res;
+
+//	}
+
+
+
+	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor) {
+
+		std::vector<NewNode> res;
+
+		BBox2 bbox;
+		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+			for (Point2 pt : poly->poly.points) {bbox.add(pt);}
+		}
+
+		int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
+		int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
+
+		int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
+		int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
+
+		int z = floor->atHeight * 100;
+
+		struct Combo {
+			HelperPoly3 poly;
+			const Floorplan::FloorOutlinePolygon* orig;
+			Combo(HelperPoly3 poly, const Floorplan::FloorOutlinePolygon* orig) : poly(poly), orig(orig) {;}
+		};
+
+		std::vector<Combo> polygons;
+		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
+			HelperPoly3 pol(*poly);
+			polygons.push_back(Combo(pol, poly));
+		}
+
+		GF3_ITER_XY
+
+				int type = GridNode::TYPE_FLOOR;
+				bool remove = false;
+				bool add = false;
+
+				for (const Combo& c : polygons) {
+					if (c.poly.contains(Point2(x,y))) {
+						if (c.orig->method == Floorplan::OutlineMethod::ADD) {add = true;}
+						if (c.orig->method == Floorplan::OutlineMethod::REMOVE) {remove = true; break;}
+						if (c.orig->outdoor) {type = GridNode::TYPE_OUTDOOR;}
+					}
+				}
+
+				if (add && !remove) {
+					GridPoint gp(x, y, z);
+					res.push_back(NewNode(gp, type));
+				}
+
+			}
+		}
+
+		return res;
+
+	}
+
+
+
+	//
+
+	//				const std::vector<NewNode> pts = getPointsOn(floor, *poly);
+	//				if (poly->method == Floorplan::OutlineMethod::ADD) {
+	//					add.insert(add.end(), pts.begin(), pts.end());
+	//				} else {
+	//					rem.insert(rem.end(), pts.begin(), pts.end());
+	//				}
+
+	//			}
+
+	static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
+		const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
+		double d00 = dot(v0, v0);
+		double d01 = dot(v0, v1);
+		double d11 = dot(v1, v1);
+		double d20 = dot(v2, v0);
+		double d21 = dot(v2, v1);
+		double denom = d00 * d11 - d01 * d01;
+		v = (d11 * d20 - d01 * d21) / denom;
+		w = (d00 * d21 - d01 * d20) / denom;
+		u = 1.0f - v - w;
+		return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0);
+	}
+
+//	void isBlocked(const GridPoint& gp) {
+//		for (Floorplan::Floor* floor : map->floors) {
+//			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
+//				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
+//				if (line) {
+//					line->
+//				}
+//			}
+//		}
+//	}
+
+	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const std::vector<Floorplan::Quad3>& quads) {
+
+		std::vector<NewNode> res;
+
+		// whole stair
+		BBox3 bboxStair;
+		for (const Floorplan::Quad3& quad : quads) {
+			bboxStair.add(quad.p1);
+			bboxStair.add(quad.p2);
+			bboxStair.add(quad.p3);
+			bboxStair.add(quad.p4);
+		}
+
+		// stair's starting and ending z (must be connected to a floor)
+		//int z1 = grid.snapZ( (floor->atHeight) * 100 );
+		//
+		int z2 = grid.snapZ( (floor->atHeight + bboxStair.getMax().z) * 100 );
+
+		// one quad
+		for (const Floorplan::Quad3& quad : quads) {
+
+			BBox3 bbox;
+			bbox.add(quad.p1);
+			bbox.add(quad.p2);
+			bbox.add(quad.p3);
+			bbox.add(quad.p4);
+
+
+			int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
+			int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
+
+			int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
+			int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
+
+			//int zFloor = floor->atHeight * 100;
+
+//			for (int y = y1; y <= y2; y += gs_cm) {
+//				const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);
+//				for (int x = x1-xx; x <= x2; x += gs_cm) {
+
+			GF3_ITER_XY
+
+					int z = 0;
+					Point2 p(x/100.0f, y/100.0f);
+
+					float u,v,w;
+					if (bary(p, quad.p1.xy(), quad.p2.xy(), quad.p3.xy(), u, v, w)) {
+						z = (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) * 100;
+					} else if (bary(p, quad.p1.xy(), quad.p3.xy(), quad.p4.xy(), u, v, w)) {
+						z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
+					} else {
+						// outside of the quad -> skip
+						//z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
+						continue;
+
+						//z = zFloor + (
+						//		(quad.p1.z*u + quad.p2.z*v + quad.p3.z*w)
+						//	) * 100;
+
+					}
+
+					//z = grid.snapZ(z);
+
+					const GridPoint gp(x, y, z);
+					const int type = GridNode::TYPE_STAIR;
+					res.push_back(NewNode(gp, type));
+
+				}
+			}
+
+		}
+
+		// scale to ensure starting at floor, and ending at floor
+
+		return res;
+
+	}
+
+};
+
+#endif // GRIDFACTORY3_H

grid/factory/v3/HelperPoly3.h

@@ -0,0 +1,101 @@
+#ifndef HELPERPOLY3_H
+#define HELPERPOLY3_H
+
+#include "../../../geo/Point2.h"
+#include "../../../geo/Point3.h"
+#include "../../../geo/BBox2.h"
+#include "../../../geo/BBox3.h"
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../grid/Grid.h"
+
+/** helper class for polygon methods */
+struct HelperPoly3 {
+
+	BBox2 bbox_cm;
+	std::vector<Point2> points_cm;
+
+	/** empty ctor */
+	HelperPoly3() {
+		;
+	}
+
+	/** ctor from floorplan-polygon */
+	HelperPoly3(const Floorplan::FloorOutlinePolygon& poly) {
+		for (Point2 p : poly.poly.points) { add(p * 100); }
+	}
+
+	/** ctor from floorplan-quad */
+	HelperPoly3(const Floorplan::Quad3& quad) {
+		add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
+	}
+
+	/** ctor from floorplan-polygon */
+	HelperPoly3(const Floorplan::Polygon2& poly) {
+		for (Point2 p : poly.points) { add(p * 100); }
+	}
+
+	void add(const Point2 p) {
+		points_cm.push_back(p);
+		bbox_cm.add(p);
+	}
+
+	void add(const Point3& p) {
+		points_cm.push_back(p.xy());
+		bbox_cm.add(p.xy());
+	}
+
+	/** does the polygon contain the given point (in cm)? */
+	bool contains(const Point2 p_cm) const {
+
+		// not within bbox? -> not within polygon
+		if (!bbox_cm.contains(p_cm)) {return false;}
+
+		// ensure the point is at least a bit outside of the polygon
+		const float x1_cm = bbox_cm.getMin().x - 17.71920;
+		const float y1_cm = bbox_cm.getMin().y - 23.10923891;
+
+		// construct line between point outside of the polygon and the point in question
+		const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y);
+
+		// determine the number of intersections
+		int hits = 0;
+		const int cnt = points_cm.size();
+		for (int i = 0; i < cnt; ++i) {
+			const Point2 p1 = points_cm[(i+0)%cnt];
+			const Point2 p2 = points_cm[(i+1)%cnt];
+			const Line2 l12(p1, p2);
+			if (l12.getSegmentIntersection(l)) {++hits;}
+		}
+
+		// inside or outside?
+		return ((hits % 2) == 1);
+
+	}
+
+	/** call a user-function for each GRID-ALIGNED point within the polygon */
+	void forEachGridPoint(const int gridSize_cm, std::function<void(int x_cm, int y_cm)> callback) const {
+
+		int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm;
+		int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm;
+
+		int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm;
+		int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm;
+
+		// process each point within the (aligned) bbox
+		for (int y = y1; y <= y2; y += gridSize_cm) {
+			for (int x = x1; x <= x2; x += gridSize_cm) {
+
+				// does this point belong to the polygon?
+				if (!contains(Point2(x,y))) {continue;}
+
+				// call the callback
+				callback(x,y);
+
+			}
+		}
+
+	}
+
+};
+
+#endif // HELPERPOLY3_H

grid/walk/v3/Helper.h

@@ -84,43 +84,67 @@ namespace GW3 {
 
 	};
 
+	/**
+	 * data-structure to track to-be-visited nodes
+	 * push_back, pop_front
+	 * as pop_front is costly, we omit the pop and use a head-index instead
+	 * memory-consumption vs speed
+	 */
+	struct ToVisit {
+		size_t nextIdx = 0;
+		std::vector<uint32_t> vec;
+		ToVisit() {vec.reserve(256);}
+		void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
+		uint32_t next() {return vec[nextIdx++];}
+		bool empty() const {return nextIdx >= vec.size();}
+	};
+
 	/** get an iterator over all nodes reachable from the given start */
-	template <typename Node> class ReachableIteratorUnsorted {
+	template <typename Node, typename Conditions> class ReachableIteratorUnsorted {
 
 	const Grid<Node>& grid;
 	const Node& start;
 
 	Node* curNode = nullptr;
 	std::unordered_set<uint32_t> visited;
-	std::vector<uint32_t> toVisit;
+	ToVisit toVisit;
+
+	Conditions cond;
 
 	public:
 
-		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
+		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) {
-			toVisit.push_back(start.getIdx());
+			toVisit.add(start.getIdx());
 		}
 
 		bool hasNext() const {
 			return !toVisit.empty();
 		}
 
-		const Node& next(const std::function<bool(const Node&)>& skip) {
+		//const Node& next(const std::function<bool(const Node&)>& skip) {
-
+		//template <typename Skip> const Node& next(const Skip skip) {
-			const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance)
+		const Node& next() {
-			toVisit.erase(toVisit.begin());
-			visited.insert(curIdx);
 
+			// get the next to-be-visited node
+			const uint32_t curIdx = toVisit.next();			//visit from inside out (needed for correct distance)
 			const Node& curNode = grid[curIdx];
 
-			for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+			// mark as "visited"
+			visited.insert(curIdx);
 
-				const int neighborIdx = curNode.getNeighborIdx(i);
+			// get all neighbors
+			const int numNeighbors = curNode.getNumNeighbors();
+			for (int i = 0; i < numNeighbors; ++i) {
+
+				const uint32_t neighborIdx = curNode.getNeighborIdx(i);
 				const Node& neighbor = grid[neighborIdx];
 
+				const bool visit = cond.visit(neighbor) ;
+
 				// not yet reached -> store distance
-				if (!skip(neighbor)) {
+				if (visit) {
 					if (visited.find(neighborIdx) == visited.end()) {
-						toVisit.push_back(neighborIdx);
+						toVisit.add(neighborIdx);
 					}
 				}
 
@@ -150,10 +174,11 @@ namespace GW3 {
 
 	public:
 
-		static GridPoint p3ToGp(const Point3 p) {
+//		static GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
-			const Point3 p100 = p*100;
+//			const Point3 p100 = p*100;
-			return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
+//			//return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
-		}
+//			return GridPoint( grid.snapX(p100.x), grid.snapY(p100.y), grid.snapZ(p100.z) );
+//		}
 
 		static Point3 gpToP3(const GridPoint gp) {
 			return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
@@ -169,10 +194,22 @@ namespace GW3 {
 			return bbox.contains(pt);
 		}
 
+//		/** does one of the given grid-nodes contains the provided point-in-question? */
+//		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+//			for (const Node* n : nodes) {
+//				if (contains(grid, n, pt)) {
+//					return n;
+//				}
+//			}
+//			return nullptr;
+//		}
+
 		/** does one of the given grid-nodes contains the provided point-in-question? */
-		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+		static const Node* contains(const Grid<Node>& grid, const std::vector<const Node*>& nodes, Point2 pt) {
 			for (const Node* n : nodes) {
-				if (contains(grid, n, pt)) {return n;}
+				if (contains(grid, n, pt)) {
+					return n;
+				}
 			}
 			return nullptr;
 		}

grid/walk/v3/Reachable.h

@@ -0,0 +1,340 @@
+#ifndef INDOOR_GW3_REACHABLE_H
+#define INDOOR_GW3_REACHABLE_H
+
+#include <vector>
+#include <set>
+#include "../../Grid.h"
+
+namespace GW3 {
+
+	#define likely(x)       __builtin_expect((x),1)
+	#define unlikely(x)     __builtin_expect((x),0)
+
+
+
+	/**
+	 * get all grid nodes that are reachable within x-edges (depth)
+	 */
+	template <typename Node> class ReachableByDepthUnsorted {
+
+		struct VisitEntry {
+			const Node* gn;
+			int depth;
+			VisitEntry() {;}
+			VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;}
+		};
+
+		struct Visits {
+			VisitEntry visits[512];// __attribute__((aligned(16)));
+			size_t head = 0;
+			size_t tail = 0;
+			VisitEntry& getNext() {
+				return visits[tail++];
+			}
+			void add(const VisitEntry& e) {
+				visits[head++] = e;
+				assert(head < 512);
+				//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
+			}
+			bool hasMore() const {
+				return head > tail;
+			}
+		};
+
+		const Grid<Node>& grid;
+
+	public:
+
+		ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
+			;
+		}
+
+		/** get all nodes reachable from start using maxDepth steps */
+		std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
+
+			std::unordered_set<const Node*> checked;
+
+			// assuming max 8 neighbors per node, we need
+			// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
+			// which is 1 + (1+2+3+4+5)*neighbors
+			// which is 1 + (n*n + n)/2*neighbors
+			// however this seems to be slow?!
+			//const int n = maxDepth + 1;
+			//const int maxEntries = (n * n + n) / 2 * 10 + 1;
+			//const int toAlloc = 4096 / sizeof(VisitEntry);
+			//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
+			//if (maxDepth > 9) {throw Exception("will not fit!");}
+
+			Visits toVisit;
+
+			// directly start with the node itself and all its neighbors
+			checked.insert(&start);
+			for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
+				const int nIdx = start.getNeighborIdx(i);
+				const Node& gnNext = grid[nIdx];
+				checked.insert(&gnNext);
+				toVisit.add(VisitEntry(&gnNext, 1));
+			}
+
+			// check all to-be-visited nodes
+			while ( likely(toVisit.hasMore()) ) {
+
+				const VisitEntry& e = toVisit.getNext();
+
+				if ( likely(e.depth <= maxDepth) ) {
+
+					const Node* gnCur = e.gn;
+					for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+						const int nIdx = gnCur->getNeighborIdx(i);
+						const Node& gnNext = grid[nIdx];
+						if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+							toVisit.add(VisitEntry(&gnNext, e.depth+1));
+							checked.insert(&gnNext);
+						}
+					}
+
+				}
+
+			}
+
+			return checked;
+
+		}
+
+	};
+
+
+	/**
+	 * get all grid nodes that are reachable within x-edges (depth)
+	 * additionally returns the needed walking distance in meter
+	 */
+	template <typename Node> class ReachableByDepthWithDistanceSorted {
+
+		struct VisitEntry {
+			const Node* gn;
+			int depth;
+			float dist_m;
+			int myIdx;
+			VisitEntry() {;}
+			VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
+				gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
+		};
+
+		struct Visits {
+			VisitEntry visits[1024];// __attribute__((aligned(16)));
+			size_t head = 0;
+			size_t tail = 0;
+			VisitEntry& getNext() {
+				return visits[tail++];
+			}
+			void add(const VisitEntry& e) {
+				visits[head++] = e;
+				assert(head < 1024);
+				//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
+			}
+			bool hasMore() const {
+				return head > tail;
+			}
+			void sort() {
+				const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
+					return e1.dist_m < e2.dist_m;
+				};
+				std::sort(&visits[tail], &visits[head], comp);
+			}
+		};
+
+		const Grid<Node>& grid;
+
+	public:
+
+		/** result */
+		struct Entry {
+
+			const Node* node;
+			const float walkDistToStart_m;
+			const int prevIdx;
+
+			Entry(const Node* node, const float dist, const size_t prevIdx) :
+				node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}
+
+			bool hasPrev() const {
+				return prevIdx >= 0;
+			}
+
+		};
+
+		ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
+			;
+		}
+
+		/** get all nodes reachable from start using maxDepth steps */
+		std::vector<Entry> get(const Node& start, const int maxDepth) {
+
+			std::unordered_set<const Node*> checked;
+			std::vector<Entry> res;
+
+			Visits toVisit;
+
+			// directly start with the node itself and all its neighbors
+			checked.insert(&start);
+			res.push_back(Entry(&start, 0, -1));
+			for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
+				const int nIdx = start.getNeighborIdx(i);
+				const Node& gnNext = grid[nIdx];
+				const float dist_m = gnNext.getDistanceInMeter(start);
+				toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size()));
+				res.push_back(Entry(&gnNext, dist_m, 0));
+				checked.insert(&gnNext);
+			}
+			toVisit.sort();
+
+			// check all to-be-visited nodes
+			while ( likely(toVisit.hasMore()) ) {
+
+				const VisitEntry& e = toVisit.getNext();
+
+				if ( likely(e.depth <= maxDepth) ) {
+
+					const Node* gnCur = e.gn;
+
+	//				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+	//					const int nIdx = gnCur->getNeighborIdx(i);
+	//					const Node& gnNext = grid[nIdx];
+	//					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+	//						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
+	//						const float dist_m = e.dist_m + nodeNodeDist_m;
+	//						toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+	//						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
+	//						checked.insert(&gnNext);
+	//					}
+	//				}
+
+	//				const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
+
+					std::vector<VisitEntry> sub;
+
+					for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
+						const int nIdx = gnCur->getNeighborIdx(i);
+						const Node& gnNext = grid[nIdx];
+						if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
+							const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
+							const float dist_m = e.dist_m + nodeNodeDist_m;
+							//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+							sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
+							res.push_back(Entry(&gnNext, dist_m, e.myIdx));
+							checked.insert(&gnNext);
+
+						}
+					}
+
+					// dijkstra.. sort the new nodes by destination to start
+					// only sorting the 8 new nodes seems enough due to the graph's layout
+					const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
+						return e1.dist_m < e2.dist_m;
+					};
+
+					std::sort(sub.begin(), sub.end(), comp);
+
+					for (const VisitEntry& e : sub) {
+						toVisit.add(e);
+					}
+
+				}
+
+				// slower with same result ;)
+				//toVisit.sort();
+
+			}
+
+			return res;
+
+		}
+
+	};
+
+
+	/**
+	 * data-structure to track to-be-visited nodes
+	 * push_back, pop_front
+	 * as pop_front is costly, we omit the pop and use a head-index instead
+	 * memory-consumption vs speed
+	 */
+	struct _ToVisit {
+		size_t nextIdx = 0;
+		std::vector<uint32_t> vec;
+		_ToVisit() {vec.reserve(256);}
+		void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
+		uint32_t next() {return vec[nextIdx++];}
+		bool empty() const {return nextIdx >= vec.size();}
+	};
+
+
+
+	/** get a list of all nodes that are reachable after checking several conditions */
+	template <typename Node, typename Conditions> class ReachableByConditionUnsorted {
+
+
+	public:
+
+
+		static std::vector<const Node*> get(const Grid<Node>& grid, const Node& start, const Conditions cond) {
+
+			//Node* curNode = nullptr;
+			std::unordered_set<uint32_t> scheduled;
+			_ToVisit toVisit;
+			toVisit.add(start.getIdx());
+
+			std::vector<const Node*> res;
+
+			while(!toVisit.empty()) {
+
+				// get the next to-be-visited node
+				const uint32_t curIdx = toVisit.next();			//visit from inside out (needed for correct distance)
+				const Node& curNode = grid[curIdx];
+
+				// process current node
+				res.push_back(&curNode);
+				scheduled.insert(curIdx);
+
+				// get all neighbors
+				const int numNeighbors = curNode.getNumNeighbors();
+				for (int i = 0; i < numNeighbors; ++i) {
+
+					const uint32_t neighborIdx = curNode.getNeighborIdx(i);
+
+					// already visited?
+					if (scheduled.find(neighborIdx) != scheduled.end()) {continue;}
+					scheduled.insert(neighborIdx);
+
+					// matches the used condition?
+					const Node& neighbor = grid[neighborIdx];
+					if (!cond.visit(neighbor)) {continue;}
+
+					// OK!
+					toVisit.add(neighborIdx);
+
+				}
+
+			}
+
+			// done
+			return res;
+
+		}
+
+
+
+		//const Node& next(const std::function<bool(const Node&)>& skip) {
+		//template <typename Skip> const Node& next(const Skip skip) {
+		const Node& next() {
+
+
+
+		}
+
+	};
+
+
+}
+
+#endif // REACHABLE_H

grid/walk/v3/ReachableSampler.h

@@ -0,0 +1,81 @@
+#ifndef INDOOR_GW3_REACHABLESAMPLER_H
+#define INDOOR_GW3_REACHABLESAMPLER_H
+
+#include "../../../math/Random.h"
+
+#include "Reachable.h"
+#include "Helper.h"
+
+namespace GW3 {
+
+	template <typename Node> class ReachableSamplerByDepth {
+
+	public:
+
+		using Entry = typename ReachableByDepthWithDistanceSorted<Node>::Entry;
+
+		struct SampleResult {
+			Point3 pos;
+			float walkDistToStart_m;
+			SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;}
+		};
+
+	private:
+
+		const Grid<Node>& grid;
+		const float gridSize_m;
+
+		const std::vector<Entry>& reachableNodes;
+
+		mutable RandomGenerator gen;
+
+		mutable std::uniform_real_distribution<float> dOffset;
+
+
+	public:
+
+		/** ctor */
+		ReachableSamplerByDepth(const Grid<Node>& grid, const std::vector<Entry>& reachableNodes) :
+			grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) {
+			;
+		}
+
+		SampleResult sample() {
+
+			std::uniform_int_distribution<int> dIdx(0, reachableNodes.size() - 1);
+
+			const int idx = dIdx(gen);
+
+			const Entry* e = &reachableNodes[idx];
+			const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]);
+			const Node* nDst = e->node;
+
+			// center of the destination node
+			const Point3 nodeCenter = Helper<Node>::gpToP3(*nDst);
+
+			// random position within destination-node
+			const float ox = dOffset(gen);
+			const float oy = dOffset(gen);
+
+			// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
+			const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
+
+			// calculate end's walking-distance towards the start
+			float distToStart_m;
+			if (ePrev1) {
+				distToStart_m = ePrev1->walkDistToStart_m + (Helper<Node>::gpToP3(*(ePrev1->node)).getDistance(end));
+			} else {
+				distToStart_m = nodeCenter.getDistance(end);
+			}
+
+			// done
+			return SampleResult(end, distToStart_m);
+
+		}
+
+
+	};
+
+}
+
+#endif // REACHABLESAMPLER_H

grid/walk/v3/Structs.h

@@ -4,18 +4,55 @@
 #include "../../../geo/Heading.h"
 #include "../../../geo/Point3.h"
 #include <vector>
+#include "../../../math/Distributions.h"
+#include "../../../grid/Grid.h"
 
 namespace GW3 {
 
+	struct StepSizes {
+
+		float stepSizeFloor_m = NAN;
+		float stepSizeStair_m = NAN;
+
+		bool isValid() const {
+			return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
+		}
+
+		template <typename Node> float inMeter(const int steps, const Point3 start, const Grid<Node>& grid) const {
+
+			Assert::isTrue(isValid(), "invalid step-sizes given");
+
+			const GridPoint gp = grid.toGridPoint(start);
+			const Node& n = grid.getNodeFor(gp);
+			if (grid.isPlain(n)) {
+				return stepSizeFloor_m * steps;
+			} else {
+				return stepSizeStair_m * steps;
+			}
+
+		}
+
+	};
+
 	/** paremters for the walk */
 	struct WalkParams {
 
+		//Distribution::Normal<float> dDistFloor;
+		//Distribution::Normal<float> dDistStair;
+
 		Point3 start;
-		float distance_m;
+		//float distance_m;
+		int numSteps;
 		Heading heading = Heading(0);
 
 		float lookFurther_m = 1.5;
 
+		StepSizes stepSizes;
+
+		template <typename Node> float getDistanceInMeter(const Grid<Node>& grid) const {
+			return stepSizes.inMeter(numSteps, start, grid);
+		}
+
 	};
 
 	/** result of the random walk */

grid/walk/v3/WalkEvaluator.h

@@ -9,6 +9,29 @@
 
 namespace GW3 {
 
+	/** describes a potential walk, which can be evaluated */
+	struct PotentialWalk {
+
+		/** initial parameters (requested walk) */
+		const WalkParams& params;
+
+		/** walk started here */
+		Point3 pStart;
+
+		/** walk ended here */
+		Point3 pEnd;
+
+		/** usually the euclidean distance start<->end but not necessarily! */
+		float walkDist_m;
+
+		/** ctor */
+		PotentialWalk(const WalkParams& params, const Point3 pStart, const Point3 pEnd, const float walkedDistance_m) :
+			params(params), pStart(pStart), pEnd(pEnd), walkDist_m(walkedDistance_m) {
+			;
+		}
+
+	};
+
 	/** interface for all evaluators that return a probability for a given walk */
 	template <typename Node> class WalkEvaluator {
 
@@ -17,7 +40,7 @@ namespace GW3 {
 		/** get the probability for the given walk */
 		//virtual double getProbability(const Walk<Node>& walk) const = 0;
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0;
+		virtual double getProbability(const PotentialWalk& walk) const = 0;
 
 	};
 
@@ -31,15 +54,13 @@ namespace GW3 {
 
 		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			(void) params;
+			const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
-			(void) pStart;
-
-			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
 			const Node& node = grid->getNodeFor(gp);
 			const double p = node.getWalkImportance();
-			return std::pow(p,10);
+			return p;
+			//return std::pow(p,10);
 
 		}
 
@@ -50,25 +71,32 @@ namespace GW3 {
 	/** evaluate the difference between head(start,end) and the requested heading */
 	template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
 
-		const double sigma;
+		const double sigma_rad;
+		const double kappa;
+		Distribution::VonMises<double> _dist;
+		Distribution::LUT<double> dist;
 
 	public:
 
-		WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
+		// kappa = 1/var = 1/sigma^2
+		// https://en.wikipedia.org/wiki/Von_Mises_distribution
+		WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
+			sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
+			;
+		}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			(void) params;
+			if (walk.pStart == walk.pEnd) {
-
-			if (pStart == pEnd) {
 				std::cout << "warn! start-position == end-positon" << std::endl;
 				return 0;
 			}
 
-			const Heading head(pStart.xy(), pEnd.xy());
+			const Heading head(walk.pStart.xy(), walk.pEnd.xy());
-			const float diff = head.getDiffHalfRAD(params.heading);
+			const float diff = head.getDiffHalfRAD(walk.params.heading);
 			//const float diff = Heading::getSignedDiff(params.heading, head);
-			return Distribution::Normal<double>::getProbability(0, sigma, diff);
+			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
+			return dist.getProbability(diff);
 
 		}
 
@@ -77,16 +105,23 @@ namespace GW3 {
 	/** evaluate the difference between distance(start, end) and the requested distance */
 	template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
 
+		const Grid<Node>& grid;
+
 		const double sigma;
 
+		const Distribution::Normal<double> dist;
+
 	public:
 
-		WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
+		WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;}
 
-		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+		virtual double getProbability(const PotentialWalk& walk) const override {
 
-			const float walkedDistance_m = pStart.getDistance(pEnd);
+			const float requestedDistance_m = walk.params.getDistanceInMeter(grid);
-			return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+			const float walkedDistance_m = walk.walkDist_m;//pStart.getDistance(pEnd);
+			const float diff = walkedDistance_m - requestedDistance_m;
+			return dist.getProbability(diff);
+			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
 
 		}
 

grid/walk/v3/Walker.h

@@ -14,6 +14,8 @@
 #include "Helper.h"
 #include "Structs.h"
 #include "WalkEvaluator.h"
+#include "Reachable.h"
+#include "ReachableSampler.h"
 
 namespace GW3 {
 
@@ -26,18 +28,22 @@ namespace GW3 {
 	public:
 
 		/** get a new destination for the given start */
-		virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
+		virtual const WalkResult getDestination(const WalkParams& params) const = 0;
 
 	};
 
 	template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
 
-		//Random::RandomGenerator rnd;
+		Grid<Node>& grid;
-
 		std::vector<WalkEvaluator<Node>*> evals;
 
 	public:
 
+		/** ctor */
+		WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
+			;
+		}
+
 		/** make the code a little more readable */
 		using Helper = GW3::Helper<Node>;
 		using Walk = typename GW3::Walk<Node>;
@@ -50,44 +56,76 @@ namespace GW3 {
 		}
 
 		/** perform the walk based on the configured setup */
-		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
+		const WalkResult getDestination(const WalkParams& params) const override {
+
+			Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
+
+			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
 
-			Assert::isNot0(params.distance_m, "walking distance must be > 0");
 
 			static std::mt19937 rndGen;
 
-			const GridPoint gpStart = Helper::p3ToGp(params.start);
+			const GridPoint gpStart = grid.toGridPoint(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
 
+			// calculate a walk's probability
+			auto getP = [&] (const Point3 dst) {
+				double p = 1;
+				const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst));
+				for (const WalkEvaluator<Node>* eval : evals) {
+					const double p1 = eval->getProbability(pWalk);
+					p *= p1;
+				}
+				return p;
+			};
+
 			// include one additional grid-cell (increased distance)
-			const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
+			//const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1);
-			ReachableSettings set;
+			const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15);
-			set.limitDistance = true;
+
-			set.dist_m = params.distance_m + secBuffer_m;
+//			ReachableSettings set;
-			set.limitHeading = false;
+//			set.limitDistance = true;
-			set.heading = params.heading;
+//			set.dist_m = params.distance_m + secBuffer_m;
-			set.maxHeadingDiff_rad = M_PI/2;
+//			set.limitHeading = false;
-			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+//			set.heading = params.heading;
+//			set.maxHeadingDiff_rad = M_PI/2;
+
+//			// get all nodes that satisfy above constraints
+//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+
+			struct Cond {
+				const float maxDist_m;
+				const Node* startNode;
+				Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
+				bool visit(const Node& n) const {
+					return (startNode->getDistanceInMeter(n)) < maxDist_m;
+				}
+			};
+			Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
+			std::vector<const Node*>  reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
 
 			WalkResult res;
 			res.heading = params.heading;
 			res.position = params.start;
 
-
+			// get the to-be-reached destination's position (using start+distance+heading)
 			const Point2 dir = res.heading.asVector();
-			const Point2 dst = params.start.xy() + (dir * params.distance_m);
+			const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid));
 
-			// is dst reachable?
+			// is above destination reachable?
 			const Node* n = Helper::contains(grid, reachableNodes, dst);
+			//const Node* n = ri.contains(dst);
 			if (n) {
 
 				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
-				const GridPoint gp = Helper::p3ToGp(p3);
+				const GridPoint gp = grid.toGridPoint(p3);
+
+
 
 				if (grid.hasNodeFor(gp)) {
 					res.position = p3;					// update position
 					//res.heading;						// keep as-is
-					//res.probability;					// keep as-is
+					res.probability *= getP(p3);		// keep as-is
 					return res;							// done
 
 				} else {
@@ -111,9 +149,11 @@ namespace GW3 {
 			const Point3 start = params.start;
 			const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
 
+			const PotentialWalk pWalk(params, start, end, start.getDistance(end));
+
 			double p = 1;
 			for (const WalkEvaluator<Node>* eval : evals) {
-				const double p1 = eval->getProbability(start, end, params);
+				const double p1 = eval->getProbability(pWalk);
 				p *= p1;
 			}
 
@@ -123,8 +163,31 @@ namespace GW3 {
 			}
 
 			res.heading = Heading(start.xy(), end.xy());
-			res.probability = p;
+			res.probability *= getP(end);
 			res.position = end;
+
+			if (!grid.hasNodeFor(grid.toGridPoint(res.position))) {
+
+				std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl;
+
+				std::cout << "issue:" << res.position.asString() << std::endl;
+
+				for (int i = -80; i <= +80; i+=10) {
+					Point3 pos = res.position + Point3(0,0,i/100.0f);
+					std::cout << pos.asString() <<  " ----- " << res.position.asString() << std::endl;
+					std::cout << (grid.toGridPoint(pos)).asString() << std::endl;
+					std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl;
+					std::cout << std::endl;
+				}
+
+				int i = 0; (void) i;
+
+			}
+
+#if (GRID_MODE == GM_BOX)
+			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
+#endif
+
 			return res;
 
 		}
@@ -142,8 +205,20 @@ namespace GW3 {
 
 		std::vector<WalkEvaluator<Node>*> evals;
 
+		Grid<Node>& grid;
+		const float gridSize_m;
+
+		mutable std::minstd_rand rndGen;
+		mutable std::uniform_real_distribution<float> dFinal;
+
 	public:
 
+		/** ctor */
+		WalkerWeightedRandom(Grid<Node>& grid) :
+			grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
+			;
+		}
+
 		/** make the code a little more readable */
 		using Helper = GW3::Helper<Node>;
 		using Walk = typename GW3::Walk<Node>;
@@ -156,101 +231,189 @@ namespace GW3 {
 		}
 
 		/** perform the walk based on the configured setup */
-		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
+		const WalkResult getDestination(const WalkParams& params) const override {
 
-			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+			const float walkDist_m = params.getDistanceInMeter(grid);
 
-			static std::minstd_rand rndGen;
+			Assert::isNot0(walkDist_m, "walking distance must be > 0");
 
 			const GridPoint gpStart = Helper::p3ToGp(params.start);
 			const Node* startNode = grid.getNodePtrFor(gpStart);
+			if (!startNode) {throw Exception("start node not found!");}
 
-//			// include one additional grid-cell (increased distance)
+			const float maxDist = walkDist_m + gridSize_m;
-//			const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
+			const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
 
-//			ReachableSettings set;
+			Point3 best; double bestP = 0;
-//			set.limitDistance = true;
+			//DrawList<Point3> drawer;
-//			set.limitHeading = true;
-//			set.dist_m = params.distance_m + secBuffer_m;
-//			set.heading = params.heading;
-//			set.maxHeadingDiff_rad = M_PI/2;
-//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
-
-			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
-			//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
-
-
-			Point3 best;
-			double bestP = 0;
-//			DrawList<Point3> drawer;
 
 			const Point3 start = params.start;
 
-			// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
-			//for (int i = 0; i < 500; ++i) {
 
-			//	const Node* dstNode = reachableNodes[dNode(rndGen)];
+			struct RICond {
-
+				const GridPoint gpStart;
-			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
+				const float maxDist;
-
+				RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;}
-
+				bool visit (const Node& n) const {
-
+					const float dist_m = n.getDistanceInMeter(gpStart);
-			ReachableIteratorUnsorted<Node> ri(grid, *startNode);
+					return dist_m < maxDist;
-			const float maxDist = params.distance_m * 1.25 + gridSize_m;
+				}
-
-			auto skip = [&] (const Node& n) {
-				const float dist_m = n.getDistanceInMeter(gpStart);
-				return dist_m > maxDist;
 			};
+			RICond riCond(gpStart, maxDist);
 
-			//for (const Node* dstNode : reachableNodes) {
+			// iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance)
-			while(ri.hasNext()) {
+			ReachableIteratorUnsorted<Node, RICond> ri(grid, *startNode, riCond);
 
-				const Node* dstNode = &ri.next(skip);
+			int numVisitedNodes = 0;
-//				const float dist_m = dstNode->getDistanceInMeter(gpStart);
 
-//				if (dist_m > maxDist) {
+
-//					break;
+			#define MODE	2
+
+			#if (MODE == 1)
+
+				double bestNodeP = 0;
+				const Node* bestNode = nullptr;
+
+				ReachableByDepthUnsorted<Node> reach(grid);
+				std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
+
+				for (const Node* dstNode : nodes) {
+					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
+					const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
+					double p = 1.0;
+					for (const WalkEvaluator<Node>* eval : evals) {
+						const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
+						p *= p1;
+					}
+					if (p > bestNodeP) {
+						bestNodeP = p;
+						bestNode = dstNode;
+					}
+				}
+
+//				while(ri.hasNext()) {
+//					const Node* dstNode = &ri.next();
+//					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
+//					double p = 1.0;
+//					for (const WalkEvaluator<Node>* eval : evals) {
+//						const double p1 = eval->getProbability(start, nodeCenter, params);
+//						p *= p1;
+//					}
+//					if (p > bestNodeP) {
+//						bestNodeP = p;
+//						bestNode = dstNode;
+//					}
 //				}
 
-				for (int i = 0; i < 25; ++i) {
+				for (int i = 0; i < 10; ++i) {
+
+					const Point3 nodeCenter = Helper::gpToP3(*bestNode);
 
 					// random position within destination-node
-					const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
+					const float ox = dFinal(rndGen);
+					const float oy = dFinal(rndGen);
 
-					// destination = node-center + offset (within the node's bbox)
+					// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
-					const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
+					const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
-
+					const float walkDist_m = end.getDistance(start);//*1.05;
-					// sanity check
-					if (start == end) {continue;}
-					if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
-						std::cout << "random destination is not part of the grid" << std::endl;
-						continue;
-					}
-
-					//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
 
 					double p = 1;
 					for (const WalkEvaluator<Node>* eval : evals) {
-						const double p1 = eval->getProbability(start, end, params);
+						const double p1 = eval->getProbability(start, end, walkDist_m, params);
 						p *= p1;
 					}
 
 					if (p > bestP) {bestP = p; best = end;}
-					//drawer.add(end, p);
 
 				}
 
-			}
+			#elif (MODE == 2)
 
-			//const Point3 end = drawer.get();
+				ReachableByDepthUnsorted<Node> reach(grid);
+				std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
+
+				// all reachable nodes
+				//while(ri.hasNext()) {
+					//const Node* dstNode = &ri.next();
+
+				for (const Node* dstNode : nodes) {
+
+					++numVisitedNodes;
+
+					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
+
+					// try multiple locations within each reachable node
+					for (int i = 0; i < 3; ++i) {
+
+						// random position within destination-node
+						const float ox = dFinal(rndGen);
+						const float oy = dFinal(rndGen);
+
+						// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
+						const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
+
+						// sanity check
+						if (start == end) {continue;}
+	//					if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
+	//						std::cout << "random destination is not part of the grid" << std::endl;
+	//						continue;
+	//					}
+						//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
+						const float walkDist_m = end.getDistance(start);//*1.05;
+
+						const PotentialWalk pWalk(params, start, end, walkDist_m);
+
+						double p = 1;
+						for (const WalkEvaluator<Node>* eval : evals) {
+							const double p1 = eval->getProbability(pWalk);
+							p *= p1;
+						}
+
+						if (p > bestP) {bestP = p; best = end;}
+	//					drawer.add(end, p);
+
+					}
+
+				}
+
+			#elif (MODE == 3)
+
+				using Reachable = ReachableByDepthWithDistanceSorted<Node>;
+				using ReachableNode = typename Reachable::Entry;
+				Reachable reach(grid);
+				std::vector<ReachableNode> reachableNodes = reach.get(*startNode, depth);
+
+				using Sampler = ReachableSamplerByDepth<Node>;
+				using SamplerResult = typename Sampler::SampleResult;
+				Sampler sampler(grid, reachableNodes);
+
+				for (int i = 0; i < 1500; ++i) {
+
+					const SamplerResult sample = sampler.sample();
+
+					double p = 1;
+					for (const WalkEvaluator<Node>* eval : evals) {
+						const double p1 = eval->getProbability(start, sample.pos, sample.walkDistToStart_m*0.94, params);
+						p *= p1;
+					}
+
+					if (p > bestP) {bestP = p; best = sample.pos;}
+
+				}
+
+			#endif
+
+			//std::cout << numVisitedNodes << std::endl;
+
+			//double drawProb = 0; const Point3 end = drawer.get(drawProb);
 			const Point3 end = best;
 			WalkResult res;
 			if (start == end) {
 				res.probability = 0;
 			} else {
 				res.heading = Heading(start.xy(), end.xy());
-				res.probability = bestP;
+				//res.probability = drawProb;		// when using DrawList
+				res.probability = bestP;	// when using bestP
 			}
 			res.position = end;
 			return res;

lib/Recast/Recast.cpp

@@ -0,0 +1,511 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#include <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <new>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+float rcSqrt(float x)
+{
+	return sqrtf(x);
+}
+
+/// @class rcContext
+/// @par
+///
+/// This class does not provide logging or timer functionality on its
+/// own.  Both must be provided by a concrete implementation
+/// by overriding the protected member functions.  Also, this class does not
+/// provide an interface for extracting log messages. (Only adding them.)
+/// So concrete implementations must provide one.
+///
+/// If no logging or timers are required, just pass an instance of this
+/// class through the Recast build process.
+///
+
+/// @par
+///
+/// Example:
+/// @code
+/// // Where ctx is an instance of rcContext and filepath is a char array.
+/// ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath);
+/// @endcode
+void rcContext::log(const rcLogCategory category, const char* format, ...)
+{
+	if (!m_logEnabled)
+		return;
+	static const int MSG_SIZE = 512;
+	char msg[MSG_SIZE];
+	va_list ap;
+	va_start(ap, format);
+	int len = vsnprintf(msg, MSG_SIZE, format, ap);
+	if (len >= MSG_SIZE)
+	{
+		len = MSG_SIZE-1;
+		msg[MSG_SIZE-1] = '\0';
+	}
+	va_end(ap);
+	doLog(category, msg, len);
+}
+
+rcHeightfield* rcAllocHeightfield()
+{
+	return new (rcAlloc(sizeof(rcHeightfield), RC_ALLOC_PERM)) rcHeightfield;
+}
+
+rcHeightfield::rcHeightfield()
+	: width()
+	, height()
+	, bmin()
+	, bmax()
+	, cs()
+	, ch()
+	, spans()
+	, pools()
+	, freelist()
+{
+}
+
+rcHeightfield::~rcHeightfield()
+{
+	// Delete span array.
+	rcFree(spans);
+	// Delete span pools.
+	while (pools)
+	{
+		rcSpanPool* next = pools->next;
+		rcFree(pools);
+		pools = next;
+	}
+}
+
+void rcFreeHeightField(rcHeightfield* hf)
+{
+	if (!hf) return;
+	hf->~rcHeightfield();
+	rcFree(hf);
+}
+
+rcCompactHeightfield* rcAllocCompactHeightfield()
+{
+	rcCompactHeightfield* chf = (rcCompactHeightfield*)rcAlloc(sizeof(rcCompactHeightfield), RC_ALLOC_PERM);
+	memset(chf, 0, sizeof(rcCompactHeightfield));
+	return chf;
+}
+
+void rcFreeCompactHeightfield(rcCompactHeightfield* chf)
+{
+	if (!chf) return;
+	rcFree(chf->cells);
+	rcFree(chf->spans);
+	rcFree(chf->dist);
+	rcFree(chf->areas);
+	rcFree(chf);
+}
+
+rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
+{
+	rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM);
+	memset(lset, 0, sizeof(rcHeightfieldLayerSet));
+	return lset;
+}
+
+void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset)
+{
+	if (!lset) return;
+	for (int i = 0; i < lset->nlayers; ++i)
+	{
+		rcFree(lset->layers[i].heights);
+		rcFree(lset->layers[i].areas);
+		rcFree(lset->layers[i].cons);
+	}
+	rcFree(lset->layers);
+	rcFree(lset);
+}
+
+
+rcContourSet* rcAllocContourSet()
+{
+	rcContourSet* cset = (rcContourSet*)rcAlloc(sizeof(rcContourSet), RC_ALLOC_PERM);
+	memset(cset, 0, sizeof(rcContourSet));
+	return cset;
+}
+
+void rcFreeContourSet(rcContourSet* cset)
+{
+	if (!cset) return;
+	for (int i = 0; i < cset->nconts; ++i)
+	{
+		rcFree(cset->conts[i].verts);
+		rcFree(cset->conts[i].rverts);
+	}
+	rcFree(cset->conts);
+	rcFree(cset);
+}
+
+rcPolyMesh* rcAllocPolyMesh()
+{
+	rcPolyMesh* pmesh = (rcPolyMesh*)rcAlloc(sizeof(rcPolyMesh), RC_ALLOC_PERM);
+	memset(pmesh, 0, sizeof(rcPolyMesh));
+	return pmesh;
+}
+
+void rcFreePolyMesh(rcPolyMesh* pmesh)
+{
+	if (!pmesh) return;
+	rcFree(pmesh->verts);
+	rcFree(pmesh->polys);
+	rcFree(pmesh->regs);
+	rcFree(pmesh->flags);
+	rcFree(pmesh->areas);
+	rcFree(pmesh);
+}
+
+rcPolyMeshDetail* rcAllocPolyMeshDetail()
+{
+	rcPolyMeshDetail* dmesh = (rcPolyMeshDetail*)rcAlloc(sizeof(rcPolyMeshDetail), RC_ALLOC_PERM);
+	memset(dmesh, 0, sizeof(rcPolyMeshDetail));
+	return dmesh;
+}
+
+void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh)
+{
+	if (!dmesh) return;
+	rcFree(dmesh->meshes);
+	rcFree(dmesh->verts);
+	rcFree(dmesh->tris);
+	rcFree(dmesh);
+}
+
+void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
+{
+	// Calculate bounding box.
+	rcVcopy(bmin, verts);
+	rcVcopy(bmax, verts);
+	for (int i = 1; i < nv; ++i)
+	{
+		const float* v = &verts[i*3];
+		rcVmin(bmin, v);
+		rcVmax(bmax, v);
+	}
+}
+
+void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h)
+{
+	*w = (int)((bmax[0] - bmin[0])/cs+0.5f);
+	*h = (int)((bmax[2] - bmin[2])/cs+0.5f);
+}
+
+/// @par
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcAllocHeightfield, rcHeightfield
+bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height,
+						 const float* bmin, const float* bmax,
+						 float cs, float ch)
+{
+	rcIgnoreUnused(ctx);
+
+	hf.width = width;
+	hf.height = height;
+	rcVcopy(hf.bmin, bmin);
+	rcVcopy(hf.bmax, bmax);
+	hf.cs = cs;
+	hf.ch = ch;
+	hf.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*)*hf.width*hf.height, RC_ALLOC_PERM);
+	if (!hf.spans)
+		return false;
+	memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height);
+	return true;
+}
+
+static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm)
+{
+	float e0[3], e1[3];
+	rcVsub(e0, v1, v0);
+	rcVsub(e1, v2, v0);
+	rcVcross(norm, e0, e1);
+	rcVnormalize(norm);
+}
+
+/// @par
+///
+/// Only sets the area id's for the walkable triangles.  Does not alter the
+/// area id's for unwalkable triangles.
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
+void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
+							 const float* verts, int nv,
+							 const int* tris, int nt,
+							 unsigned char* areas)
+{
+	rcIgnoreUnused(ctx);
+	rcIgnoreUnused(nv);
+
+	const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
+
+	float norm[3];
+
+	for (int i = 0; i < nt; ++i)
+	{
+		const int* tri = &tris[i*3];
+		int a = tri[0];
+		int b = tri[1];
+		int c = tri[2];
+		float aa = verts[6];
+		float bb = verts[7];
+		float cc = verts[8];
+
+		calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
+		// Check if the face is walkable.
+		if (norm[1] > walkableThr)
+			areas[i] = RC_WALKABLE_AREA;
+	}
+}
+
+/// @par
+///
+/// Only sets the area id's for the unwalkable triangles.  Does not alter the
+/// area id's for walkable triangles.
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
+void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
+								const float* verts, int /*nv*/,
+								const int* tris, int nt,
+								unsigned char* areas)
+{
+	rcIgnoreUnused(ctx);
+
+	const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
+
+	float norm[3];
+
+	for (int i = 0; i < nt; ++i)
+	{
+		const int* tri = &tris[i*3];
+		calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
+		// Check if the face is walkable.
+		if (norm[1] <= walkableThr)
+			areas[i] = RC_NULL_AREA;
+	}
+}
+
+int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf)
+{
+	rcIgnoreUnused(ctx);
+
+	const int w = hf.width;
+	const int h = hf.height;
+	int spanCount = 0;
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next)
+			{
+				if (s->area != RC_NULL_AREA)
+					spanCount++;
+			}
+		}
+	}
+	return spanCount;
+}
+
+/// @par
+///
+/// This is just the beginning of the process of fully building a compact heightfield.
+/// Various filters may be applied, then the distance field and regions built.
+/// E.g: #rcBuildDistanceField and #rcBuildRegions
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcAllocCompactHeightfield, rcHeightfield, rcCompactHeightfield, rcConfig
+bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb,
+							   rcHeightfield& hf, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
+
+	const int w = hf.width;
+	const int h = hf.height;
+	const int spanCount = rcGetHeightFieldSpanCount(ctx, hf);
+
+	// Fill in header.
+	chf.width = w;
+	chf.height = h;
+	chf.spanCount = spanCount;
+	chf.walkableHeight = walkableHeight;
+	chf.walkableClimb = walkableClimb;
+	chf.maxRegions = 0;
+	rcVcopy(chf.bmin, hf.bmin);
+	rcVcopy(chf.bmax, hf.bmax);
+	chf.bmax[1] += walkableHeight*hf.ch;
+	chf.cs = hf.cs;
+	chf.ch = hf.ch;
+	chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*w*h, RC_ALLOC_PERM);
+	if (!chf.cells)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h);
+		return false;
+	}
+	memset(chf.cells, 0, sizeof(rcCompactCell)*w*h);
+	chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*spanCount, RC_ALLOC_PERM);
+	if (!chf.spans)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
+		return false;
+	}
+	memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount);
+	chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*spanCount, RC_ALLOC_PERM);
+	if (!chf.areas)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount);
+		return false;
+	}
+	memset(chf.areas, RC_NULL_AREA, sizeof(unsigned char)*spanCount);
+
+	const int MAX_HEIGHT = 0xffff;
+
+	// Fill in cells and spans.
+	int idx = 0;
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcSpan* s = hf.spans[x + y*w];
+			// If there are no spans at this cell, just leave the data to index=0, count=0.
+			if (!s) continue;
+			rcCompactCell& c = chf.cells[x+y*w];
+			c.index = idx;
+			c.count = 0;
+			while (s)
+			{
+				if (s->area != RC_NULL_AREA)
+				{
+					const int bot = (int)s->smax;
+					const int top = s->next ? (int)s->next->smin : MAX_HEIGHT;
+					chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff);
+					chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
+					chf.areas[idx] = s->area;
+					idx++;
+					c.count++;
+				}
+				s = s->next;
+			}
+		}
+	}
+
+	// Find neighbour connections.
+	const int MAX_LAYERS = RC_NOT_CONNECTED-1;
+	int tooHighNeighbour = 0;
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					rcSetCon(s, dir, RC_NOT_CONNECTED);
+					const int nx = x + rcGetDirOffsetX(dir);
+					const int ny = y + rcGetDirOffsetY(dir);
+					// First check that the neighbour cell is in bounds.
+					if (nx < 0 || ny < 0 || nx >= w || ny >= h)
+						continue;
+
+					// Iterate over all neighbour spans and check if any of the is
+					// accessible from current cell.
+					const rcCompactCell& nc = chf.cells[nx+ny*w];
+					for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k)
+					{
+						const rcCompactSpan& ns = chf.spans[k];
+						const int bot = rcMax(s.y, ns.y);
+						const int top = rcMin(s.y+s.h, ns.y+ns.h);
+
+						// Check that the gap between the spans is walkable,
+						// and that the climb height between the gaps is not too high.
+						if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
+						{
+							// Mark direction as walkable.
+							const int lidx = k - (int)nc.index;
+							if (lidx < 0 || lidx > MAX_LAYERS)
+							{
+								tooHighNeighbour = rcMax(tooHighNeighbour, lidx);
+								continue;
+							}
+							rcSetCon(s, dir, lidx);
+							break;
+						}
+					}
+
+				}
+			}
+		}
+	}
+
+	if (tooHighNeighbour > MAX_LAYERS)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)",
+				 tooHighNeighbour, MAX_LAYERS);
+	}
+
+	return true;
+}
+
+/*
+static int getHeightfieldMemoryUsage(const rcHeightfield& hf)
+{
+	int size = 0;
+	size += sizeof(hf);
+	size += hf.width * hf.height * sizeof(rcSpan*);
+
+	rcSpanPool* pool = hf.pools;
+	while (pool)
+	{
+		size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL;
+		pool = pool->next;
+	}
+	return size;
+}
+
+static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf)
+{
+	int size = 0;
+	size += sizeof(rcCompactHeightfield);
+	size += sizeof(rcCompactSpan) * chf.spanCount;
+	size += sizeof(rcCompactCell) * chf.width * chf.height;
+	return size;
+}
+*/

lib/Recast/RecastAlloc.cpp

@@ -0,0 +1,86 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#include <stdlib.h>
+#include <string.h>
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+static void *rcAllocDefault(size_t size, rcAllocHint)
+{
+	return malloc(size);
+}
+
+static void rcFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static rcAllocFunc* sRecastAllocFunc = rcAllocDefault;
+static rcFreeFunc* sRecastFreeFunc = rcFreeDefault;
+
+/// @see rcAlloc, rcFree
+void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc)
+{
+	sRecastAllocFunc = allocFunc ? allocFunc : rcAllocDefault;
+	sRecastFreeFunc = freeFunc ? freeFunc : rcFreeDefault;
+}
+
+/// @see rcAllocSetCustom
+void* rcAlloc(size_t size, rcAllocHint hint)
+{
+	return sRecastAllocFunc(size, hint);
+}
+
+/// @par
+///
+/// @warning This function leaves the value of @p ptr unchanged.  So it still
+/// points to the same (now invalid) location, and not to null.
+/// 
+/// @see rcAllocSetCustom
+void rcFree(void* ptr)
+{
+	if (ptr)
+		sRecastFreeFunc(ptr);
+}
+
+/// @class rcIntArray
+///
+/// While it is possible to pre-allocate a specific array size during 
+/// construction or by using the #resize method, certain methods will 
+/// automatically resize the array as needed.
+///
+/// @warning The array memory is not initialized to zero when the size is 
+/// manually set during construction or when using #resize.
+
+/// @par
+///
+/// Using this method ensures the array is at least large enough to hold
+/// the specified number of elements.  This can improve performance by
+/// avoiding auto-resizing during use.
+void rcIntArray::doResize(int n)
+{
+	if (!m_cap) m_cap = n;
+	while (m_cap < n) m_cap *= 2;
+	int* newData = (int*)rcAlloc(m_cap*sizeof(int), RC_ALLOC_TEMP);
+	rcAssert(newData);
+	if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int));
+	rcFree(m_data);
+	m_data = newData;
+}
+

lib/Recast/RecastAlloc.h

@@ -0,0 +1,146 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#ifndef RECASTALLOC_H
+#define RECASTALLOC_H
+
+#include <stddef.h>
+
+/// Provides hint values to the memory allocator on how long the
+/// memory is expected to be used.
+enum rcAllocHint
+{
+	RC_ALLOC_PERM,		///< Memory will persist after a function call.
+	RC_ALLOC_TEMP		///< Memory used temporarily within a function.
+};
+
+/// A memory allocation function.
+//  @param[in]		size			The size, in bytes of memory, to allocate.
+//  @param[in]		rcAllocHint	A hint to the allocator on how long the memory is expected to be in use.
+//  @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
+///  @see rcAllocSetCustom
+typedef void* (rcAllocFunc)(size_t size, rcAllocHint hint);
+
+/// A memory deallocation function.
+///  @param[in]		ptr		A pointer to a memory block previously allocated using #rcAllocFunc.
+/// @see rcAllocSetCustom
+typedef void (rcFreeFunc)(void* ptr);
+
+/// Sets the base custom allocation functions to be used by Recast.
+///  @param[in]		allocFunc	The memory allocation function to be used by #rcAlloc
+///  @param[in]		freeFunc	The memory de-allocation function to be used by #rcFree
+void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc);
+
+/// Allocates a memory block.
+///  @param[in]		size	The size, in bytes of memory, to allocate.
+///  @param[in]		hint	A hint to the allocator on how long the memory is expected to be in use.
+///  @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
+/// @see rcFree
+void* rcAlloc(size_t size, rcAllocHint hint);
+
+/// Deallocates a memory block.
+///  @param[in]		ptr		A pointer to a memory block previously allocated using #rcAlloc.
+/// @see rcAlloc
+void rcFree(void* ptr);
+
+
+/// A simple dynamic array of integers.
+class rcIntArray
+{
+	int* m_data;
+	int m_size, m_cap;
+
+	void doResize(int n);
+	
+	// Explicitly disabled copy constructor and copy assignment operator.
+	rcIntArray(const rcIntArray&);
+	rcIntArray& operator=(const rcIntArray&);
+
+public:
+	/// Constructs an instance with an initial array size of zero.
+	rcIntArray() : m_data(0), m_size(0), m_cap(0) {}
+
+	/// Constructs an instance initialized to the specified size.
+	///  @param[in]		n	The initial size of the integer array.
+	rcIntArray(int n) : m_data(0), m_size(0), m_cap(0) { resize(n); }
+	~rcIntArray() { rcFree(m_data); }
+
+	/// Specifies the new size of the integer array.
+	///  @param[in]		n	The new size of the integer array.
+	void resize(int n)
+	{
+		if (n > m_cap)
+			doResize(n);
+		
+		m_size = n;
+	}
+
+	/// Push the specified integer onto the end of the array and increases the size by one.
+	///  @param[in]		item	The new value.
+	void push(int item) { resize(m_size+1); m_data[m_size-1] = item; }
+
+	/// Returns the value at the end of the array and reduces the size by one.
+	///  @return The value at the end of the array.
+	int pop()
+	{
+		if (m_size > 0)
+			m_size--;
+		
+		return m_data[m_size];
+	}
+
+	/// The value at the specified array index.
+	/// @warning Does not provide overflow protection.
+	///  @param[in]		i	The index of the value.
+	const int& operator[](int i) const { return m_data[i]; }
+
+	/// The value at the specified array index.
+	/// @warning Does not provide overflow protection.
+	///  @param[in]		i	The index of the value.
+	int& operator[](int i) { return m_data[i]; }
+
+	/// The current size of the integer array.
+	int size() const { return m_size; }
+};
+
+/// A simple helper class used to delete an array when it goes out of scope.
+/// @note This class is rarely if ever used by the end user.
+template<class T> class rcScopedDelete
+{
+	T* ptr;
+public:
+
+	/// Constructs an instance with a null pointer.
+	inline rcScopedDelete() : ptr(0) {}
+
+	/// Constructs an instance with the specified pointer.
+	///  @param[in]		p	An pointer to an allocated array.
+	inline rcScopedDelete(T* p) : ptr(p) {}
+	inline ~rcScopedDelete() { rcFree(ptr); }
+
+	/// The root array pointer.
+	///  @return The root array pointer.
+	inline operator T*() { return ptr; }
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	rcScopedDelete(const rcScopedDelete&);
+	rcScopedDelete& operator=(const rcScopedDelete&);
+};
+
+#endif

lib/Recast/RecastArea.cpp

@@ -0,0 +1,591 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#include <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+/// @par 
+/// 
+/// Basically, any spans that are closer to a boundary or obstruction than the specified radius 
+/// are marked as unwalkable.
+///
+/// This method is usually called immediately after the heightfield has been built.
+///
+/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius
+bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedTimer timer(ctx, RC_TIMER_ERODE_AREA);
+	
+	unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
+	if (!dist)
+	{
+		ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount);
+		return false;
+	}
+	
+	// Init distance.
+	memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount);
+	
+	// Mark boundary cells.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (chf.areas[i] == RC_NULL_AREA)
+				{
+					dist[i] = 0;
+				}
+				else
+				{
+					const rcCompactSpan& s = chf.spans[i];
+					int nc = 0;
+					for (int dir = 0; dir < 4; ++dir)
+					{
+						if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+						{
+							const int nx = x + rcGetDirOffsetX(dir);
+							const int ny = y + rcGetDirOffsetY(dir);
+							const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir);
+							if (chf.areas[nidx] != RC_NULL_AREA)
+							{
+								nc++;
+							}
+						}
+					}
+					// At least one missing neighbour.
+					if (nc != 4)
+						dist[i] = 0;
+				}
+			}
+		}
+	}
+	
+	unsigned char nd;
+	
+	// Pass 1
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					// (-1,0)
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (-1,-1)
+					if (rcGetCon(as, 3) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(3);
+						const int aay = ay + rcGetDirOffsetY(3);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+				if (rcGetCon(s, 3) != RC_NOT_CONNECTED)
+				{
+					// (0,-1)
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (1,-1)
+					if (rcGetCon(as, 2) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(2);
+						const int aay = ay + rcGetDirOffsetY(2);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+			}
+		}
+	}
+	
+	// Pass 2
+	for (int y = h-1; y >= 0; --y)
+	{
+		for (int x = w-1; x >= 0; --x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				
+				if (rcGetCon(s, 2) != RC_NOT_CONNECTED)
+				{
+					// (1,0)
+					const int ax = x + rcGetDirOffsetX(2);
+					const int ay = y + rcGetDirOffsetY(2);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (1,1)
+					if (rcGetCon(as, 1) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(1);
+						const int aay = ay + rcGetDirOffsetY(1);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+				if (rcGetCon(s, 1) != RC_NOT_CONNECTED)
+				{
+					// (0,1)
+					const int ax = x + rcGetDirOffsetX(1);
+					const int ay = y + rcGetDirOffsetY(1);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (-1,1)
+					if (rcGetCon(as, 0) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(0);
+						const int aay = ay + rcGetDirOffsetY(0);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+			}
+		}
+	}
+	
+	const unsigned char thr = (unsigned char)(radius*2);
+	for (int i = 0; i < chf.spanCount; ++i)
+		if (dist[i] < thr)
+			chf.areas[i] = RC_NULL_AREA;
+	
+	rcFree(dist);
+	
+	return true;
+}
+
+static void insertSort(unsigned char* a, const int n)
+{
+	int i, j;
+	for (i = 1; i < n; i++)
+	{
+		const unsigned char value = a[i];
+		for (j = i - 1; j >= 0 && a[j] > value; j--)
+			a[j+1] = a[j];
+		a[j+1] = value;
+	}
+}
+
+/// @par
+///
+/// This filter is usually applied after applying area id's using functions
+/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea.
+/// 
+/// @see rcCompactHeightfield
+bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MEDIAN_AREA);
+	
+	unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
+	if (!areas)
+	{
+		ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount);
+		return false;
+	}
+	
+	// Init distance.
+	memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount);
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA)
+				{
+					areas[i] = chf.areas[i];
+					continue;
+				}
+				
+				unsigned char nei[9];
+				for (int j = 0; j < 9; ++j)
+					nei[j] = chf.areas[i];
+				
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						if (chf.areas[ai] != RC_NULL_AREA)
+							nei[dir*2+0] = chf.areas[ai];
+						
+						const rcCompactSpan& as = chf.spans[ai];
+						const int dir2 = (dir+1) & 0x3;
+						if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
+						{
+							const int ax2 = ax + rcGetDirOffsetX(dir2);
+							const int ay2 = ay + rcGetDirOffsetY(dir2);
+							const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
+							if (chf.areas[ai2] != RC_NULL_AREA)
+								nei[dir*2+1] = chf.areas[ai2];
+						}
+					}
+				}
+				insertSort(nei, 9);
+				areas[i] = nei[4];
+			}
+		}
+	}
+	
+	memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount);
+	
+	rcFree(areas);
+	
+	return true;
+}
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId,
+				   rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MARK_BOX_AREA);
+
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					if (chf.areas[i] != RC_NULL_AREA)
+						chf.areas[i] = areaId;
+				}
+			}
+		}
+	}
+}
+
+
+static int pointInPoly(int nvert, const float* verts, const float* p)
+{
+	int i, j, c = 0;
+	for (i = 0, j = nvert-1; i < nvert; j = i++)
+	{
+		const float* vi = &verts[i*3];
+		const float* vj = &verts[j*3];
+		if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
+			(p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
+			c = !c;
+	}
+	return c;
+}
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// The y-values of the polygon vertices are ignored. So the polygon is effectively 
+/// projected onto the xz-plane at @p hmin, then extruded to @p hmax.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
+						  const float hmin, const float hmax, unsigned char areaId,
+						  rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA);
+
+	float bmin[3], bmax[3];
+	rcVcopy(bmin, verts);
+	rcVcopy(bmax, verts);
+	for (int i = 1; i < nverts; ++i)
+	{
+		rcVmin(bmin, &verts[i*3]);
+		rcVmax(bmax, &verts[i*3]);
+	}
+	bmin[1] = hmin;
+	bmax[1] = hmax;
+
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+	
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	
+	// TODO: Optimize.
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA)
+					continue;
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					float p[3];
+					p[0] = chf.bmin[0] + (x+0.5f)*chf.cs; 
+					p[1] = 0;
+					p[2] = chf.bmin[2] + (z+0.5f)*chf.cs; 
+
+					if (pointInPoly(nverts, verts, p))
+					{
+						chf.areas[i] = areaId;
+					}
+				}
+			}
+		}
+	}
+}
+
+int rcOffsetPoly(const float* verts, const int nverts, const float offset,
+				 float* outVerts, const int maxOutVerts)
+{
+	const float	MITER_LIMIT = 1.20f;
+
+	int n = 0;
+
+	for (int i = 0; i < nverts; i++)
+	{
+		const int a = (i+nverts-1) % nverts;
+		const int b = i;
+		const int c = (i+1) % nverts;
+		const float* va = &verts[a*3];
+		const float* vb = &verts[b*3];
+		const float* vc = &verts[c*3];
+		float dx0 = vb[0] - va[0];
+		float dy0 = vb[2] - va[2];
+		float d0 = dx0*dx0 + dy0*dy0;
+		if (d0 > 1e-6f)
+		{
+			d0 = 1.0f/rcSqrt(d0);
+			dx0 *= d0;
+			dy0 *= d0;
+		}
+		float dx1 = vc[0] - vb[0];
+		float dy1 = vc[2] - vb[2];
+		float d1 = dx1*dx1 + dy1*dy1;
+		if (d1 > 1e-6f)
+		{
+			d1 = 1.0f/rcSqrt(d1);
+			dx1 *= d1;
+			dy1 *= d1;
+		}
+		const float dlx0 = -dy0;
+		const float dly0 = dx0;
+		const float dlx1 = -dy1;
+		const float dly1 = dx1;
+		float cross = dx1*dy0 - dx0*dy1;
+		float dmx = (dlx0 + dlx1) * 0.5f;
+		float dmy = (dly0 + dly1) * 0.5f;
+		float dmr2 = dmx*dmx + dmy*dmy;
+		bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f;
+		if (dmr2 > 1e-6f)
+		{
+			const float scale = 1.0f / dmr2;
+			dmx *= scale;
+			dmy *= scale;
+		}
+
+		if (bevel && cross < 0.0f)
+		{
+			if (n+2 >= maxOutVerts)
+				return 0;
+			float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f;
+			outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset;
+			n++;
+			outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset;
+			n++;
+		}
+		else
+		{
+			if (n+1 >= maxOutVerts)
+				return 0;
+			outVerts[n*3+0] = vb[0] - dmx*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] - dmy*offset;
+			n++;
+		}
+	}
+	
+	return n;
+}
+
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkCylinderArea(rcContext* ctx, const float* pos,
+						const float r, const float h, unsigned char areaId,
+						rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MARK_CYLINDER_AREA);
+	
+	float bmin[3], bmax[3];
+	bmin[0] = pos[0] - r;
+	bmin[1] = pos[1];
+	bmin[2] = pos[2] - r;
+	bmax[0] = pos[0] + r;
+	bmax[1] = pos[1] + h;
+	bmax[2] = pos[2] + r;
+	const float r2 = r*r;
+	
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+	
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				
+				if (chf.areas[i] == RC_NULL_AREA)
+					continue;
+				
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; 
+					const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; 
+					const float dx = sx - pos[0];
+					const float dz = sz - pos[2];
+					
+					if (dx*dx + dz*dz < r2)
+					{
+						chf.areas[i] = areaId;
+					}
+				}
+			}
+		}
+	}
+}

lib/Recast/RecastAssert.cpp

@@ -0,0 +1,35 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#include "RecastAssert.h"
+
+#ifndef NDEBUG
+
+static rcAssertFailFunc* sRecastAssertFailFunc = 0;
+
+void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc)
+{
+	sRecastAssertFailFunc = assertFailFunc;
+}
+
+rcAssertFailFunc* rcAssertFailGetCustom()
+{
+	return sRecastAssertFailFunc;
+}
+
+#endif

lib/Recast/RecastAssert.h

@@ -0,0 +1,56 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#ifndef RECASTASSERT_H
+#define RECASTASSERT_H
+
+// Note: This header file's only purpose is to include define assert.
+// Feel free to change the file and include your own implementation instead.
+
+#ifdef NDEBUG
+
+// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/
+#	define rcAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false)
+
+#else
+
+/// An assertion failure function.
+//  @param[in]		expression  asserted expression.
+//  @param[in]		file  Filename of the failed assertion.
+//  @param[in]		line  Line number of the failed assertion.
+///  @see rcAssertFailSetCustom
+typedef void (rcAssertFailFunc)(const char* expression, const char* file, int line);
+
+/// Sets the base custom assertion failure function to be used by Recast.
+///  @param[in]		assertFailFunc	The function to be used in case of failure of #dtAssert
+void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc);
+
+/// Gets the base custom assertion failure function to be used by Recast.
+rcAssertFailFunc* rcAssertFailGetCustom();
+
+#	include <assert.h> 
+#	define rcAssert(expression) \
+		{ \
+			rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \
+			if(failFunc == NULL) { assert(expression); } \
+			else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \
+		}
+
+#endif
+
+#endif // RECASTASSERT_H

lib/Recast/RecastFilter.cpp

@@ -0,0 +1,202 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAssert.h"
+
+/// @par
+///
+/// Allows the formation of walkable regions that will flow over low lying 
+/// objects such as curbs, and up structures such as stairways. 
+/// 
+/// Two neighboring spans are walkable if: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) < waklableClimb</tt>
+/// 
+/// @warning Will override the effect of #rcFilterLedgeSpans.  So if both filters are used, call
+/// #rcFilterLedgeSpans after calling this filter. 
+///
+/// @see rcHeightfield, rcConfig
+void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb, rcHeightfield& solid)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_FILTER_LOW_OBSTACLES);
+	
+	const int w = solid.width;
+	const int h = solid.height;
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			rcSpan* ps = 0;
+			bool previousWalkable = false;
+			unsigned char previousArea = RC_NULL_AREA;
+			
+			for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next)
+			{
+				const bool walkable = s->area != RC_NULL_AREA;
+				// If current span is not walkable, but there is walkable
+				// span just below it, mark the span above it walkable too.
+				if (!walkable && previousWalkable)
+				{
+					if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb)
+						s->area = previousArea;
+				}
+				// Copy walkable flag so that it cannot propagate
+				// past multiple non-walkable objects.
+				previousWalkable = walkable;
+				previousArea = s->area;
+			}
+		}
+	}
+}
+
+/// @par
+///
+/// A ledge is a span with one or more neighbors whose maximum is further away than @p walkableClimb
+/// from the current span's maximum.
+/// This method removes the impact of the overestimation of conservative voxelization 
+/// so the resulting mesh will not have regions hanging in the air over ledges.
+/// 
+/// A span is a ledge if: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) > walkableClimb</tt>
+/// 
+/// @see rcHeightfield, rcConfig
+void rcFilterLedgeSpans(rcContext* ctx, const int walkableHeight, const int walkableClimb,
+						rcHeightfield& solid)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_FILTER_BORDER);
+
+	const int w = solid.width;
+	const int h = solid.height;
+	const int MAX_HEIGHT = 0xffff;
+	
+	// Mark border spans.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next)
+			{
+				// Skip non walkable spans.
+				if (s->area == RC_NULL_AREA)
+					continue;
+				
+				const int bot = (int)(s->smax);
+				const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT;
+				
+				// Find neighbours minimum height.
+				int minh = MAX_HEIGHT;
+
+				// Min and max height of accessible neighbours.
+				int asmin = s->smax;
+				int asmax = s->smax;
+
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					int dx = x + rcGetDirOffsetX(dir);
+					int dy = y + rcGetDirOffsetY(dir);
+					// Skip neighbours which are out of bounds.
+					if (dx < 0 || dy < 0 || dx >= w || dy >= h)
+					{
+						minh = rcMin(minh, -walkableClimb - bot);
+						continue;
+					}
+
+					// From minus infinity to the first span.
+					rcSpan* ns = solid.spans[dx + dy*w];
+					int nbot = -walkableClimb;
+					int ntop = ns ? (int)ns->smin : MAX_HEIGHT;
+					// Skip neightbour if the gap between the spans is too small.
+					if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight)
+						minh = rcMin(minh, nbot - bot);
+					
+					// Rest of the spans.
+					for (ns = solid.spans[dx + dy*w]; ns; ns = ns->next)
+					{
+						nbot = (int)ns->smax;
+						ntop = ns->next ? (int)ns->next->smin : MAX_HEIGHT;
+						// Skip neightbour if the gap between the spans is too small.
+						if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight)
+						{
+							minh = rcMin(minh, nbot - bot);
+						
+							// Find min/max accessible neighbour height. 
+							if (rcAbs(nbot - bot) <= walkableClimb)
+							{
+								if (nbot < asmin) asmin = nbot;
+								if (nbot > asmax) asmax = nbot;
+							}
+							
+						}
+					}
+				}
+				
+				// The current span is close to a ledge if the drop to any
+				// neighbour span is less than the walkableClimb.
+				if (minh < -walkableClimb)
+				{
+					s->area = RC_NULL_AREA;
+				}
+				// If the difference between all neighbours is too large,
+				// we are at steep slope, mark the span as ledge.
+				else if ((asmax - asmin) > walkableClimb)
+				{
+					s->area = RC_NULL_AREA;
+				}
+			}
+		}
+	}
+}
+
+/// @par
+///
+/// For this filter, the clearance above the span is the distance from the span's 
+/// maximum to the next higher span's minimum. (Same grid column.)
+/// 
+/// @see rcHeightfield, rcConfig
+void rcFilterWalkableLowHeightSpans(rcContext* ctx, int walkableHeight, rcHeightfield& solid)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_FILTER_WALKABLE);
+	
+	const int w = solid.width;
+	const int h = solid.height;
+	const int MAX_HEIGHT = 0xffff;
+	
+	// Remove walkable flag from spans which do not have enough
+	// space above them for the agent to stand there.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next)
+			{
+				const int bot = (int)(s->smax);
+				const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT;
+				if ((top - bot) <= walkableHeight)
+					s->area = RC_NULL_AREA;
+			}
+		}
+	}
+}

lib/Recast/RecastLayers.cpp

@@ -0,0 +1,644 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#include <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+
+// Must be 255 or smaller (not 256) because layer IDs are stored as
+// a byte where 255 is a special value.
+static const int RC_MAX_LAYERS = 63;
+static const int RC_MAX_NEIS = 16;
+
+struct rcLayerRegion
+{
+	unsigned char layers[RC_MAX_LAYERS];
+	unsigned char neis[RC_MAX_NEIS];
+	unsigned short ymin, ymax;
+	unsigned char layerId;		// Layer ID
+	unsigned char nlayers;		// Layer count
+	unsigned char nneis;		// Neighbour count
+	unsigned char base;		// Flag indicating if the region is the base of merged regions.
+};
+
+
+static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v)
+{
+	const int n = (int)an;
+	for (int i = 0; i < n; ++i)
+	{
+		if (a[i] == v)
+			return true;
+	}
+	return false;
+}
+
+static bool addUnique(unsigned char* a, unsigned char& an, int anMax, unsigned char v)
+{
+	if (contains(a, an, v))
+		return true;
+
+	if ((int)an >= anMax)
+		return false;
+
+	a[an] = v;
+	an++;
+	return true;
+}
+
+
+inline bool overlapRange(const unsigned short amin, const unsigned short amax,
+						 const unsigned short bmin, const unsigned short bmax)
+{
+	return (amin > bmax || amax < bmin) ? false : true;
+}
+
+
+
+struct rcLayerSweepSpan
+{
+	unsigned short ns;	// number samples
+	unsigned char id;	// region id
+	unsigned char nei;	// neighbour id
+};
+
+/// @par
+/// 
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// @see rcAllocHeightfieldLayerSet, rcCompactHeightfield, rcHeightfieldLayerSet, rcConfig
+bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf,
+							  const int borderSize, const int walkableHeight,
+							  rcHeightfieldLayerSet& lset)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_LAYERS);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedDelete<unsigned char> srcReg((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP));
+	if (!srcReg)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'srcReg' (%d).", chf.spanCount);
+		return false;
+	}
+	memset(srcReg,0xff,sizeof(unsigned char)*chf.spanCount);
+	
+	const int nsweeps = chf.width;
+	rcScopedDelete<rcLayerSweepSpan> sweeps((rcLayerSweepSpan*)rcAlloc(sizeof(rcLayerSweepSpan)*nsweeps, RC_ALLOC_TEMP));
+	if (!sweeps)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'sweeps' (%d).", nsweeps);
+		return false;
+	}
+	
+	
+	// Partition walkable area into monotone regions.
+	int prevCount[256];
+	unsigned char regId = 0;
+
+	for (int y = borderSize; y < h-borderSize; ++y)
+	{
+		memset(prevCount,0,sizeof(int)*regId);
+		unsigned char sweepId = 0;
+		
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA) continue;
+
+				unsigned char sid = 0xff;
+
+				// -x
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					if (chf.areas[ai] != RC_NULL_AREA && srcReg[ai] != 0xff)
+						sid = srcReg[ai];
+				}
+				
+				if (sid == 0xff)
+				{
+					sid = sweepId++;
+					sweeps[sid].nei = 0xff;
+					sweeps[sid].ns = 0;
+				}
+				
+				// -y
+				if (rcGetCon(s,3) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					const unsigned char nr = srcReg[ai];
+					if (nr != 0xff)
+					{
+						// Set neighbour when first valid neighbour is encoutered.
+						if (sweeps[sid].ns == 0)
+							sweeps[sid].nei = nr;
+						
+						if (sweeps[sid].nei == nr)
+						{
+							// Update existing neighbour
+							sweeps[sid].ns++;
+							prevCount[nr]++;
+						}
+						else
+						{
+							// This is hit if there is nore than one neighbour.
+							// Invalidate the neighbour.
+							sweeps[sid].nei = 0xff;
+						}
+					}
+				}
+				
+				srcReg[i] = sid;
+			}
+		}
+		
+		// Create unique ID.
+		for (int i = 0; i < sweepId; ++i)
+		{
+			// If the neighbour is set and there is only one continuous connection to it,
+			// the sweep will be merged with the previous one, else new region is created.
+			if (sweeps[i].nei != 0xff && prevCount[sweeps[i].nei] == (int)sweeps[i].ns)
+			{
+				sweeps[i].id = sweeps[i].nei;
+			}
+			else
+			{
+				if (regId == 255)
+				{
+					ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Region ID overflow.");
+					return false;
+				}
+				sweeps[i].id = regId++;
+			}
+		}
+		
+		// Remap local sweep ids to region ids.
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (srcReg[i] != 0xff)
+					srcReg[i] = sweeps[srcReg[i]].id;
+			}
+		}
+	}
+
+	// Allocate and init layer regions.
+	const int nregs = (int)regId;
+	rcScopedDelete<rcLayerRegion> regs((rcLayerRegion*)rcAlloc(sizeof(rcLayerRegion)*nregs, RC_ALLOC_TEMP));
+	if (!regs)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'regs' (%d).", nregs);
+		return false;
+	}
+	memset(regs, 0, sizeof(rcLayerRegion)*nregs);
+	for (int i = 0; i < nregs; ++i)
+	{
+		regs[i].layerId = 0xff;
+		regs[i].ymin = 0xffff;
+		regs[i].ymax = 0;
+	}
+	
+	// Find region neighbours and overlapping regions.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			unsigned char lregs[RC_MAX_LAYERS];
+			int nlregs = 0;
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const unsigned char ri = srcReg[i];
+				if (ri == 0xff) continue;
+				
+				regs[ri].ymin = rcMin(regs[ri].ymin, s.y);
+				regs[ri].ymax = rcMax(regs[ri].ymax, s.y);
+				
+				// Collect all region layers.
+				if (nlregs < RC_MAX_LAYERS)
+					lregs[nlregs++] = ri;
+				
+				// Update neighbours
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						const unsigned char rai = srcReg[ai];
+						if (rai != 0xff && rai != ri)
+						{
+							// Don't check return value -- if we cannot add the neighbor
+							// it will just cause a few more regions to be created, which
+							// is fine.
+							addUnique(regs[ri].neis, regs[ri].nneis, RC_MAX_NEIS, rai);
+						}
+					}
+				}
+				
+			}
+			
+			// Update overlapping regions.
+			for (int i = 0; i < nlregs-1; ++i)
+			{
+				for (int j = i+1; j < nlregs; ++j)
+				{
+					if (lregs[i] != lregs[j])
+					{
+						rcLayerRegion& ri = regs[lregs[i]];
+						rcLayerRegion& rj = regs[lregs[j]];
+
+						if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, lregs[j]) ||
+							!addUnique(rj.layers, rj.nlayers, RC_MAX_LAYERS, lregs[i]))
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
+							return false;
+						}
+					}
+				}
+			}
+			
+		}
+	}
+	
+	// Create 2D layers from regions.
+	unsigned char layerId = 0;
+	
+	static const int MAX_STACK = 64;
+	unsigned char stack[MAX_STACK];
+	int nstack = 0;
+	
+	for (int i = 0; i < nregs; ++i)
+	{
+		rcLayerRegion& root = regs[i];
+		// Skip already visited.
+		if (root.layerId != 0xff)
+			continue;
+
+		// Start search.
+		root.layerId = layerId;
+		root.base = 1;
+		
+		nstack = 0;
+		stack[nstack++] = (unsigned char)i;
+		
+		while (nstack)
+		{
+			// Pop front
+			rcLayerRegion& reg = regs[stack[0]];
+			nstack--;
+			for (int j = 0; j < nstack; ++j)
+				stack[j] = stack[j+1];
+			
+			const int nneis = (int)reg.nneis;
+			for (int j = 0; j < nneis; ++j)
+			{
+				const unsigned char nei = reg.neis[j];
+				rcLayerRegion& regn = regs[nei];
+				// Skip already visited.
+				if (regn.layerId != 0xff)
+					continue;
+				// Skip if the neighbour is overlapping root region.
+				if (contains(root.layers, root.nlayers, nei))
+					continue;
+				// Skip if the height range would become too large.
+				const int ymin = rcMin(root.ymin, regn.ymin);
+				const int ymax = rcMax(root.ymax, regn.ymax);
+				if ((ymax - ymin) >= 255)
+					 continue;
+
+				if (nstack < MAX_STACK)
+				{
+					// Deepen
+					stack[nstack++] = (unsigned char)nei;
+					
+					// Mark layer id
+					regn.layerId = layerId;
+					// Merge current layers to root.
+					for (int k = 0; k < regn.nlayers; ++k)
+					{
+						if (!addUnique(root.layers, root.nlayers, RC_MAX_LAYERS, regn.layers[k]))
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
+							return false;
+						}
+					}
+					root.ymin = rcMin(root.ymin, regn.ymin);
+					root.ymax = rcMax(root.ymax, regn.ymax);
+				}
+			}
+		}
+		
+		layerId++;
+	}
+	
+	// Merge non-overlapping regions that are close in height.
+	const unsigned short mergeHeight = (unsigned short)walkableHeight * 4;
+	
+	for (int i = 0; i < nregs; ++i)
+	{
+		rcLayerRegion& ri = regs[i];
+		if (!ri.base) continue;
+		
+		unsigned char newId = ri.layerId;
+		
+		for (;;)
+		{
+			unsigned char oldId = 0xff;
+			
+			for (int j = 0; j < nregs; ++j)
+			{
+				if (i == j) continue;
+				rcLayerRegion& rj = regs[j];
+				if (!rj.base) continue;
+				
+				// Skip if the regions are not close to each other.
+				if (!overlapRange(ri.ymin,ri.ymax+mergeHeight, rj.ymin,rj.ymax+mergeHeight))
+					continue;
+				// Skip if the height range would become too large.
+				const int ymin = rcMin(ri.ymin, rj.ymin);
+				const int ymax = rcMax(ri.ymax, rj.ymax);
+				if ((ymax - ymin) >= 255)
+				  continue;
+						  
+				// Make sure that there is no overlap when merging 'ri' and 'rj'.
+				bool overlap = false;
+				// Iterate over all regions which have the same layerId as 'rj'
+				for (int k = 0; k < nregs; ++k)
+				{
+					if (regs[k].layerId != rj.layerId)
+						continue;
+					// Check if region 'k' is overlapping region 'ri'
+					// Index to 'regs' is the same as region id.
+					if (contains(ri.layers,ri.nlayers, (unsigned char)k))
+					{
+						overlap = true;
+						break;
+					}
+				}
+				// Cannot merge of regions overlap.
+				if (overlap)
+					continue;
+				
+				// Can merge i and j.
+				oldId = rj.layerId;
+				break;
+			}
+			
+			// Could not find anything to merge with, stop.
+			if (oldId == 0xff)
+				break;
+			
+			// Merge
+			for (int j = 0; j < nregs; ++j)
+			{
+				rcLayerRegion& rj = regs[j];
+				if (rj.layerId == oldId)
+				{
+					rj.base = 0;
+					// Remap layerIds.
+					rj.layerId = newId;
+					// Add overlaid layers from 'rj' to 'ri'.
+					for (int k = 0; k < rj.nlayers; ++k)
+					{
+						if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, rj.layers[k]))
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
+							return false;
+						}
+					}
+
+					// Update height bounds.
+					ri.ymin = rcMin(ri.ymin, rj.ymin);
+					ri.ymax = rcMax(ri.ymax, rj.ymax);
+				}
+			}
+		}
+	}
+	
+	// Compact layerIds
+	unsigned char remap[256];
+	memset(remap, 0, 256);
+
+	// Find number of unique layers.
+	layerId = 0;
+	for (int i = 0; i < nregs; ++i)
+		remap[regs[i].layerId] = 1;
+	for (int i = 0; i < 256; ++i)
+	{
+		if (remap[i])
+			remap[i] = layerId++;
+		else
+			remap[i] = 0xff;
+	}
+	// Remap ids.
+	for (int i = 0; i < nregs; ++i)
+		regs[i].layerId = remap[regs[i].layerId];
+	
+	// No layers, return empty.
+	if (layerId == 0)
+		return true;
+	
+	// Create layers.
+	rcAssert(lset.layers == 0);
+	
+	const int lw = w - borderSize*2;
+	const int lh = h - borderSize*2;
+
+	// Build contracted bbox for layers.
+	float bmin[3], bmax[3];
+	rcVcopy(bmin, chf.bmin);
+	rcVcopy(bmax, chf.bmax);
+	bmin[0] += borderSize*chf.cs;
+	bmin[2] += borderSize*chf.cs;
+	bmax[0] -= borderSize*chf.cs;
+	bmax[2] -= borderSize*chf.cs;
+	
+	lset.nlayers = (int)layerId;
+	
+	lset.layers = (rcHeightfieldLayer*)rcAlloc(sizeof(rcHeightfieldLayer)*lset.nlayers, RC_ALLOC_PERM);
+	if (!lset.layers)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'layers' (%d).", lset.nlayers);
+		return false;
+	}
+	memset(lset.layers, 0, sizeof(rcHeightfieldLayer)*lset.nlayers);
+
+	
+	// Store layers.
+	for (int i = 0; i < lset.nlayers; ++i)
+	{
+		unsigned char curId = (unsigned char)i;
+
+		rcHeightfieldLayer* layer = &lset.layers[i];
+
+		const int gridSize = sizeof(unsigned char)*lw*lh;
+
+		layer->heights = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
+		if (!layer->heights)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'heights' (%d).", gridSize);
+			return false;
+		}
+		memset(layer->heights, 0xff, gridSize);
+
+		layer->areas = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
+		if (!layer->areas)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'areas' (%d).", gridSize);
+			return false;
+		}
+		memset(layer->areas, 0, gridSize);
+
+		layer->cons = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
+		if (!layer->cons)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'cons' (%d).", gridSize);
+			return false;
+		}
+		memset(layer->cons, 0, gridSize);
+		
+		// Find layer height bounds.
+		int hmin = 0, hmax = 0;
+		for (int j = 0; j < nregs; ++j)
+		{
+			if (regs[j].base && regs[j].layerId == curId)
+			{
+				hmin = (int)regs[j].ymin;
+				hmax = (int)regs[j].ymax;
+			}
+		}
+
+		layer->width = lw;
+		layer->height = lh;
+		layer->cs = chf.cs;
+		layer->ch = chf.ch;
+		
+		// Adjust the bbox to fit the heightfield.
+		rcVcopy(layer->bmin, bmin);
+		rcVcopy(layer->bmax, bmax);
+		layer->bmin[1] = bmin[1] + hmin*chf.ch;
+		layer->bmax[1] = bmin[1] + hmax*chf.ch;
+		layer->hmin = hmin;
+		layer->hmax = hmax;
+
+		// Update usable data region.
+		layer->minx = layer->width;
+		layer->maxx = 0;
+		layer->miny = layer->height;
+		layer->maxy = 0;
+		
+		// Copy height and area from compact heightfield. 
+		for (int y = 0; y < lh; ++y)
+		{
+			for (int x = 0; x < lw; ++x)
+			{
+				const int cx = borderSize+x;
+				const int cy = borderSize+y;
+				const rcCompactCell& c = chf.cells[cx+cy*w];
+				for (int j = (int)c.index, nj = (int)(c.index+c.count); j < nj; ++j)
+				{
+					const rcCompactSpan& s = chf.spans[j];
+					// Skip unassigned regions.
+					if (srcReg[j] == 0xff)
+						continue;
+					// Skip of does nto belong to current layer.
+					unsigned char lid = regs[srcReg[j]].layerId;
+					if (lid != curId)
+						continue;
+					
+					// Update data bounds.
+					layer->minx = rcMin(layer->minx, x);
+					layer->maxx = rcMax(layer->maxx, x);
+					layer->miny = rcMin(layer->miny, y);
+					layer->maxy = rcMax(layer->maxy, y);
+					
+					// Store height and area type.
+					const int idx = x+y*lw;
+					layer->heights[idx] = (unsigned char)(s.y - hmin);
+					layer->areas[idx] = chf.areas[j];
+					
+					// Check connection.
+					unsigned char portal = 0;
+					unsigned char con = 0;
+					for (int dir = 0; dir < 4; ++dir)
+					{
+						if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+						{
+							const int ax = cx + rcGetDirOffsetX(dir);
+							const int ay = cy + rcGetDirOffsetY(dir);
+							const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+							unsigned char alid = srcReg[ai] != 0xff ? regs[srcReg[ai]].layerId : 0xff;
+							// Portal mask
+							if (chf.areas[ai] != RC_NULL_AREA && lid != alid)
+							{
+								portal |= (unsigned char)(1<<dir);
+								// Update height so that it matches on both sides of the portal.
+								const rcCompactSpan& as = chf.spans[ai];
+								if (as.y > hmin)
+									layer->heights[idx] = rcMax(layer->heights[idx], (unsigned char)(as.y - hmin));
+							}
+							// Valid connection mask
+							if (chf.areas[ai] != RC_NULL_AREA && lid == alid)
+							{
+								const int nx = ax - borderSize;
+								const int ny = ay - borderSize;
+								if (nx >= 0 && ny >= 0 && nx < lw && ny < lh)
+									con |= (unsigned char)(1<<dir);
+							}
+						}
+					}
+					
+					layer->cons[idx] = (portal << 4) | con;
+				}
+			}
+		}
+		
+		if (layer->minx > layer->maxx)
+			layer->minx = layer->maxx = 0;
+		if (layer->miny > layer->maxy)
+			layer->miny = layer->maxy = 0;
+	}
+	
+	return true;
+}

lib/Recast/RecastRasterization.cpp

@@ -0,0 +1,454 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+inline bool overlapBounds(const float* amin, const float* amax, const float* bmin, const float* bmax)
+{
+	bool overlap = true;
+	overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
+	overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
+	overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
+	return overlap;
+}
+
+inline bool overlapInterval(unsigned short amin, unsigned short amax,
+							unsigned short bmin, unsigned short bmax)
+{
+	if (amax < bmin) return false;
+	if (amin > bmax) return false;
+	return true;
+}
+
+
+static rcSpan* allocSpan(rcHeightfield& hf)
+{
+	// If running out of memory, allocate new page and update the freelist.
+	if (!hf.freelist || !hf.freelist->next)
+	{
+		// Create new page.
+		// Allocate memory for the new pool.
+		rcSpanPool* pool = (rcSpanPool*)rcAlloc(sizeof(rcSpanPool), RC_ALLOC_PERM);
+		if (!pool) return 0;
+
+		// Add the pool into the list of pools.
+		pool->next = hf.pools;
+		hf.pools = pool;
+		// Add new items to the free list.
+		rcSpan* freelist = hf.freelist;
+		rcSpan* head = &pool->items[0];
+		rcSpan* it = &pool->items[RC_SPANS_PER_POOL];
+		do
+		{
+			--it;
+			it->next = freelist;
+			freelist = it;
+		}
+		while (it != head);
+		hf.freelist = it;
+	}
+	
+	// Pop item from in front of the free list.
+	rcSpan* it = hf.freelist;
+	hf.freelist = hf.freelist->next;
+	return it;
+}
+
+static void freeSpan(rcHeightfield& hf, rcSpan* ptr)
+{
+	if (!ptr) return;
+	// Add the node in front of the free list.
+	ptr->next = hf.freelist;
+	hf.freelist = ptr;
+}
+
+static bool addSpan(rcHeightfield& hf, const int x, const int y,
+					const unsigned short smin, const unsigned short smax,
+					const unsigned char area, const int flagMergeThr)
+{
+	
+	int idx = x + y*hf.width;
+	
+	rcSpan* s = allocSpan(hf);
+	if (!s)
+		return false;
+	s->smin = smin;
+	s->smax = smax;
+	s->area = area;
+	s->next = 0;
+	
+	// Empty cell, add the first span.
+	if (!hf.spans[idx])
+	{
+		hf.spans[idx] = s;
+		return true;
+	}
+	rcSpan* prev = 0;
+	rcSpan* cur = hf.spans[idx];
+	
+	// Insert and merge spans.
+	while (cur)
+	{
+		if (cur->smin > s->smax)
+		{
+			// Current span is further than the new span, break.
+			break;
+		}
+		else if (cur->smax < s->smin)
+		{
+			// Current span is before the new span advance.
+			prev = cur;
+			cur = cur->next;
+		}
+		else
+		{
+			// Merge spans.
+			if (cur->smin < s->smin)
+				s->smin = cur->smin;
+			if (cur->smax > s->smax)
+				s->smax = cur->smax;
+			
+			// Merge flags.
+			if (rcAbs((int)s->smax - (int)cur->smax) <= flagMergeThr)
+				s->area = rcMax(s->area, cur->area);
+			
+			// Remove current span.
+			rcSpan* next = cur->next;
+			freeSpan(hf, cur);
+			if (prev)
+				prev->next = next;
+			else
+				hf.spans[idx] = next;
+			cur = next;
+		}
+	}
+	
+	// Insert new span.
+	if (prev)
+	{
+		s->next = prev->next;
+		prev->next = s;
+	}
+	else
+	{
+		s->next = hf.spans[idx];
+		hf.spans[idx] = s;
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// The span addition can be set to favor flags. If the span is merged to
+/// another span and the new @p smax is within @p flagMergeThr units
+/// from the existing span, the span flags are merged.
+///
+/// @see rcHeightfield, rcSpan.
+bool rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y,
+			   const unsigned short smin, const unsigned short smax,
+			   const unsigned char area, const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	if (!addSpan(hf, x, y, smin, smax, area, flagMergeThr))
+	{
+		ctx->log(RC_LOG_ERROR, "rcAddSpan: Out of memory.");
+		return false;
+	}
+
+	return true;
+}
+
+// divides a convex polygons into two convex polygons on both sides of a line
+static void dividePoly(const float* in, int nin,
+					  float* out1, int* nout1,
+					  float* out2, int* nout2,
+					  float x, int axis)
+{
+	float d[12];
+	for (int i = 0; i < nin; ++i)
+		d[i] = x - in[i*3+axis];
+
+	int m = 0, n = 0;
+	for (int i = 0, j = nin-1; i < nin; j=i, ++i)
+	{
+		bool ina = d[j] >= 0;
+		bool inb = d[i] >= 0;
+		if (ina != inb)
+		{
+			float s = d[j] / (d[j] - d[i]);
+			out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
+			out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
+			out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
+			rcVcopy(out2 + n*3, out1 + m*3);
+			m++;
+			n++;
+			// add the i'th point to the right polygon. Do NOT add points that are on the dividing line
+			// since these were already added above
+			if (d[i] > 0)
+			{
+				rcVcopy(out1 + m*3, in + i*3);
+				m++;
+			}
+			else if (d[i] < 0)
+			{
+				rcVcopy(out2 + n*3, in + i*3);
+				n++;
+			}
+		}
+		else // same side
+		{
+			// add the i'th point to the right polygon. Addition is done even for points on the dividing line
+			if (d[i] >= 0)
+			{
+				rcVcopy(out1 + m*3, in + i*3);
+				m++;
+				if (d[i] != 0)
+					continue;
+			}
+			rcVcopy(out2 + n*3, in + i*3);
+			n++;
+		}
+	}
+
+	*nout1 = m;
+	*nout2 = n;
+}
+
+
+
+static bool rasterizeTri(const float* v0, const float* v1, const float* v2,
+						 const unsigned char area, rcHeightfield& hf,
+						 const float* bmin, const float* bmax,
+						 const float cs, const float ics, const float ich,
+						 const int flagMergeThr)
+{
+	const int w = hf.width;
+	const int h = hf.height;
+	float tmin[3], tmax[3];
+	const float by = bmax[1] - bmin[1];
+	
+	// Calculate the bounding box of the triangle.
+	rcVcopy(tmin, v0);
+	rcVcopy(tmax, v0);
+	rcVmin(tmin, v1);
+	rcVmin(tmin, v2);
+	rcVmax(tmax, v1);
+	rcVmax(tmax, v2);
+	
+	// If the triangle does not touch the bbox of the heightfield, skip the triagle.
+	if (!overlapBounds(bmin, bmax, tmin, tmax))
+		return true;
+	
+	// Calculate the footprint of the triangle on the grid's y-axis
+	int y0 = (int)((tmin[2] - bmin[2])*ics);
+	int y1 = (int)((tmax[2] - bmin[2])*ics);
+	y0 = rcClamp(y0, 0, h-1);
+	y1 = rcClamp(y1, 0, h-1);
+	
+	// Clip the triangle into all grid cells it touches.
+	float buf[7*3*4];
+	float *in = buf, *inrow = buf+7*3, *p1 = inrow+7*3, *p2 = p1+7*3;
+
+	rcVcopy(&in[0], v0);
+	rcVcopy(&in[1*3], v1);
+	rcVcopy(&in[2*3], v2);
+	int nvrow, nvIn = 3;
+	
+	for (int y = y0; y <= y1; ++y)
+	{
+		// Clip polygon to row. Store the remaining polygon as well
+		const float cz = bmin[2] + y*cs;
+		dividePoly(in, nvIn, inrow, &nvrow, p1, &nvIn, cz+cs, 2);
+		rcSwap(in, p1);
+		if (nvrow < 3) continue;
+		
+		// find the horizontal bounds in the row
+		float minX = inrow[0], maxX = inrow[0];
+		for (int i=1; i<nvrow; ++i)
+		{
+			if (minX > inrow[i*3])	minX = inrow[i*3];
+			if (maxX < inrow[i*3])	maxX = inrow[i*3];
+		}
+		int x0 = (int)((minX - bmin[0])*ics);
+		int x1 = (int)((maxX - bmin[0])*ics);
+		x0 = rcClamp(x0, 0, w-1);
+		x1 = rcClamp(x1, 0, w-1);
+
+		int nv, nv2 = nvrow;
+
+		for (int x = x0; x <= x1; ++x)
+		{
+			// Clip polygon to column. store the remaining polygon as well
+			const float cx = bmin[0] + x*cs;
+			dividePoly(inrow, nv2, p1, &nv, p2, &nv2, cx+cs, 0);
+			rcSwap(inrow, p2);
+			if (nv < 3) continue;
+			
+			// Calculate min and max of the span.
+			float smin = p1[1], smax = p1[1];
+			for (int i = 1; i < nv; ++i)
+			{
+				smin = rcMin(smin, p1[i*3+1]);
+				smax = rcMax(smax, p1[i*3+1]);
+			}
+			smin -= bmin[1];
+			smax -= bmin[1];
+			// Skip the span if it is outside the heightfield bbox
+			if (smax < 0.0f) continue;
+			if (smin > by) continue;
+			// Clamp the span to the heightfield bbox.
+			if (smin < 0.0f) smin = 0;
+			if (smax > by) smax = by;
+			
+			// Snap the span to the heightfield height grid.
+			unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, RC_SPAN_MAX_HEIGHT);
+			unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), (int)ismin+1, RC_SPAN_MAX_HEIGHT);
+			
+			if (!addSpan(hf, x, y, ismin, ismax, area, flagMergeThr))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// No spans will be added if the triangle does not overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2,
+						 const unsigned char area, rcHeightfield& solid,
+						 const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	if (!rasterizeTri(v0, v1, v2, area, solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+	{
+		ctx->log(RC_LOG_ERROR, "rcRasterizeTriangle: Out of memory.");
+		return false;
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// Spans will only be added for triangles that overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
+						  const int* tris, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+	
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	// Rasterize triangles.
+	for (int i = 0; i < nt; ++i)
+	{
+		const float* v0 = &verts[tris[i*3+0]*3];
+		const float* v1 = &verts[tris[i*3+1]*3];
+		const float* v2 = &verts[tris[i*3+2]*3];
+		// Rasterize.
+		if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+		{
+			ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// Spans will only be added for triangles that overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
+						  const unsigned short* tris, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+	
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	// Rasterize triangles.
+	for (int i = 0; i < nt; ++i)
+	{
+		const float* v0 = &verts[tris[i*3+0]*3];
+		const float* v1 = &verts[tris[i*3+1]*3];
+		const float* v2 = &verts[tris[i*3+2]*3];
+		// Rasterize.
+		if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+		{
+			ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// Spans will only be added for triangles that overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+	
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	// Rasterize triangles.
+	for (int i = 0; i < nt; ++i)
+	{
+		const float* v0 = &verts[(i*3+0)*3];
+		const float* v1 = &verts[(i*3+1)*3];
+		const float* v2 = &verts[(i*3+2)*3];
+		// Rasterize.
+		if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+		{
+			ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
+			return false;
+		}
+	}
+
+	return true;
+}

main.cpp

@@ -83,16 +83,16 @@ void wifi() {
 
 int main(int argc, char** argv) {
 
-	wifi(); return 0;
+	//wifi(); return 0;
 
 
 #ifdef WITH_TESTS
 	::testing::InitGoogleTest(&argc, argv);
 
 	// skip all tests starting with LIVE_
-    //::testing::GTEST_FLAG(filter) = "*Barometer*";
+	//::testing::GTEST_FLAG(filter) = "*Barometer*";
 
-    ::testing::GTEST_FLAG(filter) = "*Distribution*";
+	//::testing::GTEST_FLAG(filter) = "*Distribution.T*";
 	//::testing::GTEST_FLAG(filter) = "*RingBuffer*";
 
 	//::testing::GTEST_FLAG(filter) = "*Grid.*";
@@ -109,7 +109,7 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Matrix4*";
 	//::testing::GTEST_FLAG(filter) = "*Sphere3*";
 
-    //::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*";
+	::testing::GTEST_FLAG(filter) = "NavMesh*";
 	//::testing::GTEST_FLAG(filter) = "Timestamp*";
 
 	//::testing::GTEST_FLAG(filter) = "*RayTrace3*";

math/boxkde/BoxGaus.h

@@ -3,6 +3,7 @@
 #include <cmath>
 #include <vector>
 
+#include "Image2D.h"
 #include "BoxSizes.h"
 
 template <class T>
@@ -28,7 +29,7 @@ struct BoxGaus
 	{
         BoxSizes<T> bsX(sigmaX, nFilt);
         BoxSizes<T> bsY(sigmaY, nFilt);
-		std::vector<T> buffer(input.size());
+        std::vector<T> buffer(input.size());
 
         assertMsg((2 * bsX.wl + 1 < w) && (2 * bsX.wl + 1 < h), "Box-Filter size in X direction is too big");
         assertMsg((2 * bsX.wu + 1 < w) && (2 * bsX.wu + 1 < h), "Box-Filter size in X direction is too big");
@@ -89,7 +90,7 @@ private:
             for (size_t j = 0; j <= r; j++)
             {
                 val += src[ri] - fv;
-                dst[j + i*w] = val * iarr;
+                dst[i + j*w] = val * iarr;
 
                 ri += w;
                 ti += w;
@@ -99,7 +100,7 @@ private:
             for (size_t j = r + 1; j < h - r; j++)
             {
                 val += src[ri] - src[li];
-                dst[j + i*w] = val * iarr;
+                dst[i + j*w] = val * iarr;
 
                 li += w;
                 ri += w;
@@ -110,12 +111,14 @@ private:
             for (size_t j = h - r; j < h; j++)
             {
                 val += lv - src[li];
-                dst[j + i*w] = val * iarr;
+                dst[i + j*w] = val * iarr;
 
                 li += w;
                 ti += w;
             }
         }
+
+        int test = 0;
     }
 };
 

math/boxkde/Image2D.h

@@ -174,7 +174,6 @@ struct Image2D : public ImageView2D<TValue>
         assertMsg(data.size() == width*height, "Sizes must be the same");
         this->values = values_vec.data();
     }
-    
 
           std::vector<TValue>& data()       { return values_vec; }
     const std::vector<TValue>& data() const { return values_vec; }

math/boxkde/benchmark.h

@@ -2,6 +2,9 @@
 #include <chrono>
 #include <string>
 #include <iostream>
+#include <vector>
+#include <algorithm>
+#include <numeric>
 
 template <typename F>
 static void benchmark(std::string name, size_t count, F&& lambda)
@@ -102,4 +105,4 @@ public:
 
 		lapTime = clock::now();
 	}
-};
+};

math/distribution/Normal.h

@@ -26,8 +26,13 @@ namespace Distribution {
 		Normal(const T mu, const T sigma) :
 			mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(RANDOM_SEED), dist(mu,sigma) {
 
+#warning "analyze issue when coping an existing distribution and using draw() afterwards. this seems to yield issues"
+
 		}
 
+		/** do not allow copy. this will not work as expected for std::normal_distribution when using draw() ?! */
+		//Normal(const Normal& o) = delete;
+
 		/** get probability for the given value */
 		T getProbability(const T val) const {
 			const T b = -0.5 * ((val-mu)/sigma) * ((val-mu)/sigma);

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/NavMesh.h

@@ -0,0 +1,121 @@
+#ifndef NAV_MESH_H
+#define NAV_MESH_H
+
+#include "NavMeshTriangle.h"
+#include <vector>
+#include "../geo/BBox3.h"
+#include <random>
+#include "../math/DrawList.h"
+#include "NavMeshRandom.h"
+#include "NavMeshLocation.h"
+
+namespace NM {
+
+	template <typename Tria> class NavMesh {
+
+		/** all triangles within the mesh */
+		std::vector<Tria*> triangles;
+
+		BBox3 bbox;
+
+	public:
+
+		/** ctor */
+		NavMesh() {
+
+		}
+
+		/** dtor */
+		~NavMesh() {
+			for (const Tria* t : triangles) {delete t;}
+			triangles.clear();
+		}
+
+		/** the overall bounding-box */
+		const BBox3 getBBox() const {
+			return bbox;
+		}
+
+		/** add a new triangle */
+		void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) {
+			triangles.push_back(new Tria(p1,p2,p3,type));
+			bbox.add(p1);
+			bbox.add(p2);
+			bbox.add(p3);
+		}
+
+		/** get the triangle this point belongs to (if any) */
+		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 within NavMesh: " + pos.asString());
+		}
+
+		/** connect both triangles */
+		void connectBiDir(int idx1, int idx2) {
+			connectUniDir(idx1,idx2);
+			connectUniDir(idx2,idx1);
+		}
+
+		/** connect both triangles */
+		void connectUniDir(int idxFrom, int idxTo) {
+			Tria* tria = triangles[idxFrom];
+			tria->addNeighbor(triangles[idxTo]);
+		}
+
+		/** allows for-each iteration over all included triangles */
+		decltype(triangles.begin()) begin() {return triangles.begin();}
+
+		/** allows for-each iteration over all included triangles */
+		decltype(triangles.end()) end() {return triangles.end();}
+
+		/** array access */
+		Tria* operator [] (const size_t idx) {
+			Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds");
+			return triangles[idx];
+		}
+
+		/** get the number of triangles used */
+		size_t getNumTriangles() const {
+			return triangles.size();
+		}
+
+		/** ---------------- MISC ---------------- */
+
+
+		NavMeshRandom<Tria> getRandom() {
+			return NavMeshRandom<Tria>(triangles);
+		}
+
+	//	/** ---------------- 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 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 Tria& tria, const size_t nth) const {
+	//		const Tria& neighbor = triangles[tria._neighbors[nth]];
+	//		return (Tria&) neighbor;
+	//	}
+
+	};
+
+}
+
+#endif

navMesh/NavMeshDebug.h

@@ -0,0 +1,128 @@
+#ifndef NAVMESHDEBUG_H
+#define NAVMESHDEBUG_H
+
+#include "NavMesh.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#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 {
+
+	/**
+	 * debug plot NavMeshes
+	 */
+	class NavMeshDebug {
+
+	public:
+
+		K::Gnuplot gp;
+		K::GnuplotSplot plot;
+		K::GnuplotSplotElementLines lines;
+		K::GnuplotSplotElementPoints border;
+		K::GnuplotSplotElementColorPoints particles;
+		K::GnuplotSplotElementLines pathEstimated;
+
+	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 <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"));
+
+			const BBox3 bbox = nm.getBBox();
+
+			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));
+
+			//stairs in eigene group? vlt gehen dann auch die dellen weg?
+
+			for (const Tria* tria : nm) {
+				const uint8_t type = tria->getType();
+				if (type < 0 || type > 5) {
+					throw std::runtime_error("out of type-bounds");
+				}
+				K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
+				pol->add(K::GnuplotCoordinate3(tria->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST));
+				pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST));
+				pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST));
+				pol->close();
+				pol->setZIndex(tria->getP3().z);
+				plot.getObjects().add(pol);
+
+				//for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
+				//	const Tria* o = nm.getNeighbor(tria, i);
+				//	const Point3 p1 = tria->getCenter();
+				//	const Point3 p2 = o.getCenter();
+				//	//lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
+				//}
+
+				for (const NavMeshTriangle* o : *tria) {
+					const Point3 p1 = tria->getCenter();
+					const Point3 p2 = o->getCenter();
+				//	lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
+				}
+
+			}
+
+			plot.getObjects().reOrderByZIndex();
+
+		}
+
+
+		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";
+		}
+
+	};
+
+}
+
+#endif // NAVMESHDEBUG_H

navMesh/NavMeshFactory.h

@@ -0,0 +1,745 @@
+#ifndef NAV_MESH_FACTORY_H
+#define NAV_MESH_FACTORY_H
+
+#include "../floorplan/v2/Floorplan.h"
+#include "../floorplan/v2/FloorplanHelper.h"
+
+#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 {
+
+
+	class NavMeshPoly {
+
+		struct GPCPolygon : gpc_polygon {
+			GPCPolygon() {
+				num_contours = 0;
+				contour = nullptr;
+				hole = nullptr;
+			}
+			~GPCPolygon() {
+				if (contour) {
+					gpc_free_polygon(this);
+					//free(contour->vertex); contour->vertex = nullptr;
+				}
+				free(contour); contour = nullptr;
+				free(hole); hole = nullptr;
+
+			}
+			GPCPolygon& operator = (const GPCPolygon& o) = delete;
+			GPCPolygon& operator = (GPCPolygon& o) {
+				this->contour = o.contour;
+				this->hole = o.hole;
+				this->num_contours = o.num_contours;
+				o.contour = nullptr;
+				o.hole = nullptr;
+				return *this;
+			}
+		};
+
+	private:
+
+		GPCPolygon state;
+		float z;
+
+	public:
+
+		NavMeshPoly(float z) : z(z) {
+			;
+		}
+
+		void add(const Floorplan::Polygon2& poly) {
+			GPCPolygon cur = toGPC(poly);
+			gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
+		}
+
+		void remove(const Floorplan::Polygon2& poly) {
+			GPCPolygon cur = toGPC(poly);
+			gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
+		}
+
+		std::vector<std::vector<Point3>> get() {
+
+			gpc_tristrip res;
+			res.num_strips = 0;
+			res.strip = nullptr;
+
+			//res.strip = (gpc_vertex_list*) malloc(1024);
+			gpc_polygon_to_tristrip(&state, &res);
+
+			std::vector<std::vector<Point3>> trias;
+
+			for (int i = 0; i < res.num_strips; ++i) {
+				gpc_vertex_list lst = res.strip[i];
+				for (int j = 2; j < lst.num_vertices; ++j) {
+					std::vector<Point3> tria;
+					gpc_vertex& v1 = lst.vertex[j-2];
+					gpc_vertex& v2 = lst.vertex[j-1];
+					gpc_vertex& v3 = lst.vertex[j];
+					tria.push_back(Point3(v1.x, v1.y, z));
+					tria.push_back(Point3(v2.x, v2.y, z));
+					tria.push_back(Point3(v3.x, v3.y, z));
+					trias.push_back(tria);
+				}
+
+			}
+
+			gpc_free_tristrip(&res);
+
+			return std::move(trias);
+
+		}
+
+	private:
+
+		GPCPolygon toGPC(Floorplan::Polygon2 poly) {
+
+			std::vector<gpc_vertex> verts;
+			for (Point2 p2 : poly.points) {
+				gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
+				verts.push_back(vert);
+			}
+
+			GPCPolygon gpol;
+			gpc_vertex_list list;
+			list.num_vertices = verts.size();
+			list.vertex = verts.data();
+			gpc_add_contour(&gpol, &list, 0);
+
+			return gpol;
+
+		}
+
+	};
+
+
+
+
+
+
+	struct TriangleIn {
+		Point3 p1;
+		Point3 p2;
+		Point3 p3;
+		uint8_t type;
+		TriangleIn(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : p1(p1), p2(p2), p3(p3), type(type) {;}
+	};
+
+	struct TriangleOut {
+
+		Point3 p1;
+		Point3 p2;
+		Point3 p3;
+
+		int numNeighbors = 0;
+		int neighbors[3];	// each triangle has max 3 neighbors
+
+		TriangleOut(const Point3 p1, const Point3 p2, const Point3 p3) : p1(p1), p2(p2), p3(p3), neighbors() {;}
+
+		Point3 center() const {
+			return (p1+p2+p3) / 3;
+		}
+
+	};
+
+#define NMF_STEPS	8
+
+	template <typename Tria> class NavMeshFactory {
+
+	private:
+
+		NavMesh<Tria>* dst = nullptr;
+
+		const NavMeshSettings& settings;
+
+		std::vector<TriangleIn> triangles;
+
+	public:
+
+		NavMeshFactory(NavMesh<Tria>* dst, const NavMeshSettings& settings) : dst(dst), settings(settings) {
+
+		}
+
+		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, listener);
+		}
+
+//		/** 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) {
+
+			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);
+
+					// 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);
+						}
+					}
+
+					// 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<Floorplan::FloorObstacleLine*>(obs);
+						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);
+					}
+
+				}
+
+			}
+
+			// 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);	// slightly grow to ensure connection?!
+				for (const Floorplan::Quad3& quad : quads) {
+
+					// 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, 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) {
+
+				// swap YZ and polygon order
+				int startVert = vData.size() / 3;
+
+				// invert triangle ? (CW vs CCW)
+				// ensure normal points UP
+				const Point3 norm = cross((t.p2-t.p1), (t.p3-t.p1));
+				if (norm.z > 0) {
+					tData.push_back(startVert + 0);
+					tData.push_back(startVert + 2);
+					tData.push_back(startVert + 1);
+				} else {
+					tData.push_back(startVert + 0);
+					tData.push_back(startVert + 1);
+					tData.push_back(startVert + 2);
+				}
+
+				typeData.push_back(t.type);
+
+				vData.push_back(t.p1.x);
+				vData.push_back(t.p1.z);
+				vData.push_back(t.p1.y);
+
+				vData.push_back(t.p2.x);
+				vData.push_back(t.p2.z);
+				vData.push_back(t.p2.y);
+
+				vData.push_back(t.p3.x);
+				vData.push_back(t.p3.z);
+				vData.push_back(t.p3.y);
+
+			}
+
+			unsigned char* m_triareas = typeData.data();
+			const float* verts = vData.data();
+			const int* tris = tData.data();
+
+			int ntris = tData.size() / 3;
+			int nverts = vData.size() / 3;
+
+
+			//unsigned char* m_triareas;
+			rcHeightfield* m_solid;
+			rcCompactHeightfield* m_chf;
+			rcContourSet* m_cset;
+			rcPolyMesh* m_pmesh;
+			rcConfig m_cfg;
+			rcPolyMeshDetail* m_dmesh;
+			rcContext* m_ctx = new rcContext();
+
+//			float m_cellSize = 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 = 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?
+
+				// Set the area where the navigation will be build.
+				// Here the bounds of the input mesh are used, but the
+				// area could be specified by an user defined box, etc.
+				rcVcopy(m_cfg.bmin, bmin);
+				rcVcopy(m_cfg.bmax, bmax);
+				rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height);
+
+				// Reset build times gathering.
+				m_ctx->resetTimers();
+
+				// Start the build process.
+				m_ctx->startTimer(RC_TIMER_TOTAL);
+
+				m_ctx->log(RC_LOG_PROGRESS, "Building navigation:");
+				m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height);
+				m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f);
+
+				//
+				// Step 2. Rasterize input polygon soup.
+				//
+
+				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) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
+					return false;
+				}
+				if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch)) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
+					return false;
+				}
+
+				// Allocate array that can hold triangle area types.
+				// If you have multiple meshes you need to process, allocate
+				// and array which can hold the max number of triangles you need to process.
+	//			m_triareas = new unsigned char[ntris];
+	//			if (!m_triareas)
+	//			{
+	//				m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris);
+	//				return false;
+	//			}
+
+				// Find triangles which are walkable based on their slope and rasterize them.
+				// If your input data is multiple meshes, you can transform them here, calculate
+				// the are type for each of the meshes and rasterize them.
+				//memset(m_triareas, 0, ntris*sizeof(unsigned char));
+				//rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas);
+				if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb)) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles.");
+					return false;
+				}
+
+				bool m_keepInterResults = false;
+				bool m_filterLowHangingObstacles = false;
+				bool m_filterLedgeSpans = false;
+				bool m_filterWalkableLowHeightSpans = false;
+
+				// 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
+				// as well as filter spans where the character cannot possibly stand.
+				if (m_filterLowHangingObstacles)
+					rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid);
+				if (m_filterLedgeSpans)
+					rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
+				if (m_filterWalkableLowHeightSpans)
+					rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid);
+
+
+				// Step 4. Partition walkable surface to simple regions.
+				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) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
+					return false;
+				}
+				if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf)) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
+					return false;
+				}
+
+				//if (!m_keepInterResults) {
+					rcFreeHeightField(m_solid);
+					m_solid = 0;
+				//}
+
+				// Erode the walkable area by agent radius.
+				if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf))
+				{
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
+					return false;
+				}
+
+				// (Optional) Mark areas.
+		//		const ConvexVolume* vols = m_geom->getConvexVolumes();
+		//		for (int i  = 0; i < m_geom->getConvexVolumeCount(); ++i)
+		//			rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);
+
+
+				// Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas.
+				// There are 3 martitioning methods, each with some pros and cons:
+				// 1) Watershed partitioning
+				//   - the classic Recast partitioning
+				//   - creates the nicest tessellation
+				//   - usually slowest
+				//   - partitions the heightfield into nice regions without holes or overlaps
+				//   - the are some corner cases where this method creates produces holes and overlaps
+				//      - holes may appear when a small obstacles is close to large open area (triangulation can handle this)
+				//      - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail
+				//   * generally the best choice if you precompute the nacmesh, use this if you have large open areas
+				// 2) Monotone partioning
+				//   - fastest
+				//   - partitions the heightfield into regions without holes and overlaps (guaranteed)
+				//   - creates long thin polygons, which sometimes causes paths with detours
+				//   * use this if you want fast navmesh generation
+				// 3) Layer partitoining
+				//   - quite fast
+				//   - partitions the heighfield into non-overlapping regions
+				//   - relies on the triangulation code to cope with holes (thus slower than monotone partitioning)
+				//   - produces better triangles than monotone partitioning
+				//   - does not have the corner cases of watershed partitioning
+				//   - can be slow and create a bit ugly tessellation (still better than monotone)
+				//     if you have large open areas with small obstacles (not a problem if you use tiles)
+				//   * good choice to use for tiled navmesh with medium and small sized tiles
+
+
+				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)) {
+						m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
+						return false;
+					}
+
+					// Partition the walkable surface into simple regions without holes.
+					if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) {
+						m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions.");
+						return false;
+					}
+					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)) {
+						m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions.");
+						return false;
+					}
+
+					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)) {
+						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) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
+					return false;
+				}
+				if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset)) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
+					return false;
+				}
+
+				//
+				// Step 6. Build polygons mesh from contours.
+				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) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
+					return false;
+				}
+				if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh)) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
+					return false;
+				}
+
+				//
+				// Step 7. Create detail mesh which allows to access approximate height on each polygon.
+				if (listener) {listener->onNavMeshBuildUpdateMajor("building details");}
+				if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 7);}
+
+				m_dmesh = rcAllocPolyMeshDetail();
+				if (!m_dmesh) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'.");
+					return false;
+				}
+
+				if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh)) {
+					m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh.");
+					return false;
+				}
+
+				//if (!m_keepInterResults) {
+					rcFreeCompactHeightfield(m_chf);
+					m_chf = 0;
+					rcFreeContourSet(m_cset);
+					m_cset = 0;
+				//}
+
+
+		//	std::vector<TriangleOut> res;
+
+			const float* orig = m_pmesh->bmin;
+
+			// https://github.com/recastnavigation/recastnavigation/blob/master/Docs/Extern/Recast_api.txt
+			for (int i = 0; i < m_pmesh->npolys; ++i) {
+
+				const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2];
+
+				const uint8_t type = m_pmesh->areas[i];
+
+	//			Each entry is <tt>2 * #nvp</tt> in length. The first half of the entry
+	//			contains the indices of the polygon. The first instance of #RC_MESH_NULL_IDX
+	//			indicates the end of the indices for the entry. The second half contains
+	//			indices to neighbor polygons. A value of #RC_MESH_NULL_IDX indicates no
+	//			connection for the associated edge. (I.e. The edge is a solid border.)
+
+	//			we only use exactly 3 vertices per polygon, no iteration needed
+
+	//			for (int j = 0; j < m_pmesh->nvp; ++j) {
+	//				if (p[j] == RC_MESH_NULL_IDX) {break;}
+
+	//				const unsigned short* v = &m_pmesh->verts[p[j]*3];
+	//				const float x = orig[0] + v[0]*m_pmesh->cs;
+	//				const float z = orig[1] + v[1]*m_pmesh->ch;
+	//				const float y = orig[2] + v[2]*m_pmesh->cs;
+
+	//				pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST));
+
+	//			}
+
+				// un-swap Y/Z
+				const unsigned short* v0 = &m_pmesh->verts[p[0]*3];
+				const Point3 p0(orig[0] + v0[0]*m_pmesh->cs, orig[2] + v0[2]*m_pmesh->cs, orig[1] + v0[1]*m_pmesh->ch);
+
+				const unsigned short* v1 = &m_pmesh->verts[p[1]*3];
+				const Point3 p1(orig[0] + v1[0]*m_pmesh->cs, orig[2] + v1[2]*m_pmesh->cs, orig[1] + v1[1]*m_pmesh->ch);
+
+				const unsigned short* v2 = &m_pmesh->verts[p[2]*3];
+				const Point3 p2(orig[0] + v2[0]*m_pmesh->cs, orig[2] + v2[2]*m_pmesh->cs, orig[1] + v2[1]*m_pmesh->ch);
+
+				dst->add(p0,p1,p2,type);
+
+			}
+
+			// now, connect neighbors
+			for (int i = 0; i < m_pmesh->npolys; ++i) {
+
+				const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2];
+
+				// find all neighbor polygons using their index
+				for (int j = 0; j < m_pmesh->nvp; ++j) {
+					int jj = j + m_pmesh->nvp;					// offset, 2nd half of the array [size: 2*nvp]
+					if (p[jj] == RC_MESH_NULL_IDX) {continue;}	// no neighbor for the current edge!
+					const int idx = p[jj];
+					dst->connectUniDir(i, idx);
+				}
+
+			}
+
+			return true;
+
+		}
+
+
+		/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
+		Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const {
+			const 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
+			const Point2 p2 = line->from - perp * thickness_m/2;	// start-down
+			const Point2 p3 = line->to + perp * thickness_m/2;		// end-up
+			const Point2 p4 = line->to - perp * thickness_m/2;		// end-down
+			Floorplan::Polygon2 res;
+			res.points.push_back(p1);
+			res.points.push_back(p2);
+			res.points.push_back(p4);
+			res.points.push_back(p3);
+			return res;
+		}
+
+		/** 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;
+		}
+
+	};
+
+}
+
+#endif

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/NavMeshLocation.h

@@ -0,0 +1,37 @@
+#ifndef NAVMESHLOCATION_H
+#define NAVMESHLOCATION_H
+
+#include "../geo/Point3.h"
+
+class NavMeshTriangle;
+
+namespace NM {
+
+	/**
+	 * as Point3 -> Triangle (on Mesh) lookups are expensive,
+	 * we try to combine both information (point -> triangle)
+	 * most of the time using this structure
+	 */
+	template <typename Tria> struct NavMeshLocation {
+
+		/** point within the world (in meter) */
+		Point3 pos;
+
+		/** NavMeshTriangle the point belongs to */
+		const Tria* tria;
+
+		/** empty ctor */
+		NavMeshLocation() : pos(0,0,0), tria(nullptr) {
+			;
+		}
+
+		/** ctor */
+		NavMeshLocation(const Point3 pos, const Tria* tria) : pos(pos), tria(tria) {
+			;
+		}
+
+	};
+
+}
+
+#endif // NAVMESHLOCATION_H

navMesh/NavMeshRandom.h

@@ -0,0 +1,112 @@
+#ifndef NAVMESHRANDOM_H
+#define NAVMESHRANDOM_H
+
+#include <random>
+#include <vector>
+#include "../math/DrawList.h"
+#include "../geo/Point3.h"
+#include "../misc/PerfCheck.h"
+
+#include "NavMeshLocation.h"
+
+namespace NM {
+
+	/**
+	 * randomly pick points within the area of the nav-mesh.
+	 * points are picked evenly:
+	 * bigger triangles are used more often
+	 *
+	 */
+	template <typename Tria> class NavMeshRandom {
+
+		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;
+
+
+		uint32_t nextSeed() {
+			static uint32_t seed = 0;
+			return ++seed;
+		}
+
+	public:
+
+		/** ctor (const/non-const using T) */
+		template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
+
+			// 1st = almost always the same number?!
+			gen(); 	gen();
+
+			// construct a DrawList (probability = size[area] of the triangle
+			// bigger triangles must be choosen more often
+			for (size_t idx = 0; idx < srcTriangles.size(); ++idx) {
+				this->triangles.push_back(srcTriangles[idx]);
+				this->lst.add(idx, srcTriangles[idx]->getArea());
+			}
+
+		}
+
+		/** draw a random point */
+		NavMeshLocation<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];
+
+			// 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<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;
+
+					}
+				}
+
+			}
+
+		}
+
+	};
+
+}
+
+#endif // NAVMESHRANDOM_H

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

@@ -0,0 +1,230 @@
+#ifndef NAVMESHTRIANGLE_H
+#define NAVMESHTRIANGLE_H
+
+#include "../geo/Point3.h"
+#include "../geo/Point2.h"
+
+namespace NM {
+
+	/**
+	 * represents one triangle within the NavMesh
+	 * each Triangle has up to 3 neighbors (one per edge)
+	 *
+	 * for performance enhancements,
+	 * some memeber attributes are pre-calculated once
+	 */
+	class NavMeshTriangle {
+
+	private:
+
+		template<typename> friend class NavMesh;
+
+		const Point3 p1;
+		const Point3 p2;
+		const Point3 p3;
+		const uint8_t type;
+
+		NavMeshTriangle* _neighbors[3];
+		int _numNeighbors;
+
+	private:	// precalculated stuff
+
+		Point2 v0;
+		Point2 v1;
+		float dot00;
+		float dot01;
+		float dot11;
+		double invDenom;
+		float area;
+
+		float minZ;
+		float maxZ;
+
+		const Point3 center;
+		const Point3 v12;
+		const Point3 v13;
+
+	public:
+
+		/** ctor */
+		NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
+			p1(p1), p2(p2), p3(p3), type(type),
+			_neighbors(), _numNeighbors(0),
+			center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
+
+			precompute();
+
+		}
+
+		/** get the triangle's type */
+		uint8_t getType() const {return type;}
+
+		Point3 getP1() const {return p1;}
+
+		Point3 getP2() const {return p2;}
+
+		Point3 getP3() const {return p3;}
+
+
+		/** 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);
+		}
+
+		/** is the triangle plain? (same Z for all points) */
+		bool isPlain() const {
+			const float d1 = std::abs(p1.z - p2.z);
+			const float d2 = std::abs(p2.z - p3.z);
+			return (d1 < 0.1) && (d2 < 0.1);
+		}
+
+		const NavMeshTriangle* const* begin() const {return &_neighbors[0];}
+
+		const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}
+
+		Point3 getPoint(const float u, const float v) const {
+			return p1 + (v12*u) + (v13*v);
+		}
+
+		/** does the triangle contain the given 3D point? */
+		bool contains(const Point3 p) const {
+			return	(minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
+		}
+
+		/** does the triangle contain the given 2D point? */
+		bool contains(const Point2 p) const {
+
+			const Point2 v2 = p - p1.xy();
+
+			// Compute dot products
+			float dot02 = dot(v0, v2);
+			float dot12 = dot(v1, v2);
+
+			// Compute barycentric coordinates
+			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+			// Check if point is in triangle
+			return (u >= 0) && (v >= 0) && (u + v <= 1);
+
+		}
+
+		/** estimate the correct z-value for the given 2D point */
+		Point3 toPoint3(const Point2 p) const {
+
+			const Point2 v2 = p - p1.xy();
+
+			// Compute dot products
+			float dot02 = dot(v0, v2);
+			float dot12 = dot(v1, v2);
+
+			// Compute barycentric coordinates
+			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+			const Point3 res = getPoint(v,u);
+			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
+
+		}
+
+
+
+		/** get the triangle's size */
+		float getArea() const {
+			return area;
+		}
+
+		/** get the triangle's center-point */
+		Point3 getCenter() const {
+			return center;
+		}
+
+
+
+	private:
+
+		/** perform some pre-calculations to speed things up */
+		void precompute() {
+
+	#warning "TODO, z buffer"
+			minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15;	// TODO the builder does not align on the same height as we did
+			maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;
+
+			// Compute vectors
+			v0 = p3.xy() - p1.xy();
+			v1 = p2.xy() - p1.xy();
+
+			// Compute dot products
+			dot00 = dot(v0, v0);
+			dot01 = dot(v0, v1);
+			dot11 = dot(v1, v1);
+
+			// Compute barycentric coordinates
+			invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);
+
+
+
+
+			const float a = (p2-p1).length();
+			const float b = (p3-p1).length();
+			const float c = (p2-p3).length();
+			const float s = 0.5f * (a+b+c);
+			area = std::sqrt( s * (s-a) * (s-b) * (s-c) );
+
+		}
+
+	protected:
+
+		void addNeighbor(NavMeshTriangle* o) {
+			Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
+			_neighbors[_numNeighbors] = o;
+			++_numNeighbors;
+		}
+
+
+	};
+
+}
+
+#endif // NAVMESHTRIANGLE_H

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

@@ -0,0 +1,103 @@
+#ifndef NAVMESHSUB_H
+#define NAVMESHSUB_H
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../NavMeshRandom.h"
+
+#include <vector>
+#include <unordered_set>
+
+namespace NM {
+
+	template <typename Tria> class NavMeshSub {
+
+		std::vector<const Tria*> toVisit;
+
+	public:
+
+		NavMeshSub(const NavMeshLocation<Tria>& loc, float radius_m) {
+			build(loc,radius_m);
+		}
+
+		/** does this submesh contain the given point? */
+		bool contains(const Point2 p2) const {
+			for (const Tria* t : toVisit) {
+				if (t->contains(p2)) {return true;}
+			}
+			return false;
+		}
+
+		/** 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) {
+				if (t->contains(p2)) {return t;}
+			}
+			return nullptr;
+		}
+
+		/** perform random operations on the submesh */
+		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) {
+//				toVisit.push_back( (const Tria*)n );
+//			}
+//			size_t next = 1;	// start with the first neighbor (skip starting triangle itself)
+
+			size_t next = 0;
+			while (next < toVisit.size()) {
+
+				// next triangle
+				const NavMeshTriangle* cur = toVisit[next]; ++next;
+
+				// neighbors
+				for (const auto* n : *cur) {
+					const Tria* t = (const Tria*) n;
+					//const float dist = loc.pos.getDistance(n->getCenter());
+					const float dist = n->getDistanceApx(loc.pos);
+					if (dist > radius_m) {continue;}
+					if (visited.find(t) != visited.end()) {continue;}
+					toVisit.push_back(t);
+					visited.insert(t);
+				}
+
+			}
+
+		}
+
+
+	};
+
+}
+
+#endif // NAVMESHSUB_H

navMesh/walk/NavMeshWalkEval.h

@@ -0,0 +1,141 @@
+#ifndef NAVMESHWALKEVAL_H
+#define NAVMESHWALKEVAL_H
+
+#include "NavMeshWalkParams.h"
+#include "../NavMeshLocation.h"
+#include "../../math/Distributions.h"
+#include "../../misc/PerfCheck.h"
+
+namespace NM {
+
+	template <typename Tria> struct NavMeshPotentialWalk {
+
+		NavMeshWalkParams<Tria> requested;
+
+		NavMeshLocation<Tria> end;
+
+		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
+			;
+		}
+
+		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
+			;
+		}
+
+	};
+
+	/**
+	 * evaluate a NavMeshWalk from -> to = probability
+	 */
+	template <typename Tria> class NavMeshWalkEval {
+
+	public:
+
+		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;
+
+	};
+
+
+
+
+
+
+
+	/**
+	 * evaluate the difference between head(start,end) and the requested heading
+	 */
+	template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {
+
+		const double sigma_rad;
+		const double kappa;
+		Distribution::VonMises<double> _dist;
+		Distribution::LUT<double> dist;
+
+	public:
+
+		// kappa = 1/var = 1/sigma^2
+		// https://en.wikipedia.org/wiki/Von_Mises_distribution
+		WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
+			sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
+			;
+		}
+
+		virtual double getProbability(const NavMeshPotentialWalk<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 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
+	 */
+	template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {
+
+		const double sigma;
+
+		const Distribution::Normal<double> dist;
+
+	public:
+
+		WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
+
+		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;
+			return dist.getProbability(diff);
+			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+
+		}
+
+	};
+
+
+}
+
+#endif // NAVMESHWALKEVAL_H

navMesh/walk/NavMeshWalkParams.h

@@ -0,0 +1,76 @@
+#ifndef NAVMESHWALKPARAMS_H
+#define NAVMESHWALKPARAMS_H
+
+#include "../../geo/Heading.h"
+#include "../NavMeshLocation.h"
+#include "../NavMeshType.h"
+
+namespace NM {
+
+	/** configure pedestrian StepSizes */
+	struct StepSizes {
+
+		float stepSizeFloor_m = NAN;
+		float stepSizeStair_m = NAN;
+
+		bool isValid() const {
+			return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
+		}
+
+		template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const {
+
+			Assert::isTrue(isValid(), "invalid step-sizes given");
+
+			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;
+//			}
+
+		}
+
+	};
+
+	/** configure walking from -> to */
+	template <typename Tria> struct NavMeshWalkParams {
+
+		/** walk starts here (pos/tria) */
+		NavMeshLocation<Tria> start;
+
+		/** direction to walk to */
+		Heading heading;
+
+		/** number of steps to walk */
+		int numSteps;
+
+		/** configuration for pedestrian's step-sizes */
+		StepSizes stepSizes;
+
+
+		/** empty ctor */
+		NavMeshWalkParams() : heading(0) {;}
+
+		/** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */
+		float getToBeWalkedDistance() const {
+			if (_toBeWalkedDistance != _toBeWalkedDistance) {
+				_toBeWalkedDistance = stepSizes.inMeter(numSteps, start);
+			}
+			return _toBeWalkedDistance;
+		}
+
+	private:
+
+		// precalc
+		mutable float _toBeWalkedDistance = NAN;
+
+	};
+
+}
+
+#endif // NAVMESHWALKPARAMS_H

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

@@ -0,0 +1,114 @@
+#ifndef NAVMESHWALKSIMPLE_H
+#define NAVMESHWALKSIMPLE_H
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
+
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
+
+namespace NM {
+
+	template <typename Tria> class NavMeshWalkSimple {
+
+	private:
+
+		const NavMesh<Tria>& mesh;
+
+		std::vector<NavMeshWalkEval<Tria>*> evals;
+
+		int hits = 0;
+		int misses = 0;
+
+	public:
+
+
+		/** single result */
+		struct ResultEntry {
+			NavMeshLocation<Tria> location;
+			Heading heading;
+			double probability;
+			ResultEntry() : heading(0) {;}
+		};
+
+		/** list of results */
+		using ResultList = std::vector<ResultEntry>;
+
+	public:
+
+		/** ctor */
+		NavMeshWalkSimple(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) {
+
+			ResultEntry re;
+
+			// to-be-walked distance;
+			const float toBeWalkedDist = params.getToBeWalkedDistance();
+			const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
+
+			// construct reachable region
+			NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+			// get the to-be-reached destination's position (using start+distance+heading)
+			const Point2 dir = params.heading.asVector();
+			const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
+
+			const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+			// is above destination reachable?
+			if (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<Tria> 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;
+
+			}
+
+			const int total = (hits + misses);
+			if (total % 10000 == 0) {
+				std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
+			}
+
+			// calculate probability
+			const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
+			re.probability = 1.0;
+			for (const NavMeshWalkEval<Tria>* eval : evals) {
+				const double p1 = eval->getProbability(pwalk);
+				re.probability *= p1;
+			}
+
+			// done
+			return re;
+
+		}
+
+		ResultList getMany(const NavMeshWalkParams<Tria>& params) {
+			return {getOne(params)};
+		}
+
+
+	};
+
+}
+
+#endif // NAVMESHWALKSIMPLE_H

sensors/imu/MagnetometerData.h

@@ -36,7 +36,7 @@ struct MagnetometerData {
 	}
 
 	float magnitude() const {
-        return std::sqrt( x*x + y*y + z*z );
+		return std::sqrt( x*x + y*y + z*z );
 	}
 
 	MagnetometerData& operator += (const MagnetometerData& o) {
@@ -73,9 +73,9 @@ private:
 };
 
 namespace std {
-    inline MagnetometerData sqrt(const MagnetometerData& o) {
+	inline MagnetometerData sqrt(const MagnetometerData& o) {
-        return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
+		return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
-    }
+	}
 }
 
 #endif // INDOOR_IMU_MAGNETOMETERDATA_H

sensors/imu/StepDetection.h

@@ -36,7 +36,7 @@ private:
 	Timestamp blockUntil;
 	bool waitForUp = false;
 
-    const Timestamp blockTime = Timestamp::fromMS(250);		// 150-250 looks good
+	const Timestamp blockTime;								// 150-250 looks good
     const float upperThreshold = +0.4*0.6f;					// + is usually smaller than down (look at graphs)
     const float lowerThreshold = -1.5*0.6f;					// the 0.8 is for testing!
 
@@ -57,7 +57,7 @@ private:
 public:
 
 	/** ctor */
-	StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
+	StepDetection(const Timestamp blockTime = Timestamp::fromMS(200)) : blockTime(blockTime), avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
 
 		#ifdef WITH_DEBUG_PLOT
 			gp << "set autoscale xfix\n";

sensors/offline/FilePlayer.h

@@ -75,13 +75,25 @@ namespace Offline {
 			thread.join();
 		}
 
+
+		/** manual ticking */
+		int tickPos = 0;
+		void tick() {
+			const std::vector<Entry>& events = reader->getEntries();
+			const Entry& e = events[tickPos];
+			trigger(Timestamp::fromMS(e.ts), e);
+			++tickPos;
+		}
+
 	private:
 
+
+
 		/** background loop */
 		void loop() {
 
 			// get all sensor events from the offline file
-			const std::vector<Entry> events = reader->getEntries();
+			const std::vector<Entry>& events = reader->getEntries();
 
 			// reference time (system vs. first-event)
 			Timestamp tsRef1 = Timestamp::fromMS(events.front().ts);
@@ -104,23 +116,7 @@ namespace Offline {
 					if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));}
 				}
 
-				// event index
+				trigger(ts, e);
-				const size_t idx = e.idx;
-
-#warning "some sensors todo:"
-				switch(e.type) {
-					case Sensor::ACC:			listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
-					case Sensor::BARO:			listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
-					case Sensor::BEACON:		break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
-					case Sensor::COMPASS:		listener->onCompass(ts, reader->getCompass()[idx].data); break;
-					case Sensor::MAGNETOMETER:	listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
-					case Sensor::GPS:			listener->onGPS(ts, reader->getGPS()[idx].data); break;
-					case Sensor::GRAVITY:		listener->onGravity(ts, reader->getGravity()[idx].data); break;
-					case Sensor::GYRO:			listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
-					case Sensor::LIN_ACC:		break;//listener->on(ts, reader->getBarometer()[idx].data); break;
-					case Sensor::WIFI:			listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
-					default:					throw Exception("code error. found not-yet-implemented sensor");
-				}
 
 			}
 
@@ -129,8 +125,34 @@ namespace Offline {
 
 		}
 
+
+
+		void trigger(const Timestamp ts, const Entry& e) {
+
+			const int idx = e.idx;
+
+			#warning "some sensors todo:"
+			switch(e.type) {
+				case Sensor::ACC:			listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
+				case Sensor::BARO:			listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
+				case Sensor::BEACON:		break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
+				case Sensor::COMPASS:		listener->onCompass(ts, reader->getCompass()[idx].data); break;
+				case Sensor::MAGNETOMETER:	listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
+				case Sensor::GPS:			listener->onGPS(ts, reader->getGPS()[idx].data); break;
+				case Sensor::GRAVITY:		listener->onGravity(ts, reader->getGravity()[idx].data); break;
+				case Sensor::GYRO:			listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
+				case Sensor::LIN_ACC:		break;//listener->on(ts, reader->getBarometer()[idx].data); break;
+				case Sensor::WIFI:			listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
+				default:					throw Exception("code error. found not-yet-implemented sensor");
+			}
+
+		}
+
+
 	};
 
+
+
 }
 
 #endif // FILEPLAYER_H

smc/smoothing/BackwardSimulation.h

@@ -169,6 +169,7 @@ namespace SMC {
 
                     State est = estimation->estimate(smoothedParticles);
                     estimatedStates.push_back(est);
+                    continue;
                 }
 
                 // compute weights using the transition model

smc/smoothing/FastKDESmoothing.h

@@ -76,17 +76,17 @@ namespace SMC {
         /** ctor */
         FastKDESmoothing(int numParticles, const Floorplan::IndoorMap* map, const int gridsize_cm, const Point2 bandwith) {
             this->numParticles = numParticles;
-            backwardParticles.reserve(numParticles);
+            //backwardParticles.reserve(numParticles);
-            firstFunctionCall = true;
+            this->firstFunctionCall = true;
 
-            const Point3 maxBB = FloorplanHelper::getBBox(map).getMax();
+            const Point3 maxBB = FloorplanHelper::getBBox(map).getMax() * 100.0;
-            const Point3 minBB = FloorplanHelper::getBBox(map).getMin();
+            const Point3 minBB = FloorplanHelper::getBBox(map).getMin() * 100.0;
-            bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
+            this->bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
 
             // Create histogram
             size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_cm);
             size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_cm);
-            grid = Grid2D<float>(bb, nBinsX, nBinsY);
+            this->grid = Grid2D<float>(bb, nBinsX, nBinsY);
 
             this->bandwith = bandwith;
         }
@@ -186,7 +186,7 @@ namespace SMC {
 
                     State est = estimation->estimate(smoothedParticles);
                     estimatedStates.push_back(est);
-                    return est;
+                    continue;
                 }
 
                 // transition p(q_t+1* | q_t): so we are performing again a forward transition step.
@@ -215,7 +215,7 @@ namespace SMC {
                 smoothedParticles = forwardHistory.getParticleSet(i);
                 for(Particle<State> p : smoothedParticles){
                     p.weight = p.weight * grid.fetch(p.state.position.x_cm, p.state.position.y_cm);
-                    Assert::isNot0(p.weight, "smoothed particle has zero weight");
+                    //Assert::isNot0(p.weight, "smoothed particle has zero weight");
                 }
 
                 //normalization

synthetic/SyntheticPath.h

@@ -4,18 +4,23 @@
 #include "../math/Interpolator.h"
 #include "../floorplan/v2/Floorplan.h"
 #include "../floorplan/v2/FloorplanHelper.h"
+#include "../floorplan/v2/FloorplanHelper.h"
 
 /** allows interpolation along a synthetic path */
 class SyntheticPath : private Interpolator<float, Point3> {
 
 	using Base = Interpolator<float, Point3>;
 	using Entry = Base::InterpolatorEntry;
+	const Floorplan::IndoorMap* map;
+
 
 public:
 
 	/** create path using the given ground-truth points from the map */
 	void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {
 
+		this->map = map;
+
 		// get all ground-truth points from the map
 		auto gtps = FloorplanHelper::getGroundTruthPoints(map);
 		float dist = 0;
@@ -38,6 +43,8 @@ public:
 		return Base::getEntries();
 	}
 
+
+
 	/** smooth harsh angles */
 	void smooth(float delta = 1, int numRuns = 1) {
 
@@ -104,6 +111,18 @@ 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);
+		const Point3 pos2 = getPosAfterDistance(distance + 0.1);
+		const float delta = std::abs(pos1.z - pos2.z);
+		return delta < 0.01;
+	}
+
 };
 
 #endif // INDOOR_SYNTEHTICPATH_H

synthetic/SyntheticSteps.h

@@ -25,26 +25,39 @@ private:
 	/** the walker to listen to */
 	SyntheticWalker* walker;
 
-	/** the pedestrian's step-size (in meter) */
+	///** the pedestrian's step-size (in meter) */
-	float stepSize_m = 0.7;
+	//float stepSize_m = 0;
+
+	///** when walking stairs, the step size is much smaller */
+	//float stepSizeStair_m = 0;
 
 	float lastStepAtDistance = 0;
 
 	Timestamp refStepPattern;
-	Interpolator<Timestamp, AccelerometerData> stepPattern;
+	Interpolator<Timestamp, AccelerometerData> stepPatternPlain;
+	Interpolator<Timestamp, AccelerometerData> stepPatternStair;
 
 	Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
 	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
 	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
 
+
 	int stepPatternPos = -1;
 
 	std::vector<Listener*> listeners;
 
+	//float stepSize_m;
+	//float stepSizeSigma_m;
+	float noiseLevel;
+	Distribution::Normal<float> dNextStep;
+	Distribution::Normal<float> dNextStepStair;
+
 public:
 
 	/** ctor with the walker to follow */
-	SyntheticSteps(SyntheticWalker* walker) {
+	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) {
 
 		walker->addListener(this);
 		dX.setSeed(1);
@@ -64,10 +77,16 @@ public:
 //			AccelerometerData acc(x,y,z);
 //			stepPattern.add(Timestamp::fromMS(i), acc);
 //		}
-		stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
+
-		stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
+		stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
-		stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
+		stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
-		stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
+		stepPatternPlain.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
+		stepPatternPlain.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
+
+		stepPatternStair.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
+		stepPatternStair.add(Timestamp::fromMS(200), AccelerometerData(0, 0.6, 4));
+		stepPatternStair.add(Timestamp::fromMS(300), AccelerometerData(0.5, -0.6, -3.5));
+		stepPatternStair.add(Timestamp::fromMS(350), AccelerometerData(0, 0, 0));
 
 	}
 
@@ -79,18 +98,20 @@ public:
 protected:
 
 
-	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
+	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override {
 
 		(void) curPos;
-		const float nextStepAt = (lastStepAtDistance + stepSize_m);
+		const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw());
+		const auto stepPattern = (type == SyntheticWalker::Type::FLOOR) ? (stepPatternPlain) : (stepPatternStair);
+		const float nextStepAt = lastStepAtDistance + distAdd;
 
 		// 1st, start with random noise on the accelerometer
 		const float x = dX.draw();
 		const float y = dY.draw();
 		const float z = dZ.draw();
-		const AccelerometerData base(0, 4, 9.7);
+		const AccelerometerData aBase(0, 4, 9.7);
-		const AccelerometerData noise(x, y, z);
+		const AccelerometerData aNoise(x, y, z);
-		AccelerometerData acc = base + noise;
+		AccelerometerData acc = aBase + aNoise * noiseLevel;
 
 		// is it time to inject a "step" into the accelerometer data?
 		if (walkedDistance > nextStepAt) {
@@ -105,7 +126,7 @@ protected:
 				refStepPattern = Timestamp::fromMS(0);
 			} else {
 				const AccelerometerData step = stepPattern.get(curPatPos);
-				acc = base + noise*2.5f + step;
+				acc = aBase + (aNoise * noiseLevel) + step;
 			}
 		}
 

synthetic/SyntheticTurns.h

@@ -7,7 +7,7 @@
 #include "../sensors/imu/GyroscopeData.h"
 #include "../geo/Heading.h"
 
-#include "../math/distribution/Normal.h"
+#include "../math/Distributions.h"
 
 /**
  * simulates acceleromter and gyroscope data
@@ -39,15 +39,23 @@ private:
 
 	Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
 	Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
-	Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10);	// heading error, slightly biased
 
+	Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
+
+	//float headingDrift_rad;
+	//float headingSigma_rad;
+	float noiseLevel;
+	Distribution::Normal<float> dHeadErr;
 
 	std::vector<Listener*> listeners;
 
 public:
 
 	/** ctor with the walker to follow */
-	SyntheticTurns(SyntheticWalker* walker) {
+	SyntheticTurns(SyntheticWalker* walker, const float headingDrift_rad = 0, const float headingSigma_rad = 0, const float noiseLevel = 0) :
+		//headingDrift_rad(headingDrift_rad), headingSigma_rad(headingSigma_rad),
+		noiseLevel(noiseLevel + 0.00001f), dHeadErr(headingDrift_rad, headingSigma_rad) {
+
 		walker->addListener(this);
 		dAccX.setSeed(1);
 		dAccY.setSeed(3);
@@ -55,6 +63,7 @@ public:
 		dGyroX.setSeed(7);
 		dGyroY.setSeed(9);
 		dGyroZ.setSeed(11);
+
 	}
 
 	/** attach a listener to this provider */
@@ -68,7 +77,7 @@ protected:
 	Heading desiredHead = Heading(0);
 	Heading curHead = Heading(0);
 	Point3 lastPos = Point3(NAN, NAN, NAN);
-	float change;
+	double change = 0;
 
 	inline float clamp(const float val, const float min, const float max) {
 		if (val < min) {return min;}
@@ -76,7 +85,7 @@ protected:
 		return val;
 	}
 
-	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
+	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override {
 
 		// time sine last onWalk();
 		if (lastTs.isZero()) {lastTs = walkedTime; return;}
@@ -86,36 +95,42 @@ protected:
 		if (lastPos.x != lastPos.x) {
 			lastPos = curPos;
 		} else {
-			desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
+			desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();
 			lastPos = curPos;
 		}
 
 		// difference between current-heading and desired-heading
-		const float diffRad = Heading::getSignedDiff(curHead, desiredHead);
+		const double maxChange = dMaxChange.draw();
+		const double diffRad = Heading::getSignedDiff(curHead, desiredHead);
+		//change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange);
+		change = clamp(diffRad / 25, -maxChange, +maxChange);
 
-		// slowly change the current heading to match the desired one
+
-		const float maxChange = dMaxChange.draw();
+//		// slowly change the current heading to match the desired one
-		const float toChange = clamp(diffRad, -maxChange, +maxChange);
+//		//const double maxChange = dMaxChange.draw();
-		//if (change < toChange) {change += toChange*0.01;}
+//		//const double toChange = clamp(diffRad, -maxChange, +maxChange);
-		if (change > toChange) {change *= 0.93;}
+//		const double toChange = diffRad;
-		if (change < toChange) {change += dChange.draw()/10000;}
+//		//if (change < toChange) {change += toChange*0.01;}
-		//if (change > toChange) {change -= dChange.draw();}
+//		if (change > toChange) {change *= 0.93;}
+//		//if (change < toChange) {change += dChange.draw()/10000;}	// does not work for small changes?!
+//		if (change < toChange) {change += (toChange-change) * 0.07;}
+//		//if (change > toChange) {change -= dChange.draw();}
 
 
 
 		curHead += change;
 
 		// convert to gyro's radians-per-second
-		const float radPerSec = change * 1000 / deltaTs.ms();;
+		const double radPerSec = change * 1000 / deltaTs.ms();;
 
-		const float accX = 0.00 + dAccX.draw();
+		const float accX = 0.00 + dAccX.draw() * (noiseLevel);
-		const float accY = 0.00 + dAccY.draw();
+		const float accY = 0.00 + dAccY.draw() * (noiseLevel);
-		const float accZ = 9.81 + dAccZ.draw();
+		const float accZ = 9.81 + dAccZ.draw() * (noiseLevel);
 		AccelerometerData acc(accX, accY, accZ);
 
-		const float gyroX = dGyroX.draw();
+		const float gyroX = dGyroX.draw() * (noiseLevel);
-		const float gyroY = dGyroY.draw();
+		const float gyroY = dGyroY.draw() * (noiseLevel);
-		const float gyroZ = dGyroZ.draw() + radPerSec;
+		const float gyroZ = dGyroZ.draw() * (noiseLevel) + radPerSec;
 		GyroscopeData gyro(gyroX, gyroY, gyroZ);
 
 		for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}

synthetic/SyntheticWalker.h

@@ -8,9 +8,14 @@ class SyntheticWalker {
 
 public:
 
+	enum class Type {
+		FLOOR,
+		NON_FLOOR,
+	};
+
 	class Listener {
 	public:
-		virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0;
+		virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos, const Type type) = 0;
 	};
 
 private:
@@ -44,6 +49,10 @@ public:
 		this->listeners.push_back(l);
 	}
 
+	bool done() {
+		return path.doneAtDistance(this->walkedDistance);
+	}
+
 	/** increment the walk */
 	Point3 tick(const Timestamp timePassed) {
 
@@ -55,11 +64,13 @@ public:
 
 		// get the current position along the path
 		const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance);
+		const bool isPlainPart = path.isPlain(this->walkedDistance);
+		const Type type = (isPlainPart) ? (Type::FLOOR) : (Type::NON_FLOOR);
 
 		Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() );
 
 		// inform listener
-		for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);}
+		for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath, type);}
 
 		return curPosOnPath;
 

tests/math/filter/TestButter.cpp

@@ -23,234 +23,234 @@
 
 TEST(Butterworth, offlineSinus) {
 
-    //input data
+	//input data
-    std::minstd_rand gen;
+	std::minstd_rand gen;
-    std::uniform_real_distribution<double> noise (-0.1, +0.1);
+	std::uniform_real_distribution<double> noise (-0.1, +0.1);
 
-    int size = 1100; //Fs
+	int size = 1100; //Fs
-    double* input = new double[size];
+	double* input = new double[size];
-    double* output = new double[size];
+	double* output = new double[size];
 
-    // 17.5hz sin signal with random noise [-0.1, 0.1]
+	// 17.5hz sin signal with random noise [-0.1, 0.1]
-    for( int i=0; i < size; ++i ){
+	for( int i=0; i < size; ++i ){
-        input[i] = sin(0.1 * i) + noise(gen);
+		input[i] = sin(0.1 * i) + noise(gen);
-    }
+	}
 
-    //butterworth
+	//butterworth
-    Filter::ButterworthLP<double> butter(size,20,5);
+	Filter::ButterworthLP<double> butter(size,20,5);
-    butter.stepInitialization(0);
+	butter.stepInitialization(0);
-    butter.filter(input, output, size, 0, true);
+	butter.filter(input, output, size, 0, true);
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
-    K::GnuplotPlotElementLines linesInput;
+	K::GnuplotPlotElementLines linesInput;
-    K::GnuplotPlotElementLines linesOutput;
+	K::GnuplotPlotElementLines linesOutput;
 
-    for(int i=0; i < size-1; ++i){
+	for(int i=0; i < size-1; ++i){
 
-        K::GnuplotPoint2 input_p1(i, input[i]);
+		K::GnuplotPoint2 input_p1(i, input[i]);
-        K::GnuplotPoint2 input_p2(i+1, input[i+1]);
+		K::GnuplotPoint2 input_p2(i+1, input[i+1]);
 
-        K::GnuplotPoint2 output_p1(i, output[i]);
+		K::GnuplotPoint2 output_p1(i, output[i]);
-        K::GnuplotPoint2 output_p2(i+1, output[i+1]);
+		K::GnuplotPoint2 output_p2(i+1, output[i+1]);
 
-        linesInput.addSegment(input_p1, input_p2);
+		linesInput.addSegment(input_p1, input_p2);
-        linesOutput.addSegment(output_p1, output_p2);
+		linesOutput.addSegment(output_p1, output_p2);
-    }
+	}
 	linesOutput.getStroke().getColor().setHexStr("#00FF00");
 
-    plot.add(&linesInput);
+	plot.add(&linesInput);
-    plot.add(&linesOutput);
+	plot.add(&linesOutput);
 
-    gp.draw(plot);
+	gp.draw(plot);
-    gp.flush();
+	gp.flush();
 
-    sleep(10);
+	sleep(10);
 }
 
 TEST(Butterworth, onlineSinus) {
 
-    int size = 1100; //Fs
+	int size = 1100; //Fs
-    double* input = new double[size];
+	double* input = new double[size];
-    double* output = new double[size];
+	double* output = new double[size];
 
-    Filter::ButterworthLP<double> butter(size,20,5);
+	Filter::ButterworthLP<double> butter(size,20,5);
-    butter.stepInitialization(0);
+	butter.stepInitialization(0);
 
-    //input data
+	//input data
-    std::minstd_rand gen;
+	std::minstd_rand gen;
-    std::uniform_real_distribution<double> noise (-0.1, +0.1);
+	std::uniform_real_distribution<double> noise (-0.1, +0.1);
 
-    // 17.5hz sin signal with random noise [-0.1, 0.1]
+	// 17.5hz sin signal with random noise [-0.1, 0.1]
-    for( int i=0; i < size; ++i ){
+	for( int i=0; i < size; ++i ){
-        input[i] = sin(0.1 * i) + noise(gen);
+		input[i] = sin(0.1 * i) + noise(gen);
 
-        output[i] = butter.process(input[i]);
+		output[i] = butter.process(input[i]);
-    }
+	}
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
-    K::GnuplotPlotElementLines linesInput;
+	K::GnuplotPlotElementLines linesInput;
-    K::GnuplotPlotElementLines linesOutput;
+	K::GnuplotPlotElementLines linesOutput;
 
-    for(int i=0; i < size-1; ++i){
+	for(int i=0; i < size-1; ++i){
 
-        K::GnuplotPoint2 input_p1(i, input[i]);
+		K::GnuplotPoint2 input_p1(i, input[i]);
-        K::GnuplotPoint2 input_p2(i+1, input[i+1]);
+		K::GnuplotPoint2 input_p2(i+1, input[i+1]);
 
-        K::GnuplotPoint2 output_p1(i, output[i]);
+		K::GnuplotPoint2 output_p1(i, output[i]);
-        K::GnuplotPoint2 output_p2(i+1, output[i+1]);
+		K::GnuplotPoint2 output_p2(i+1, output[i+1]);
 
-        linesInput.addSegment(input_p1, input_p2);
+		linesInput.addSegment(input_p1, input_p2);
-        linesOutput.addSegment(output_p1, output_p2);
+		linesOutput.addSegment(output_p1, output_p2);
-    }
+	}
 	linesOutput.getStroke().getColor().setHexStr("#00FF00");
 
-    plot.add(&linesInput);
+	plot.add(&linesInput);
-    plot.add(&linesOutput);
+	plot.add(&linesOutput);
 
-    gp.draw(plot);
+	gp.draw(plot);
-    gp.flush();
+	gp.flush();
 
-    sleep(1);
+	sleep(1);
 }
 
 TEST(Butterworth, offlineOctaveBaro) {
 
 
-    double* input = new double[100000];
+	double* input = new double[100000];
-    double* output = new double[100000];
+	double* output = new double[100000];
 
-    Interpolator<int, double> interp;
+	Interpolator<int, double> interp;
 
-    //read file
+	//read file
-    std::string line;
+	std::string line;
-    std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
+	std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
-    std::ifstream infile(filename);
+	std::ifstream infile(filename);
 
-    int counter = 0;
+	int counter = 0;
-    while (std::getline(infile, line))
+	while (std::getline(infile, line))
-    {
+	{
-      std::istringstream iss(line);
+	  std::istringstream iss(line);
-      int ts;
+	  int ts;
-      double value;
+	  double value;
 
-      while (iss >> ts >> value) {
+	  while (iss >> ts >> value) {
 
-        interp.add(ts, value);
+		interp.add(ts, value);
 
-        while(interp.getMaxKey() > counter*20 ){
+		while(interp.getMaxKey() > counter*20 ){
-            double interpValue = interp.get(counter*20);
+			double interpValue = interp.get(counter*20);
 
-            input[counter] = interpValue;
+			input[counter] = interpValue;
-            //std::cout << counter*20 << " " << interpValue << " i" << std::endl;
+			//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
-            ++counter;
+			++counter;
-        }
+		}
 
-        //std::cout << ts << " " << value << " r" << std::endl;
+		//std::cout << ts << " " << value << " r" << std::endl;
 
-      }
+	  }
-    }
+	}
 
-    Filter::ButterworthLP<double> butter(50,0.2,2);
+	Filter::ButterworthLP<double> butter(50,0.2,2);
-    butter.filter(input, output, counter, 938.15, true);
+	butter.filter(input, output, counter, 938.15, true);
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
-    K::GnuplotPlotElementLines linesInput;
+	K::GnuplotPlotElementLines linesInput;
-    K::GnuplotPlotElementLines linesOutput;
+	K::GnuplotPlotElementLines linesOutput;
 
-    for(int i=0; i < counter-1; ++i){
+	for(int i=0; i < counter-1; ++i){
 
-        K::GnuplotPoint2 input_p1(i, input[i]);
+		K::GnuplotPoint2 input_p1(i, input[i]);
-        K::GnuplotPoint2 input_p2(i+1, input[i+1]);
+		K::GnuplotPoint2 input_p2(i+1, input[i+1]);
 
-        K::GnuplotPoint2 output_p1(i, output[i]);
+		K::GnuplotPoint2 output_p1(i, output[i]);
-        K::GnuplotPoint2 output_p2(i+1, output[i+1]);
+		K::GnuplotPoint2 output_p2(i+1, output[i+1]);
 
-        linesInput.addSegment(input_p1, input_p2);
+		linesInput.addSegment(input_p1, input_p2);
-        linesOutput.addSegment(output_p1, output_p2);
+		linesOutput.addSegment(output_p1, output_p2);
-    }
+	}
 	linesOutput.getStroke().getColor().setHexStr("#00FF00");
 
-    plot.add(&linesInput);
+	plot.add(&linesInput);
-    plot.add(&linesOutput);
+	plot.add(&linesOutput);
 
-    gp.draw(plot);
+	gp.draw(plot);
-    gp.flush();
+	gp.flush();
 
-    sleep(1);
+	sleep(1);
 
 }
 
 TEST(Butterworth, onlineOctaveBaro) {
 
-    std::vector<double> input;
+	std::vector<double> input;
-    std::vector<double> output;
+	std::vector<double> output;
 
-    Interpolator<int, double> interp;
+	Interpolator<int, double> interp;
 
-    Filter::ButterworthLP<double> butter(50,0.02,2);
+	Filter::ButterworthLP<double> butter(50,0.02,2);
-    butter.stepInitialization(938.15);
+	butter.stepInitialization(938.15);
 
-    //read file
+	//read file
-    std::string line;
+	std::string line;
-    std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
+	std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
-    std::ifstream infile(filename);
+	std::ifstream infile(filename);
 
-    int counter = 1;
+	int counter = 1;
-    while (std::getline(infile, line))
+	while (std::getline(infile, line))
-    {
+	{
-      std::istringstream iss(line);
+	  std::istringstream iss(line);
-      int ts;
+	  int ts;
-      double value;
+	  double value;
 
-      while (iss >> ts >> value) {
+	  while (iss >> ts >> value) {
 
-        interp.add(ts, value);
+		interp.add(ts, value);
 
-        while(interp.getMaxKey() > counter*20 ){
+		while(interp.getMaxKey() > counter*20 ){
-            double interpValue = interp.get(counter*20);
+			double interpValue = interp.get(counter*20);
 
-            //std::cout << counter*20 << " " << interpValue << " i" << std::endl;
+			//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
 
-            input.push_back(interpValue);
+			input.push_back(interpValue);
 
-            output.push_back(butter.process(interpValue));
+			output.push_back(butter.process(interpValue));
 
-            ++counter;
+			++counter;
-        }
+		}
 
-        //std::cout << ts << " " << value << " r" << std::endl;
+		//std::cout << ts << " " << value << " r" << std::endl;
 
-      }
+	  }
-    }
+	}
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
-    K::GnuplotPlotElementLines linesInput;
+	K::GnuplotPlotElementLines linesInput;
-    K::GnuplotPlotElementLines linesOutput;
+	K::GnuplotPlotElementLines linesOutput;
 
-    for(int i=0; i < input.size()-1; ++i){
+	for(int i=0; i < input.size()-1; ++i){
 
-        K::GnuplotPoint2 input_p1(i, input[i]);
+		K::GnuplotPoint2 input_p1(i, input[i]);
-        K::GnuplotPoint2 input_p2(i+1, input[i+1]);
+		K::GnuplotPoint2 input_p2(i+1, input[i+1]);
 
-        K::GnuplotPoint2 output_p1(i, output[i]);
+		K::GnuplotPoint2 output_p1(i, output[i]);
-        K::GnuplotPoint2 output_p2(i+1, output[i+1]);
+		K::GnuplotPoint2 output_p2(i+1, output[i+1]);
 
-        linesInput.addSegment(input_p1, input_p2);
+		linesInput.addSegment(input_p1, input_p2);
-        linesOutput.addSegment(output_p1, output_p2);
+		linesOutput.addSegment(output_p1, output_p2);
-    }
+	}
 	linesOutput.getStroke().getColor().setHexStr("#00FF00");
 
-    plot.add(&linesInput);
+	plot.add(&linesInput);
-    plot.add(&linesOutput);
+	plot.add(&linesOutput);
 
-    gp.draw(plot);
+	gp.draw(plot);
-    gp.flush();
+	gp.flush();
 
-    sleep(1);
+	sleep(1);
 }
 
 

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

@@ -0,0 +1,40 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../navMesh/NavMeshFactory.h"
+using namespace NM;
+
+TEST(NavMeshFactory, 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;
+
+	NavMeshSettings set;
+	NavMesh<NM::NavMeshTriangle> nm;
+	NavMeshFactory<NM::NavMeshTriangle> fac(&nm,set);
+	fac.build(&map);
+
+	ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5);
+	ASSERT_NEAR(0, 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(2, nm.getNumTriangles());
+
+//	ASSERT_EQ(nm.getNeighbor(0,0), nm[1]);
+//	ASSERT_EQ(nm.getNeighbor(1,0), nm[0]);
+
+}
+
+#endif

tests/navMesh/TestNavMeshSub.cpp

@@ -0,0 +1,87 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../navMesh/NavMeshFactory.h"
+#include "../../navMesh/walk/NavMeshSub.h"
+using namespace NM;
+
+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;
+
+	NavMeshSettings set;
+	NavMesh<NM::NavMeshTriangle> nm;
+	NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
+	fac.build(&map);
+
+	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));
+		}
+
+	}
+
+
+
+}
+
+#endif

tests/navMesh/TestNavMeshTriangle.cpp

@@ -0,0 +1,36 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../navMesh/NavMeshTriangle.h"
+using namespace NM;
+
+TEST(NavMeshTriangle, contains) {
+
+	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)));
+	ASSERT_TRUE(t1.contains(Point3(0,1,0)));
+	ASSERT_TRUE(t1.contains(Point3(0.5,0.5,0)));
+
+	ASSERT_FALSE(t1.contains(Point3(0.501,0.5,0)));
+	ASSERT_FALSE(t1.contains(Point3(0.5,0.501,0)));
+	ASSERT_FALSE(t1.contains(Point3(1,1,0)));
+
+}
+
+TEST(NavMeshTriangle, area) {
+
+	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), 1);
+	ASSERT_NEAR(0.5, t2.getArea(), 0.0001);
+
+}
+
+
+
+
+#endif

tests/sensors/imu/TestMotionDetection.cpp

@@ -1,4 +1,4 @@
-#ifdef WITH_TESTS
+#ifdef TODO_________WITH_TESTS
 
 #include "../../Tests.h"
 
@@ -9,191 +9,190 @@
 /** visualize the motionAxis */
 TEST(MotionDetection, motionAxis) {
 
-    MotionDetection md;
+	MotionDetection md;
 
-    //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
+	//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
 
-    //Walking with smartphone straight and always parallel to motion axis
+	//Walking with smartphone straight and always parallel to motion axis
-    //std::string filename = getDataFile("motion/straight_potrait.csv");
+	//std::string filename = getDataFile("motion/straight_potrait.csv");
 
-    //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
+	//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
-    std::string filename = getDataFile("motion/straight_landscape_left.csv");
+	std::string filename = getDataFile("motion/straight_landscape_left.csv");
-    //std::string filename = getDataFile("motion/straight_landscape_right.csv");
+	//std::string filename = getDataFile("motion/straight_landscape_right.csv");
 
-    //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
+	//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
-    //std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
+	//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
 
-    //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
+	//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
-    //std::string filename = getDataFile("motion/rounds_potrait.csv");
+	//std::string filename = getDataFile("motion/rounds_potrait.csv");
 
-    //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
+	//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
-    //std::string filename = getDataFile("motion/rounds_landscape.csv");
+	//std::string filename = getDataFile("motion/rounds_landscape.csv");
 
-    //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
+	//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
-    //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
+	//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
 
-    //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
+	//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
-    //std::string filename = getDataFile("motion/rounds_pocket.csv");
+	//std::string filename = getDataFile("motion/rounds_pocket.csv");
 
-    //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
+	//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
-    //std::string filename = getDataFile("motion/table_flat.csv");
+	//std::string filename = getDataFile("motion/table_flat.csv");
 
 	Offline::FileReader fr(filename);
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
 
-    gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
+	gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
 
 
-    Eigen::Vector2f curVec;
+	Eigen::Vector2f curVec;
-    float motionAxisAngleRad;
+	float motionAxisAngleRad;
-    Timestamp ts;
+	Timestamp ts;
-    Timestamp lastTs;
+	Timestamp lastTs;
 
-    //calc motion axis
+	//calc motion axis
 	for (const Offline::Entry& e : fr.getEntries()) {
 
-        ts = Timestamp::fromMS(e.ts);
+		ts = Timestamp::fromMS(e.ts);
 
 		if (e.type == Offline::Sensor::LIN_ACC) {
-            md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
+			md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
 
 		} else if (e.type == Offline::Sensor::GRAVITY) {
-            md.addGravity(ts, fr.getGravity()[e.idx].data);
+			md.addGravity(ts, fr.getGravity()[e.idx].data);
-            curVec = md.getCurrentMotionAxis();
+			curVec = md.getCurrentMotionAxis();
-            motionAxisAngleRad = md.getMotionChangeInRad();
+			motionAxisAngleRad = md.getMotionChangeInRad();
-        }
+		}
 
-        // start with the first available timestamp
+		// start with the first available timestamp
-        if (lastTs.isZero()) {lastTs = ts;}
+		if (lastTs.isZero()) {lastTs = ts;}
 
-        if(ts - lastTs > Timestamp::fromMS(500)) {
+		if(ts - lastTs > Timestamp::fromMS(500)) {
 
-            lastTs = ts;
+			lastTs = ts;
 
-            K::GnuplotPoint2 raw_p1(0, 0);
+			K::GnuplotPoint2 raw_p1(0, 0);
-            K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
+			K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
-            K::GnuplotPlotElementLines motionLines;
+			K::GnuplotPlotElementLines motionLines;
-            motionLines.addSegment(raw_p1, raw_p2);
+			motionLines.addSegment(raw_p1, raw_p2);
-            plot.add(&motionLines);
+			plot.add(&motionLines);
 
-            gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159  << "' at screen 0.1,0.1\n";
+			gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159  << "' at screen 0.1,0.1\n";
 
-            gp.draw(plot);
+			gp.draw(plot);
-            gp.flush();
+			gp.flush();
-            //usleep(5000*33);
+			//usleep(5000*33);
-        }
+		}
-    }
+	}
 
-    //was passiert bei grenzwerten. 90° oder sowas.
+	//was passiert bei grenzwerten. 90° oder sowas.
-    //wie stabil ist die motion axis eigentlich?
+	//wie stabil ist die motion axis eigentlich?
-    //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
+	//erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
-    //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
+	//wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
 
 }
 
 /** comparing motionAngle and turnAngle */
 TEST(MotionDetection, motionAngle) {
 
-    MotionDetection md;
+	MotionDetection md;
-    PoseDetection pd;
+	TurnDetection td;
-    TurnDetection td(&pd);
 
-    //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
+	//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
 
-    //Walking with smartphone straight and always parallel to motion axis
+	//Walking with smartphone straight and always parallel to motion axis
-    std::string filename = getDataFile("motion/straight_potrait.csv");
+	std::string filename = getDataFile("motion/straight_potrait.csv");
 
-    //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
+	//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
-    //std::string filename = getDataFile("motion/straight_landscape_left.csv");
+	//std::string filename = getDataFile("motion/straight_landscape_left.csv");
-    //std::string filename = getDataFile("motion/straight_landscape_right.csv");
+	//std::string filename = getDataFile("motion/straight_landscape_right.csv");
 
-    //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
+	//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
-    //std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
+	//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
 
-    //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
+	//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
-    //std::string filename = getDataFile("motion/rounds_potrait.csv");
+	//std::string filename = getDataFile("motion/rounds_potrait.csv");
 
-    //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
+	//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
-    //std::string filename = getDataFile("motion/rounds_landscape.csv");
+	//std::string filename = getDataFile("motion/rounds_landscape.csv");
 
-    //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
+	//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
-    //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
+	//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
 
-    //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
+	//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
-    //std::string filename = getDataFile("motion/rounds_pocket.csv");
+	//std::string filename = getDataFile("motion/rounds_pocket.csv");
 
-    //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
+	//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
-    //std::string filename = getDataFile("motion/table_flat.csv");
+	//std::string filename = getDataFile("motion/table_flat.csv");
 
 	Offline::FileReader fr(filename);
-    Timestamp ts;
+	Timestamp ts;
 
-    //save for later plotting
+	//save for later plotting
-    std::vector<float> delta_motionAngles;
+	std::vector<float> delta_motionAngles;
-    std::vector<float> delta_turnAngles;
+	std::vector<float> delta_turnAngles;
 
-    //calc motion axis
+	//calc motion axis
 	for (const Offline::Entry& e : fr.getEntries()) {
 
-        ts = Timestamp::fromMS(e.ts);
+		ts = Timestamp::fromMS(e.ts);
 
 		if (e.type == Offline::Sensor::LIN_ACC) {
-            md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
+			md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
 
 		} else if (e.type == Offline::Sensor::GRAVITY) {
-            md.addGravity(ts, fr.getGravity()[e.idx].data);
+			md.addGravity(ts, fr.getGravity()[e.idx].data);
-            delta_motionAngles.push_back(md.getMotionChangeInRad());
+			delta_motionAngles.push_back(md.getMotionChangeInRad());
 
 		} else if (e.type == Offline::Sensor::ACC) {
 			const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
-            pd.addAccelerometer(ts, _acc.data);
+			td.addAccelerometer(ts, _acc.data);
 
 		} else if (e.type == Offline::Sensor::GYRO) {
 			const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
-            delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
+			delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
-        }
+		}
 
-    }
+	}
 
-    //draw motion
+	//draw motion
-    static K::Gnuplot gpMotion;
+	static K::Gnuplot gpMotion;
-    K::GnuplotPlot plotMotion;
+	K::GnuplotPlot plotMotion;
-    K::GnuplotPlotElementLines motionLines;
+	K::GnuplotPlotElementLines motionLines;
 
-    for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
+	for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
 
-        K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
+		K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
-        K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
+		K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
-        motionLines.addSegment(raw_p1, raw_p2);
+		motionLines.addSegment(raw_p1, raw_p2);
 
-    }
+	}
 
-    gpMotion << "set title 'Motion Detection'\n";
+	gpMotion << "set title 'Motion Detection'\n";
-    plotMotion.add(&motionLines);
+	plotMotion.add(&motionLines);
-    gpMotion.draw(plotMotion);
+	gpMotion.draw(plotMotion);
-    gpMotion.flush();
+	gpMotion.flush();
 
 
-    //draw rotation
+	//draw rotation
-    static K::Gnuplot gpTurn;
+	static K::Gnuplot gpTurn;
-    K::GnuplotPlot plotTurn;
+	K::GnuplotPlot plotTurn;
-    K::GnuplotPlotElementLines turnLines;
+	K::GnuplotPlotElementLines turnLines;
 
-    for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
+	for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
 
-        K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
+		K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
-        K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
+		K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
-        turnLines.addSegment(raw_p1, raw_p2);
+		turnLines.addSegment(raw_p1, raw_p2);
-    }
+	}
 
-    gpTurn << "set title 'Turn Detection'\n";
+	gpTurn << "set title 'Turn Detection'\n";
-    plotTurn.add(&turnLines);
+	plotTurn.add(&turnLines);
-    gpTurn.draw(plotTurn);
+	gpTurn.draw(plotTurn);
-    gpTurn.flush();
+	gpTurn.flush();
 
-    sleep(1);
+	sleep(1);
 
 }
 

tests/sensors/imu/TestTurnDetection.cpp

@@ -6,34 +6,33 @@
 
 TEST(TurnDetection, rotationMatrix) {
 
-    Vector3 dst(0, 0, 1);
+	Vector3 dst(0, 0, 1);
-    Vector3 src(1, 1, 0);
+	Vector3 src(1, 1, 0); src = src.normalized();
-    src = src.normalized();
 
 	// get a matrix that rotates "src" into "dst"
-    Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
+	Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
 
-    Vector3 res = rot * src;
+	Vector3 res = rot * src;
 
-    ASSERT_NEAR(dst.x, res.x, 0.01);
+	ASSERT_NEAR(dst.x, res.x, 0.01);
-    ASSERT_NEAR(dst.y, res.y, 0.01);
+	ASSERT_NEAR(dst.y, res.y, 0.01);
-    ASSERT_NEAR(dst.z, res.z, 0.01);
+	ASSERT_NEAR(dst.z, res.z, 0.01);
 
 }
 
 TEST(TurnDetection, gyroRotate) {
 
 
-    Vector3 zAxis(0, 0, 1);
+	Vector3 zAxis(0, 0, 1);
-    Vector3 acc(0, 7.0, 7.0);
+	Vector3 acc(0, 7.0, 7.0);
 
-    Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
+	Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
 
-    Vector3 gyro(0, 60, 60);
+	Vector3 gyro(0, 60, 60);
 
-    Vector3 gyro2(0, 0, 84);
+	Vector3 gyro2(0, 0, 84);
 
-    Vector3 gyro3 = rot * gyro;
+	Vector3 gyro3 = rot * gyro;
 
 	ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
 
@@ -42,11 +41,10 @@ TEST(TurnDetection, gyroRotate) {
 
 TEST(TurnDetection, xx) {
 
-    Vector3 dst(0, 0, 1);
+	Vector3 dst(0, 0, 1);
-    Vector3 src(0.0, 2.9, -10.0);
+	Vector3 src(0.0, 2.9, -10.0); src = src.normalized();		// sample accelerometer readings
-    src = src.normalized();		// sample accelerometer readings
 
-    Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
+	Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
 
 //	Eigen::Vector3f x; x << 1, 0, 0;
 //	Eigen::Vector3f z = src.normalized();
@@ -57,14 +55,14 @@ TEST(TurnDetection, xx) {
 //	rot.row(1) = y;
 //	rot.row(2) = z;
 
-    Vector3 res = rot * src;
+	Vector3 res = rot * src;
 //	ASSERT_NEAR(dst(0), res(0), 0.01);
 //	ASSERT_NEAR(dst(1), res(1), 0.01);
 //	ASSERT_NEAR(dst(2), res(2), 0.01);
 
-    Vector3 gyro(0, 10, 30);
+	Vector3 gyro(0, 10, 30);
 
-    Vector3 gyro2 = rot * gyro;
+	Vector3 gyro2 = rot * gyro;
 	int i = 0; (void) i;
 
 }

tests/sensors/pressure/TestBarometer.cpp

@@ -1,4 +1,4 @@
-#ifdef WITH_TESTS
+#ifdef TODO_______WITH_TESTS
 
 #include "../../Tests.h"
 
@@ -115,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) {
 
 	}
 
-    sleep(1);
+	sleep(1);
 
 
 	// tendence must be clear and smaller than the sigma
@@ -124,120 +124,119 @@ TEST(Barometer, LIVE_tendence2) {
 }
 
 TEST(Barometer, Activity) {
-    ActivityButterPressure act;
+	ActivityButterPressure act;
 
-    //read file
+	//read file
-    std::string line;
+	std::string line;
 	std::string filename = getDataFile("barometer/baro1.dat");
-    std::ifstream infile(filename);
+	std::ifstream infile(filename);
 
-    std::vector<ActivityButterPressure::History> actHist;
+	std::vector<ActivityButterPressure::History> actHist;
-    std::vector<ActivityButterPressure::History> rawHist;
+	std::vector<ActivityButterPressure::History> rawHist;
 
-    while (std::getline(infile, line))
+	while (std::getline(infile, line))
-    {
+	{
-        std::istringstream iss(line);
+		std::istringstream iss(line);
-        int ts;
+		int ts;
-        double value;
+		double value;
 
-        while (iss >> ts >> value) {
+		while (iss >> ts >> value) {
-            act.add(Timestamp::fromMS(ts), BarometerData(value));
+			ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value));
-            Activity currentAct = act.get();
+			rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
-            rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
+			actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct)));
-            actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData((int)currentAct)));
+		}
-        }
+	}
-    }
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::Gnuplot gpRaw;
+	K::Gnuplot gpRaw;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
-    K::GnuplotPlot plotRaw;
+	K::GnuplotPlot plotRaw;
-    K::GnuplotPlotElementLines rawLines;
+	K::GnuplotPlotElementLines rawLines;
-    K::GnuplotPlotElementLines resultLines;
+	K::GnuplotPlotElementLines resultLines;
 
-    for(int i=0; i < actHist.size()-1; ++i){
+	for(int i=0; i < actHist.size()-1; ++i){
 
-        //raw
+		//raw
-        K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
+		K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
-        K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
+		K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
 
-        rawLines.addSegment(raw_p1, raw_p2);
+		rawLines.addSegment(raw_p1, raw_p2);
 
-        //results
+		//results
-        K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
+		K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
-        K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
+		K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
 
-        resultLines.addSegment(input_p1, input_p2);
+		resultLines.addSegment(input_p1, input_p2);
-    }
+	}
 
-    plotRaw.add(&rawLines);
+	plotRaw.add(&rawLines);
-    plot.add(&resultLines);
+	plot.add(&resultLines);
 
-    gp.draw(plot);
+	gp.draw(plot);
-    gp.flush();
+	gp.flush();
 
-    gpRaw.draw(plotRaw);
+	gpRaw.draw(plotRaw);
-    gpRaw.flush();
+	gpRaw.flush();
 
-    sleep(5);
+	sleep(5);
 
 }
 
 TEST(Barometer, ActivityPercent) {
 
-    ActivityButterPressurePercent act;
+	ActivityButterPressurePercent act;
 
-    //read file
+	//read file
-    std::string line;
+	std::string line;
-    std::string filename = getDataFile("barometer/baro1.dat");
+	std::string filename = getDataFile("barometer/baro1.dat");
-    std::ifstream infile(filename);
+	std::ifstream infile(filename);
 
-    std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
+	std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
-    std::vector<double> rawHist;
+	std::vector<double> rawHist;
 
-    while (std::getline(infile, line))
+	while (std::getline(infile, line))
-    {
+	{
-        std::istringstream iss(line);
+		std::istringstream iss(line);
-        int ts;
+		int ts;
-        double value;
+		double value;
 
-        while (iss >> ts >> value) {
+		while (iss >> ts >> value) {
-            ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
+			ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
-            rawHist.push_back(value);
+			rawHist.push_back(value);
-            actHist.push_back(activity);
+			actHist.push_back(activity);
-        }
+		}
-    }
+	}
 
-    K::Gnuplot gp;
+	K::Gnuplot gp;
-    K::Gnuplot gpRaw;
+	K::Gnuplot gpRaw;
-    K::GnuplotPlot plot;
+	K::GnuplotPlot plot;
-    K::GnuplotPlot plotRaw;
+	K::GnuplotPlot plotRaw;
-    K::GnuplotPlotElementLines rawLines;
+	K::GnuplotPlotElementLines rawLines;
-    K::GnuplotPlotElementLines resultLines;
+	K::GnuplotPlotElementLines resultLines;
 
-    for(int i=0; i < actHist.size()-1; ++i){
+	for(int i=0; i < actHist.size()-1; ++i){
 
-        K::GnuplotPoint2 raw_p1(i, rawHist[i]);
+		K::GnuplotPoint2 raw_p1(i, rawHist[i]);
-        K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
+		K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
 
-        rawLines.addSegment(raw_p1, raw_p2);
+		rawLines.addSegment(raw_p1, raw_p2);
 
-        K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
+		K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
-        K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
+		K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
 
-        resultLines.addSegment(input_p1, input_p2);
+		resultLines.addSegment(input_p1, input_p2);
-    }
+	}
 
-    plotRaw.add(&rawLines);
+	plotRaw.add(&rawLines);
-    plot.add(&resultLines);
+	plot.add(&resultLines);
 
-    gp.draw(plot);
+	gp.draw(plot);
-    gp.flush();
+	gp.flush();
 
-    gpRaw.draw(plotRaw);
+	gpRaw.draw(plotRaw);
-    gpRaw.flush();
+	gpRaw.flush();
 
-    sleep(5);
+	sleep(5);
 }