From 3c72bc814c4d30db19dbcd4f2f0740b890c67e6b Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 17 Jan 2018 16:36:37 +0100 Subject: [PATCH] worked on nav-mesh added dijkstra support for nav mesh some minor changes to distributions minor fixes --- CMakeLists.txt | 8 +- main.cpp | 2 +- math/Distributions.h | 1 + math/distribution/Const.h | 30 ++++- math/distribution/Normal.h | 6 +- math/distribution/Uniform.h | 6 + nav/dijkstra/Dijkstra.h | 7 +- navMesh/NavMeshDebug.h | 34 ++++- navMesh/NavMeshTriangle.h | 147 +++++++++++++++----- navMesh/meta/NavMeshDijkstra.h | 179 +++++++++++++++++++++++++ navMesh/walk/NavMeshWalkRandom.h | 4 +- navMesh/walk/NavMeshWalkSemiDirected.h | 124 +++++++++++++++++ navMesh/walk/NavMeshWalkSemiRandom.h | 6 +- tests/navMesh/TestNavMeshDijkstra.cpp | 111 +++++++++++++++ tests/navMesh/TestNavMeshTriangle.cpp | 79 +++++++++++ 15 files changed, 696 insertions(+), 48 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bb9cca7..8536689 100755 --- 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( diff --git a/main.cpp b/main.cpp index ea7aebb..c7515fe 100755 --- 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*"; diff --git a/math/Distributions.h b/math/Distributions.h index 40d49a4..1c5a3d8 100644 --- 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 diff --git a/math/distribution/Const.h b/math/distribution/Const.h index 30bd63a..0cefdfb 100644 --- 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 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 diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index a519feb..632a524 100644 --- 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() ?! */ diff --git a/math/distribution/Uniform.h b/math/distribution/Uniform.h index 6c17452..af5a873 100644 --- 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); diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index f1ffbe3..12f19d5 100644 --- 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* 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* getNode(const T* userNode) const { + auto it = nodes.find(userNode); return (unlikely(it == nodes.end())) ? (nullptr) : (it->second); } diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h index deedec0..a5b7cdb 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -9,9 +9,12 @@ #include #include #include +#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 void addDijkstra(NavMesh& mesh) { + + // ensure Tria extends NavMeshTriangleDijkstra + StaticAssert::AinheritsB(); + + NavMeshRandom rnd = mesh.getRandom(); + + for (int i = 0; i < 900; ++i) { + NavMeshLocation 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 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"; diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index 60f8358..eb21bd1 100644 --- 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 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); diff --git a/navMesh/meta/NavMeshDijkstra.h b/navMesh/meta/NavMeshDijkstra.h index 7416bd5..bc0c101 100644 --- 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 +#include +#include + +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 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 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 static void stamp(NavMesh& mesh, const Point3 dst) { + + // ensure Tria extends NavMeshTriangleDijkstra + StaticAssert::AinheritsB(); + + // point3 to mesh location + //NavMeshLocation endLoc = mesh.getLocation(dst); + + std::vector nodes = net(mesh); + Node* end = nodes[0]; // TODO; + + NodeAccess acc; + + // dijkstra + Dijkstra 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 static std::vector net(NavMesh& mesh) { + + std::vector 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 diff --git a/navMesh/walk/NavMeshWalkRandom.h b/navMesh/walk/NavMeshWalkRandom.h index db625c4..083db2c 100644 --- a/navMesh/walk/NavMeshWalkRandom.h +++ b/navMesh/walk/NavMeshWalkRandom.h @@ -64,7 +64,7 @@ namespace NM { NavMeshPotentialWalk 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 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"); diff --git a/navMesh/walk/NavMeshWalkSemiDirected.h b/navMesh/walk/NavMeshWalkSemiDirected.h index 494545f..27c35bd 100644 --- 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 class NavMeshWalkSemiDirected { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + public: + + + /** single result */ + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + /** list of results */ + using ResultList = std::vector; + + public: + + /** ctor */ + NavMeshWalkSemiDirected(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + + + ResultEntry getOne(const NavMeshWalkParams& params) { + + ResultEntry re; + + static Distribution::Uniform dHead(-0.10, +0.10, 1337); + static Distribution::Uniform 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 reachable(params.start, searchRegion); + + while(true) { + + // not to be found.. plan B + if (++runs > 8) { + NM::NavMeshRandom 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 pwalk(params, re.location); + re.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + re.probability *= p1; + } + + // done + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) { + return {getOne(params)}; + } + + + }; + +} + #endif // NAVMESHWALKSEMIDIRECTED_H diff --git a/navMesh/walk/NavMeshWalkSemiRandom.h b/navMesh/walk/NavMeshWalkSemiRandom.h index b3c4866..f4b95c6 100644 --- 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(); diff --git a/tests/navMesh/TestNavMeshDijkstra.cpp b/tests/navMesh/TestNavMeshDijkstra.cpp index e69de29..2c8243b 100644 --- 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 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 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 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 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 diff --git a/tests/navMesh/TestNavMeshTriangle.cpp b/tests/navMesh/TestNavMeshTriangle.cpp index 2051bfe..e1159dd 100644 --- 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 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 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 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