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

minor changes to floorplan

Browse Source 
fixed some compile issues
worked on nav-meshes
added some tests
This commit is contained in:
FrankE
2018-01-16 12:41:05 +01:00
parent fee6cd3496
commit 55061ef0da
24 changed files with 1288 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
navMesh/NavMeshDebug.h
View File

@@ -7,6 +7,7 @@
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
namespace NM {
@@ -18,27 +19,61 @@ namespace NM {
public:
template <typename Tria> static void show(NavMesh<Tria>& nm) {
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementPoints border;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementLines pathEstimated;
K::GnuplotFill gFill[3] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#111111"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaaa"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaff"), 1)
};
private:
K::GnuplotFill gFill[6] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed
};
public:
NavMeshDebug() {
gp << "set view equal xy\n";
plot.add(&lines); lines.setShowPoints(true);
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");
}
void draw() {
gp.draw(plot);
gp.flush();
}
template <typename T> void showParticles(const std::vector<T>& particles) {
this->particles.clear();
double min = +999;
double max = -999;
for (const T& p : particles) {
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
const double prob = std::pow(p.weight, 0.25);
this->particles.add(p3, prob);
if (prob > max) {max = prob;}
if (prob < min) {min = prob;}
}
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"));
K::Gnuplot gp;
gp << "set view equal xy\n";
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true);
K::GnuplotSplotElementPoints points; plot.add(&points);
const BBox3 bbox = nm.getBBox();
points.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
points.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
border.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0));
// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
@@ -47,7 +82,7 @@ namespace NM {
for (const Tria* tria : nm) {
const uint8_t type = tria->getType();
if (type < 0 || type > 2) {
if (type < 0 || type > 5) {
throw std::runtime_error("out of type-bounds");
}
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
@@ -75,10 +110,15 @@ namespace NM {
plot.getObjects().reOrderByZIndex();
gp.draw(plot);
gp.flush();
sleep(1);
}
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";
}
void setCurPos(const Point3 pt) {
gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n";
}
};