101 lines
2.2 KiB
C++
101 lines
2.2 KiB
C++
#ifndef VIS_H
|
|
#define VIS_H
|
|
|
|
#include <KLib/misc/gnuplot/Gnuplot.h>
|
|
#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 <Indoor/geo/Length.h>
|
|
#include <Indoor/floorplan/Floor.h>
|
|
|
|
class Vis {
|
|
|
|
public:
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot splot;
|
|
K::GnuplotSplotElementLines floors;
|
|
K::GnuplotSplotElementColorPoints gridNodes;
|
|
K::GnuplotSplotElementLines gridEdges;
|
|
K::GnuplotSplotElementPoints particles;
|
|
|
|
public:
|
|
|
|
Vis() {
|
|
|
|
gp << "set hidden3d front\n";
|
|
gp << "set view equal xy\n";
|
|
gp << "set ticslevel 0\n";
|
|
gp << "set cbrange[0.8:2.0]\n";
|
|
|
|
// attach all layers
|
|
splot.add(&floors);
|
|
splot.add(&gridNodes);
|
|
splot.add(&gridEdges);
|
|
splot.add(&particles);
|
|
|
|
}
|
|
|
|
/** add all obstacles of the given floor to the provided height */
|
|
Vis& addFloor(const Floor& f, const LengthF height) {
|
|
|
|
// add each wall
|
|
for (const Line2& l : f.getObstacles()) {
|
|
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
|
|
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
|
|
floors.addSegment(p1, p2);
|
|
}
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
/** add the grid to the plot */
|
|
template <typename T> Vis& addGrid(Grid<T>& grid) {
|
|
|
|
float max = 0;
|
|
for (const T& n1 : grid) {
|
|
if (n1.distToTarget > max) {max = n1.distToTarget;}
|
|
}
|
|
gp << "set cbrange[0.0:1.0]\n";
|
|
//gp << "set cbrange[0.8:1.3]\n";
|
|
|
|
for (const T& n1 : grid) {
|
|
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
|
//const float color = n1.imp;
|
|
//const float color = n1.distToTarget/max;
|
|
const float color = 0;
|
|
gridNodes.add(p1, color);
|
|
// for (const T& n2 : grid.neighbors(n1)) {
|
|
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
|
// gridEdges.addSegment(p1, p2);
|
|
// }
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
void removeGrid() {
|
|
gridNodes.clear();;
|
|
}
|
|
|
|
template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
|
|
particles.clear();;
|
|
for (const GridWalkState<T>& n : states) {
|
|
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/** show (plot) the current setup */
|
|
void show() {
|
|
gp.draw(splot);
|
|
gp.flush();
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // VIS_H
|