kazu
04d8ae8c74
- some should be the same as previous commit (sorry!) - some should be new: LINT checks, ...?
146 lines
3.5 KiB
C++
146 lines
3.5 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#include "../Tests.h"
|
|
|
|
#include "Plot.h"
|
|
#include "../../grid/factory/GridImportance.h"
|
|
#include "../../grid/factory/GridFactory.h"
|
|
#include "../../floorplan/FloorplanFactorySVG.h"
|
|
#include "../../nav/dijkstra/Dijkstra.h"
|
|
|
|
#include "../../grid/walk/GridWalkState.h"
|
|
#include "../../grid/walk/GridWalkWeighted.h"
|
|
#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"
|
|
|
|
|
|
//TEST(Walk, plot) {
|
|
TEST(Walk, DISABLED_plot) {
|
|
|
|
Grid<GP> g(20);
|
|
GridFactory<GP> gf(g);
|
|
|
|
bool use3D = true;
|
|
|
|
// load floorplan
|
|
FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
|
|
Floor f1 = fpf.getFloor("1");
|
|
Floor f2 = fpf.getFloor("2");
|
|
Stairs s1_2 = fpf.getStairs("1_2");
|
|
|
|
|
|
|
|
// build the grid
|
|
gf.addFloor(f1, 20);
|
|
if (use3D) {
|
|
gf.addFloor(f2, 340);
|
|
gf.addStairs(s1_2, 20, 340);
|
|
}
|
|
|
|
|
|
|
|
//TODO: write as something simpler? e.g. as box with rotation??
|
|
PlatformStair pStair;
|
|
pStair.platform = BBox2(Point2(200, 200), Point2(400,500));
|
|
pStair.s1 = Stair(Line2( 600,200, 600,350 ), Point2(-200,0));
|
|
pStair.s2 = Stair(Line2( 600,400, 600,500 ), Point2(-250,0));
|
|
//gf.buildPlatformStair(pStair, 20, 340);
|
|
|
|
gf.removeIsolated();
|
|
|
|
// calculate node importance based on the floorplan (walls, ...)
|
|
GridImportance gi;
|
|
gi.addImportance(g, 20);
|
|
gi.addImportance(g, 340);
|
|
|
|
// dijkstra mapper
|
|
class TMP {
|
|
Grid<GP>& grid;
|
|
public:
|
|
TMP(Grid<GP>& grid) : grid(grid) {;}
|
|
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
|
|
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
|
|
float getWeightBetween(const GP& n1, const GP& n2) const {
|
|
float d = (n1.inCentimeter() - n2.inCentimeter()).length(2.0);
|
|
//if (d > 20) {d*= 1.30;}
|
|
return d / std::pow(n2.imp, 3);
|
|
}
|
|
} tmp(g);
|
|
|
|
|
|
|
|
// // calculate shortest path
|
|
// Dijkstra<GP> d;
|
|
const GP* start = &g.getNodeFor(GridPoint(700,200,20));
|
|
const GP* end;
|
|
if (use3D) {
|
|
end = &g.getNodeFor(GridPoint(1200,200,340));
|
|
} else {
|
|
end = &g.getNodeFor(GridPoint(1300,1300,20));
|
|
}
|
|
// d.build(end, start, tmp);
|
|
|
|
// gi.addDistanceToTarget(g, d);
|
|
|
|
// // add the path's importance to the grid
|
|
// gi.addImportance(g, d.getNode(start), d.getNode(end));
|
|
|
|
// // plot path
|
|
// K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
|
|
// DijkstraNode<GP>* dn = d.getNode(start);
|
|
// while (dn->previous != nullptr) {
|
|
// path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
|
|
// dn = dn->previous;
|
|
// }
|
|
|
|
|
|
|
|
// walk
|
|
GridWalkLightAtTheEndOfTheTunnel<GP> walk(g, tmp, *end);
|
|
|
|
std::minstd_rand gen;
|
|
std::normal_distribution<float> dWalk(0.3, 0.3);
|
|
std::normal_distribution<float> dTurn(0.3, 0.3);
|
|
|
|
K::GnuplotSplotElementPoints pStates; pStates.getColor().setHexStr("#880000");
|
|
|
|
// setup starting states
|
|
std::vector<GridWalkState<GP>> states;
|
|
for (int i = 0; i < 1000; ++i) {
|
|
states.push_back( GridWalkState<GP>( start, Headings::UP ) );
|
|
}
|
|
|
|
Plot p;
|
|
|
|
// show the grid
|
|
//p.build(g);
|
|
|
|
p.addFloor(f1, 20);
|
|
if (use3D) {
|
|
p.addFloor(f2, 340);
|
|
}
|
|
//p.splot.add(&path);
|
|
p.splot.add(&pStates);
|
|
p.nodes.setPointSize(0.2);
|
|
|
|
if (!use3D) {
|
|
p.gp << "set view 0,0\n";
|
|
}
|
|
p.gp << "set view equal xy\n";
|
|
|
|
// walk random distances
|
|
for (int i = 0; i < 5000; ++i) {
|
|
pStates.clear();
|
|
for (GridWalkState<GP>& state : states) {
|
|
state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen), Activity::UNKNOWN);
|
|
pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10));
|
|
}
|
|
p.gp.draw(p.splot);
|
|
p.gp.flush();
|
|
usleep(1000*80);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
#endif
|