313 lines
11 KiB
C++
313 lines
11 KiB
C++
#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>
|
|
#include "../NavMeshDebug.h"
|
|
|
|
namespace NM {
|
|
|
|
/** distance/neighbor-to-the target for each of the 3 triangle edge points */
|
|
class NavMeshTriangleDijkstra {
|
|
|
|
public:
|
|
|
|
/** next hop towards the pedestrian's target */
|
|
struct ToTarget {
|
|
struct NextTarget {
|
|
const NavMeshTriangle* tria = nullptr;
|
|
int pointIndex;
|
|
Point3 point() const {
|
|
switch(pointIndex) {
|
|
case 0: return tria->getP1();
|
|
case 1: return tria->getP2();
|
|
case 2: return tria->getP3();
|
|
case 3: return tria->getCenter();
|
|
default: throw Exception("invalid point index");
|
|
}
|
|
}
|
|
template <typename Tria> ToTarget hop() const {
|
|
const Tria* t = (const Tria*)tria;
|
|
switch(pointIndex) {
|
|
case 0: return t->spFromP1;
|
|
case 1: return t->spFromP2;
|
|
case 2: return t->spFromP3;
|
|
case 3: return t->spFromCenter;
|
|
default: throw Exception("invalid point index");
|
|
}
|
|
}
|
|
} next;
|
|
float distance = 0;
|
|
};
|
|
|
|
ToTarget spFromCenter;
|
|
ToTarget spFromP1;
|
|
ToTarget spFromP2;
|
|
ToTarget spFromP3;
|
|
|
|
/** interpolate the distance towards the garget for the given point */
|
|
template <typename UserTriangleClass> float getDistanceToDestination(const Point3 p) const {
|
|
// this one is a little bit awkward.. normally NavMeshTriangleDijkstra should extend NavMeshTriangle..
|
|
// however, this often yields issues for user-classes, extending NavMeshTriangle more than once
|
|
StaticAssert::AinheritsB<UserTriangleClass, NavMeshTriangle>();
|
|
const UserTriangleClass* userClass = static_cast<const UserTriangleClass*>(this); // must inherit NavMeshTriangle
|
|
return userClass->interpolate(p, spFromP1.distance, spFromP2.distance, spFromP3.distance);
|
|
}
|
|
|
|
/** get the next neighbor-point/triangle for the given point */
|
|
template <typename Tria> NavMeshTriangleDijkstra::ToTarget nearestHop(const Point3 p) const {
|
|
const Tria* tria = static_cast<const Tria*>(this);
|
|
const Point3 pc = tria->getCenter();
|
|
const Point3 p1 = tria->getP1();
|
|
const Point3 p2 = tria->getP2();
|
|
const Point3 p3 = tria->getP3();
|
|
const float dc = p.getDistance(pc);
|
|
const float d1 = p.getDistance(p1);
|
|
const float d2 = p.getDistance(p2);
|
|
const float d3 = p.getDistance(p3);
|
|
// if (dc < d1 && dc < d2 && dc < d3) {return NavMeshLocation<Tria>(pc, static_cast<const Tria*>(spFromCenter.next));}
|
|
// if (d1 < dc && d1 < d2 && d1 < d3) {return NavMeshLocation<Tria>(p1, static_cast<const Tria*>(spFromP1.next));}
|
|
// if (d2 < dc && d2 < d1 && d2 < d3) {return NavMeshLocation<Tria>(p2, static_cast<const Tria*>(spFromP2.next));}
|
|
// if (d3 < dc && d3 < d1 && d3 < d2) {return NavMeshLocation<Tria>(p3, static_cast<const Tria*>(spFromP3.next));}
|
|
if (dc < d1 && dc < d2 && dc < d3) {return spFromCenter;}
|
|
if (d1 < dc && d1 < d2 && d1 < d3) {return spFromP1;}
|
|
if (d2 < dc && d2 < d1 && d2 < d3) {return spFromP2;}
|
|
if (d3 < dc && d3 < d1 && d3 < d2) {return spFromP3;}
|
|
throw Exception("invalid code-path detected");
|
|
}
|
|
|
|
/** get the complete path from p towards to pedestrian's destination */
|
|
template <typename Tria> std::vector<NavMeshLocation<Tria>> getPathToDestination(const Point3 p) const {
|
|
std::vector<NavMeshLocation<Tria>> path;
|
|
// NavMeshLocation<Tria> cur(p, static_cast<const Tria*>(this));
|
|
// path.push_back(cur);
|
|
// while(true) {
|
|
// //NavMeshLocation<Tria> step = prev.tria->nextHop(prev.pos);//static_cast<const NavMeshTriangleDijkstra*>(prev.tria)->next(prev.pos);
|
|
// NavMeshLocation<Tria> step = static_cast<const NavMeshTriangleDijkstra*>(prev.tria)->nextHop<Tria>(prev.pos);
|
|
|
|
// path.push_back(step);
|
|
// if (step.tria == nullptr) {break;} // reached end
|
|
// prev = step;
|
|
// }
|
|
// return path;
|
|
|
|
// starting point
|
|
NavMeshLocation<Tria> first(p, static_cast<const Tria*>(this));
|
|
path.push_back(first);
|
|
|
|
// first hop towards the destination
|
|
NavMeshTriangleDijkstra::ToTarget cur = nearestHop<Tria>(p);
|
|
|
|
// iterate all hops towards the destination
|
|
while(cur.next.tria) {
|
|
const NavMeshLocation<Tria> loc(cur.next.point(), (const Tria*)cur.next.tria);
|
|
path.push_back(loc);
|
|
cur = cur.next.hop<Tria>();
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
};
|
|
|
|
// // 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 TemporalNode {
|
|
Point3 pt; // a point within the map
|
|
const NavMeshTriangle* triangle; // the triangle the point belongs to
|
|
int pointIndex; // [0-2] (3 edge points) or 3 [center]
|
|
std::vector<TemporalNode*> neighbors;
|
|
TemporalNode(const Point3 pt, const NavMeshTriangle* triangle, int pointIndex) : pt(pt), triangle(triangle), pointIndex(pointIndex) {;}
|
|
bool operator == (const TemporalNode& 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 TemporalNode* n) {return n->pt.getDistance(pos) < 0.0001;}
|
|
NodeComp(const Point3 pos) : pos(pos) {;}
|
|
};
|
|
|
|
struct NodeAccess {
|
|
int getNumNeighbors(const TemporalNode& n) const {return n.neighbors.size();}
|
|
const TemporalNode* getNeighbor(const TemporalNode& n, const int idx) const {return n.neighbors[idx];}
|
|
float getWeightBetween(const TemporalNode& n1, const TemporalNode& n2) const {return n1.pt.getDistance(n2.pt);}
|
|
};
|
|
|
|
public:
|
|
|
|
/** attach distance/triangle-to-target to the ToTarget struct */
|
|
static void set(NavMeshTriangleDijkstra::ToTarget& t, const DijkstraNode<TemporalNode>* n) {
|
|
t.distance = n->cumWeight;
|
|
t.next.tria = (n->previous) ? (n->previous->element->triangle) : (nullptr);
|
|
t.next.pointIndex = (n->previous) ? (n->previous->element->pointIndex) : (-1);
|
|
}
|
|
|
|
template <typename Tria> static void stamp(NavMesh<Tria>& mesh, const Point3 dst) {
|
|
|
|
// ensure Tria extends NavMeshTriangleDijkstra
|
|
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
|
|
|
|
// build finer mesh for dijkstra
|
|
std::vector<TemporalNode*> nodes = net(mesh);
|
|
|
|
// point3 to mesh location
|
|
NavMeshLocation<Tria> endLoc = mesh.getLocation(dst);
|
|
auto it = std::find_if(nodes.begin(), nodes.end(), NodeComp(endLoc.tria->getCenter()));
|
|
if (it == nodes.end()) {throw Exception("end node not found");}
|
|
TemporalNode* end = *it;
|
|
|
|
|
|
// Node* end = nodes[0]; // TODO;
|
|
|
|
NodeAccess acc;
|
|
|
|
// dijkstra
|
|
Dijkstra<TemporalNode> 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) {set(t->spFromCenter, dnCenter);}
|
|
|
|
auto* dn1 = dijkstra.getNode(*it1);
|
|
if (dn1 != nullptr) {set(t->spFromP1, dn1);}
|
|
|
|
auto* dn2 = dijkstra.getNode(*it2);
|
|
if (dn2 != nullptr) {set(t->spFromP2, dn2);}
|
|
|
|
auto* dn3 = dijkstra.getNode(*it3);
|
|
if (dn3 != nullptr) {set(t->spFromP3, dn3);}
|
|
|
|
}
|
|
|
|
|
|
|
|
NavMeshDebug dbg;
|
|
dbg.addMesh(mesh);
|
|
dbg.addDijkstra(mesh);
|
|
dbg.draw();
|
|
|
|
|
|
int zzz = 0; (void) zzz;
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
* a normal navigation mesh only connects adjacent triangles (can be thought of as "from center to center")
|
|
* however, later on, we need a distance estimation for any point within the triangle.
|
|
* we thus need dijkstra to estimate the distance for every edge of the triangle (to allow for barycentric interpolation)
|
|
* we thus build a temporal graph which contains all triangle centers and edge-points.
|
|
* Note: many triangles share the same edge-points!
|
|
* likewise, all possible connections are drawn.
|
|
*/
|
|
template <typename Tria> static std::vector<TemporalNode*> net(NavMesh<Tria>& mesh) {
|
|
|
|
std::vector<TemporalNode*> 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 TemporalNode(t->getCenter(), t, 3));}
|
|
|
|
//Node n1(t->getP1());
|
|
auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
|
|
if (it1 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP1(), t, 0));}
|
|
|
|
//Node n2(t->getP2());
|
|
auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
|
|
if (it2 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP2(), t, 1));}
|
|
|
|
//Node n3(t->getP1());
|
|
auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));
|
|
if (it3 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP3(), t, 2));}
|
|
|
|
}
|
|
|
|
// 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 TemporalNode* n1 : nodes) {
|
|
for (const TemporalNode* 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
|