94 lines
2.3 KiB
C++
94 lines
2.3 KiB
C++
#ifndef WALKVIZ_H
|
|
#define WALKVIZ_H
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/math/Interpolator.h>
|
|
|
|
#include <KLib/misc/gnuplot/Gnuplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
|
|
|
|
struct WalkViz {
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotPlot plot;
|
|
|
|
K::GnuplotPlotElementPoints nodes;
|
|
K::GnuplotPlotElementLines outline;
|
|
K::GnuplotPlotElementLines obstacles;
|
|
K::GnuplotPlotElementPoints particles;
|
|
K::GnuplotPlotElementLines path;
|
|
|
|
WalkViz() {
|
|
plot.add(&obstacles);
|
|
plot.add(&outline);
|
|
plot.add(&nodes); nodes.setPointType(7); nodes.setPointSize(1); nodes.getColor().setHexStr("#999999");
|
|
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.1);
|
|
plot.add(&path); path.getStroke().setWidth(2);
|
|
gp << "set size ratio -1\n";
|
|
}
|
|
|
|
void add(Floorplan::FloorOutlinePolygon* poly) {
|
|
Floorplan::Polygon2 pol2 = poly->poly;
|
|
const int num = pol2.points.size();
|
|
for (int i = 0; i < num; ++i) {
|
|
const Point2 pt1 = pol2.points[(i+0)];
|
|
const Point2 pt2 = pol2.points[(i+1)%num];
|
|
K::GnuplotPoint2 gp1(pt1.x*100, pt1.y*100);
|
|
K::GnuplotPoint2 gp2(pt2.x*100, pt2.y*100);
|
|
outline.addSegment(gp1, gp2);
|
|
}
|
|
}
|
|
|
|
void addObstacle(Floorplan::FloorObstacle* obs) {
|
|
|
|
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
|
Floorplan::FloorObstacleCircle* circle = dynamic_cast<Floorplan::FloorObstacleCircle*>(obs);
|
|
|
|
if (line) {
|
|
K::GnuplotPoint2 gp1(line->from.x*100, line->from.y*100);
|
|
K::GnuplotPoint2 gp2(line->to.x*100, line->to.y*100);
|
|
obstacles.addSegment(gp1,gp2);
|
|
}
|
|
|
|
}
|
|
|
|
void addFloor(Floorplan::Floor* floor) {
|
|
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
|
addObstacle(obs);
|
|
}
|
|
}
|
|
|
|
void addMap(Floorplan::IndoorMap* map) {
|
|
for (Floorplan::Floor* floor : map->floors) {
|
|
addFloor(floor);
|
|
}
|
|
}
|
|
|
|
template <typename Node> void addGrid(Grid<Node>& grid) {
|
|
for (const Node& n : grid) {
|
|
K::GnuplotPoint2 gp(n.x_cm, n.y_cm);
|
|
nodes.add(gp);
|
|
}
|
|
}
|
|
|
|
void showPath(const Interpolator<float, Point3>& interpol) {
|
|
for (const auto& entry : interpol.getEntries()) {
|
|
const Point3 p = entry.value;
|
|
K::GnuplotPoint2 gp(p.x * 100 , p.y*100);
|
|
path.add(gp);
|
|
}
|
|
}
|
|
|
|
void draw() {
|
|
gp.draw(plot);
|
|
gp.flush();
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
#endif // WALKVIZ_H
|