worked on nav-mesh

added dijkstra support for nav mesh
some minor changes to distributions
minor fixes

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