kazu
9947dced15
added new helper methods/classes (e.g. for heading) new test cases optimize the dijkstra cleanups/refactoring added timed-benchmarks to the log many more...
151 lines
3.1 KiB
C++
151 lines
3.1 KiB
C++
#ifndef PLOT_H
|
|
#define PLOT_H
|
|
|
|
#include "../../grid/Grid.h"
|
|
#include <set>
|
|
|
|
#include <KLib/misc/gnuplot/Gnuplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
|
|
|
#include "../../floorplan/Floor.h"
|
|
|
|
class GP : public GridNode, public GridPoint {
|
|
public:
|
|
float imp = 1.0f;
|
|
float impPath = 1.0f;
|
|
float distToTarget = 1.0;
|
|
public:
|
|
GP() : GridNode(), GridPoint() {;}
|
|
GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
|
|
};
|
|
|
|
class Plot {
|
|
|
|
public:
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot splot;
|
|
|
|
K::GnuplotSplotElementColorPoints nodes;
|
|
K::GnuplotSplotElementLines lines;
|
|
K::GnuplotSplotElementLines floors;
|
|
|
|
/** ctor */
|
|
Plot() {
|
|
|
|
gp << "set ticslevel 0\n";
|
|
gp << "set view equal xyz\n";
|
|
gp << "set cbrange[0.5:1.5]\n";
|
|
gp << "set palette gray negative\n";
|
|
|
|
//gp << "set hidden3d front\n";
|
|
|
|
splot.add(&nodes);
|
|
|
|
splot.add(&floors);
|
|
floors.setLineWidth(2);
|
|
floors.setColorHex("#008800");
|
|
|
|
}
|
|
|
|
template <int gridSize_cm, typename T> Plot& showGrid(Grid<gridSize_cm, T>& g) {
|
|
addEdges(g);
|
|
addNodes(g);
|
|
return *this;
|
|
}
|
|
|
|
template <int gridSize_cm, typename T> Plot& addEdges(Grid<gridSize_cm, T>& g) {
|
|
|
|
// prevent adding edges twice
|
|
std::set<size_t> done;
|
|
|
|
for (GP& n1 : g) {
|
|
for (const T& n2 : g.neighbors(n1)) {
|
|
size_t edge = g.getUID(n1) ^ g.getUID(n2);
|
|
if (done.find(edge) == done.end()) {
|
|
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
|
K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
|
lines.addSegment(p1, p2);
|
|
done.insert(edge);
|
|
}
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
template <int gridSize_cm, typename T> Plot& addNodes(Grid<gridSize_cm, T>& g) {
|
|
|
|
for (GP& n1 : g) {
|
|
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
|
nodes.add(p1, 1.0);
|
|
}
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
template <int gridSize_cm, typename T> Plot& build(Grid<gridSize_cm, T>& g) {
|
|
|
|
std::set<uint64_t> done;
|
|
|
|
int cnt = 0;
|
|
for (int i = 0; i < g.getNumNodes(); ++i) {
|
|
const GP& n1 = g[i];
|
|
//const float color = std::pow(n1.distToTarget,0.1);
|
|
const float color = n1.imp * n1.impPath;
|
|
nodes.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), color);
|
|
|
|
for (const T& n2 : g.neighbors(n1)) {
|
|
//for (int n = 0; n < n1.getNumNeighbors(); ++n) {
|
|
// const GP& n2 = n1.getNeighbor(n, g);
|
|
if (done.find(g.getUID(n2)) == done.end()) {
|
|
K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
|
K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
|
lines.addSegment(p1, p2);
|
|
++cnt;
|
|
}
|
|
}
|
|
|
|
done.insert(g.getUID(n1));
|
|
|
|
}
|
|
|
|
nodes.setPointSize(1.0);
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
/** remove all floors from the plot */
|
|
Plot& resetFloors() {
|
|
floors.clear();
|
|
return *this;
|
|
}
|
|
|
|
/** add the given floor to the plot */
|
|
Plot& addFloor(Floor& f, const float z_cm) {
|
|
for (const Line2& l : f.getObstacles()) {
|
|
K::GnuplotPoint3 p1(l.p1.x, l.p1.y, z_cm+10);
|
|
K::GnuplotPoint3 p2(l.p2.x, l.p2.y, z_cm+10);
|
|
floors.addSegment(p1, p2);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
Plot& fire() {
|
|
gp.draw(splot);
|
|
gp.flush();
|
|
sleep(1000);
|
|
return *this;
|
|
}
|
|
|
|
};
|
|
|
|
|
|
#endif // PLOT_H
|