158 lines
5.0 KiB
C++
158 lines
5.0 KiB
C++
#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 = 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) {;}
|
|
};
|
|
|
|
|
|
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);
|
|
GridFactory<MyNode> gridFac(grid);
|
|
|
|
|
|
FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);
|
|
|
|
Floor f0 = fpFac.getFloor("floor_0");
|
|
Floor f1 = fpFac.getFloor("floor_1");
|
|
Floor f2 = fpFac.getFloor("floor_2");
|
|
Floor f3 = fpFac.getFloor("floor_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));
|
|
const LengthF h2 = LengthF::cm(align(360+340));
|
|
const LengthF h3 = LengthF::cm(align(360+340+340));
|
|
|
|
gridFac.addFloor(f0, h0.cm());
|
|
gridFac.addFloor(f1, h1.cm());
|
|
gridFac.addFloor(f2, h2.cm());
|
|
gridFac.addFloor(f3, h3.cm());
|
|
|
|
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);
|
|
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);
|
|
|
|
return 0;
|
|
|
|
}
|