began putting everything together
This commit is contained in:
164
code/main.cpp
164
code/main.cpp
@@ -1,146 +1,62 @@
|
||||
#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"
|
||||
#include "Helper.h"
|
||||
#include "MyGridNode.h"
|
||||
#include "Helper.h"
|
||||
#include "DijkstraMapper.h"
|
||||
|
||||
namespace Settings {
|
||||
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
|
||||
const int gridSize_cm = 20;
|
||||
}
|
||||
#include "eval/Eval.h"
|
||||
#include "eval/EvalBase.h"
|
||||
|
||||
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) {;}
|
||||
};
|
||||
Settings settings;
|
||||
|
||||
void testModelWalk() {
|
||||
|
||||
int align(const int val) {
|
||||
return val / Settings::gridSize_cm * Settings::gridSize_cm;
|
||||
}
|
||||
Grid<MyGridNode> grid(MiscSettings::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);
|
||||
}
|
||||
};
|
||||
Helper::FHWSFloors floors = Helper::getFloors();
|
||||
Helper::buildTheGrid(grid, floors);
|
||||
|
||||
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()));
|
||||
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
|
||||
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
|
||||
|
||||
Vis vis;
|
||||
vis.addFloor(f0, h0);
|
||||
vis.addFloor(f1, h1);
|
||||
vis.addFloor(f2, h2);
|
||||
vis.addFloor(f3, h3);
|
||||
vis.addFloor(floors.f0, floors.h0);
|
||||
vis.addFloor(floors.f1, floors.h1);
|
||||
vis.addFloor(floors.f2, floors.h2);
|
||||
vis.addFloor(floors.f3, floors.h3);
|
||||
|
||||
// vis.gp << "set xrange [1000:4000]\n";
|
||||
// vis.gp << "set yrange [1000:4000]\n";
|
||||
// vis.gp << "set zrange [0:600]\n";
|
||||
|
||||
// switch between different grid-walkers
|
||||
GridWalkRandomHeadingUpdate<MyGridNode> walk;
|
||||
//GridWalkRandomHeadingUpdateAdv<MyGridNode> walk;
|
||||
//GridWalkPushForward<MyGridNode> walk;
|
||||
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk(grid, DijkstraMapper(grid), end);
|
||||
|
||||
|
||||
std::vector<GridWalkState<MyGridNode>> states;
|
||||
for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState<MyGridNode>(&start, Heading::rnd())); }
|
||||
|
||||
// track the number-of-visits for each node to draw something like a particle-heat-map?
|
||||
|
||||
//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())); }
|
||||
|
||||
// show the importance factors
|
||||
// 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) {
|
||||
for (GridWalkState<MyGridNode>& state : states) {
|
||||
state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
|
||||
}
|
||||
usleep(1000*80);
|
||||
@@ -152,6 +68,20 @@ int main(void) {
|
||||
|
||||
sleep(1000);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int main(void) {
|
||||
|
||||
//testModelWalk();
|
||||
|
||||
Eval eval;
|
||||
eval.setEval1();
|
||||
eval.run();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user