added several grid-walks

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...
2016-01-24 18:59:06 +01:00
parent cdf97322f8
commit 9947dced15
30 changed files with 1406 additions and 94 deletions
--- a/tests/grid/TestWalk.cpp
+++ b/tests/grid/TestWalk.cpp
@@ -0,0 +1,127 @@
+#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) {
+
+	Grid<20, GP> g;
+	GridFactory<20, 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);
+	}
+	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<20, GP>& grid;
+	public:
+		TMP(Grid<20, 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 = ((Point3)n1 - (Point3)n2).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> dist(0.3, 0.3);
+
+	K::GnuplotSplotElementPoints pStates; pStates.setColorHex("#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;
+	//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(dist(gen)));
+			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