diff --git a/IO/GeoFileParser.cpp b/IO/GeoFileParser.cpp index 96b1275adc8a73fe31e3d46d71ac9846fdfc2d8b..b1e8f05b3785ba153d50c17a24284d3357fd601c 100644 --- a/IO/GeoFileParser.cpp +++ b/IO/GeoFileParser.cpp @@ -470,7 +470,7 @@ bool GeoFileParser::parseDoorNode(TiXmlElement * xDoor, int id, Building* buildi building->GetTransition(id)->TempClose(); break; case DoorState::Error: - Log->Write("WARNING:\t Unknown door state: <%s>. open or close. Default: open", + Log->Write("WARNING:\t Unknown door state: <%s>. open, close or temp_close. Default: open", stateStr.c_str()); building->GetTransition(id)->Open(); break; @@ -926,6 +926,7 @@ std::shared_ptr GeoFileParser::parseTrainTimeTableNode(TiXmlElem float arrival_time = xmltof(e->Attribute("arrival_time"), -1); float departure_time = xmltof(e->Attribute("departure_time"), -1); + // @todo: check these values for correctness e.g. arrival < departure Log->Write("INFO:\tTrain time table:"); Log->Write("INFO:\t id: %d", id); Log->Write("INFO:\t type: %s", type.c_str()); @@ -953,6 +954,8 @@ std::shared_ptr GeoFileParser::parseTrainTimeTableNode(TiXmlElem train_start, train_end, platform_id, + false, + false, }); return trainTimeTab; diff --git a/Simulation.cpp b/Simulation.cpp index 211fd400ce169988151087658ad4f82c0ad2cb90..2564c1b272279b7d4ede6bba958426c1c7867839 100644 --- a/Simulation.cpp +++ b/Simulation.cpp @@ -575,19 +575,36 @@ double Simulation::RunBody(double maxSimTime) //here we could place router-tasks (calc new maps) that can use multiple cores AND we have 't' //update quickestRouter if (_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)) { - FFRouter* ffrouter = dynamic_cast(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)); if(geometryChanged) { - ffrouter->Init(_building.get()); - std::cout << KBLU << " Init router in simulation\n" << RESET; - } + FFRouter* ffrouter2 = dynamic_cast(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)); + ffrouter2->Init(_building.get()); + // debug + fs::path f("tmp_"+std::to_string(t)+"_"+_config->GetGeometryFile()); + std::string filename = f.string(); + std::cout << "\n Write geometry --> " << filename.c_str() << "\n"; + _building->SaveGeometry(filename); + // + double _deltaH = _building->GetConfig()->get_deltaH(); + double _wallAvoidDistance = _building->GetConfig()->get_wall_avoid_distance(); + bool _useWallAvoidance = _building->GetConfig()->get_use_wall_avoidance(); + + if(auto dirlocff = dynamic_cast(_building->GetConfig()->get_dirStrategy())){ + Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ..."); + dirlocff->Init(_building.get(), _deltaH, _wallAvoidDistance, _useWallAvoidance); + Log->Write("INFO:\t Init DirectionLOCALFloorfield done"); + } - if (ffrouter->MustReInit()) { - ffrouter->ReInit(); - ffrouter->SetRecalc(t); + std::cout << KBLU << " Init router in simulation\n" << RESET; + } + else{ + FFRouter* ffrouter = dynamic_cast(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)); + if (ffrouter->MustReInit()) { + ffrouter->ReInit(); + ffrouter->SetRecalc(t); + } } - } // here the used routers are update, when needed due to external changes @@ -703,18 +720,24 @@ bool Simulation::WriteTrajectories(std::string trajectoryName) * add doors * set _routingEngine->setNeedUpdate(true); */ -bool Simulation::correctGeometry(std::shared_ptr building, std::string trainType, Point TrackStart, Point TrackEnd) +bool Simulation::correctGeometry(std::shared_ptr building, std::shared_ptr tab) { - std::cout << "enter with train " << trainType.c_str() << "\n"; - std::cout<< KBLU << "Enter correctGeometry: Building Has " << building->GetAllTransitions().size() << " Transitions\n" << RESET; - //auto platforms = building->GetPlatforms(); - SubRoom * subroom; - int room_id, subroom_id; - auto mytrack = building->GetTrackWalls(TrackStart, TrackEnd, room_id, subroom_id); - std::cout << "room: " << room_id << " subroom_id " << subroom_id << "\n" ; - Room* room = building->GetRoom(room_id); - subroom = room->GetSubRoom(subroom_id);//todo safety check - int transition_id = 10000; + //auto platforms = building->GetPlatforms(); + int trainId = tab->id; + std::string trainType = tab->type; + Point TrackStart = tab->pstart; + Point TrackEnd = tab->pend; + SubRoom * subroom; + int room_id, subroom_id; + auto mytrack = building->GetTrackWalls(TrackStart, TrackEnd, room_id, subroom_id); + Room* room = building->GetRoom(room_id); + subroom = room->GetSubRoom(subroom_id);//todo safety check + int transition_id = 10000; + + std::cout << "enter with train " << trainType.c_str() << "\n"; + std::cout<< KBLU << "Enter correctGeometry: Building Has " << building->GetAllTransitions().size() << " Transitions\n" << RESET; + std::cout << "room: " << room_id << " subroom_id " << subroom_id << "\n" ; + if(mytrack.empty() || subroom == nullptr) return false; @@ -730,7 +753,7 @@ bool Simulation::correctGeometry(std::shared_ptr building, std::string subroom->GetUID(); auto walls = subroom->GetAllWalls(); // debugging - std::cout << "------\n"; + // std::cout << "------\n"; for(auto pw: pws) { auto pw1 = pw.first; @@ -739,9 +762,9 @@ bool Simulation::correctGeometry(std::shared_ptr building, std::string auto w1 = pw1.second; auto p2 = pw2.first; auto w2 = pw2.second; - std::cout << "p1 " << p1.toString() << ", wall: " << w1.toString() << "\n"; - std::cout << "p2 " << p2.toString() << ", wall: " << w2.toString() << "\n"; - std::cout << "------\n"; + // std::cout << "p1 " << p1.toString() << ", wall: " << w1.toString() << "\n"; + // std::cout << "p2 " << p2.toString() << ", wall: " << w2.toString() << "\n"; + // std::cout << "------\n"; // case 1 Point P; if(w1 == w2) @@ -758,10 +781,6 @@ bool Simulation::correctGeometry(std::shared_ptr building, std::string e->SetSubRoom1(subroom); subroom->AddTransition(e);// danger area building->AddTransition(e);// danger area - std::cout << "added transition\n"; - std::cout << "open: " << e->IsOpen() << "\n" ; - std::cout << "Close: " << e->IsClose() << "\n"; - std::cout << "TempClose: " << e->IsTempClose() << "\n" ; std::cout<< KGRN << "Transition added. Building Has " << building->GetAllTransitions().size() << " Transitions\n" << RESET; double dist_pt1 = (w1.GetPoint1() - e->GetPoint1()).NormSquare(); double dist_pt2 = (w1.GetPoint1() - e->GetPoint2()).NormSquare(); @@ -781,10 +800,10 @@ bool Simulation::correctGeometry(std::shared_ptr building, std::string Wall NewWall(w1.GetPoint1(), A); Wall NewWall1(w1.GetPoint2(), B); // add new lines to be controled against overlap with exits - building->TempAddedWalls.push_back(NewWall); - building->TempAddedWalls.push_back(NewWall1); - building->TempAddedDoors.push_back(*e); - building->TempRemovedWalls.push_back(w1); + building->TempAddedWalls[trainId].push_back(NewWall); + building->TempAddedWalls[trainId].push_back(NewWall1); + building->TempAddedDoors[trainId].push_back(*e); + building->TempRemovedWalls[trainId].push_back(w1); subroom->AddWall(NewWall); subroom->AddWall(NewWall1); subroom->RemoveWall(w1); @@ -963,31 +982,40 @@ int Simulation::GetMaxSimTime() const{ // return true is changes are made to the geometry bool Simulation::TrainTraffic() { - // here open transition that should be closed - // TODO fix, opens door everytime... bool trainHere = false; + bool trainLeave = false; std::string trainType = ""; Point trackStart, trackEnd; + int trainId = 0; auto now = Pedestrian::GetGlobalTime(); - static int once =1; for(auto && tab: TrainTimeTables) { - if( (now>=tab.second->tin) && (now<=tab.second->tout) ) + trainType = tab.second->type; + trainId = tab.second->id; + trackStart = tab.second->pstart; + trackEnd = tab.second->pend; + if(!tab.second->arrival && (now >= tab.second->tin) && (now <= tab.second->tout)) { trainHere = true; - trainType = tab.second->type; - trackStart = tab.second->pstart; - trackEnd = tab.second->pend; - continue; + TrainTimeTables.at(trainId)->arrival = true; + std::cout << KRED << "Arrival: TRAIN " << trainType << " at time: " << now << "\n" << RESET; + correctGeometry(_building, tab.second); + + } + else if(tab.second->arrival && now >= tab.second->tout) + { + std::cout <resetGeometry(tab.second); + trainLeave = true; + TrainTimeTables.at(trainId)->arrival = false; + } } // todo: correctgeometry on arrival of a train. Reset it on departure of train. - if(trainHere && once) + if(trainHere || trainLeave) { - correctGeometry(_building, trainType, trackStart, trackEnd); - std::cout << KRED << ">> update router\n" << RESET; - once=0; return true; } diff --git a/Simulation.h b/Simulation.h index 2d3aa41d967cb5ca3b4825b8f24440e2dabc653f..d8ed0718880f958d6c69ab7ef4a01802f0753f99 100644 --- a/Simulation.h +++ b/Simulation.h @@ -192,7 +192,7 @@ public: int GetMaxSimTime() const; void incrementCountTraj(); - bool correctGeometry(std::shared_ptr building, std::string trainType, Point TrackStart, Point TrackEnd); + bool correctGeometry(std::shared_ptr building, std::shared_ptr); bool WriteTrajectories(std::string trajName); bool TrainTraffic(); diff --git a/geometry/Building.cpp b/geometry/Building.cpp index 5950a9df551d2b020cc28fb2385e1de1a40a7f02..76951d1acbb8a2f958b24cba34d9c1ec898bf3da 100644 --- a/geometry/Building.cpp +++ b/geometry/Building.cpp @@ -410,7 +410,7 @@ const std::vector Building::GetTrackWalls(Point TrackStart, Point TrackEnd { track_id=track.first; int commonPoints = 0; - std::cout << "\t track " << track.first << "\n"; + // std::cout << "\t track " << track.first << "\n"; auto walls = track.second; for(auto wall: walls) { @@ -439,13 +439,15 @@ const std::vector Building::GetTrackWalls(Point TrackStart, Point TrackEnd room_id = _platforms.at(platform_id)->rid; subroom_id = _platforms.at(platform_id)->sid; mytrack = _platforms.at(platform_id)->tracks[track_id]; - std::cout << "track has walls: " << mytrack.size() << "\n"; - std::cout << "platform " << platform_id << " track " << track_id << "\n"; - std::cout << "room " << room_id << " subroom " << subroom_id << "\n"; + // std::cout << "track has walls: " << mytrack.size() << "\n"; + // std::cout << "platform " << platform_id << " track " << track_id << "\n"; + // std::cout << "room " << room_id << " subroom " << subroom_id << "\n"; } else { - std::cout << "could not find any track! \n"; + std::cout << "could not find any track! Exit.\n"; + exit(-1); + } return mytrack; } @@ -490,107 +492,144 @@ const std::vector > Building::GetIntersectionPo /* std::cout << "intersection at :" << interPoint2.toString() << "\n"; */ } } // tracks - std::cout << "door: " << door.toString() << ", intersections: " << nintersections << "\n"; + // std::cout << "door: " << door.toString() << ", intersections: " << nintersections << "\n"; if(nintersections == 2) pws.push_back(std::make_pair(pw1, pw2)); }// doors return pws; } -bool Building::resetTempVectors() +// reset changes made by trainTimeTable[id] +bool Building::resetGeometry(std::shared_ptr tab) { + // this function is composed of three copy/pasted blocks. int room_id, subroom_id; SubRoom * subroom; // remove temp added walls - for(auto wall: TempAddedWalls) + for(auto id_wall: TempAddedWalls) { - for (auto platform: _platforms) - { - auto tracks = platform.second->tracks; - room_id = platform.second->rid; - subroom_id = platform.second->sid; - for(auto r: GetAllRooms()) - { - if(r.first != room_id) - continue; - subroom = r.second->GetSubRoom(subroom_id); - } - - for (auto track : tracks) - { - auto walls = track.second; - for(auto trackWall : walls) - { - if (trackWall == wall) - { - std::cout<< "todo\n"; + int i = id_wall.first; + if(i != tab->id) continue; + auto tempWalls = id_wall.second; + for(auto wall: tempWalls) + { + for (auto platform: _platforms) + { + auto tracks = platform.second->tracks; + room_id = platform.second->rid; + subroom_id = platform.second->sid; + for(auto r: GetAllRooms()) + { + if(r.first != room_id) + continue; + subroom = r.second->GetSubRoom(subroom_id); + } + + for (auto track : tracks) + { + auto walls = track.second; + for(auto trackWall : walls) + { + if (trackWall == wall) + { subroom->RemoveWall(wall); - } - } - } - } + auto it = std::find(tempWalls.begin(), tempWalls.end(), wall); + if (it != tempWalls.end()) + { + // std::cout<< KGRN << "delete wall ..." << RESET <tracks; - room_id = platform.second->rid; - subroom_id = platform.second->sid; - for(auto r: GetAllRooms()) - { - if(r.first != room_id) - continue; - subroom = r.second->GetSubRoom(subroom_id); - } - for (auto track : tracks) - { - auto walls = track.second; - for(auto trackWall : walls) - { - if (trackWall == wall) - { - subroom->AddWall(wall); - std::cout<< "todo\n"; - } - } - } - } + int i = id_wall.first; + if(i != tab->id) continue; + auto tempWalls = id_wall.second; + + for(auto wall: tempWalls) + { + for (auto platform: _platforms) + { + auto tracks = platform.second->tracks; + room_id = platform.second->rid; + subroom_id = platform.second->sid; + for(auto r: GetAllRooms()) + { + if(r.first != room_id) + continue; + subroom = r.second->GetSubRoom(subroom_id); + } + for (auto track : tracks) + { + auto walls = track.second; + for(auto trackWall : walls) + { + if (trackWall == wall) + { + subroom->AddWall(wall); + auto it = std::find(tempWalls.begin(), tempWalls.end(), wall); + if (it != tempWalls.end()) + { + // std::cout<< KGRN << "delete wall ..." << RESET <tracks; - room_id = platform.second->rid; - subroom_id = platform.second->sid; - for(auto r: GetAllRooms()) - { if(r.first != room_id) - continue; - subroom = r.second->GetSubRoom(subroom_id); - } - - for (auto track : tracks) - { - auto walls = track.second; - for(auto trackWall : walls) - { - if (trackWall == door) - { + int i = id_wall.first; + if(i != tab->id) continue; + auto tempWalls = id_wall.second; + + for(auto door: tempWalls) + { + for (auto platform: _platforms) + { + auto tracks = platform.second->tracks; + room_id = platform.second->rid; + subroom_id = platform.second->sid; + for(auto r: GetAllRooms()) + { if(r.first != room_id) + continue; + subroom = r.second->GetSubRoom(subroom_id); + } + + for (auto track : tracks) + { + auto walls = track.second; + for(auto trackWall : walls) + { + if (trackWall == door) + { subroom->RemoveTransition(&door); - std::cout<< "todo\n"; - } - } - } - } + auto it = std::find(tempWalls.begin(), tempWalls.end(), door); + if (it != tempWalls.end()) + { + // std::cout<< KGRN << "delete wall ..." << RESET < TempAddedWalls; - std::vector TempRemovedWalls; - std::vector TempAddedDoors; - bool resetTempVectors(); - + std::map > TempAddedWalls; // map to trainTimeTable + std::map > TempRemovedWalls; + std::map > TempAddedDoors; // Building(const std::string &, const std::string &, RoutingEngine &, PedDistributor &, double); Building(Configuration* config, PedDistributor& pedDistributor); - + bool resetGeometry(std::shared_ptr tab); /// destructor virtual ~Building(); @@ -406,7 +406,7 @@ private: * @param subroom * @return bool */ -bool RemoveOverlappingDoors( + bool RemoveOverlappingDoors( const std::shared_ptr& subroom) const; /** @} */ // end of group diff --git a/routing/DirectionStrategy.cpp b/routing/DirectionStrategy.cpp index 1d0be8c56e55803804816b023a3073af2d17d5e6..fbe75b8fc88584b111c4d0cd7a529c1755810a1b 100644 --- a/routing/DirectionStrategy.cpp +++ b/routing/DirectionStrategy.cpp @@ -167,7 +167,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const #if DEBUG printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID()); - printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY()); + printf("nextPointOn Line: %f %f\n", NextPointOnLine._x, NextPointOnLine._y); #endif double dist; int inear = -1; @@ -186,7 +186,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const #if DEBUG printf("Check wall number %d. Dist = %f (%f)\n", i, dist, minDist); - printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY()); + printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1()._x,walls[i].GetPoint1()._y, walls[i].GetPoint2()._x,walls[i].GetPoint2()._y); #endif } @@ -207,7 +207,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const iObs = obs; #if DEBUG printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist); - printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY()); + printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1()._x,owalls[i].GetPoint1()._y, owalls[i].GetPoint2()._x,owalls[i].GetPoint2()._y); #endif } }//walls of obstacle @@ -226,7 +226,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const // angle = tmpDirection.GetDeviationAngle(owalls[inear].enlarge(2*ped->GetLargerAxis())); #if DEBUG - printf("COLLISION WITH OBSTACLE %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1().GetX(),owalls[inear].GetPoint1().GetY(), owalls[inear].GetPoint2().GetX(),owalls[inear].GetPoint2().GetY()); + printf("COLLISION WITH OBSTACLE %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1()._x,owalls[inear].GetPoint1()._y, owalls[inear].GetPoint2()._x,owalls[inear].GetPoint2()._y); #endif } //iObs @@ -234,7 +234,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const angle = tmpDirection.GetDeviationAngle(walls[inear].Enlarge(2*ped->GetLargerAxis())); #if DEBUG - printf("COLLISION WITH WALL %f %f --- %f %f\n===========\n",walls[inear].GetPoint1().GetX(),walls[inear].GetPoint1().GetY(), walls[inear].GetPoint2().GetX(),walls[inear].GetPoint2().GetY()); + printf("COLLISION WITH WALL %f %f --- %f %f\n===========\n",walls[inear].GetPoint1()._x,walls[inear].GetPoint1()._y, walls[inear].GetPoint2()._x,walls[inear].GetPoint2()._y); #endif } //else }//inear @@ -263,9 +263,9 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const #if DEBUG printf("inear=%d, iObs=%d, minDist=%f\n", inear, iObs, minDist); printf("PED=%d\n", ped->GetID()); - printf ("MC Posx = %.2f, Posy=%.2f, Lot=[%.2f, %.2f]\n", ped->GetPos().GetX(), ped->GetPos().GetY(), NextPointOnLine.GetX(), NextPointOnLine.GetY()); - printf("MC p1=[%.2f, %.2f] p2=[%.2f, %.2f]\n", p1.GetX(), p1.GetY(), p2.GetX(), p2.GetY()); - printf("angle=%f, G=[%.2f, %.2f]\n", angle, G.GetX(), G.GetY()); + printf ("MC Posx = %.2f, Posy=%.2f, Lot=[%.2f, %.2f]\n", ped->GetPos()._x, ped->GetPos()._y, NextPointOnLine._x, NextPointOnLine._y); + printf("MC p1=[%.2f, %.2f] p2=[%.2f, %.2f]\n", p1._x, p1._y, p2._x, p2._y); + printf("angle=%f, G=[%.2f, %.2f]\n", angle, G._x, G._y); printf("\n----------\nLEAVE function with PED=%d\n----------\n",ped->GetID()); // getc(stdin); @@ -292,7 +292,8 @@ Point DirectionFloorfield::GetTarget(Room* room, Pedestrian* ped) const { UNUSED(room); #if DEBUG - if (initDone && (ffviafm != nullptr)) { + + if (1) { #endif // DEBUG Point p; @@ -351,7 +352,7 @@ DirectionFloorfield::~DirectionFloorfield() { Point DirectionLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const { #if DEBUG - if (initDone && (ffviafm != nullptr)) { + if (1) { #endif // DEBUG Point p; @@ -367,6 +368,7 @@ Point DirectionLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const // if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0) { // p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()); // } + Point P = p + ped->GetPos(); return (p + ped->GetPos()); #if DEBUG @@ -374,7 +376,7 @@ Point DirectionLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const #endif // DEBUG //this should not execute: - //std::cerr << "Failure in DirectionFloorfield::GetTarget!!" << std::endl; + std::cerr << "Failure in DirectionFloorfield::GetTarget!!" << std::endl; // exit(EXIT_FAILURE); } @@ -456,7 +458,7 @@ Point DirectionSubLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const { (void)room; // silence warning #if DEBUG - if (initDone && (ffviafm != nullptr)) { + if (1) { #endif // DEBUG Point p; diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp index 1e1186b74e2501daf45d2bd67a37d700ab589e99..9a533d8754f6ae72e1ec75b149114da4b914a5cd 100644 --- a/routing/ff_router/ffRouter.cpp +++ b/routing/ff_router/ffRouter.cpp @@ -108,7 +108,7 @@ bool FFRouter::Init(Building* building) _goalToLineUIDmap = _globalFF->getGoalToLineUIDmap(); _goalToLineUIDmap2 = _globalFF->getGoalToLineUIDmap2(); _goalToLineUIDmap3 = _globalFF->getGoalToLineUIDmap3(); - //_globalFF->writeGoalFF("goal.vtk", goalIDs); + _globalFF->writeGoalFF("goal.vtk", goalIDs); } //get all door UIDs _allDoorUIDs.clear();