From 07d739ebb7a7f018e675b99de1762fcf1f098cd5 Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 27 Jan 2016 21:39:21 +0100 Subject: [PATCH] changed the floorplan (adjust stairs) added the new stairs to the code changed the visualisation --- code/CMakeLists.txt | 3 +- code/Vis.h | 39 ++++- code/main.cpp | 115 +++++++++++++- code/plan.svg | 377 +++++++++++++++++++++++--------------------- 4 files changed, 337 insertions(+), 197 deletions(-) diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index a138ae4..c5c9690 100755 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -64,8 +64,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g - -O0 - + -O2 -DWITH_TESTS -DWITH_ASSERTIONS diff --git a/code/Vis.h b/code/Vis.h index bbe6ea7..8b185dd 100644 --- a/code/Vis.h +++ b/code/Vis.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -16,8 +17,9 @@ public: K::Gnuplot gp; K::GnuplotSplot splot; K::GnuplotSplotElementLines floors; - K::GnuplotSplotElementPoints gridNodes; + K::GnuplotSplotElementColorPoints gridNodes; K::GnuplotSplotElementLines gridEdges; + K::GnuplotSplotElementPoints particles; public: @@ -26,11 +28,13 @@ public: gp << "set hidden3d front\n"; gp << "set view equal xy\n"; gp << "set ticslevel 0\n"; + gp << "set cbrange[0.8:2.0]\n"; // attach all layers splot.add(&floors); splot.add(&gridNodes); splot.add(&gridEdges); + splot.add(&particles); } @@ -50,13 +54,36 @@ public: /** add the grid to the plot */ template Vis& addGrid(Grid& grid) { + + float max = 0; + for (const T& n1 : grid) { + if (n1.distToTarget > max) {max = n1.distToTarget;} + } + gp << "set cbrange[0.0:1.0]\n"; + //gp << "set cbrange[0.8:1.3]\n"; + for (const T& n1 : grid) { const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); - gridNodes.add(p1); - for (const T& n2 : grid.neighbors(n1)) { - const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); - gridEdges.addSegment(p1, p2); - } + //const float color = n1.imp; + //const float color = n1.distToTarget/max; + const float color = 0; + gridNodes.add(p1, color); +// for (const T& n2 : grid.neighbors(n1)) { +// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); +// gridEdges.addSegment(p1, p2); +// } + } + return *this; + } + + void removeGrid() { + gridNodes.clear();; + } + + template Vis& showStates(std::vector>& states) { + particles.clear();; + for (const GridWalkState& n : states) { + particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm)); } return *this; } diff --git a/code/main.cpp b/code/main.cpp index 76cd605..6b10fdd 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -1,14 +1,21 @@ #include #include +#include +#include +#include +#include +#include #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& grid; +public: + DijkstraMapper(Grid& 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 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 walk; + //GridWalkRandomHeadingUpdateAdv walk; + //GridWalkPushForward walk; + GridWalkLightAtTheEndOfTheTunnel walk(grid, DijkstraMapper(grid), end); + + + std::vector> states; + for (int i = 0; i < 2000; ++i) { states.push_back(GridWalkState(&start, Heading::rnd())); } + +// vis.addGrid(grid); +// vis.show(); +// sleep(100); +// vis.removeGrid(); + + Distribution::Normal wDist(0.3, 0.3); + //std::minstd_rand gen(1234); + //std::normal_distribution dist(0.6, 0.3); + + while(true) { + for (GridWalkState& state : states) { + state = walk.getDestination(grid, state, std::abs(wDist.draw()) ); + } + usleep(1000*80); + vis.showStates(states); + vis.show(); + } + + sleep(1000); diff --git a/code/plan.svg b/code/plan.svg index 71adfcd..8ec6368 100755 --- a/code/plan.svg +++ b/code/plan.svg @@ -42,23 +42,23 @@ guidetolerance="10" inkscape:pageopacity="0" inkscape:pageshadow="2" - inkscape:window-width="1366" - inkscape:window-height="672" + inkscape:window-width="1600" + inkscape:window-height="782" id="namedview3234" showgrid="true" inkscape:zoom="2.4027176" - inkscape:cx="422.22336" - inkscape:cy="1231.7432" + inkscape:cx="555.925" + inkscape:cy="388.39069" inkscape:window-x="0" - inkscape:window-y="33" + inkscape:window-y="0" inkscape:window-maximized="1" - inkscape:current-layer="layer11" + inkscape:current-layer="layer16" inkscape:object-nodes="true" units="px" showborder="true" inkscape:snap-to-guides="true" inkscape:snap-text-baseline="true" - showguides="true" + showguides="false" inkscape:guide-bbox="true" inkscape:snap-nodes="true" inkscape:snap-smooth-nodes="true"> @@ -238,7 +238,7 @@ inkscape:groupmode="layer" id="layer2" inkscape:label="roomnames_2" - style="display:inline"> + style="display:none"> + style="display:inline"> - + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> - + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> + + + + + + + + + @@ -2867,21 +2909,6 @@ d="m 2462.5984,460.62989 0,276.37795" id="path12918" inkscape:connector-curvature="0" /> - - - + + + + + + @@ -3055,7 +3112,7 @@ inkscape:connector-curvature="0" /> @@ -3545,24 +3602,6 @@ inkscape:connector-curvature="0" transform="translate(0,-4.2364502e-5)" sodipodi:nodetypes="cc" /> - - - + + + + + + @@ -4352,30 +4421,42 @@ inkscape:connector-curvature="0" transform="translate(0,-4.2364502e-5)" sodipodi:nodetypes="cc" /> - - - + + + + + + - + sodipodi:nodetypes="ccc" /> - - + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" + d="m 503.14961,457.08658 -46.063,0 0,276.37795" + id="path4992" + inkscape:connector-curvature="0" + sodipodi:nodetypes="ccc" /> + style="display:none"> + sodipodi:nodetypes="ccc" /> - - - - + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" + d="m 503.14961,460.62989 -56.69292,0 0,276.37795" + id="path5000" + inkscape:connector-curvature="0" /> - - - - - +