FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

changed the floorplan (adjust stairs)

Browse Source 
added the new stairs to the code
changed the visualisation
This commit is contained in:
FrankE
2016-01-27 21:39:21 +01:00
parent f3856d9473
commit 07d739ebb7
4 changed files with 337 additions and 197 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
code/main.cpp
View File

@@ -1,14 +1,21 @@
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/factory/GridImportance.h>
#include "Vis.h"
namespace Settings {
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
const int gridSize_cm = 200;
const int gridSize_cm = 20;
}
struct MyNode : public GridNode, public GridPoint {
float distToTarget = 1.0;
float imp = 1.0;
public:
MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};
@@ -18,6 +25,20 @@ int align(const int val) {
return val / Settings::gridSize_cm * Settings::gridSize_cm;
}
// dijkstra mapper
class DijkstraMapper {
Grid<MyNode>& grid;
public:
DijkstraMapper(Grid<MyNode>& grid) : grid(grid) {;}
int getNumNeighbors(const MyNode& node) const {return node.getNumNeighbors();}
const MyNode* getNeighbor(const MyNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const MyNode& n1, const MyNode& n2) const {
float d = ((Point3)n1 - (Point3)n2).length(2.0);
//if (d > 20) {d*= 1.30;}
return d / std::pow(n2.imp, 3);
}
};
int main(void) {
Grid<MyNode> grid(Settings::gridSize_cm);
@@ -31,9 +52,9 @@ int main(void) {
Floor f2 = fpFac.getFloor("floor_2");
Floor f3 = fpFac.getFloor("floor_3");
Stairs f01 = fpFac.getStairs("staircase_0_1");
Stairs f12 = fpFac.getStairs("staircase_1_2");
Stairs f23 = fpFac.getStairs("staircase_2_3");
Stairs s01 = fpFac.getStairs("staircase_0_1");
Stairs s12 = fpFac.getStairs("staircase_1_2");
Stairs s23 = fpFac.getStairs("staircase_2_3");
const LengthF h0 = LengthF::cm(align(0));
const LengthF h1 = LengthF::cm(align(360));
@@ -44,12 +65,90 @@ int main(void) {
gridFac.addFloor(f1, h1.cm());
gridFac.addFloor(f2, h2.cm());
gridFac.addFloor(f3, h3.cm());
//gridFac.removeIsolated();
gridFac.addStairs(s01, h0.cm(), h1.cm());
gridFac.addStairs(s12, h1.cm(), h2.cm());
gridFac.addStairs(s23, h2.cm(), h3.cm());
// maybe the two sides are wrong?
PlatformStair psUpperLeft;
psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
gridFac.buildPlatformStair(psUpperLeft, h0.cm(), h1.cm());
gridFac.buildPlatformStair(psUpperLeft, h1.cm(), h2.cm());
gridFac.buildPlatformStair(psUpperLeft, h2.cm(), h3.cm());
// vis.gp << "set xrange [1100:1800]\n";
// vis.gp << "set yrange [4500:5200]\n";
PlatformStair psUpperRight;
psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
gridFac.buildPlatformStair(psUpperRight, h0.cm(), h1.cm());
gridFac.buildPlatformStair(psUpperRight, h1.cm(), h2.cm());
gridFac.buildPlatformStair(psUpperRight, h2.cm(), h3.cm());
// vis.gp << "set xrange [6100:6900]\n";
// vis.gp << "set yrange [4500:5200]\n";
PlatformStair psLowerLeft;
psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
gridFac.buildPlatformStair(psLowerLeft, h0.cm(), h1.cm());
gridFac.buildPlatformStair(psLowerLeft, h1.cm(), h2.cm());
gridFac.buildPlatformStair(psLowerLeft, h2.cm(), h3.cm());
// vis.gp << "set xrange [1300:2100]\n";
// vis.gp << "set yrange [400:1400]\n";
gridFac.removeIsolated( (MyNode&)grid.getNodeFor(GridPoint(300,300,h0.cm())) );
GridImportance gridImp;
gridImp.addImportance(grid, h0.cm());
gridImp.addImportance(grid, h1.cm());
gridImp.addImportance(grid, h2.cm());
gridImp.addImportance(grid, h3.cm());
MyNode& start = (MyNode&)grid.getNodeFor(GridPoint(500,300,h0.cm()));
MyNode& end = (MyNode&)grid.getNodeFor(GridPoint(7000,5000,h3.cm()));
Vis vis;
vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3);
vis.addGrid(grid);
vis.show();
vis.addFloor(f0, h0);
vis.addFloor(f1, h1);
vis.addFloor(f2, h2);
vis.addFloor(f3, h3);
//GridWalkRandomHeadingUpdate<MyNode> walk;
//GridWalkRandomHeadingUpdateAdv<MyNode> walk;
//GridWalkPushForward<MyNode> walk;
GridWalkLightAtTheEndOfTheTunnel<MyNode> walk(grid, DijkstraMapper(grid), end);
std::vector<GridWalkState<MyNode>> states;
for (int i = 0; i < 2000; ++i) { states.push_back(GridWalkState<MyNode>(&start, Heading::rnd())); }
// vis.addGrid(grid);
// vis.show();
// sleep(100);
// vis.removeGrid();
Distribution::Normal<float> wDist(0.3, 0.3);
//std::minstd_rand gen(1234);
//std::normal_distribution<float> dist(0.6, 0.3);
while(true) {
for (GridWalkState<MyNode>& state : states) {
state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
}
usleep(1000*80);
vis.showStates(states);
vis.show();
}
sleep(1000);