337 lines
12 KiB
C++
337 lines
12 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#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);}
|
||
};
|
||
|
||
struct NodeAccessFavorCenter {
|
||
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 {
|
||
|
||
if(n2.pointIndex == 3){
|
||
return n1.pt.getDistance(n2.pt);
|
||
} else {
|
||
return n1.pt.getDistance(n2.pt) / 0.2f;
|
||
}
|
||
}
|
||
};
|
||
|
||
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;
|
||
NodeAccessFavorCenter 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
|