Indoor/navMesh/meta/NavMeshDijkstra.h

#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