worked on nav-mesh

added dijkstra support for nav mesh some minor changes to distributions minor fixes
2018-01-17 16:36:37 +01:00
parent e81e8c112d
commit 3c72bc814c
15 changed files with 696 additions and 48 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,7 +30,7 @@ FILE(GLOB HEADERS
 	./*/*/*/*.h
 	./*/*/*/*/*.h
 	./*/*/*/*/*/*.h
-        ./*/*/*/*/*/*/*.h
+	./*/*/*/*/*/*/*.h
 	./tests/data/*
 	./tests/data/*/*
 	./tests/data/*/*/*
@@ -42,8 +42,8 @@ FILE(GLOB SOURCES
 	./*/*.cpp
 	./*/*/*.cpp
 	./*/*/*/*.cpp
-        ./*/*/*/*/*.cpp
-        ./*/*/*/*/*/*.cpp
+	./*/*/*/*/*.cpp
+	./*/*/*/*/*/*.cpp
 )

 FIND_PACKAGE( OpenMP REQUIRED)
@@ -101,7 +101,7 @@ ADD_EXECUTABLE(

 SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3)
 INCLUDE_DIRECTORIES(/usr/include/libnl3/)
-SET(EXTRA_LIBS ${EXTRA_LIBS} iw)
+#SET(EXTRA_LIBS ${EXTRA_LIBS} iw)

 # needed external libraries
 TARGET_LINK_LIBRARIES(
--- a/main.cpp
+++ b/main.cpp
@@ -109,7 +109,7 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Matrix4*";
 	//::testing::GTEST_FLAG(filter) = "*Sphere3*";

-	::testing::GTEST_FLAG(filter) = "NavMesh*";
+	::testing::GTEST_FLAG(filter) = "NavMeshD*";
 	//::testing::GTEST_FLAG(filter) = "Timestamp*";

 	//::testing::GTEST_FLAG(filter) = "*RayTrace3*";
--- a/math/Distributions.h
+++ b/math/Distributions.h
@@ -11,5 +11,6 @@
 #include "distribution/NormalN.h"
 #include "distribution/Rectangular.h"
 #include "distribution/NormalCDF.h"
+#include "distribution/Const.h"

 #endif // DISTRIBUTIONS_H
--- a/math/distribution/Const.h
+++ b/math/distribution/Const.h
@@ -1,4 +1,28 @@
-#ifndef CONST_H
-#define CONST_H
+#ifndef DISTRIBUTION_CONST_H
+#define DISTRIBUTION_CONST_H

-#endif // CONST_H
+
+namespace Distribution {
+
+	/** uniform distribution */
+	template <typename T> class Const {
+
+		const T val;
+
+	public:
+
+		/** ctor */
+		Const(const T val) : val(val) {
+
+		}
+
+		/** get a constant value */
+		T draw() {
+			return val;
+		}
+
+	};
+
+}
+
+#endif // DISTRIBUTION_CONST_H
--- a/math/distribution/Normal.h
+++ b/math/distribution/Normal.h
@@ -25,9 +25,11 @@ namespace Distribution {
 		/** ctor */
 		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"
-
+		/** ctor with seed */
+		Normal(const T mu, const T sigma, const uint32_t seed) :
+			mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(seed), dist(mu,sigma) {
 		}

 		/** do not allow copy. this will not work as expected for std::normal_distribution when using draw() ?! */
--- a/math/distribution/Uniform.h
+++ b/math/distribution/Uniform.h
@@ -27,6 +27,12 @@ namespace Distribution {
 		Uniform(const T min, const T max) : gen(RANDOM_SEED), dist(min, max) {

 		}
+
+		/** ctor with seed */
+		Uniform(const T min, const T max, const uint32_t seed) : gen(seed), dist(min, max) {
+
+		}
+
 		/** get a uniformaly distributed random number */
 		T draw() {
 			return dist(gen);
--- a/nav/dijkstra/Dijkstra.h
+++ b/nav/dijkstra/Dijkstra.h
@@ -31,7 +31,12 @@ public:

 	/** get the dijkstra-pendant for the given user-node. null if none matches */
 	inline const DijkstraNode<T>* getNode(const T& userNode) const {
-		auto it = nodes.find(&userNode);
+		return getNode(&userNode);
+	}
+
+	/** get the dijkstra-pendant for the given user-node. null if none matches */
+	inline const DijkstraNode<T>* getNode(const T* userNode) const {
+		auto it = nodes.find(userNode);
 		return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
 	}

--- a/navMesh/NavMeshDebug.h
+++ b/navMesh/NavMeshDebug.h
@@ -9,9 +9,12 @@
 #include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
 #include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
 #include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
+#include "../math/Distributions.h"

 namespace NM {

+	class NavMeshTriangleDijkstra;
+
 	/**
 	 * debug plot NavMeshes
 	 */
@@ -25,6 +28,7 @@ namespace NM {
 		K::GnuplotSplotElementPoints border;
 		K::GnuplotSplotElementColorPoints particles;
 		K::GnuplotSplotElementLines pathEstimated;
+		K::GnuplotSplotElementColorPoints distances;

 	private:

@@ -45,7 +49,7 @@ namespace NM {
 			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");
-
+			plot.add(&distances); distances.setPointSize(2); distances.setPointType(7);
 		}

 		void draw() {
@@ -112,6 +116,34 @@ namespace NM {

 		}

+		template <typename Tria> void addDijkstra(NavMesh<Tria>& mesh) {
+
+			// ensure Tria extends NavMeshTriangleDijkstra
+			StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
+
+			NavMeshRandom<Tria> rnd = mesh.getRandom();
+
+			for (int i = 0; i < 900; ++i) {
+				NavMeshLocation<Tria> loc = rnd.draw();
+				float v = loc.tria->interpolate(loc.pos, loc.tria->distAtP1, loc.tria->distAtP2, loc.tria->distAtP3);
+				distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v);
+			}
+
+
+//			Distribution::Uniform<float> dist (-0.5, +0.5);
+//			for (const Tria* t : mesh) {
+//				const Point3 posC = t->getCenter();
+//				distances.add(K::GnuplotPoint3(posC.x+dist.draw(), posC.y+dist.draw(), posC.z), t->distAtCenter);
+//				const Point3 pos1 = t->getP1();
+//				distances.add(K::GnuplotPoint3(pos1.x+dist.draw(), pos1.y+dist.draw(), pos1.z), t->distAtP1);
+//				const Point3 pos2 = t->getP2();
+//				distances.add(K::GnuplotPoint3(pos2.x+dist.draw(), pos2.y+dist.draw(), pos2.z), t->distAtP2);
+//				const Point3 pos3 = t->getP3();
+//				distances.add(K::GnuplotPoint3(pos3.x+dist.draw(), pos3.y+dist.draw(), pos3.z), t->distAtP3);
+//			}
+
+		}
+

 		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";
--- a/navMesh/NavMeshTriangle.h
+++ b/navMesh/NavMeshTriangle.h
@@ -4,6 +4,11 @@
 #include "../geo/Point3.h"
 #include "../geo/Point2.h"

+// fast barycentric code
+// https://stackoverflow.com/questions/25385361/point-within-a-triangle-barycentric-co-ordinates#25386102
+// https://i.stack.imgur.com/8VODS.png
+// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
+
 namespace NM {

 	/**
@@ -29,12 +34,13 @@ namespace NM {

 	private:	// precalculated stuff

-		Point2 v0;
-		Point2 v1;
-		float dot00;
-		float dot01;
-		float dot11;
-		double invDenom;
+//		Point2 v0;
+//		Point2 v1;
+//		float dot00;
+//		float dot01;
+//		float dot11;
+//		double invDenom;
+
 		float area;

 		float minZ;
@@ -44,13 +50,16 @@ namespace NM {
 		const Point3 v12;
 		const Point3 v13;

+		const double _det;
+
 	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) {
+			center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1),
+			_det(1.0*(p2.y - p3.y)*(p1.x - p3.x) + (p3.x - p2.x)*(p1.y - p3.y)) {

 			precompute();

@@ -66,6 +75,13 @@ namespace NM {
 		Point3 getP3() const {return p3;}


+		/** get the number of known neighbors for this triangle */
+		int getNumNeighbors() const {return _numNeighbors;}
+
+		/** get the idx-th neighbor */
+		const NavMeshTriangle* getNeighbor(const int idx) const {return _neighbors[idx];}
+
+
 		/** get the distance between the given point and the triangle using approximate tests */
 		float getDistanceApx(const Point3 pt) const {

@@ -123,6 +139,61 @@ namespace NM {
 			return p1 + (v12*u) + (v13*v);
 		}

+
+		/** 2D UV */
+		void getUV(const Point2 p, float& u, float& v) const {
+
+			// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
+			const Point2 v0 = p2.xy()	- p1.xy();
+			const Point2 v1 = p3.xy()	- p1.xy();
+			const Point2 v2 = p			- p1.xy();
+			const float den = v0.x * v1.y - v1.x * v0.y;
+			u = (v2.x * v1.y - v1.x * v2.y) / den;
+			v = (v0.x * v2.y - v2.x * v0.y) / den;
+
+		}
+
+		/** 2D UVW */
+		void getUVW(const Point2 p, float& u, float& v, float& w) const {
+			getUV(p,u,v);
+			w = 1-u-v;
+		}
+
+		/** 3D UV */
+		void getUV(const Point3 p, float& u, float& v) const {
+
+			const Point3 v0 = p2 - p1;
+			const Point3 v1 = p3 - p1;
+			const Point3 v2 = p  - p1;
+			const float d00 = dot(v0, v0);
+			const float d01 = dot(v0, v1);
+			const float d11 = dot(v1, v1);
+			const float d20 = dot(v2, v0);
+			const float d21 = dot(v2, v1);
+			const float denom = d00 * d11 - d01 * d01;
+			u = (d11 * d20 - d01 * d21) / denom;
+			v = (d00 * d21 - d01 * d20) / denom;
+			//w = 1.0f - v - w;
+
+			int xx = 0; (void) xx;
+
+		}
+
+		/** 3D UVW */
+		void getUVW(const Point3 p, float& u, float& v, float& w) const {
+			getUV(p,u,v);
+			w = 1-u-v;
+		}
+
+		template <typename T> T interpolate(const Point3 p, const T val1, const T val2, const T val3) const {
+
+			float u, v, w;
+			getUVW(p.xy(),u,v,w);
+
+			return (w*val1) + (u*val2) + (v*val3);
+
+		}
+
 		/** does the triangle contain the given 3D point? */
 		bool contains(const Point3 p) const {
 			return	(minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
@@ -131,15 +202,18 @@ namespace NM {
 		/** does the triangle contain the given 2D point? */
 		bool contains(const Point2 p) const {

-			const Point2 v2 = p - p1.xy();
+//			const Point2 v2 = p - p1.xy();

-			// Compute dot products
-			float dot02 = dot(v0, v2);
-			float dot12 = dot(v1, v2);
+//			// 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;
+//			// Compute barycentric coordinates
+//			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+//			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+			float u, v;
+			getUV(p, u, v);

 			// Check if point is in triangle
 			return (u >= 0) && (v >= 0) && (u + v <= 1);
@@ -149,17 +223,20 @@ namespace NM {
 		/** estimate the correct z-value for the given 2D point */
 		Point3 toPoint3(const Point2 p) const {

-			const Point2 v2 = p - p1.xy();
+//			const Point2 v2 = p - p1.xy();

-			// Compute dot products
-			float dot02 = dot(v0, v2);
-			float dot12 = dot(v1, v2);
+//			// 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;
+//			// Compute barycentric coordinates
+//			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+//			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

-			const Point3 res = getPoint(v,u);
+			float u, v;
+			getUV(p, u, v);
+
+			const Point3 res = getPoint(u,v);
 			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");

@@ -180,6 +257,14 @@ namespace NM {
 			return center;
 		}

+		/** cast to string */
+		operator std::string() const {return asString();}
+
+		/** get as string */
+		std::string asString() const {
+			return "(" + std::to_string(center.x) + "," + std::to_string(center.y) + "," + std::to_string(center.z) + ")";
+		}
+


 	private:
@@ -191,17 +276,17 @@ namespace NM {
 			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 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 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);
+//			// Compute barycentric coordinates
+//			invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);



--- a/navMesh/meta/NavMeshDijkstra.h
+++ b/navMesh/meta/NavMeshDijkstra.h
@@ -1,4 +1,183 @@
 #ifndef NAVMESHDIJKSTRA_H
 #define NAVMESHDIJKSTRA_H

+#include "../NavMesh.h"
+#include "../../nav/dijkstra/Dijkstra.h"
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+
+namespace NM {
+
+	/** distance to the target for each of the 3 triangle edge points */
+	struct NavMeshTriangleDijkstra {
+		float distAtCenter;
+		float distAtP1;
+		float distAtP2;
+		float distAtP3;
+	};
+
+//	// mapper
+//	template <typename Tria> struct Access {
+//		int getNumNeighbors(const Tria& t) const {return t.getNumNeighbors();}
+//		const Tria* getNeighbor(const Tria& t, const int idx) const {return (Tria*)t.getNeighbor(idx);}
+//		float getWeightBetween(const Tria& t1, const Tria& t2) const {return t1.getCenter().getDistance(t2.getCenter());}
+//	};
+
+	/** add distance-to-target infos for the triangles */
+	class NavMeshDijkstra {
+
+
+		struct Node {
+			Point3 pt;
+			std::vector<Node*> neighbors;
+			Node(const Point3 pt) : pt(pt) {;}
+			bool operator == (const Node& o ) {return o.pt == pt;}
+			bool operator == (const Point3& pos ) {return pt == pos;}
+			operator std::string() const {return asString();}
+			std::string asString() const {
+				return "(" + std::to_string(pt.x) + "," + std::to_string(pt.y) + "," + std::to_string(pt.z) + ")";
+			}
+		};
+
+		struct NodeComp {
+			Point3 pos;
+			//bool operator () (const Node* n) {return n->pt == pos;}
+			bool operator () (const Node* n) {return n->pt.getDistance(pos) < 0.0001;}
+			NodeComp(const Point3 pos) : pos(pos) {;}
+		};
+
+		struct NodeAccess {
+			int getNumNeighbors(const Node& n) const {return n.neighbors.size();}
+			const Node* getNeighbor(const Node& n, const int idx) const {return n.neighbors[idx];}
+			float getWeightBetween(const Node& n1, const Node& n2) const {return n1.pt.getDistance(n2.pt);}
+		};
+
+	public:
+
+		template <typename Tria> static void stamp(NavMesh<Tria>& mesh, const Point3 dst) {
+
+			// ensure Tria extends NavMeshTriangleDijkstra
+			StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
+
+			// point3 to mesh location
+			//NavMeshLocation<Tria> endLoc = mesh.getLocation(dst);
+
+			std::vector<Node*> nodes = net(mesh);
+			Node* end = nodes[0];	// TODO;
+
+			NodeAccess acc;
+
+			// dijkstra
+			Dijkstra<Node> dijkstra;
+			dijkstra.build(end, acc);
+
+			for (Tria* t : mesh) {
+
+				auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter()));
+				auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
+				auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
+				auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));
+
+				auto* dnCenter = dijkstra.getNode(*itCenter);
+				if (dnCenter != nullptr) {t->distAtCenter = dnCenter->cumWeight;}
+
+				auto* dn1 = dijkstra.getNode(*it1);
+				if (dn1 != nullptr) {t->distAtP1 = dn1->cumWeight;}
+
+				auto* dn2 = dijkstra.getNode(*it2);
+				if (dn2 != nullptr) {t->distAtP2 = dn2->cumWeight;}
+
+				auto* dn3 = dijkstra.getNode(*it3);
+				if (dn3 != nullptr) {t->distAtP3 = dn3->cumWeight;}
+
+			}
+
+		}
+
+
+		template <typename Tria> static std::vector<Node*> net(NavMesh<Tria>& mesh) {
+
+			std::vector<Node*> nodes;
+
+			// 1) add all triangle nodes (center, p1, p2, p3)
+			for (const Tria* t : mesh) {
+
+				auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter()));
+				if (itCenter == nodes.end()) {nodes.push_back(new Node(t->getCenter()));}
+
+				//Node n1(t->getP1());
+				auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
+				if (it1 == nodes.end()) {nodes.push_back(new Node(t->getP1()));}
+
+				//Node n2(t->getP2());
+				auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
+				if (it2 == nodes.end()) {nodes.push_back(new Node(t->getP2()));}
+
+				//Node n3(t->getP1());
+				auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));
+				if (it3 == nodes.end()) {nodes.push_back(new Node(t->getP3()));}
+
+			}
+
+			// 2) connect all possible nodes
+			for (const Tria* t : mesh) {
+
+				auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter()));
+				auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
+				auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
+				auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));
+
+				(*itCenter)->neighbors.push_back(*it1);
+				(*itCenter)->neighbors.push_back(*it2);
+				(*itCenter)->neighbors.push_back(*it3);
+
+				(*it1)->neighbors.push_back(*itCenter);
+				(*it1)->neighbors.push_back(*it2);
+				(*it1)->neighbors.push_back(*it3);
+
+				(*it2)->neighbors.push_back(*itCenter);
+				(*it2)->neighbors.push_back(*it1);
+				(*it2)->neighbors.push_back(*it3);
+
+				(*it3)->neighbors.push_back(*itCenter);
+				(*it3)->neighbors.push_back(*it1);
+				(*it3)->neighbors.push_back(*it2);
+
+				// neighbors
+				for (const auto* n : *t) {
+					auto itCenter2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(n->getCenter()));
+					(*itCenter)->neighbors.push_back(*itCenter2);
+				}
+
+			}
+
+
+
+
+			K::Gnuplot gp;
+			K::GnuplotSplot plot;
+			K::GnuplotSplotElementLines lines; plot.add(&lines);
+
+			for (const Node* n1 : nodes) {
+				for (const Node* n2 : n1->neighbors) {
+					const K::GnuplotPoint3 gp1(n1->pt.x, n1->pt.y, n1->pt.z);
+					const K::GnuplotPoint3 gp2(n2->pt.x, n2->pt.y, n2->pt.z);
+					lines.addSegment(gp1, gp2);
+				}
+			}
+
+			gp.draw(plot);
+			gp.flush();;
+
+			int xxxx = 0; (void) xxxx;
+
+			return nodes;
+
+		}
+
+	};
+
+}
+
 #endif // NAVMESHDIJKSTRA_H
--- a/navMesh/walk/NavMeshWalkRandom.h
+++ b/navMesh/walk/NavMeshWalkRandom.h
@@ -64,7 +64,7 @@ namespace NM {
 			NavMeshPotentialWalk<Tria> pwalk(params);

 			// improve quality (the higher, the better)
-			for (int i = 0; i < 25; ++i) {
+			for (int i = 0; i < 50; ++i) {

 				PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");

@@ -107,7 +107,7 @@ namespace NM {
 			NavMeshPotentialWalk<Tria> pwalk(params);

 			// improve quality (the higher, the better)
-			for (int i = 0; i < 25; ++i) {
+			for (int i = 0; i < 50; ++i) {

 				PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");

--- a/navMesh/walk/NavMeshWalkSemiDirected.h
+++ b/navMesh/walk/NavMeshWalkSemiDirected.h
@@ -1,4 +1,128 @@
 #ifndef NAVMESHWALKSEMIDIRECTED_H
 #define NAVMESHWALKSEMIDIRECTED_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 NavMeshWalkSemiDirected {
+
+	private:
+
+		const NavMesh<Tria>& mesh;
+
+		std::vector<NavMeshWalkEval<Tria>*> evals;
+
+	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 */
+		NavMeshWalkSemiDirected(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;
+
+			static Distribution::Uniform<float> dHead(-0.10, +0.10, 1337);
+			static Distribution::Uniform<float> dDist(-0.10, +0.10, 7331);
+
+			float impactHead = 1;
+			float impactDist = 1;
+			int runs = 0;
+
+			// construct reachable region once
+			// must be large enough to also contain increased distances!
+			const float searchRegion = 2.0 + params.getToBeWalkedDistance() * 1.1;
+			const NavMeshSub<Tria> reachable(params.start, searchRegion);
+
+			while(true) {
+
+				// not to be found.. plan B
+				if (++runs > 8) {
+					NM::NavMeshRandom<Tria> rnd = reachable.getRandom();
+					re.location = rnd.drawWithin(params.start.pos, params.getToBeWalkedDistance());
+					re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
+					break;
+				}
+
+				// to-be-walked distance;
+				const float modHead = dHead.draw() * impactHead;
+				const float modDist = dDist.draw() * impactDist;
+				impactHead += 3;
+				impactDist += 1.05;
+
+				const float toBeWalkedDist = params.getToBeWalkedDistance() + modDist;
+				const Heading head = params.heading + modHead;
+
+				if (toBeWalkedDist < 0.01)			{continue;}
+				if (toBeWalkedDist > searchRegion)	{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 * toBeWalkedDist);
+				const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+				// is above destination reachable?
+				if (dstTria) {
+
+					re.heading = head;											// adjust walked heading
+					re.location.pos = dstTria->toPoint3(dst);					// new destination position
+					re.location.tria = dstTria;									// new destination triangle
+					break;
+
+				}
+
+			}
+
+			// 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 // NAVMESHWALKSEMIDIRECTED_H
--- a/navMesh/walk/NavMeshWalkSemiRandom.h
+++ b/navMesh/walk/NavMeshWalkSemiRandom.h
@@ -68,13 +68,13 @@ namespace NM {
 			re.location = pwalk.end;


-			for (int i = 0; i < 25; ++i) {
+			for (int i = 0; i < 10; ++i) {

 				const float distance = params.getToBeWalkedDistance() * dDist.draw();
 				const Heading head = params.heading + dHead.draw();

 				// only forward!
-				if (distance < 0.01) {continue;}
+				if (distance < 0.01) {--i; continue;}

 				// get the to-be-reached destination's position (using start+distance+heading)
 				const Point2 dir = head.asVector();
@@ -123,7 +123,7 @@ namespace NM {
 				const Heading head = params.heading + dHead.draw();

 				// only forward!
-				if (distance < 0.01) {continue;}
+				if (distance < 0.01) {--i; continue;}

 				// get the to-be-reached destination's position (using start+distance+heading)
 				const Point2 dir = head.asVector();
--- a/tests/navMesh/TestNavMeshDijkstra.cpp
+++ b/tests/navMesh/TestNavMeshDijkstra.cpp
@@ -0,0 +1,111 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../navMesh/NavMeshFactory.h"
+#include "../../navMesh/walk/NavMeshSub.h"
+#include "../../navMesh/meta/NavMeshDijkstra.h"
+#include "../../navMesh/NavMeshDebug.h"
+
+#include "../../floorplan/v2/FloorplanReader.h"
+
+
+using namespace NM;
+
+struct MyNMT1231902345 : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra {
+
+	MyNMT1231902345(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : NavMeshTriangle(p1, p2, p3, type) {;}
+
+};
+
+TEST(NavMeshDijkstra, build) {
+
+	NavMeshSettings set;
+	NavMesh<MyNMT1231902345> nm;
+
+
+
+	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.8) {
+		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;
+
+	Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
+	remove.outdoor = false;
+	remove.method = Floorplan::OutlineMethod::REMOVE;
+	remove.poly.points.push_back(Point2(+4,-3));
+	remove.poly.points.push_back(Point2(+11,-3));
+	remove.poly.points.push_back(Point2(+11,+1));
+	remove.poly.points.push_back(Point2(+4,+1));
+
+	Floorplan::FloorOutlinePolygon remove2; floor.outline.push_back(&remove2);
+	remove2.outdoor = false;
+	remove2.method = Floorplan::OutlineMethod::REMOVE;
+	remove2.poly.points.push_back(Point2(-11,-2));
+	remove2.poly.points.push_back(Point2(-2,-2));
+	remove2.poly.points.push_back(Point2(-2,+2));
+	remove2.poly.points.push_back(Point2(-11,+2));
+
+	NavMeshFactory<MyNMT1231902345> fac(&nm, set);
+	fac.build(&map);
+
+//	nm.add(Point3(0,0,0), Point3(10,10,0), Point3(0,10,0), 1);
+//	nm.add(Point3(0,10,0), Point3(10,10,0), Point3(0,20,0), 1);
+//	nm.add(Point3(0,20,0), Point3(10,10,0), Point3(10,20,0), 1);
+
+
+	NavMeshDebug dbg;
+	dbg.addMesh(nm);
+	dbg.draw();
+
+	NM::NavMeshDijkstra::stamp(nm, Point3(4,4,0));
+
+	dbg.addDijkstra(nm);
+	dbg.draw();
+
+	int xxx = 0; (void) xxx;
+
+}
+
+
+TEST(NavMeshDijkstra, build2) {
+
+	NavMeshSettings set;
+	NavMesh<MyNMT1231902345> nm;
+
+	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");
+	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/SHL41_nm.xml");
+	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/museum31.xml");
+
+
+
+
+	NavMeshFactory<MyNMT1231902345> fac(&nm, set);
+	fac.build(map);
+
+	NavMeshDebug dbg;
+	dbg.addMesh(nm);
+	dbg.draw();
+
+	NM::NavMeshDijkstra::stamp(nm, Point3(4,4,0));
+
+	dbg.addDijkstra(nm);
+	dbg.draw();
+
+	int xxx = 0; (void) xxx;
+
+}
+
+
+#endif
--- a/tests/navMesh/TestNavMeshTriangle.cpp
+++ b/tests/navMesh/TestNavMeshTriangle.cpp
@@ -5,6 +5,70 @@
 #include "../../navMesh/NavMeshTriangle.h"
 using namespace NM;

+
+
+
+TEST(NavMeshTriangle, uv_2D_on_2D) {
+
+	NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);		// flat triangle
+	ASSERT_NEAR(0.5, t1.getArea(), 0.0001);
+
+	float u, v;
+
+	std::vector<float> testVals = {1.0,0.0,  0.0,1.0,  0.4,0.4,  0.8,0.1,  0.1,0.8,  0.3,0.6,  0.6,0.3};
+
+	// point -> uv -> point
+	for (size_t i = 0; i < testVals.size(); i+=2) {
+		Point2 pt1(testVals[i], testVals[i+1]);
+		t1.getUV(pt1, u, v);
+		Point3 pt2 = t1.getPoint(u,v);
+		ASSERT_NEAR(pt1.x, pt2.x, 0.0001);
+		ASSERT_NEAR(pt1.y, pt2.y, 0.0001);
+	}
+
+}
+
+
+TEST(NavMeshTriangle, uv_2D_on_3D) {
+
+	NavMeshTriangle t1(Point3(0,0,0), Point3(2,0,0), Point3(0,1,1), 1);		// non-flat triangle
+
+	float u, v;
+
+	std::vector<float> testVals = {1.0,0.0,  0.0,1.0,  0.4,0.4,  0.8,0.1,  0.1,0.8,  0.3,0.6,  0.6,0.3};
+
+	// point -> uv -> point
+	for (size_t i = 0; i < testVals.size(); i+=2) {
+		Point2 pt1(testVals[i], testVals[i+1]);
+		t1.getUV(pt1, u, v);
+		Point3 pt2 = t1.getPoint(u,v);
+		ASSERT_NEAR(pt1.x, pt2.x, 0.0001);
+		ASSERT_NEAR(pt1.y, pt2.y, 0.0001);
+	}
+
+}
+
+
+TEST(NavMeshTriangle, uv_3D_on_3D) {
+
+	NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,1), 1);		// non-flat triangle
+
+	float u, v;
+
+	std::vector<float> testVals = {1.0,0.0,0.0,  0.0,1.0,1.0};
+
+	// point -> uv -> point
+	for (size_t i = 0; i < testVals.size(); i+=3) {
+		Point3 pt1(testVals[i], testVals[i+1], testVals[i+2]);
+		t1.getUV(pt1, u, v);
+		Point3 pt2 = t1.getPoint(u,v);
+		ASSERT_NEAR(pt1.x, pt2.x, 0.0001);
+		ASSERT_NEAR(pt1.y, pt2.y, 0.0001);
+		ASSERT_NEAR(pt1.z, pt2.z, 0.0001);
+	}
+
+}
+
 TEST(NavMeshTriangle, contains) {

 	NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);
@@ -31,6 +95,21 @@ TEST(NavMeshTriangle, area) {
 }


+TEST(NavMeshTriangle, interpolate) {
+
+	NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);
+
+	ASSERT_NEAR(1.0f, t1.interpolate(Point3(0,0,0), 1.0, 2.0, 3.0), 0.001);
+	ASSERT_NEAR(2.0f, t1.interpolate(Point3(1,0,0), 1.0, 2.0, 3.0), 0.001);
+	ASSERT_NEAR(3.0f, t1.interpolate(Point3(0,1,0), 1.0, 2.0, 3.0), 0.001);
+
+	ASSERT_NEAR(1.5f, t1.interpolate(Point3(0.5,0,0), 1.0, 2.0, 3.0), 0.01);	// between 1/2
+	ASSERT_NEAR(1.9f, t1.interpolate(Point3(0.9,0,0), 1.0, 2.0, 3.0), 0.01);
+
+	ASSERT_NEAR(2.0f, t1.interpolate(Point3(0,0.5,0), 1.0, 2.0, 3.0), 0.01);	// between 1/3
+	ASSERT_NEAR(2.8f, t1.interpolate(Point3(0,0.9,0), 1.0, 2.0, 3.0), 0.01);
+
+}


 #endif