FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

merged

Browse Source
This commit is contained in:
toni
2018-01-24 11:44:59 +01:00
parent b207e5e0f2 1a1f249e9b
commit 4663c7d9e5
22 changed files with 1025 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
CMakeLists.txt
View File

@@ -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(

43
floorplan/v2/FloorplanReader.h
View File

@@ -36,9 +36,8 @@ namespace Floorplan {
if (res != tinyxml2::XMLError::XML_SUCCESS) {
throw Exception(
std::string() + "error while loading XML " + file + "\n" +
((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
);
((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : ("")));
}
IndoorMap* map = parse(doc);
return map;
@@ -53,8 +52,8 @@ namespace Floorplan {
if (res != tinyxml2::XMLError::XML_SUCCESS) {
throw Exception(
std::string() + "error while parsing XML\n" +
((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
);
}
IndoorMap* map = parse(doc);
@@ -63,7 +62,7 @@ namespace Floorplan {
private:
#define FOREACH_NODE(out, in) for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() )
#define FOREACH_NODE(out, in) for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() )
static void assertNode(const std::string& node, const XMLElem* el) {
std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'";
@@ -138,7 +137,7 @@ namespace Floorplan {
if (std::string("pois") == n->Name()) {floor->pois = parseFloorPOIs(n);}
if (std::string("stairs") == n->Name()) {floor->stairs = parseFloorStairs(n);}
if (std::string("elevators") == n->Name()) {floor->elevators = parseFloorElevators(n);}
if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);}
if (std::string("gtpoints") == n->Name()) {floor->gtpoints = parseFloorGroundTruthPoints(n);}
}
return floor;
}
@@ -226,22 +225,22 @@ namespace Floorplan {
}
/** parse the <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec;
FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
}
return vec;
}
/** parse the <gtpoints> tag */
static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
std::vector<GroundTruthPoint*> vec;
FOREACH_NODE(n, el) {
if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
}
return vec;
}
/** parse a <gtpoint> tag */
static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
GroundTruthPoint* gtp = new GroundTruthPoint();
gtp->id = el->IntAttribute("id");
gtp->pos = parsePoint3(el);
return gtp;
}
/** parse a <gtpoint> tag */
static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
GroundTruthPoint* gtp = new GroundTruthPoint();
gtp->id = el->IntAttribute("id");
gtp->pos = parsePoint3(el);
return gtp;
}
/** parse the <accesspoints> tag */

2
main.cpp
View File

@@ -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*";

1
math/Distributions.h
View File

@@ -11,5 +11,6 @@
#include "distribution/NormalN.h"
#include "distribution/Rectangular.h"
#include "distribution/NormalCDF.h"
#include "distribution/Const.h"
#endif // DISTRIBUTIONS_H

28
math/distribution/Const.h Normal file
View File

@@ -0,0 +1,28 @@
#ifndef DISTRIBUTION_CONST_H
#define DISTRIBUTION_CONST_H
namespace Distribution {
/** uniform distribution */
template <typename T> 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

6
math/distribution/Normal.h
View File

@@ -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() ?! */

6
math/distribution/Uniform.h
View File

@@ -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);

6
math/random/RandomGenerator.h
View File

@@ -1,5 +1,5 @@
#ifndef RANDOM_Random::RandomGenerator_H
#define RANDOM_Random::RandomGenerator_H
#ifndef RANDOM_Random_RandomGenerator_H
#define RANDOM_Random_RandomGenerator_H
#include <random>
#include <chrono>
@@ -26,4 +26,4 @@ namespace Random {
}
#endif // K_MATH_RANDOM_Random::RandomGenerator_H
#endif // RANDOM_Random_RandomGenerator_H

7
nav/dijkstra/Dijkstra.h
View File

@@ -31,7 +31,12 @@ public:
/** get the dijkstra-pendant for the given user-node. null if none matches */
inline const DijkstraNode<T>* 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<T>* getNode(const T* userNode) const {
auto it = nodes.find(userNode);
return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
}

18
navMesh/NavMesh.h
View File

@@ -45,7 +45,7 @@ namespace NM {
}
/** get the triangle this point belongs to (if any) */
NavMeshLocation<Tria> getLocation(const Point3 pos) {
NavMeshLocation<Tria> getLocation(const Point3 pos) const {
for (const Tria* tria : triangles) {
if (tria->contains(pos)) {
return NavMeshLocation<Tria>(pos, tria);
@@ -54,6 +54,19 @@ namespace NM {
throw Exception("location not found within NavMesh: " + pos.asString());
}
/** get the triangle/point on the mesh that is nearest to the given location */
NavMeshLocation<Tria> getLocationNearestTo(const Point3 pos) const {
auto comp = [pos] (const Tria* t1, const Tria* t2) {
//return t1->getCenter().getDistance(pos) < t2->getCenter().getDistance(pos);
return t1->getDistanceApx(pos) < t2->getDistanceApx(pos);
};
auto it = std::min_element(triangles.begin(), triangles.end(), comp);
const Tria* best = *it;
Point3 rPos = best->toPoint3Near(pos.xy());
return NavMeshLocation<Tria>(rPos, best);
}
/** connect both triangles */
void connectBiDir(int idx1, int idx2) {
connectUniDir(idx1,idx2);
@@ -86,7 +99,8 @@ namespace NM {
/** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandom() {
/** get a random-generator for several mesh-actions */
NavMeshRandom<Tria> getRandom() const {
return NavMeshRandom<Tria>(triangles);
}

50
navMesh/NavMeshDebug.h
View File

@@ -9,9 +9,12 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
#include "../math/Distributions.h"
namespace NM {
class NavMeshTriangleDijkstra;
/**
* debug plot NavMeshes
*/
@@ -25,6 +28,8 @@ namespace NM {
K::GnuplotSplotElementPoints border;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementLines pathEstimated;
K::GnuplotSplotElementColorPoints distances;
K::GnuplotSplotElementLines shortestPath;
private:
@@ -45,7 +50,8 @@ 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(0.75); distances.setPointType(7);
plot.add(&shortestPath); shortestPath.getStroke().setWidth(3);
}
void draw() {
@@ -66,6 +72,8 @@ namespace NM {
}
plot.getAxisCB().setRange(min, max + 0.000001);
}
template <typename Tria> void addMesh(NavMesh<Tria>& nm) {
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
@@ -112,6 +120,46 @@ namespace NM {
}
template <typename Tria> void addDijkstra(NavMesh<Tria>& mesh) {
distances.clear();
// ensure Tria extends NavMeshTriangleDijkstra
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
NavMeshRandom<Tria> rnd = mesh.getRandom();
for (int i = 0; i < 5000; ++i) {
NavMeshLocation<Tria> loc = rnd.draw();
float v = loc.tria->interpolate(loc.pos, loc.tria->spFromP1.distance, loc.tria->spFromP2.distance, loc.tria->spFromP3.distance);
distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v);
}
// Distribution::Uniform<float> 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);
// }
}
template <typename Tria> void addDijkstra(std::vector<NM::NavMeshLocation<Tria>>& path) {
shortestPath.clear();
for (auto& e : path) {
K::GnuplotPoint3 gp(e.pos.x, e.pos.y, e.pos.z);
shortestPath.add(gp);
}
}
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";

2
navMesh/NavMeshFactory.h
View File

@@ -12,7 +12,7 @@
#include "../lib/gpc/gpc.cpp.h"
#include "../lib/Recast/Recast.h"
#include <string.h> // memset
namespace NM {

2
navMesh/NavMeshRandom.h
View File

@@ -36,6 +36,8 @@ namespace NM {
/** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
Assert::isFalse(srcTriangles.empty(), "no triangles given. mesh is empty");
// 1st = almost always the same number?!
gen(); gen();

4
navMesh/NavMeshSettings.h
View File

@@ -36,7 +36,9 @@ namespace NM {
/** algorithm choice */
SamplePartitionType partitionType = SamplePartitionType::SAMPLE_PARTITION_WATERSHED;
float regionMinSize = 2;//8; // (isolated) regions smaller than this will not be rendered?!
float regionMinSize = 8; // (isolated) regions smaller than this will not be rendered?!
const float regionMergeSize = 20; //??
const int vertsPerPoly = 3;//6.0f;
const float detailSampleDist = 6.0f;

168
navMesh/NavMeshTriangle.h
View File

@@ -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 {
/**
@@ -27,14 +32,15 @@ namespace NM {
NavMeshTriangle* _neighbors[3];
int _numNeighbors;
private: // precalculated stuff
protected: // 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,65 @@ 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;
}
/** barycentric interpolation at Point p for val1@p1, val2@p2, val3@p3 */
template <typename T> 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 +206,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 +227,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");
@@ -168,6 +249,21 @@ namespace NM {
}
/** nearest point on the triangle */
Point3 toPoint3Near(const Point2 p) const {
float u, v;
getUV(p, u, v);
if (u < 0) {u = 0;}
if (u > 1) {u = 1;}
if (v < 0) {v = 0;}
if (v > 1) {v = 1;}
return getPoint(u,v);
}
/** get the triangle's size */
@@ -180,6 +276,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 +295,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);

312
navMesh/meta/NavMeshDijkstra.h Normal file
View File

@@ -0,0 +1,312 @@
#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

39
navMesh/walk/NavMeshWalkEval.h
View File

@@ -5,6 +5,9 @@
#include "../NavMeshLocation.h"
#include "../../math/Distributions.h"
#include "../../misc/PerfCheck.h"
#include "../../Assertions.h"
#include "../meta/NavMeshDijkstra.h"
namespace NM {
@@ -44,7 +47,7 @@ namespace NM {
/**
* evaluate the difference between head(start,end) and the requested heading
*/
template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {
template <typename Tria> class WalkEvalHeadingStartEndVonMises : public NavMeshWalkEval<Tria> {
const double sigma_rad;
const double kappa;
@@ -55,7 +58,7 @@ namespace NM {
// kappa = 1/var = 1/sigma^2
// https://en.wikipedia.org/wiki/Von_Mises_distribution
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
WalkEvalHeadingStartEndVonMises(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
;
}
@@ -136,6 +139,38 @@ namespace NM {
};
/**
* higher probability for potints that approach the target location
*/
template <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {
const double p;
public:
WalkEvalApproachesTarget(const double pApproaching = 0.65) : p(pApproaching) {
;
}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
// sanity check
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
const NavMeshLocation<Tria> locStart = walk.requested.start;
const NavMeshLocation<Tria> locEnd = walk.end;
const float distFromNew = locEnd.tria-> template getDistanceToDestination<Tria>(locEnd.pos);
const float distFromOld = locStart.tria-> template getDistanceToDestination<Tria>(locStart.pos);
const double pRemain = 1.0 / (2+distFromNew);
return ((distFromNew <= distFromOld) ? (p) : (1.0 - p)) + std::pow(pRemain, 0.2);
}
};
}
#endif // NAVMESHWALKEVAL_H

4
navMesh/walk/NavMeshWalkRandom.h
View File

@@ -64,7 +64,7 @@ namespace NM {
NavMeshPotentialWalk<Tria> 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<Tria> 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");

128
navMesh/walk/NavMeshWalkSemiDirected.h Normal file
View File

@@ -0,0 +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 <typename Tria> class NavMeshWalkSemiDirected {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
/** list of results */
using ResultList = std::vector<ResultEntry>;
public:
/** ctor */
NavMeshWalkSemiDirected(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
ResultEntry re;
static Distribution::Uniform<float> dHead(-0.10, +0.10, 1337);
static Distribution::Uniform<float> 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<Tria> reachable(params.start, searchRegion);
while(true) {
// not to be found.. plan B
if (++runs > 8) {
NM::NavMeshRandom<Tria> 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<Tria> pwalk(params, re.location);
re.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
re.probability *= p1;
}
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
return {getOne(params)};
}
};
}
#endif // NAVMESHWALKSEMIDIRECTED_H

6
navMesh/walk/NavMeshWalkSemiRandom.h
View File

@@ -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();

183
tests/navMesh/TestNavMeshDijkstra.cpp Normal file
View File

@@ -0,0 +1,183 @@
#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<MyNMT1231902345> 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<MyNMT1231902345> 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<MyNMT1231902345> 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<MyNMT1231902345> 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;
}
TEST(NavMeshDijkstra, path) {
NavMeshSettings set;
NavMesh<MyNMT1231902345> 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));
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/mnt/vm/paper/diss/data/maps/map_stair1.xml");
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");
NavMeshFactory<MyNMT1231902345> fac(&nm, set);
fac.build(map);
NavMeshDebug dbg;
dbg.addMesh(nm);
dbg.draw();
NM::NavMeshRandom<MyNMT1231902345> rnd = nm.getRandom();
const Point3 dst = rnd.draw().pos;
for (int i = 0; i < 1000; ++i) {
NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw();
start.tria->getDistanceToDestination<MyNMT1231902345>(start.pos); // just a compiler-test
NM::NavMeshDijkstra::stamp(nm, dst);
//NM::NavMeshLocation<MyNMT1231902345> start = start;//nm.getLocation(Point3(0,-6,0));
std::vector<NM::NavMeshLocation<MyNMT1231902345>> path = start.tria->getPathToDestination<MyNMT1231902345>(start.pos);
std::cout << path.size() << std::endl;
dbg.addDijkstra(nm);
dbg.addDijkstra(path);
dbg.draw();
sleep(1);
}
}
#endif

79
tests/navMesh/TestNavMeshTriangle.cpp
View File

@@ -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<float> 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<float> 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<float> 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