kazu
04d8ae8c74
- some should be the same as previous commit (sorry!) - some should be new: LINT checks, ...?
88 lines
2.2 KiB
C++
88 lines
2.2 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#include "../Tests.h"
|
|
|
|
#include "../../grid/factory/GridImportance.h"
|
|
#include "../../grid/factory/GridFactory.h"
|
|
#include "../../floorplan/FloorplanFactorySVG.h"
|
|
#include "../../nav/dijkstra/Dijkstra.h"
|
|
#include "../../grid/walk/GridWalkWeighted.h"
|
|
|
|
#include "Plot.h"
|
|
|
|
TEST(TestAll, Nav) {
|
|
|
|
|
|
Grid<GP> g(20);
|
|
|
|
// 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.5);
|
|
//if (d > 20) {d*= 1.30;}
|
|
return d / std::pow(n2.imp, 3);
|
|
}
|
|
} tmp(g);
|
|
|
|
GridFactory<GP> gf(g);
|
|
|
|
// 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);
|
|
gf.addFloor(f2, 340);
|
|
gf.addStairs(s1_2, 20, 340);
|
|
gf.removeIsolated();
|
|
|
|
// calculate node importance based on the floorplan (walls, ...)
|
|
GridImportance gi;
|
|
//gi.addImportance(g, 20);
|
|
//gi.addImportance(g, 340);
|
|
|
|
// calculate dijkstra using aforementioned importance
|
|
Dijkstra<GP> d;
|
|
const GP& start = g.getNodeFor(GridPoint(500,200,20));
|
|
const GP& end = g.getNodeFor(GridPoint(1200,200,340));
|
|
//const GP& end = g.getNodeFor(GridPoint(1300,1300,20));
|
|
d.build(&start, &end, tmp);
|
|
|
|
// add the path's importance to the grid
|
|
gi.addImportance(g, d.getNode(start), d.getNode(end));
|
|
|
|
// plot path
|
|
K::GnuplotSplotElementLines path; path.getStroke().getColor().setHexStr("#0000ff"); path.getStroke().setWidth(2);
|
|
const DijkstraNode<GP>* dn = d.getNode(end);
|
|
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
|
|
// GridWalkWeighted<GP> walk;
|
|
// Log::add("test", "walking");
|
|
// for (int i = 0; i < 5000; ++i) {
|
|
// walk.getDestination(g, &start, 1.0, Headings::UP);
|
|
// }
|
|
// Log::add("test", "done");
|
|
|
|
Plot p;
|
|
p.build(g);
|
|
//p.showGrid(g);
|
|
p.addFloor(f1, 20);
|
|
p.addFloor(f2, 340);
|
|
p.splot.add(&path);
|
|
p.fire();
|
|
|
|
}
|
|
|
|
#endif
|