280 lines
8.2 KiB
C++
280 lines
8.2 KiB
C++
#ifndef PAPERVISIMPORTANCE_H
|
|
#define PAPERVISIMPORTANCE_H
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
#include <Indoor/grid/factory/GridFactory.h>
|
|
#include <Indoor/grid/factory/GridImportance.h>
|
|
|
|
#include <Indoor/floorplan/FloorplanFactorySVG.h>
|
|
|
|
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
|
|
|
|
|
|
|
|
#include <Indoor/nav/dijkstra/Dijkstra.h>
|
|
#include <Indoor/nav/dijkstra/DijkstraPath.h>
|
|
|
|
#include "PaperPlot.h"
|
|
#include "PaperPlot2D.h"
|
|
|
|
#include "../MyGridNode.h"
|
|
#include "../Settings.h"
|
|
#include "../DijkstraMapper.h"
|
|
|
|
PaperPlot2D::Size s1 = PaperPlot2D::Size(4,1.5);
|
|
PaperPlot2D::Size s2 = PaperPlot2D::Size(4,1.7);
|
|
|
|
class PaperVisImportance {
|
|
|
|
public:
|
|
|
|
|
|
|
|
static float clamp(const float in, const float min, const float max) {
|
|
if (in < min) {return min;}
|
|
if (in > max) {return max;}
|
|
return in;
|
|
}
|
|
|
|
// use node-importance as grid-color
|
|
struct ColorizerImp {
|
|
float get(const MyGridNode& n) const {return n.imp;}
|
|
bool skip(const MyGridNode& n) const {(void) n; return false;}
|
|
};
|
|
|
|
// use node-distance as grid-color
|
|
struct ColorizerDist {
|
|
float get(const MyGridNode& n) const {return n.distToTarget;}
|
|
bool skip(const MyGridNode& n) const {(void) n; return false;}
|
|
};
|
|
|
|
// use num-visited as grid-color
|
|
struct ColorizeHeat {
|
|
int maxCnt; int cutoff;
|
|
ColorizeHeat(const int maxCnt, const int cutoff) : maxCnt(maxCnt), cutoff(cutoff) {;}
|
|
float get(const MyGridNode& n) const {return (n.cnt > maxCnt) ? (maxCnt) : (n.cnt);}
|
|
bool skip(const MyGridNode& n) const {return n.cnt < cutoff;} // skip to reduce plot size
|
|
};
|
|
|
|
|
|
static void createImportance() {
|
|
|
|
// load the floorplan
|
|
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
|
|
Floor f0 = fpFac.getFloor("test1");
|
|
const LengthF h0 = LengthF::cm(0);
|
|
|
|
// add the floorplan to the grid
|
|
Grid<MyGridNode> grid(20);
|
|
GridFactory<MyGridNode> gridFac(grid);
|
|
|
|
gridFac.addFloor(f0, h0.cm());
|
|
|
|
// remove all isolated nodes not attached to 300,300,floor0
|
|
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(400,400,h0.cm()) ) );
|
|
|
|
// stamp importance information onto the grid-nodes
|
|
GridImportance gridImp;
|
|
gridImp.addImportance(grid, h0.cm());
|
|
|
|
|
|
{
|
|
PaperPlot2D plot("floorplan_importance.eps", s1);
|
|
plot.setRanges(0,5100, 0,2100);
|
|
//plot.addFloor(f0);
|
|
plot.addGrid(grid, ColorizerImp());
|
|
plot.gp << "set colorbox\n";
|
|
plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0.6\n";
|
|
plot.show();
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void createPath() {
|
|
|
|
// load the floorplan
|
|
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
|
|
Floor f0 = fpFac.getFloor("test1");
|
|
const LengthF h0 = LengthF::cm(0);
|
|
|
|
// add the floorplan to the grid
|
|
Grid<MyGridNode> grid(20);
|
|
GridFactory<MyGridNode> gridFac(grid);
|
|
|
|
gridFac.addFloor(f0, h0.cm());
|
|
|
|
// remove all isolated nodes not attached to 300,300,floor0
|
|
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
|
|
|
|
|
|
|
|
// start and end
|
|
//const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
|
|
//const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
|
|
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0));
|
|
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0));
|
|
|
|
// build all shortest path to reach th target
|
|
Dijkstra<MyGridNode> dijkstra;
|
|
DijkstraMapper accImp(grid);
|
|
DijkstraMapperNormal accNormal(grid);
|
|
|
|
// path without importance
|
|
dijkstra.build(&gnStart, accNormal);
|
|
DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
|
|
|
|
// stamp importance information onto the grid-nodes
|
|
GridImportance gridImp;
|
|
gridImp.addImportance(grid, h0.cm());
|
|
|
|
// path WITH importance
|
|
dijkstra.build(&gnStart, accImp);
|
|
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
|
|
|
|
// build plot
|
|
K::GnuplotPlotElementLines gpPath1a; gpPath1a.setLineWidth(6); gpPath1a.setColorHex("#000000");
|
|
K::GnuplotPlotElementLines gpPath1b; gpPath1b.setLineWidth(2); gpPath1b.setColorHex("#ffffff");
|
|
K::GnuplotPlotElementLines gpPath2a; gpPath2a.setLineWidth(7); gpPath2a.setColorHex("#000000");
|
|
K::GnuplotPlotElementLines gpPath2b; gpPath2b.setLineWidth(2); gpPath2b.setColorHex("#ffffff");
|
|
|
|
for (DijkstraNode<MyGridNode>* dn : pathNormal) {
|
|
gpPath1a.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
|
|
gpPath1b.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
|
|
}
|
|
for (DijkstraNode<MyGridNode>* dn : pathImp) {
|
|
gpPath2a.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
|
|
gpPath2b.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
|
|
}
|
|
|
|
// plot the 2 paths
|
|
{
|
|
PaperPlot2D plot("floorplan_paths.eps", s1);
|
|
plot.floors.setColorHex("#777777");
|
|
plot.setRanges(0,5100, 0,2100);
|
|
plot.addFloor(f0);
|
|
plot.plot.add(&gpPath1a); //gpPath1a.setCustomAttr("dashtype 2");
|
|
plot.plot.add(&gpPath1b); gpPath1b.setCustomAttr("dashtype 2");
|
|
plot.plot.add(&gpPath2a);
|
|
plot.plot.add(&gpPath2b);
|
|
plot.show();
|
|
}
|
|
|
|
// stamp distance information onto the grid
|
|
// attach a corresponding weight-information to each user-grid-node
|
|
for (MyGridNode& node : grid) {
|
|
const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
|
|
node.distToTarget = dn->cumWeight;
|
|
}
|
|
|
|
// walk
|
|
GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk (grid, accImp, gnStart);
|
|
|
|
for (int i = 0; i < 30000; ++i) {
|
|
|
|
if (i % 250 == 0) {std::cout << i << std::endl;}
|
|
const MyGridNode& nStart = gnEnd;
|
|
GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
|
|
GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
|
|
(void) sEnd;
|
|
|
|
}
|
|
|
|
// plot the heat-map
|
|
{
|
|
PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s2);
|
|
plot.setRanges(0,5220, 0,2100);
|
|
plot.floors.setColorHex("#000000");
|
|
//plot.gp << "set palette gray negative\n";
|
|
plot.gp << "set palette defined ( 0 'white', 1 'blue', 2 'orange', 3 'yellow' )\n";
|
|
plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0\n";
|
|
plot.addFloor(f0);
|
|
plot.addGrid(grid, ColorizeHeat(7000, 50));
|
|
plot.plot.add(&gpPath1a); //gpPath1a.setCustomAttr("dashtype 2");
|
|
plot.plot.add(&gpPath1b); gpPath1b.setCustomAttr("dashtype 2");
|
|
plot.plot.add(&gpPath2a);
|
|
plot.plot.add(&gpPath2b);
|
|
plot.gp << "set object circle at 300,300 size 40,40 front\n";
|
|
plot.gp << "set object circle at 4700,1300 size 40,40 front\n";
|
|
plot.show();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
static void showDijkstraDistance() {
|
|
|
|
// load the floorplan
|
|
FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
|
|
Floor f0 = fpFac.getFloor("test1");
|
|
const LengthF h0 = LengthF::cm(0);
|
|
|
|
// add the floorplan to the grid
|
|
Grid<MyGridNode> grid(20);
|
|
GridFactory<MyGridNode> gridFac(grid);
|
|
|
|
gridFac.addFloor(f0, h0.cm());
|
|
|
|
// remove all isolated nodes not attached to 300,300,floor0
|
|
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );
|
|
|
|
|
|
|
|
// start and end
|
|
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0));
|
|
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0));
|
|
|
|
// build all shortest path to reach th target
|
|
Dijkstra<MyGridNode> dijkstra;
|
|
|
|
// stamp importance information onto the grid-nodes
|
|
GridImportance gridImp;
|
|
gridImp.addImportance(grid, h0.cm());
|
|
|
|
// path WITH importance
|
|
DijkstraMapper accImp(grid);
|
|
dijkstra.build(&gnStart, accImp);
|
|
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
|
|
|
|
// stamp distance information onto the grid
|
|
// attach a corresponding weight-information to each user-grid-node
|
|
for (MyGridNode& node : grid) {
|
|
const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
|
|
node.distToTarget = dn->cumWeight;
|
|
}
|
|
|
|
// plot the heat-map
|
|
{
|
|
PaperPlot2D plot;
|
|
plot.setRanges(0,5220, 0,2100);
|
|
plot.floors.setColorHex("#000000");
|
|
//plot.gp << "set palette gray negative\n";
|
|
plot.gp << "set palette defined ( 0 'white', 1 'blue', 2 'orange', 3 'yellow' )\n";
|
|
plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0\n";
|
|
plot.addFloor(f0);
|
|
plot.addGrid(grid, ColorizerDist());
|
|
plot.gp << "set object circle at 300,300 size 40,40 front\n";
|
|
plot.gp << "set object circle at 4700,1300 size 40,40 front\n";
|
|
plot.show();
|
|
sleep(1000);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // PAPERVISIMPORTANCE_H
|