diff --git a/IO/GeoFileParser.cpp b/IO/GeoFileParser.cpp index 9592ce8df0f764dfb7fab0b1a7439098e84e1422..9594cb38295fc9469231f25c903e0b07e1e23def 100644 --- a/IO/GeoFileParser.cpp +++ b/IO/GeoFileParser.cpp @@ -50,6 +50,11 @@ void GeoFileParser::LoadBuilding(Building* building) Log->Write("ERROR:\t could not load extra traffic information!"); exit(EXIT_FAILURE); } + if(!LoadTrainInfo(building)) + { + Log->Write("ERROR:\t could not load train information!"); + exit(EXIT_FAILURE); + } } bool GeoFileParser::LoadGeometry(Building* building) @@ -199,6 +204,7 @@ bool GeoFileParser::LoadGeometry(Building* building) for (TiXmlElement* xPolyVertices = xSubRoom->FirstChildElement("polygon"); xPolyVertices; xPolyVertices = xPolyVertices->NextSiblingElement("polygon")) { + std::string wall_type = xmltoa(xPolyVertices->Attribute("type"), "wall"); for (TiXmlElement* xVertex = xPolyVertices->FirstChildElement( "vertex"); xVertex && xVertex!=xPolyVertices->LastChild("vertex"); @@ -208,7 +214,7 @@ bool GeoFileParser::LoadGeometry(Building* building) double y1 = xmltof(xVertex->Attribute("py")); double x2 = xmltof(xVertex->NextSiblingElement("vertex")->Attribute("px")); double y2 = xmltof(xVertex->NextSiblingElement("vertex")->Attribute("py")); - subroom->AddWall(Wall(Point(x1, y1), Point(x2, y2))); + subroom->AddWall(Wall(Point(x1, y1), Point(x2, y2), wall_type)); //printf("%0.2f %0.2f %0.2f %0.2f\n",x1,y1,x2,y2); } @@ -327,7 +333,7 @@ bool GeoFileParser::LoadGeometry(Building* building) } } else{ - Log->Write("INFO:\tNot parsing transition from file %s"); + Log->Write("INFO:\tNot parsing transition from file"); } Log->Write("INFO:\tGot %d transitions", building-> GetAllTransitions().size()); @@ -464,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; @@ -791,6 +797,220 @@ Goal* GeoFileParser::parseWaitingAreaNode(TiXmlElement * e) return wa; } +bool GeoFileParser::LoadTrainInfo(Building* building) +{ + Log->Write("--------\nINFO:\tLoading the train info"); + + TiXmlDocument doc(_configuration->GetProjectFile()); + if (!doc.LoadFile()) { + Log->Write("ERROR: \t%s", doc.ErrorDesc()); + Log->Write("ERROR: \t could not parse the project file"); + return false; + } + TiXmlElement* xRootNode = doc.RootElement(); + if (!xRootNode) { + Log->Write("ERROR:\tRoot element does not exist"); + return false; + } + if (!xRootNode->FirstChild("train_constraints")) { + Log->Write("WARNING:\tNo train constraints were found. Continue."); + } + bool resTTT = true; + bool resType = true; + if(xRootNode->FirstChild("train_constraints")) + { + resTTT = LoadTrainTimetable(building, xRootNode); + resType = LoadTrainType(building, xRootNode); + } + + return (resTTT && resType); +} +bool GeoFileParser::LoadTrainTimetable(Building* building, TiXmlElement * xRootNode) +{ + TiXmlNode* xTTTNode = xRootNode->FirstChild("train_constraints")->FirstChild("train_time_table"); + std::string TTTFilename; + if(xTTTNode) + { + fs::path p(_configuration->GetProjectRootDir()); + TTTFilename = xTTTNode->FirstChild()->ValueStr(); + p /= TTTFilename; + TTTFilename = p.string(); + Log->Write("INFO:\tTrain Timetable file <%s> will be parsed", TTTFilename.c_str()); + } + else return true; + + + TiXmlDocument docTTT(TTTFilename); + if (!docTTT.LoadFile()) { + Log->Write("ERROR: \t%s", docTTT.ErrorDesc()); + Log->Write("ERROR: \t could not parse the train timetable file."); + return false; + } + TiXmlElement* xTTT = docTTT.RootElement(); + if (!xTTT) { + Log->Write("ERROR:\tRoot element does not exist in TTT file."); + return false; + } + if (xTTT->ValueStr() != "train_time_table") { + Log->Write("ERROR:\tParsing train timetable file. Root element value is not 'train_time_table'."); + return false; + } + for (TiXmlElement* e = xTTT->FirstChildElement("train"); e; + e = e->NextSiblingElement("train")) { + std::shared_ptr TTT = parseTrainTimeTableNode(e); + + if (TTT) { + building->AddTrainTimeTable(TTT); + } + } + return true; +} +bool GeoFileParser::LoadTrainType(Building* building, TiXmlElement * xRootNode) +{ + TiXmlNode* xTTNode = xRootNode->FirstChild("train_constraints")->FirstChild("train_types"); + std::string TTFilename; + if(xTTNode) + { + fs::path p(_configuration->GetProjectRootDir()); + TTFilename = xTTNode->FirstChild()->ValueStr(); + p /= TTFilename; + TTFilename = p.string(); + Log->Write("INFO:\tTrain Type file <%s> will be parsed", TTFilename.c_str()); + } + else return true; + + + TiXmlDocument docTT(TTFilename); + if (!docTT.LoadFile()) { + Log->Write("ERROR: \t%s", docTT.ErrorDesc()); + Log->Write("ERROR: \t could not parse the train type file."); + return false; + } + TiXmlElement* xTT = docTT.RootElement(); + if (!xTT) { + Log->Write("ERROR:\tRoot element does not exist in TT file."); + return false; + } + if (xTT->ValueStr() != "train_type") { + Log->Write("ERROR:\tParsing train type file. Root element value is not 'train_type'."); + return false; + } + for (TiXmlElement* e = xTT->FirstChildElement("train"); e; + e = e->NextSiblingElement("train")) { + std::shared_ptr TT = parseTrainTypeNode(e); + if (TT) { + building->AddTrainType(TT); + } + } + return true; + +} + +std::shared_ptr GeoFileParser::parseTrainTimeTableNode(TiXmlElement * e) +{ + Log->Write("INFO:\tLoading train time table NODE"); + std::string caption = xmltoa(e->Attribute("caption"), "-1"); + int id = xmltoi(e->Attribute("id"), -1); + std::string type = xmltoa(e->Attribute("type"), "-1"); + int room_id = xmltoi(e->Attribute("room_id"), -1); + int subroom_id = xmltoi(e->Attribute("subroom_id"), -1); + int platform_id = xmltoi(e->Attribute("platform_id"), -1); + float track_start_x = xmltof(e->Attribute("track_start_x"), -1); + float track_start_y = xmltof(e->Attribute("track_start_y"), -1); + float track_end_x = xmltof(e->Attribute("track_end_x"), -1); + float track_end_y = xmltof(e->Attribute("track_end_y"), -1); + + float train_start_x = xmltof(e->Attribute("train_start_x"),-1); + float train_start_y = xmltof(e->Attribute("train_start_y"), -1); + float train_end_x = xmltof(e->Attribute("train_end_x"), -1); + float train_end_y = xmltof(e->Attribute("train_end_y"), -1); + + 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()); + Log->Write("INFO:\t room_id: %d", room_id); + Log->Write("INFO:\t subroom_id: %d", subroom_id); + Log->Write("INFO:\t platform_id: %d", platform_id); + Log->Write("INFO:\t track_start: [%.2f, %.2f]", track_start_x, track_start_y); + Log->Write("INFO:\t track_end: [%.2f, %.2f]", track_end_x, track_end_y); + Log->Write("INFO:\t arrival_time: %.2f", arrival_time); + Log->Write("INFO:\t departure_time: %.2f", departure_time); + Point track_start(track_start_x, track_start_y); + Point track_end(track_end_x, track_end_y); + Point train_start(train_start_x, train_start_y); + Point train_end(train_end_x, train_end_y); + std::shared_ptr trainTimeTab = std::make_shared( + TrainTimeTable{ + id, + type, + room_id, + subroom_id, + arrival_time, + departure_time, + track_start, + track_end, + train_start, + train_end, + platform_id, + false, + false, + }); + + return trainTimeTab; +} + +std::shared_ptr GeoFileParser::parseTrainTypeNode(TiXmlElement * e) +{ + Log->Write("INFO:\tLoading train type"); + // int T_id = xmltoi(e->Attribute("id"), -1); + std::string type = xmltoa(e->Attribute("type"), "-1"); + int agents_max = xmltoi(e->Attribute("agents_max"), -1); + float length = xmltof(e->Attribute("length"), -1); + // std::shared_ptr t = new Transition(); + // std::shared_ptr doors; + Transition t; + std::vector doors; + + for (TiXmlElement* xDoor = e->FirstChildElement("door"); xDoor; + xDoor = xDoor->NextSiblingElement("door")) { + int D_id = xmltoi(xDoor->Attribute("id"), -1); + float x1 = xmltof(xDoor->FirstChildElement("vertex")->Attribute("px"), -1); + float y1 = xmltof(xDoor->FirstChildElement("vertex")->Attribute("py"), -1); + float x2 = xmltof(xDoor->LastChild("vertex")->ToElement()->Attribute("px"), -1); + float y2 = xmltof(xDoor->LastChild("vertex")->ToElement()->Attribute("py"), -1); + Point start(x1, y1); + Point end(x2, y2); + float outflow = xmltof(xDoor->Attribute("outflow"), -1); + float dn = xmltoi(xDoor->Attribute("dn"), -1); + t.SetID(D_id); + t.SetCaption(type + std::to_string(D_id)); + t.SetPoint1(start); + t.SetPoint2(end); + t.SetOutflowRate(outflow); + t.SetDN(dn); + doors.push_back(t); + } + Log->Write("INFO:\t type: %s", type.c_str()); + Log->Write("INFO:\t capacity: %d", agents_max); + Log->Write("INFO:\t number of doors: %d", doors.size()); + for(auto d: doors) + { + Log->Write("INFO\t door (%d): %s | %s", d.GetID(), d.GetPoint1().toString().c_str(), d.GetPoint2().toString().c_str()); + } + + std::shared_ptr Type = std::make_shared( + TrainType{ + type, + agents_max, + length, + doors, + }); + return Type; + +} GeoFileParser::~GeoFileParser() { diff --git a/IO/GeoFileParser.h b/IO/GeoFileParser.h index 21b7b162011676d8f0d44e87a3c6d8dbc5ee1022..11b8787272ebaf542a949f2f24f653b214fd7640 100644 --- a/IO/GeoFileParser.h +++ b/IO/GeoFileParser.h @@ -44,6 +44,11 @@ public: Goal* parseGoalNode(TiXmlElement * e); Transition* parseTransitionNode(TiXmlElement * xTrans, Building * building); Goal* parseWaitingAreaNode(TiXmlElement * e); + bool LoadTrainInfo(Building* building); + bool LoadTrainTimetable(Building* building, TiXmlElement * xRootNode); + bool LoadTrainType(Building* building, TiXmlElement * xRootNode); + std::shared_ptr parseTrainTypeNode(TiXmlElement * e); + std::shared_ptr parseTrainTimeTableNode(TiXmlElement * e); private: Configuration* _configuration; diff --git a/IO/IODispatcher.cpp b/IO/IODispatcher.cpp index 97dc4d43942f3d61a9a7b7a7feb17798dec6be70..377c9873325c6bf0369b1ac074aa40255dc97cc6 100644 --- a/IO/IODispatcher.cpp +++ b/IO/IODispatcher.cpp @@ -370,16 +370,69 @@ std::string getEventFileName(const std::string & GetProjectFile) TiXmlNode* xMainNode = doc.RootElement(); string eventfile = ""; if (xMainNode->FirstChild("events_file")) { - ret = xMainNode->FirstChild("events_file")->FirstChild()->Value(); - Log->Write("INFO: \tevents <" + eventfile + ">"); + ret = xMainNode->FirstChild("events_file")->FirstChild()->ValueStr(); + Log->Write("INFO: \tevents <" + ret + ">"); } else { Log->Write("INFO: \tNo events found"); return ret; } return ret; } + // + // ttt.xml + // train_types.xml + // +std::string getTrainTimeTableFileName(const std::string & GetProjectFile) +{ + std::string ret=""; + + TiXmlDocument doc(GetProjectFile); + if (!doc.LoadFile()) { + Log->Write("ERROR: \t%s", doc.ErrorDesc()); + Log->Write("ERROR: \tGetTrainTimeTable could not parse the project file"); + return ret; + } + TiXmlNode* xMainNode = doc.RootElement(); + string tttfile = ""; + if (xMainNode->FirstChild("train_constraints")) { + TiXmlNode * xFileNode = xMainNode->FirstChild("train_constraints")->FirstChild("train_time_table"); + + if(xFileNode) + ret = xFileNode->FirstChild()->ValueStr(); + Log->Write("INFO: \ttrain_time_table <" + ret + ">"); + } else { + Log->Write("INFO: \tNo events no ttt file found"); + return ret; + } + return ret; +} + + +std::string getTrainTypeFileName(const std::string & GetProjectFile) +{ + std::string ret=""; + + TiXmlDocument doc(GetProjectFile); + if (!doc.LoadFile()) { + Log->Write("ERROR: \t%s", doc.ErrorDesc()); + Log->Write("ERROR: \tGetTrainType could not parse the project file"); + return ret; + } + TiXmlNode* xMainNode = doc.RootElement(); + string tttfile = ""; + if (xMainNode->FirstChild("train_constraints")) { + auto xFileNode = xMainNode->FirstChild("train_constraints")->FirstChild("train_types"); + if(xFileNode) + ret = xFileNode->FirstChild()->ValueStr(); + Log->Write("INFO: \ttrain_types <" + ret + ">"); + } else { + Log->Write("INFO: \tNo events no train types file found"); + return ret; + } + return ret; +} std::string getGoalFileName(const std::string & GetProjectFile) { std::string ret=""; @@ -411,6 +464,8 @@ void TrajectoriesFLAT::WriteHeader(long nPeds, double fps, Building* building, i std::string sourceFileName = getSourceFileName(building->GetProjectFilename()); std::string goalFileName = getGoalFileName(building->GetProjectFilename()); std::string eventFileName = getEventFileName(building->GetProjectFilename()); + std::string trainTimeTableFileName = getTrainTimeTableFileName(building->GetProjectFilename()); + std::string trainTypeFileName = getTrainTypeFileName(building->GetProjectFilename()); (void) seed; (void) nPeds; char tmp[100] = ""; sprintf(tmp, "#description: jpscore (%s)", JPSCORE_VERSION); @@ -436,6 +491,16 @@ void TrajectoriesFLAT::WriteHeader(long nPeds, double fps, Building* building, i sprintf(tmp,"#events: %s", eventFileName.c_str()); Write(tmp); } + if( trainTimeTableFileName != "") + { + sprintf(tmp,"#trainTimeTable: %s", trainTimeTableFileName.c_str()); + Write(tmp); + } + if( trainTypeFileName != "") + { + sprintf(tmp,"#trainType: %s", trainTypeFileName.c_str()); + Write(tmp); + } Write("#ID: the agent ID"); Write("#FR: the current frame"); Write("#X,Y,Z: the agents coordinates (in metres)"); diff --git a/IO/IniFileParser.cpp b/IO/IniFileParser.cpp index 433f9237b0093a812ca3de4386e494c0c2f2bca8..9cc4d50051128e23a803cb34fabf821997e5fe6b 100644 --- a/IO/IniFileParser.cpp +++ b/IO/IniFileParser.cpp @@ -1635,6 +1635,9 @@ bool IniFileParser::ParseStrategyNodeToObject(const TiXmlNode& strategyNode) }; _config->set_dirStrategy(dynamic_cast(_exit_strategy.get())); break; + case 12: + _exit_strategy = std::shared_ptr(new DirectionTrain()); + break; default: _exit_strategy = std::shared_ptr(new DirectionMinSeperationShorterLine()); diff --git a/Simulation.cpp b/Simulation.cpp index 954465b21d0532b24c9358187bc5501f1d252a90..87508c782d0ea1986823bf4ad516e416db4a7bfc 100644 --- a/Simulation.cpp +++ b/Simulation.cpp @@ -37,8 +37,6 @@ #include "pedestrian/AgentsQueue.h" #include "pedestrian/AgentsSourcesManager.h" #include "geometry/WaitingArea.h" -#include -namespace fs = std::filesystem; #ifdef _OPENMP @@ -46,12 +44,16 @@ namespace fs = std::filesystem; #define omp_get_thread_num() 0 #define omp_get_max_threads() 1 #endif - +namespace fs = std::filesystem; using namespace std; OutputHandler* Log; Trajectories* outputTXT; - +// todo: add these variables to class simulation +std::map > TrainTypes; +std::map > TrainTimeTables; +std::map trainOutflow; +//-------- Simulation::Simulation(Configuration* args) :_config(args) { @@ -68,6 +70,7 @@ Simulation::Simulation(Configuration* args) _fps = 1; _em = nullptr; _gotSources = false; + _trainConstraints = false; _maxSimTime = 100; // _config = args; } @@ -246,7 +249,35 @@ bool Simulation::InitArgs() if (!_operationalModel->Init(_building.get())) return false; Log->Write("INFO:\t Init Operational Model done"); + Log->Write("Got %d Train Types", _building->GetTrainTypes().size()); + for(auto&& TT: _building->GetTrainTypes()) + { + Log->Write("INFO\ttype : %s",TT.second->type.c_str()); + Log->Write("INFO\tMax : %d",TT.second->nmax); + Log->Write("INFO\tnumber doors : %d\n",TT.second->doors.size()); + } + if(_building->GetTrainTimeTables().size()) + Log->Write("INFO:\tGot %d Train Time Tables",_building->GetTrainTimeTables().size()); + else + Log->Write("WARNING:\tGot %d Train Time Tables",_building->GetTrainTimeTables().size()); + for(auto&& TT: _building->GetTrainTimeTables()) + { + Log->Write("INFO\tid : %d",TT.second->id); + Log->Write("INFO\ttype : %s",TT.second->type.c_str()); + Log->Write("INFO\troom id : %d",TT.second->rid); + Log->Write("INFO\ttin : %.2f%",TT.second->tin); + Log->Write("INFO\ttout : %.2f",TT.second->tout); + Log->Write("INFO\ttrack start : (%.2f, %.2f)",TT.second->pstart._x,TT.second->pstart._y); + Log->Write("INFO\ttrack end : (%.2f, %.2f)",TT.second->pend._x,TT.second->pend._y); + Log->Write("INFO\ttrain start : (%.2f, %.2f)",TT.second->tstart._x, TT.second->tstart._y); + Log->Write("INFO\ttrain end : (%.2f, %.2f)\n",TT.second->tend._x, TT.second->tend._y); + } + //@todo: these variables are global + TrainTypes = _building->GetTrainTypes(); + TrainTimeTables = _building->GetTrainTimeTables(); + _trainConstraints = (bool) TrainTimeTables.size(); + //----- // Give the DirectionStrategy the chance to perform some initialization. // This should be done after the initialization of the operationalModel // because then, invalid pedestrians have been deleted and FindExit() @@ -324,7 +355,7 @@ void Simulation::UpdateRoutesAndLocations() //set the new room if needed if ((ped->GetFinalDestination() == FINAL_DEST_OUT) - && (room->GetCaption() == "outside")) { //TODO Hier aendern fuer inside goals? + && (room->GetCaption() == "outside")) { //TODO Hier aendern fuer inside goals? #pragma omp critical(Simulation_Update_pedsToRemove) pedsToRemove.insert(ped); } else if ((ped->GetFinalDestination() != FINAL_DEST_OUT) @@ -358,7 +389,7 @@ void Simulation::UpdateRoutesAndLocations() if(_gotSources) ped->FindRoute(); //finally actualize the route - if ( !_gotSources && ped->FindRoute() == -1 ) { + if ( !_gotSources && ped->FindRoute() == -1 && !_trainConstraints) { //a destination could not be found for that pedestrian Log->Write("ERROR: \tCould not find a route for pedestrian %d in room %d and subroom %d", ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID()); @@ -529,6 +560,8 @@ double Simulation::RunBody(double maxSimTime) // main program loop while ((_nPeds || (!_agentSrcManager.IsCompleted()&& _gotSources) ) && tGetRouter(ROUTING_FF_QUICKEST)) { - FFRouter* ffrouter = dynamic_cast(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)); - if (ffrouter->MustReInit()) { - ffrouter->ReInit(); - ffrouter->SetRecalc(t); + if(geometryChanged) + { + // debug + fs::path f("tmp_"+std::to_string(t)+"_"+_config->GetGeometryFile()); + std::string filename = f.string(); + std::cout << "\nUpdate geometry. New geometry --> " << filename.c_str() << "\n"; + + std::cout<< KGRN << "Enter correctGeometry: Building Has " << _building->GetAllTransitions().size() << " Transitions\n" << RESET; + _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"); + } } + else{ // quickest needs update even if NeedsUpdate() is false + FFRouter* ffrouter = dynamic_cast(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)); + if(ffrouter != nullptr) + if (ffrouter->MustReInit()) { + ffrouter->ReInit(); + ffrouter->SetRecalc(t); + } } // here the used routers are update, when needed due to external changes if (_routingEngine->NeedsUpdate()){ + std::cout << KBLU << " Init router in simulation\n" << RESET; _routingEngine->UpdateRouter(); } @@ -571,29 +626,8 @@ double Simulation::RunBody(double maxSimTime) // write the trajectories if (0==frameNr%writeInterval) { - _iod->WriteFrame(frameNr/writeInterval, _building.get()); - - if(_config-> GetFileFormat() == FORMAT_PLAIN) - { - fs::path p = _config->GetTrajectoriesFile(); - int sf = fs::file_size(p); - if(sf>_maxFileSize*1024*1024) - { - std::string extention = p.extension().string(); - _countTraj++; - char tmp_traj_name[100]; - sprintf(tmp_traj_name,"%s_%.4d_%s", TrajectoryName.stem().string().c_str(), _countTraj, extention.c_str()); - _config->SetTrajectoriesFile(tmp_traj_name); - Log->Write("INFO:\tNew trajectory file <%s>", tmp_traj_name); - OutputHandler* file = new FileHandler(_config->GetTrajectoriesFile().c_str()); - outputTXT->SetOutputHandler(file); - -//_config->GetProjectRootDir()+"_1_"+_config->GetTrajectoriesFile()); - // _config->SetTrajectoriesFile(name); - _iod->WriteHeader(_nPeds, _fps, _building.get(), _seed, _countTraj); - // _iod->WriteGeometry(_building.get()); - } - } + _iod->WriteFrame(frameNr/writeInterval, _building.get()); + WriteTrajectories(TrajectoryName.stem().string()); } if(!_gotSources && !_periodic && _config->print_prog_bar()) @@ -623,8 +657,12 @@ double Simulation::RunBody(double maxSimTime) } #endif - // here open transition that should be closed - // TODO fix, opens door everytime... + //init train trainOutfloww + for (auto tab : TrainTimeTables) + { + trainOutflow[tab.first] = 0; + } + // regulate flow for (auto& itr: _building->GetAllTransitions()) { Transition* Trans = itr.second; @@ -632,26 +670,323 @@ double Simulation::RunBody(double maxSimTime) { if ((Trans->GetMaxDoorUsage() != (std::numeric_limits::max)()) || (Trans->GetOutflowRate() != (std::numeric_limits::max)()) ){ -// || (Trans->GetOutflowRate() != std::numeric_limits::max)){ - Trans->UpdateClosingTime( _deltaT); - if(Trans->GetClosingTime() <= _deltaT){ - Trans->changeTemporaryState(); + Trans->UpdateClosingTime( _deltaT); + if(Trans->GetClosingTime() <= _deltaT){ + Trans->changeTemporaryState(); } - Log-> Write("INFO:\tReset state of door %d, Time=%.2f", Trans->GetID(), Pedestrian::GetGlobalTime()); - } + }// normal transition } - } + //----------- + // regulate train doorusage + std::string transType = Trans->GetType(); + if (Trans->IsOpen() && transType.rfind("Train", 0) == 0) + { + std::vector strs; + boost::split(strs, transType, boost::is_any_of("_"),boost::token_compress_on); + int id = atoi(strs[1].c_str()); + std::string type = Trans->GetCaption(); + trainOutflow[id] += Trans->GetDoorUsage(); + if(trainOutflow[id] >= TrainTypes[type]->nmax) + { + std::cout << "INFO:\tclosing train door "<< transType.c_str() << " at "<< Pedestrian::GetGlobalTime() << " capacity " << TrainTypes[type]->nmax<< "\n"; + Log->Write("INFO:\tclosing train door %s at t=%.2f. Flow = %.2f (Train Capacity %d)", transType.c_str(), Pedestrian::GetGlobalTime(), trainOutflow[id], TrainTypes[type]->nmax); + Trans->Close(); + } + } + //----------- + }// Transitions if(frameNr % 1000 == 0) { - Log->Write("INFO:\tUpdate door statistics at t=%.2f", t); - PrintStatistics(t); + Log->Write("INFO:\tUpdate door statistics at t=%.2f", t); + PrintStatistics(t); } - - }// while time return t; } +bool Simulation::WriteTrajectories(std::string trajectoryName) +{ + if(_config-> GetFileFormat() == FORMAT_PLAIN) + { + fs::path p = _config->GetTrajectoriesFile(); + int sf = fs::file_size(p); + if(sf>_maxFileSize*1024*1024) + { + std::string extention = p.extension().string(); + this->incrementCountTraj(); + char tmp_traj_name[100]; + sprintf(tmp_traj_name,"%s_%.4d_%s", trajectoryName.c_str(), _countTraj, extention.c_str()); + _config->SetTrajectoriesFile(tmp_traj_name); + Log->Write("INFO:\tNew trajectory file <%s>", tmp_traj_name); + OutputHandler* file = new FileHandler(_config->GetTrajectoriesFile().c_str()); + outputTXT->SetOutputHandler(file); + +//_config->GetProjectRootDir()+"_1_"+_config->GetTrajectoriesFile()); + // _config->SetTrajectoriesFile(name); + _iod->WriteHeader(_nPeds, _fps, _building.get(), _seed, _countTraj); + // _iod->WriteGeometry(_building.get()); + } + } + return true; +} +// | | +// *-------------* <---door +// | | +// | | +// | | +// | | +// | | +//*-----x-------------x--------* <- wall +// | | + + +/* + * in this function this happens: +* remove walls +* add walls +* add doors +* set _routingEngine->setNeedUpdate(true); +*/ +bool Simulation::correctGeometry(std::shared_ptr building, std::shared_ptr tab) +{ + //auto platforms = building->GetPlatforms(); + int trainId = tab->id; + std::string trainType = tab->type; + Point TrackStart = tab->pstart; + Point TrainStart = tab->tstart; + 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); + if(subroom == nullptr) + { + Log->Write("ERROR:\t Simulation::correctGeometry got wrong room_id|subroom_id (%d|%d). TrainId %d", room_id, subroom_id, trainId); + exit(EXIT_FAILURE); + } + static int transition_id = 10000; // randomly high number + + 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; + + + auto train = building->GetTrainTypes().at(trainType); + auto doors = train->doors; + for(auto && d: doors) + { + auto newX = d.GetPoint1()._x + TrainStart._x + TrackStart._x; + auto newY = d.GetPoint1()._y + TrainStart._y + TrackStart._y; + d.SetPoint1(Point(newX, newY)); + newX = d.GetPoint2()._x + TrainStart._x + TrackStart._x; + newY = d.GetPoint2()._y + TrainStart._y + TrackStart._y; + d.SetPoint2(Point(newX, newY)); + } + for(auto d: doors) + { + Log->Write("Train %s %d. Transformed coordinates of doors: %s -- %s", trainType.c_str(), trainId, d.GetPoint1().toString().c_str(), d.GetPoint2().toString().c_str()); + } + + // std::vector > + auto pws = building->GetIntersectionPoints(doors, mytrack); + if(pws.empty()) + std::cout << KRED << "simulation::correctGeometry: pws are empty. Something went south with train doors\n" << RESET; + + auto walls = subroom->GetAllWalls(); + //--- + for(auto pw: pws) + { + auto pw1 = pw.first; + auto pw2 = pw.second; + auto p1 = pw1.first; + 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"; + // case 1 + Point P; + if(w1 == w2) + { + std::cout << "EQUAL\n"; + Transition* e = new Transition(); + e->SetID(transition_id++); + e->SetCaption(trainType); + e->SetPoint1(p1); + e->SetPoint2(p2); + std::string transType = "Train_"+std::to_string(tab->id)+"_"+std::to_string(tab->tin)+"_"+std::to_string(tab->tout); + e->SetType(transType); + room->AddTransitionID(e->GetUniqueID());// danger area + e->SetRoom1(room); + e->SetSubRoom1(subroom); + subroom->AddTransition(e);// danger area + building->AddTransition(e);// danger area + /* std::cout << KRED << "Trans added: " << e->toString() << "\n" << RESET; */ + + /* 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(); + Point A, B; + + if(dist_pt1GetPoint1(); + B = e->GetPoint2(); + } + else + { + A = e->GetPoint2(); + B = e->GetPoint1(); + } + + Wall NewWall(w1.GetPoint1(), A); + Wall NewWall1(w1.GetPoint2(), B); + NewWall.SetType(w1.GetType()); + NewWall1.SetType(w1.GetType()); + + // add new lines to be controled against overlap with exits + if(NewWall.GetLength() > J_EPS_DIST) + { + building->TempAddedWalls[trainId].push_back(NewWall); + subroom->AddWall(NewWall); + } + + else + std::cout << KRED << ">> WALL did not add: " << NewWall.toString() << "\n" << RESET ; + + if(NewWall1.GetLength() > J_EPS_DIST) + { + building->TempAddedWalls[trainId].push_back(NewWall1); + subroom->AddWall(NewWall1); + } + else + std::cout << KRED << ">> WALL did not add: " << NewWall1.toString() << "\n" << RESET ; + + building->TempAddedDoors[trainId].push_back(*e); + building->TempRemovedWalls[trainId].push_back(w1); + + subroom->RemoveWall(w1); + + + /* std::cout << KRED << "WALL added " << NewWall1.toString() << "\n" << RESET ; */ + /* std::cout << KRED << "WALL removed " << w1.toString() << "\n" << RESET ; */ + /* getc(stdin); */ + + //room->AddTransitionID(e->GetUniqueID()); + } + else if(w1.ShareCommonPointWith(w2, P)) + { + + std::cout << "ONE POINT COMON\n"; + //------------ transition -------- + Transition* e = new Transition(); + e->SetID(transition_id++); + e->SetCaption(trainType); + e->SetPoint1(p1); + e->SetPoint2(p2); + std::string transType = "Train_"+std::to_string(tab->id)+"_"+std::to_string(tab->tin)+"_"+std::to_string(tab->tout); + e->SetType(transType); + room->AddTransitionID(e->GetUniqueID());// danger area + e->SetRoom1(room); + e->SetSubRoom1(subroom); + + subroom->AddTransition(e);// danger area + building->AddTransition(e);// danger area + //-------------------------------- + Point N, M; + if(w1.GetPoint1()==P) + N = w1.GetPoint2(); + else + N = w1.GetPoint1(); + + if(w2.GetPoint1()==P) + M = w2.GetPoint2(); + else + M = w2.GetPoint1(); + + Wall NewWall(N, p1); + Wall NewWall1(M, p2); + NewWall.SetType(w1.GetType()); + NewWall1.SetType(w2.GetType()); + // changes to building + building->TempAddedWalls[trainId].push_back(NewWall); + building->TempAddedWalls[trainId].push_back(NewWall1); + building->TempAddedDoors[trainId].push_back(*e); + building->TempRemovedWalls[trainId].push_back(w1); + building->TempRemovedWalls[trainId].push_back(w2); + subroom->AddWall(NewWall); + subroom->AddWall(NewWall1); + subroom->RemoveWall(w1); + subroom->RemoveWall(w2); + /* std::cout << KRED << ". WALL added " << NewWall.toString() << "\n" << RESET ; */ + /* std::cout << KRED << "WALL added " << NewWall1.toString() << "\n" << RESET ; */ + /* std::cout << KRED << "WALL removed " << w1.toString() << "\n" << RESET ; */ + /* std::cout << KRED << "WALL removed " << w2.toString() << "\n" << RESET ; */ + /* getc(stdin); */ + } + else // disjoint + { + std::cout << "DISJOINT\n"; + //------------ transition -------- + Transition* e = new Transition(); + e->SetID(transition_id++); + e->SetCaption(trainType); + e->SetPoint1(p1); + e->SetPoint2(p2); + std::string transType = "Train_"+std::to_string(tab->id)+"_"+std::to_string(tab->tin)+"_"+std::to_string(tab->tout); + e->SetType(transType); + room->AddTransitionID(e->GetUniqueID());// danger area + e->SetRoom1(room); + e->SetSubRoom1(subroom); + + subroom->AddTransition(e);// danger area + building->AddTransition(e);// danger area + //-------------------------------- + // find points on w1 and w2 between p1 and p2 + // (A, B) + Point A, B; + if(e->isBetween(w1.GetPoint1())) + A = w1.GetPoint2(); + else + A = w1.GetPoint1(); + + if(e->isBetween(w2.GetPoint1())) + B = w2.GetPoint2(); + else + B = w2.GetPoint1(); + + Wall NewWall(A, p1); + Wall NewWall1(B, p2); + NewWall.SetType(w1.GetType()); + NewWall1.SetType(w2.GetType()); + // remove walls between + for(auto wall: mytrack) + { + if(e->isBetween(wall.GetPoint1()) || e->isBetween(wall.GetPoint2())) + { + building->TempRemovedWalls[trainId].push_back(wall); + subroom->RemoveWall(wall); + } + } + // changes to building + building->TempAddedWalls[trainId].push_back(NewWall); + building->TempAddedWalls[trainId].push_back(NewWall1); + building->TempAddedDoors[trainId].push_back(*e); + subroom->AddWall(NewWall); + subroom->AddWall(NewWall1); + + // remove walls w1 and w2 + } + } + _routingEngine->setNeedUpdate(true); + return true; + +} void Simulation::RunFooter() { // writing the footer @@ -780,6 +1115,11 @@ void Simulation::UpdateFlowAtDoors(const Pedestrian& ped) const } } +void Simulation::incrementCountTraj() +{ + _countTraj++; +} + AgentsSourcesManager& Simulation::GetAgentSrcManager() { return _agentSrcManager; @@ -793,3 +1133,45 @@ Building* Simulation::GetBuilding() int Simulation::GetMaxSimTime() const{ return _maxSimTime; } +// return true is changes are made to the geometry +bool Simulation::TrainTraffic() +{ + bool trainHere = false; + bool trainLeave = false; + std::string trainType = ""; + Point trackStart, trackEnd; + int trainId = 0; + auto now = Pedestrian::GetGlobalTime(); + for(auto && tab: TrainTimeTables) + { + 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; + 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; + + } + } + if(trainHere || trainLeave) + { + return true; + } + + /* std::cout<< KRED << "Check: Building Has " << _building->GetAllTransitions().size() << " Transitions\n" << RESET; */ + + return false; + +} diff --git a/Simulation.h b/Simulation.h index 28247849e9992e0a05e409cf4fc4587c92e7ed2e..d8ed0718880f958d6c69ab7ef4a01802f0753f99 100644 --- a/Simulation.h +++ b/Simulation.h @@ -49,6 +49,9 @@ #include "events/EventManager.h" #include "pedestrian/AgentsSourcesManager.h" #include "general/Configuration.h" +#include + + //Forward declarations //class AgentsSourcesManager; @@ -88,6 +91,8 @@ private: int _maxSimTime; bool _gotSources; // is true if we got some sources. Otherwise, false. + bool _trainConstraints; // true if inifile has some train constraints + // bool _printPB; // print progressbar public: /** @@ -185,6 +190,11 @@ public: */ void UpdateDoorticks() const; int GetMaxSimTime() const; + void incrementCountTraj(); + + bool correctGeometry(std::shared_ptr building, std::shared_ptr); + bool WriteTrajectories(std::string trajName); + bool TrainTraffic(); int _countTraj=0; // count number of TXT trajectories to produce double _maxFileSize; // in MB diff --git a/geometry/Building.cpp b/geometry/Building.cpp index 9121df654872850cbe58048fe280bd871a4bced9..0008a8b86df70388e8593bc51e842fb7d2bcd2c8 100644 --- a/geometry/Building.cpp +++ b/geometry/Building.cpp @@ -57,62 +57,62 @@ using namespace std; Building::Building() { - _caption = "no_caption"; - _geometryFilename = ""; - _routingEngine = nullptr; - _linkedCellGrid = nullptr; - _savePathway = false; + _caption = "no_caption"; + _geometryFilename = ""; + _routingEngine = nullptr; + _linkedCellGrid = nullptr; + _savePathway = false; } #ifdef _SIMULATOR Building::Building(Configuration* configuration, PedDistributor& pedDistributor) - :_configuration(configuration), - _routingEngine( - configuration->GetRoutingEngine()), - _caption("no_caption") +:_configuration(configuration), + _routingEngine( + configuration->GetRoutingEngine()), + _caption("no_caption") { - _savePathway = false; - _linkedCellGrid = nullptr; + _savePathway = false; + _linkedCellGrid = nullptr; #ifdef _JPS_AS_A_SERVICE - if (_configuration->GetRunAsService()) { - std::unique_ptr parser(new GeometryFromProtobufLoader(_configuration)); - parser->LoadBuilding(this); - } - else + if (_configuration->GetRunAsService()) { + std::unique_ptr parser(new GeometryFromProtobufLoader(_configuration)); + parser->LoadBuilding(this); + } + else #endif - { - std::unique_ptr parser(new GeoFileParser(_configuration)); - parser->LoadBuilding(this); - } + { + std::unique_ptr parser(new GeoFileParser(_configuration)); + parser->LoadBuilding(this); + } - if (!InitGeometry()) { - Log->Write("ERROR:\t could not initialize the geometry!"); - exit(EXIT_FAILURE); - } + if (!InitGeometry()) { + Log->Write("ERROR:\t could not initialize the geometry!"); + exit(EXIT_FAILURE); + } - //TODO: check whether traffic info can be loaded before InitGeometry if so call it in LoadBuilding instead and make - //TODO: LoadTrafficInfo private [gl march '16] + //TODO: check whether traffic info can be loaded before InitGeometry if so call it in LoadBuilding instead and make + //TODO: LoadTrafficInfo private [gl march '16] - if (!pedDistributor.Distribute(this)) { - Log->Write("ERROR:\tcould not distribute the pedestrians\n"); - exit(EXIT_FAILURE); - } - InitGrid(); + if (!pedDistributor.Distribute(this)) { + Log->Write("ERROR:\tcould not distribute the pedestrians\n"); + exit(EXIT_FAILURE); + } + InitGrid(); - if (!_routingEngine->Init(this)) { - Log->Write("ERROR:\t could not initialize the routers!"); - exit(EXIT_FAILURE); - } + if (!_routingEngine->Init(this)) { + Log->Write("ERROR:\t could not initialize the routers!"); + exit(EXIT_FAILURE); + } - if (!SanityCheck()) { - Log->Write("ERROR:\t There are sanity errors in the geometry file"); - exit(EXIT_FAILURE); - } + if (!SanityCheck()) { + Log->Write("ERROR:\t There are sanity errors in the geometry file"); + exit(EXIT_FAILURE); + } // SaveGeometry("/home/laemmel/Desktop/geo.xml"); } @@ -121,41 +121,41 @@ Building::Building(Configuration* configuration, PedDistributor& pedDistributor) Building::~Building() { - // - // for (int i = 0; i < GetNumberOfRooms(); i++) - // delete _rooms[i]; + // + // for (int i = 0; i < GetNumberOfRooms(); i++) + // delete _rooms[i]; #ifdef _SIMULATOR - for (unsigned int p = 0; p<_allPedestians.size(); p++) { - delete _allPedestians[p]; - } - _allPedestians.clear(); - delete _linkedCellGrid; + for (unsigned int p = 0; p<_allPedestians.size(); p++) { + delete _allPedestians[p]; + } + _allPedestians.clear(); + delete _linkedCellGrid; #endif - if (_pathWayStream.is_open()) - _pathWayStream.close(); - - for (map::const_iterator iter = _crossings.begin(); - iter!=_crossings.end(); ++iter) { - delete iter->second; - } - for (map::const_iterator iter = _transitions.begin(); - iter!=_transitions.end(); ++iter) { - delete iter->second; - } - for (map::const_iterator iter = _hLines.begin(); - iter!=_hLines.end(); ++iter) { - delete iter->second; - } - for (map::const_iterator iter = _goals.begin(); - iter!=_goals.end(); ++iter) { - delete iter->second; - } + if (_pathWayStream.is_open()) + _pathWayStream.close(); + + for (map::const_iterator iter = _crossings.begin(); + iter!=_crossings.end(); ++iter) { + delete iter->second; + } + for (map::const_iterator iter = _transitions.begin(); + iter!=_transitions.end(); ++iter) { + delete iter->second; + } + for (map::const_iterator iter = _hLines.begin(); + iter!=_hLines.end(); ++iter) { + delete iter->second; + } + for (map::const_iterator iter = _goals.begin(); + iter!=_goals.end(); ++iter) { + delete iter->second; + } } Configuration* Building::GetConfig() const { - return _configuration; + return _configuration; } ///************************************************************ @@ -163,251 +163,600 @@ Configuration* Building::GetConfig() const { // ************************************************************/ void Building::SetCaption(const std::string& s) { - _caption = s; + _caption = s; } /************************************************************* getters - ************************************************************/ +************************************************************/ string Building::GetCaption() const { - return _caption; + return _caption; } RoutingEngine* Building::GetRoutingEngine() const { - return _configuration->GetRoutingEngine().get(); + return _configuration->GetRoutingEngine().get(); } int Building::GetNumberOfRooms() const { - return (int) _rooms.size(); + return (int) _rooms.size(); } int Building::GetNumberOfGoals() const { - return (int) (_transitions.size()+_hLines.size()+_crossings.size()); + return (int) (_transitions.size()+_hLines.size()+_crossings.size()); } const std::map >& Building::GetAllRooms() const { - return _rooms; + return _rooms; } Room* Building::GetRoom(int index) const { - //todo: obsolete since the check is done by .at() - if (_rooms.count(index)==0) { - Log->Write("ERROR: Wrong 'index' in CBuiling::GetRoom() Room ID: %d size: %d", index, _rooms.size()); - Log->Write("\tControl your rooms ID and make sure they are in the order 0, 1, 2,.. "); - return nullptr; - } - //return _rooms[index]; - return _rooms.at(index).get(); + //todo: obsolete since the check is done by .at() + if (_rooms.count(index)==0) { + Log->Write("ERROR: Wrong 'index' in CBuiling::GetRoom() Room ID: %d size: %d", index, _rooms.size()); + Log->Write("\tControl your rooms ID and make sure they are in the order 0, 1, 2,.. "); + return nullptr; + } + //return _rooms[index]; + return _rooms.at(index).get(); } LCGrid* Building::GetGrid() const { - return _linkedCellGrid; + return _linkedCellGrid; } void Building::AddRoom(Room* room) { - _rooms[room->GetID()] = std::shared_ptr(room); + _rooms[room->GetID()] = std::shared_ptr(room); } void Building::AddSurroundingRoom() { - Log->Write("INFO: \tAdding the room 'outside' "); - // first look for the geometry boundaries - double x_min = FLT_MAX; - double x_max = -FLT_MAX; - double y_min = FLT_MAX; - double y_max = -FLT_MAX; - //finding the bounding of the grid - // and collect the pedestrians - - for (auto&& itr_room: _rooms) { - for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { - for (auto&& wall:itr_subroom.second->GetAllWalls()) { - double x1 = wall.GetPoint1()._x; - double y1 = wall.GetPoint1()._y; - double x2 = wall.GetPoint2()._x; - double y2 = wall.GetPoint2()._y; - - double xmax = (x1>x2) ? x1 : x2; - double xmin = (x1>x2) ? x2 : x1; - double ymax = (y1>y2) ? y1 : y2; - double ymin = (y1>y2) ? y2 : y1; - - x_min = (xmin<=x_min) ? xmin : x_min; - x_max = (xmax>=x_max) ? xmax : x_max; - y_max = (ymax>=y_max) ? ymax : y_max; - y_min = (ymin<=y_min) ? ymin : y_min; - } - } - } - - for (auto&& itr_goal:_goals) { - for (auto&& wall: itr_goal.second->GetAllWalls()) { - double x1 = wall.GetPoint1()._x; - double y1 = wall.GetPoint1()._y; - double x2 = wall.GetPoint2()._x; - double y2 = wall.GetPoint2()._y; - - double xmax = (x1>x2) ? x1 : x2; - double xmin = (x1>x2) ? x2 : x1; - double ymax = (y1>y2) ? y1 : y2; - double ymin = (y1>y2) ? y2 : y1; - - x_min = (xmin<=x_min) ? xmin : x_min; - x_max = (xmax>=x_max) ? xmax : x_max; - y_max = (ymax>=y_max) ? ymax : y_max; - y_min = (ymin<=y_min) ? ymin : y_min; - } - } - //make the grid slightly larger. - x_min = x_min-10.0; - x_max = x_max+10.0; - y_min = y_min-10.0; - y_max = y_max+10.0; - - SubRoom* bigSubroom = new NormalSubRoom(); - bigSubroom->SetRoomID((int) _rooms.size()); - bigSubroom->SetSubRoomID(0); // should be the single subroom - bigSubroom->AddWall(Wall(Point(x_min, y_min), Point(x_min, y_max))); - bigSubroom->AddWall(Wall(Point(x_min, y_max), Point(x_max, y_max))); - bigSubroom->AddWall(Wall(Point(x_max, y_max), Point(x_max, y_min))); - bigSubroom->AddWall(Wall(Point(x_max, y_min), Point(x_min, y_min))); - - Room* bigRoom = new Room(); - bigRoom->AddSubRoom(bigSubroom); - bigRoom->SetCaption("outside"); - bigRoom->SetID((int) _rooms.size()); - AddRoom(bigRoom); + Log->Write("INFO: \tAdding the room 'outside' "); + // first look for the geometry boundaries + double x_min = FLT_MAX; + double x_max = -FLT_MAX; + double y_min = FLT_MAX; + double y_max = -FLT_MAX; + //finding the bounding of the grid + // and collect the pedestrians + + for (auto&& itr_room: _rooms) { + for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { + for (auto&& wall:itr_subroom.second->GetAllWalls()) { + double x1 = wall.GetPoint1()._x; + double y1 = wall.GetPoint1()._y; + double x2 = wall.GetPoint2()._x; + double y2 = wall.GetPoint2()._y; + + double xmax = (x1>x2) ? x1 : x2; + double xmin = (x1>x2) ? x2 : x1; + double ymax = (y1>y2) ? y1 : y2; + double ymin = (y1>y2) ? y2 : y1; + + x_min = (xmin<=x_min) ? xmin : x_min; + x_max = (xmax>=x_max) ? xmax : x_max; + y_max = (ymax>=y_max) ? ymax : y_max; + y_min = (ymin<=y_min) ? ymin : y_min; + } + } + } + + for (auto&& itr_goal:_goals) { + for (auto&& wall: itr_goal.second->GetAllWalls()) { + double x1 = wall.GetPoint1()._x; + double y1 = wall.GetPoint1()._y; + double x2 = wall.GetPoint2()._x; + double y2 = wall.GetPoint2()._y; + + double xmax = (x1>x2) ? x1 : x2; + double xmin = (x1>x2) ? x2 : x1; + double ymax = (y1>y2) ? y1 : y2; + double ymin = (y1>y2) ? y2 : y1; + + x_min = (xmin<=x_min) ? xmin : x_min; + x_max = (xmax>=x_max) ? xmax : x_max; + y_max = (ymax>=y_max) ? ymax : y_max; + y_min = (ymin<=y_min) ? ymin : y_min; + } + } + //make the grid slightly larger. + x_min = x_min-10.0; + x_max = x_max+10.0; + y_min = y_min-10.0; + y_max = y_max+10.0; + + SubRoom* bigSubroom = new NormalSubRoom(); + bigSubroom->SetRoomID((int) _rooms.size()); + bigSubroom->SetSubRoomID(0); // should be the single subroom + bigSubroom->AddWall(Wall(Point(x_min, y_min), Point(x_min, y_max))); + bigSubroom->AddWall(Wall(Point(x_min, y_max), Point(x_max, y_max))); + bigSubroom->AddWall(Wall(Point(x_max, y_max), Point(x_max, y_min))); + bigSubroom->AddWall(Wall(Point(x_max, y_min), Point(x_min, y_min))); + + Room* bigRoom = new Room(); + bigRoom->AddSubRoom(bigSubroom); + bigRoom->SetCaption("outside"); + bigRoom->SetID((int) _rooms.size()); + AddRoom(bigRoom); } bool Building::InitGeometry() { - Log->Write("INFO: \tInit Geometry"); - correct(); - for (auto&& itr_room: _rooms) { - for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { - //create a close polyline out of everything - vector goals = vector(); - - // collect all crossings - for (auto&& cros:itr_subroom.second->GetAllCrossings()) { - goals.push_back(cros); - } - //collect all transitions - for (auto&& trans:itr_subroom.second->GetAllTransitions()) { - goals.push_back(trans); - } - // initialize the poly - if (!itr_subroom.second->ConvertLineToPoly(goals)) - return false; - itr_subroom.second->CalculateArea(); - - //do the same for the obstacles that are closed - for (auto&& obst:itr_subroom.second->GetAllObstacles()) { - //if (obst->GetClosed() == 1) - if (!obst->ConvertLineToPoly()) - return false; - } - - //here we can create a boost::geometry::model::polygon out of the vector objects created above - itr_subroom.second->CreateBoostPoly(); - - double minElevation = FLT_MAX; - double maxElevation = -FLT_MAX; - for (auto&& wall:itr_subroom.second->GetAllWalls()) { - const Point& P1 = wall.GetPoint1(); - const Point& P2 = wall.GetPoint2(); - if (minElevation>itr_subroom.second->GetElevation(P1)) { - minElevation = itr_subroom.second->GetElevation(P1); - } - - if (maxElevationGetElevation(P1)) { - maxElevation = itr_subroom.second->GetElevation(P1); - } - - if (minElevation>itr_subroom.second->GetElevation(P2)) { - minElevation = itr_subroom.second->GetElevation(P2); - } - - if (maxElevationGetElevation(P2)) { - maxElevation = itr_subroom.second->GetElevation(P2); - } - } - itr_subroom.second->SetMaxElevation(maxElevation); - itr_subroom.second->SetMinElevation(minElevation); - } - } - - - // look and save the neighbor subroom for improving the runtime - // that information is already present in the crossing/transitions - - for (const auto& cross: _crossings) { - SubRoom* s1 = cross.second->GetSubRoom1(); - SubRoom* s2 = cross.second->GetSubRoom2(); - if (s1) s1->AddNeighbor(s2); - if (s2) s2->AddNeighbor(s1); - } - - for (const auto& trans: _transitions) { - SubRoom* s1 = trans.second->GetSubRoom1(); - SubRoom* s2 = trans.second->GetSubRoom2(); - if (s1) s1->AddNeighbor(s2); - if (s2) s2->AddNeighbor(s1); - } - - InitInsideGoals(); - Log->Write("INFO: \tInit Geometry successful!!!\n"); - - for (auto& transItr : _transitions){ - Transition* trans = transItr.second; - std::cout << trans->GetID() << " is open " << trans->IsOpen() << std::endl; - std::cout << trans->GetID() << " is close " << trans->IsClose() << std::endl; - std::cout << trans->GetID() << " is temp_close " << trans->IsTempClose() << std::endl; - - } - return true; -} - -bool Building::InitInsideGoals() + Log->Write("INFO: \tInit Geometry"); + correct(); + for (auto&& itr_room: _rooms) { + for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { + //create a close polyline out of everything + vector goals = vector(); + + // collect all crossings + for (auto&& cros:itr_subroom.second->GetAllCrossings()) { + goals.push_back(cros); + } + //collect all transitions + for (auto&& trans:itr_subroom.second->GetAllTransitions()) { + goals.push_back(trans); + } + // initialize the poly + if (!itr_subroom.second->ConvertLineToPoly(goals)) + return false; + itr_subroom.second->CalculateArea(); + + //do the same for the obstacles that are closed + for (auto&& obst:itr_subroom.second->GetAllObstacles()) { + //if (obst->GetClosed() == 1) + if (!obst->ConvertLineToPoly()) + return false; + } + + //here we can create a boost::geometry::model::polygon out of the vector objects created above + itr_subroom.second->CreateBoostPoly(); + + double minElevation = FLT_MAX; + double maxElevation = -FLT_MAX; + for (auto&& wall:itr_subroom.second->GetAllWalls()) { + const Point& P1 = wall.GetPoint1(); + const Point& P2 = wall.GetPoint2(); + if (minElevation>itr_subroom.second->GetElevation(P1)) { + minElevation = itr_subroom.second->GetElevation(P1); + } + + if (maxElevationGetElevation(P1)) { + maxElevation = itr_subroom.second->GetElevation(P1); + } + + if (minElevation>itr_subroom.second->GetElevation(P2)) { + minElevation = itr_subroom.second->GetElevation(P2); + } + + if (maxElevationGetElevation(P2)) { + maxElevation = itr_subroom.second->GetElevation(P2); + } + } + itr_subroom.second->SetMaxElevation(maxElevation); + itr_subroom.second->SetMinElevation(minElevation); + } + } + + + // look and save the neighbor subroom for improving the runtime + // that information is already present in the crossing/transitions + + for (const auto& cross: _crossings) { + SubRoom* s1 = cross.second->GetSubRoom1(); + SubRoom* s2 = cross.second->GetSubRoom2(); + if (s1) s1->AddNeighbor(s2); + if (s2) s2->AddNeighbor(s1); + } + + for (const auto& trans: _transitions) { + SubRoom* s1 = trans.second->GetSubRoom1(); + SubRoom* s2 = trans.second->GetSubRoom2(); + if (s1) s1->AddNeighbor(s2); + if (s2) s2->AddNeighbor(s1); + } + + InitInsideGoals(); + InitPlatforms(); + //--- + for (auto platform: _platforms) + { + std::cout << "\n platform " << platform.first << ", "<< platform.second->id << "\n"; + std::cout << "\t rid " << platform.second->rid << "\n"; + auto tracks = platform.second->tracks; + for(auto track: tracks) + { + std::cout << "\t track " << track.first << "\n"; + auto walls = track.second; + for(auto wall: walls) + { + std::cout << "\t\t wall: " << wall.GetType() << ". " << wall.GetPoint1().toString() << " | " << wall.GetPoint2().toString() << "\n"; + } + } + + } + Log->Write("INFO: \tInit Geometry successful!!!\n"); + + // for (auto& transItr : _transitions){ + // Transition* trans = transItr.second; + // std::cout << trans->GetID() << " is open " << trans->IsOpen() << std::endl; + // std::cout << trans->GetID() << " is close " << trans->IsClose() << std::endl; + // std::cout << trans->GetID() << " is temp_close " << trans->IsTempClose() << std::endl; + + // } + + + return true; +} +const std::vector Building::GetTrackWalls(Point TrackStart, Point TrackEnd, int & room_id, int & subroom_id) const +{ + bool trackFound = false; + int track_id = -1; + int platform_id = -1; + std::vector mytrack; + for(auto platform: _platforms) + { + platform_id = platform.second->id; + auto tracks = platform.second->tracks; + for(auto track: tracks) + { + track_id=track.first; + int commonPoints = 0; + // std::cout << "\t track " << track.first << "\n"; + auto walls = track.second; + for(auto wall: walls) + { + Point P1 = wall.GetPoint1(); + Point P2 = wall.GetPoint2(); + //std::cout << "\t\t wall: " << wall.GetType() << ". " << wall.GetPoint1().toString() << " | " << wall.GetPoint2().toString() << "\n"; + if (P1 == TrackStart) + commonPoints++; + if(P1 == TrackEnd) + commonPoints++; + if(P2 == TrackStart) + commonPoints++; + if(P2 == TrackEnd) + commonPoints++; + } + if(commonPoints == 2 ) + { + trackFound = true; + break; + } + } // tracks + if (trackFound) break; + } // plattforms + if(trackFound) + { + 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"; + } + else + { + std::cout << "could not find any track! Exit.\n"; + exit(-1); + + } + return mytrack; +} + +const std::vector > Building::GetIntersectionPoints(const std::vector doors, const std::vector mytrack) const +{ + const int scaleFactor = 1000; // very long orthogonal walls to train's doors + std::vector > pws; + // every door has two points. + // for every point -> get pair + // collect pairs of pairs + for(auto door: doors) + { + // std::cout << "================================\n"; + // std::cout << "door: " << door.toString() << "\n"; + PointWall pw1, pw2; + int nintersections = 0; + auto n = door.NormalVec(); + auto p11 = door.GetPoint1() + n*scaleFactor; + auto p12 = door.GetPoint1() - n*scaleFactor; + auto p21 = door.GetPoint2() + n*scaleFactor; + auto p22 = door.GetPoint2() - n*scaleFactor; + auto normalWall1 = Wall(p11, p12); + auto normalWall2 = Wall(p21, p22); + // std::cout << "normal wall 1: " << normalWall1.toString() << "\n"; + // std::cout << "normal wall 2: " << normalWall2.toString() << "\n"; + for(auto twall: mytrack) + { + + Point interPoint1, interPoint2; + auto res = normalWall1.IntersectionWith(twall, interPoint1); + auto res2 = normalWall2.IntersectionWith(twall, interPoint2); + // std::cout << " res " << res << " res2 " << res2 << "\n"; + if(res == 1) + { + if(!twall.NearlyHasEndPoint(interPoint1)) + { + // std::cout << "res twall " << twall.toString() << "\n"; + pw1 = std::make_pair(interPoint1, twall); + nintersections++; + // std::cout << "intersection at :" << interPoint1.toString() << "\n"; + } + else // end point + { + if(res2 == 0) + { + + // std::cout << "twall " << twall.toString() << "\n"; + // std::cout << "YY intersection 1 at :" << interPoint1.toString() << "\n"; + // std::cout << "YY intersection 2 at :" << interPoint2.toString() << "\n"; + + } + else{ + + // std::cout << "res twall " << twall.toString() << "\n"; + pw1 = std::make_pair(interPoint1, twall); + nintersections++; + // std::cout << "BB: intersection at :" << interPoint1.toString() << "\n"; + } + } + } + if(res2 == 1) + { + if(!twall.NearlyHasEndPoint(interPoint2)) + { + // std::cout << "res2 twall " << twall.toString() << "\n"; + pw2 = std::make_pair(interPoint2, twall); + nintersections++; + // std::cout << "intersection at :" << interPoint2.toString() << "\n"; + } + else + { + if(res == 0) + { + // std::cout << "twall " << twall.toString() << "\n"; + // std::cout << "XX intersection 1 at :" << interPoint1.toString() << "\n"; + // std::cout << "XX intersection 2 at :" << interPoint2.toString() << "\n"; + } + else{ + // std::cout << "res2 twall " << twall.toString() << "\n"; + pw2 = std::make_pair(interPoint2, twall); + nintersections++; + // std::cout << "CC intersection at :" << interPoint2.toString() << "\n"; + } + } + } + // std::cout << "door: " << door.toString() << ", intersections: " << nintersections << "\n"; + + + } // tracks + // std::cout << "door: " << door.toString() << ", intersections: " << nintersections << "\n"; + // std::cout << "================================\n"; + + if(nintersections == 2) + pws.push_back(std::make_pair(pw1, pw2)); + + else + { + std::cout << KRED << "Error in GetIntersection. Should be 2 but got " << nintersections << "\n"; + exit(-1); + } + }// doors + // getc(stdin); + + return pws; +} +// 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; + + // std::cout << "enter resetGeometry with tab id: " << tab->id << "\n" ; + // std::cout << "temp Added Walls\n"; + // for(auto id_wall: TempAddedWalls) + // { + // std::cout << "i " << id_wall.first << "\n"; + // auto walls = id_wall.second; + // for(auto wall: walls ) + // std::cout << wall.toString() << "\n"; + // } + // std::cout << "temp Removed Walls\n"; + // for(auto id_wall: TempRemovedWalls) + // { + // std::cout << "i " << id_wall.first << "\n"; + // auto walls = id_wall.second; + // for(auto wall: walls ) + // std::cout << wall.toString() << "\n"; + // } + // std::cout << "temp Added Doors\n"; + // for(auto id_wall: TempAddedDoors) + // { + // std::cout << "i " << id_wall.first << "\n"; + // auto walls = id_wall.second; + // for(auto wall: walls ) + // std::cout << wall.toString() << "\n"; + // } + // getc(stdin); + + // remove temp added walls + auto tempWalls = TempAddedWalls[tab->id]; + for(auto it=tempWalls.begin(); it!=tempWalls.end(); ) + { + auto wall = *it; + if (it != tempWalls.end()) + { + tempWalls.erase(it); + } + for (auto platform: _platforms) + { + // auto tracks = platform.second->tracks; + room_id = platform.second->rid; + subroom_id = platform.second->sid; + // auto room = this->GetAllRooms().at(room_id); + SubRoom * subroom = this->GetAllRooms().at(room_id)->GetAllSubRooms().at(subroom_id).get(); + // std::cout << "----\n"; + // for(auto subWall: subroom->GetAllWalls()) + // std::cout << ">> "<< subWall.toString() << "\n"; + // std::cout << "----\n"; + for(auto subWall: subroom->GetAllWalls()) + { + if(subWall == wall) + { + // if everything goes right, then we should enter this + // if. We already erased from tempWalls! + subroom->RemoveWall(wall); + // std::cout << KGRN << "RESET REMOVE wall " << wall.toString() << "\n" << RESET; + }//if + }//subroom + }//platforms + } + TempAddedWalls[tab->id] = tempWalls; + +/* // add remove walls */ + auto tempRemovedWalls = TempRemovedWalls[tab->id]; + for(auto it=tempRemovedWalls.begin(); it!=tempRemovedWalls.end(); ) + { + auto wall = *it; + if (it != tempRemovedWalls.end()) + { + tempRemovedWalls.erase(it); + } + for (auto platform: _platforms) + { + auto tracks = platform.second->tracks; + room_id = platform.second->rid; + subroom_id = platform.second->sid; + SubRoom * subroom = this->GetAllRooms().at(room_id)->GetAllSubRooms().at(subroom_id).get(); + for (auto track : tracks) + { + auto walls = track.second; + for(auto trackWall : walls) + { + if (trackWall == wall) + { + subroom->AddWall(wall); + // std::cout << KGRN << "ADD BACK wall " << wall.toString() << "\n" << RESET; + } + } + } + } + } + TempRemovedWalls[tab->id] = tempRemovedWalls; + +/* // remove added doors */ + auto tempDoors = TempAddedDoors[tab->id]; + for(auto it=tempDoors.begin(); it!=tempDoors.end(); ) + { + auto door = *it; + if (it != tempDoors.end()) + { + tempDoors.erase(it); + } + for (auto platform: _platforms) + { + auto tracks = platform.second->tracks; + room_id = platform.second->rid; + subroom_id = platform.second->sid; + SubRoom * subroom = this->GetAllRooms().at(room_id)->GetAllSubRooms().at(subroom_id).get(); + for(auto subTrans: subroom->GetAllTransitions()) + { + if (*subTrans == door) + { + // Trnasitions are added to subrooms and building!! + subroom->RemoveTransition(subTrans); + this->RemoveTransition(subTrans); + // std::cout << KGRN << "RESET remove door " << door.toString() << "\n" << RESET; + } + + } + } + } + TempAddedDoors[tab->id] = tempDoors; + // std::cout << "leave resetGeometry with tab id: " << tab->id << "\n" ; + // std::cout << "temp Added Walls: " << TempAddedWalls[tab->id].size() << "\n"; + // std::cout << "temp Removed Walls: " << TempRemovedWalls[tab->id].size()<< "\n"; + // std::cout << "temp Added Doors: " <id].size() << "\n"; + return true; +} +bool Building::InitPlatforms() { - bool found = false; - for (auto& goalItr : _goals){ - Goal* goal = goalItr.second; - if (goal->GetRoomID() == -1){ - found = true; - std::cout << "Goal " << goal->GetId() << " is outside" << std::endl; - continue; - } - - for (auto& roomItr : _rooms){ - Room* room = roomItr.second.get(); - - if (goal->GetRoomID() != room->GetID()){ - continue; - } - - for (auto& subRoomItr : room->GetAllSubRooms()){ - SubRoom* subRoom = subRoomItr.second.get(); - - if ((goal->GetSubRoomID() == subRoom->GetSubRoomID()) && (subRoom->IsInSubRoom(goal->GetCentroid()))){ - std::cout << "Goal " << goal->GetId() << " is in subroom " << subRoom->GetUID() << std::endl; - Crossing* crossing = goal->GetCentreCrossing(); - subRoom->AddCrossing(crossing); - crossing->SetRoom1(room); - crossing->SetSubRoom1(subRoom); - crossing->SetSubRoom2(subRoom); - AddCrossing(crossing); - found = true; - break; + int num_platform = -1; + for (auto& roomItr : _rooms) + { + Room* room = roomItr.second.get(); + for (auto& subRoomItr : room->GetAllSubRooms()) + { + auto subRoom = subRoomItr.second.get(); + int subroom_id = subRoom->GetSubRoomID(); + if(subRoom->GetType() != "Platform" ) continue; + std::map > tracks; + num_platform++; + for (auto&& wall:subRoom->GetAllWalls()) + { + if(wall.GetType().find("track") == std::string::npos) continue; + // add wall to track + std::vector strs; + boost::split(strs, wall.GetType(), boost::is_any_of("-"),boost::token_compress_on); + if(strs.size() <= 1) continue; + int n = atoi(strs[1].c_str()); + /* std::cout << "caption: " << wall.GetCaption().c_str() << " > " << n << "\n"; */ + /* if(tracks.count[n] == 0) */ + /* { */ + tracks[n].push_back(wall); + /* } */ + } //walls + std::shared_ptr p = std::make_shared( + Platform{ + num_platform, + room->GetID(), + subroom_id, + tracks, + }); + AddPlatform(p); + + }//subroom + }//rooms +} + bool Building::InitInsideGoals() + { + bool found = false; + for (auto& goalItr : _goals){ + Goal* goal = goalItr.second; + if (goal->GetRoomID() == -1){ + found = true; + std::cout << "Goal " << goal->GetId() << " is outside" << std::endl; + continue; + } + + for (auto& roomItr : _rooms){ + Room* room = roomItr.second.get(); + + if (goal->GetRoomID() != room->GetID()){ + continue; + } + + for (auto& subRoomItr : room->GetAllSubRooms()){ + SubRoom* subRoom = subRoomItr.second.get(); + + if ((goal->GetSubRoomID() == subRoom->GetSubRoomID()) && (subRoom->IsInSubRoom(goal->GetCentroid()))){ + std::cout << "Goal " << goal->GetId() << " is in subroom " << subRoom->GetUID() << std::endl; + Crossing* crossing = goal->GetCentreCrossing(); + subRoom->AddCrossing(crossing); + crossing->SetRoom1(room); + crossing->SetSubRoom1(subRoom); + crossing->SetSubRoom2(subRoom); + AddCrossing(crossing); + found = true; + break; // for (auto& cross : subRoom->GetAllCrossings()){ // std::cout << "Crossing Subroom: " << cross->GetUniqueID() << std::endl; // } @@ -416,139 +765,139 @@ bool Building::InitInsideGoals() // std::cout << "Crossing Building: " << cross.second->GetUniqueID() << std::endl; // } - } - } - } + } + } + } - if (!found){ - Log->Write("Warning: \t Goal %d seems to have no subroom and is not outside, please check your input", - goal->GetId()); - } - found = false; - } + if (!found){ + Log->Write("Warning: \t Goal %d seems to have no subroom and is not outside, please check your input", + goal->GetId()); + } + found = false; + } - Log->Write("INFO: \tInitInsideGoals successful!!!\n"); + Log->Write("INFO: \tInitInsideGoals successful!!!\n"); - return true; -} + return true; + } bool Building::correct() const { - auto t_start = std::chrono::high_resolution_clock::now(); - Log->Write("INFO:\tenter correct ..."); - bool removed = false; - - for(auto&& room: this->GetAllRooms()) { - for(auto&& subroom: room.second->GetAllSubRooms()) { - // -- remove exits *on* walls - removed = RemoveOverlappingDoors(subroom.second); - // -------------------------- - // -- remove overlapping walls - auto walls = subroom.second->GetAllWalls(); // this call - // should be - // after - // eliminating - // nasty exits + auto t_start = std::chrono::high_resolution_clock::now(); + Log->Write("INFO:\tenter correct ..."); + bool removed = false; + + for(auto&& room: this->GetAllRooms()) { + for(auto&& subroom: room.second->GetAllSubRooms()) { + // -- remove exits *on* walls + removed = RemoveOverlappingDoors(subroom.second); + // -------------------------- + // -- remove overlapping walls + auto walls = subroom.second->GetAllWalls(); // this call + // should be + // after + // eliminating + // nasty exits #if DEBUG - std::cout<< "\n" << KRED << "correct Room " << room.first << " Subroom " << subroom.first << RESET << std::endl; + std::cout<< "\n" << KRED << "correct Room " << room.first << " Subroom " << subroom.first << RESET << std::endl; #endif - for(auto const & bigWall: walls) //self checking - { - // std::cout << "BigWall: " << std::endl; - // bigWall.WriteToErrorLog(); - //special treatment for doors + for(auto const & bigWall: walls) //self checking + { + // std::cout << "BigWall: " << std::endl; + // bigWall.WriteToErrorLog(); + //special treatment for doors ///// - std::vector WallPieces; - WallPieces = SplitWall(subroom.second, bigWall); - if(!WallPieces.empty()) - removed = true; + std::vector WallPieces; + WallPieces = SplitWall(subroom.second, bigWall); + if(!WallPieces.empty()) + removed = true; #if DEBUG -z std::cout << "Wall pieces size : " << WallPieces.size() << std::endl; - for(auto w:WallPieces) - w.WriteToErrorLog(); + z std::cout << "Wall pieces size : " << WallPieces.size() << std::endl; + for(auto w:WallPieces) + w.WriteToErrorLog(); #endif - int ok=0; - while(!ok) - { - ok = 1; // ok ==1 means no new pieces are found - for (auto wallPiece: WallPieces) - { - std::vector tmpWallPieces; - tmpWallPieces = SplitWall(subroom.second, wallPiece); - if(!tmpWallPieces.empty()) + int ok=0; + while(!ok) + { + ok = 1; // ok ==1 means no new pieces are found + for (auto wallPiece: WallPieces) { - // std::cout << "set ok because tmp size =" << tmpWallPieces.size() << std::endl; - ok = 0; /// stay in the loop - // append tmpWallPieces to WallPiece - WallPieces.insert(std::end(WallPieces),std::begin(tmpWallPieces), std::end(tmpWallPieces)); - // remove the line since it was split already - auto it = std::find(WallPieces.begin(), WallPieces.end(), wallPiece); - if (it != WallPieces.end()) + std::vector tmpWallPieces; + tmpWallPieces = SplitWall(subroom.second, wallPiece); + if(!tmpWallPieces.empty()) { - // std::cout<< KGRN << "delete wall ..." << RESET <> Piece: " << std::endl; - t.WriteToErrorLog(); - } + std::cout << "ok "<< ok << std::endl; + std::cout << "new while Wall peces size : " << WallPieces.size() << std::endl; + std::cout << "====" << std::endl; + + for(auto t: WallPieces){ + std::cout << ">> Piece: " << std::endl; + t.WriteToErrorLog(); + } #endif - // getc(stdin); - }// while - // remove - // duplicates fromWllPiecs - if(!WallPieces.empty()) - { - //remove duplicaes from wallPiecess - - auto end = WallPieces.end(); - for (auto it = WallPieces.begin(); it != end; ++it) { - end = std::remove(it + 1, end, *it); - } - WallPieces.erase(end, WallPieces.end()); + // getc(stdin); + }// while + // remove + // duplicates fromWllPiecs + if(!WallPieces.empty()) + { + //remove duplicaes from wallPiecess + + auto end = WallPieces.end(); + for (auto it = WallPieces.begin(); it != end; ++it) { + end = std::remove(it + 1, end, *it); + } + WallPieces.erase(end, WallPieces.end()); #if DEBUG - std::cout << "..removing duplicates pieces..\n"; - for(auto t: WallPieces){ - std::cout << ">>>> Piece: " << std::endl; - t.WriteToErrorLog(); - } + std::cout << "..removing duplicates pieces..\n"; + for(auto t: WallPieces){ + std::cout << ">>>> Piece: " << std::endl; + t.WriteToErrorLog(); + } #endif - // remove big wall and add one wallpiece to walls - ReplaceBigWall(subroom.second, bigWall, WallPieces); - } - }// bigLine - }//s - }//r - - if(removed) - { - fs::path f("correct_"+this->GetConfig()->GetGeometryFile()); - //fs::path p(this->GetConfig()->GetProjectRootDir()); - //p = p / f; - std::string filename = f.string(); - if(SaveGeometry(filename)) - this->GetConfig()->SetGeometryFile(filename); - } - - auto t_end = std::chrono::high_resolution_clock::now(); - double elapsedTimeMs = std::chrono::duration(t_end-t_start).count(); - Log->Write("INFO:\tLeave geometry correct with success (%.3f s)", elapsedTimeMs); - return true; + // remove big wall and add one wallpiece to walls + ReplaceBigWall(subroom.second, bigWall, WallPieces); + } + }// bigLine + }//s + }//r + + if(removed) + { + fs::path f("correct_"+this->GetConfig()->GetGeometryFile()); + //fs::path p(this->GetConfig()->GetProjectRootDir()); + //p = p / f; + std::string filename = f.string(); + if(SaveGeometry(filename)) + this->GetConfig()->SetGeometryFile(filename); + } + + auto t_end = std::chrono::high_resolution_clock::now(); + double elapsedTimeMs = std::chrono::duration(t_end-t_start).count(); + Log->Write("INFO:\tLeave geometry correct with success (%.3f s)", elapsedTimeMs); + return true; } bool Building::RemoveOverlappingDoors(const std::shared_ptr& subroom) const { #if DEBUG - std::cout << KRED << "\nEnter RemoveOverlappingDoors with subroom " << subroom->GetRoomID() << "," << subroom->GetSubRoomID() << RESET<<"\n"; + std::cout << KRED << "\nEnter RemoveOverlappingDoors with subroom " << subroom->GetRoomID() << "," << subroom->GetSubRoomID() << RESET<<"\n"; #endif - bool removed = false; // did we remove anything? + bool removed = false; // did we remove anything? vector exits = vector(); // transitions+crossings auto walls = subroom->GetAllWalls(); vector tmpWalls = vector(); //splited big walls are stored here @@ -568,62 +917,62 @@ bool Building::RemoveOverlappingDoors(const std::shared_ptr& subroom) c #endif while(!walls.empty()) { - auto wall = walls.back(); - walls.pop_back(); - isBigWall = false; - for(auto e=exits.begin(); e!=exits.end(); ) - { - if(wall.NearlyInLineSegment(e->GetPoint1()) && wall.NearlyInLineSegment(e->GetPoint2())) - { - isBigWall = true; // mark walls as big - double dist_pt1 = (wall.GetPoint1() - e->GetPoint1()).NormSquare(); - double dist_pt2 = (wall.GetPoint1() - e->GetPoint2()).NormSquare(); - Point A, B; - - if(dist_pt1GetPoint1(); - B = e->GetPoint2(); - } - else - { - A = e->GetPoint2(); - B = e->GetPoint1(); - } - - Wall NewWall(wall.GetPoint1(), A); - Wall NewWall1(wall.GetPoint2(), B); - // NewWall.WriteToErrorLog(); - // NewWall1.WriteToErrorLog(); - // add new lines to be controled against overlap with exits - walls.push_back(NewWall); - walls.push_back(NewWall1); - subroom->RemoveWall(wall); - exits.erase(e); // we don't need to check this exit again - removed = true; - break; // we are done with this wall. get next wall. - - }// if - else - { - e++; - } - } // exits - if(!isBigWall) - tmpWalls.push_back(wall); + auto wall = walls.back(); + walls.pop_back(); + isBigWall = false; + for(auto e=exits.begin(); e!=exits.end(); ) + { + if(wall.NearlyInLineSegment(e->GetPoint1()) && wall.NearlyInLineSegment(e->GetPoint2())) + { + isBigWall = true; // mark walls as big + double dist_pt1 = (wall.GetPoint1() - e->GetPoint1()).NormSquare(); + double dist_pt2 = (wall.GetPoint1() - e->GetPoint2()).NormSquare(); + Point A, B; + + if(dist_pt1GetPoint1(); + B = e->GetPoint2(); + } + else + { + A = e->GetPoint2(); + B = e->GetPoint1(); + } + + Wall NewWall(wall.GetPoint1(), A); + Wall NewWall1(wall.GetPoint2(), B); + // NewWall.WriteToErrorLog(); + // NewWall1.WriteToErrorLog(); + // add new lines to be controled against overlap with exits + walls.push_back(NewWall); + walls.push_back(NewWall1); + subroom->RemoveWall(wall); + exits.erase(e); // we don't need to check this exit again + removed = true; + break; // we are done with this wall. get next wall. + + }// if + else + { + e++; + } + } // exits + if(!isBigWall) + tmpWalls.push_back(wall); }// while // copy walls in subroom for(auto const & wall: tmpWalls) { - subroom->AddWall(wall); + subroom->AddWall(wall); } #if DEBUG std::cout << "\nnew Subroom: " << std::endl; for(auto w: subroom->GetAllWalls()) - w.WriteToErrorLog(); // AddWall won't add existing walls + w.WriteToErrorLog(); // AddWall won't add existing walls std::cout << KGRN << "\nLEAVE with removed=: "<< removed << RESET << std::endl; getc(stdin); @@ -633,436 +982,495 @@ bool Building::RemoveOverlappingDoors(const std::shared_ptr& subroom) c } std::vector Building::SplitWall(const std::shared_ptr& subroom, const Wall& bigWall) const{ - std::vector WallPieces; + std::vector WallPieces; #if DEBUG - std::cout << subroom->GetSubRoomID() << "collect wall pieces with " << std::endl; - bigWall.WriteToErrorLog(); + std::cout << subroom->GetSubRoomID() << "collect wall pieces with " << std::endl; + bigWall.WriteToErrorLog(); #endif - auto walls = subroom->GetAllWalls(); - auto crossings = subroom->GetAllCrossings(); - auto transitions = subroom->GetAllTransitions(); - // TODO: Hlines too? - vector walls_and_exits = vector(); - // @todo: check if this is GetAllGoals()? - // collect all crossings - for (auto&& cros:crossings) - walls_and_exits.push_back(*cros); - //collect all transitions - for (auto&& trans: transitions) - walls_and_exits.push_back(*trans); - for (auto&& wall: walls) - walls_and_exits.push_back(wall); - - for(auto const & other: walls_and_exits) - { + auto walls = subroom->GetAllWalls(); + auto crossings = subroom->GetAllCrossings(); + auto transitions = subroom->GetAllTransitions(); + // TODO: Hlines too? + vector walls_and_exits = vector(); + // @todo: check if this is GetAllGoals()? + // collect all crossings + for (auto&& cros:crossings) + walls_and_exits.push_back(*cros); + //collect all transitions + for (auto&& trans: transitions) + walls_and_exits.push_back(*trans); + for (auto&& wall: walls) + walls_and_exits.push_back(wall); + + for(auto const & other: walls_and_exits) + { #if DEBUG - std::cout << other.toString() << "\n"; + std::cout << other.toString() << "\n"; #endif - if((bigWall == other) || (bigWall.ShareCommonPointWith(other))) continue; - Point intersectionPoint; + if((bigWall == other) || (bigWall.ShareCommonPointWith(other))) continue; + Point intersectionPoint; - if(bigWall.IntersectionWith(other, intersectionPoint)) - { - if(intersectionPoint == bigWall.GetPoint1() || intersectionPoint == bigWall.GetPoint2()) continue; + if(bigWall.IntersectionWith(other, intersectionPoint)) + { + if(intersectionPoint == bigWall.GetPoint1() || intersectionPoint == bigWall.GetPoint2()) continue; #if DEBUG - std::cout << "BIG\n"; - std::cout << bigWall.GetPoint1()._x << " " << bigWall.GetPoint1()._y << "\n"; - std::cout << bigWall.GetPoint2()._x << " " << bigWall.GetPoint2()._y << "\n"; - std::cout<< "intersectin with: " << std::endl; - std::cout << other.toString() << "\n"; - std::cout << intersectionPoint._x <<" " <> Intersection at Point: " << s.c_str() << "\n"; + std::cout << "BIG\n"; + std::cout << bigWall.GetPoint1()._x << " " << bigWall.GetPoint1()._y << "\n"; + std::cout << bigWall.GetPoint2()._x << " " << bigWall.GetPoint2()._y << "\n"; + std::cout<< "intersectin with: " << std::endl; + std::cout << other.toString() << "\n"; + std::cout << intersectionPoint._x <<" " <> Intersection at Point: " << s.c_str() << "\n"; #endif - //Point NAN_p(J_NAN, J_NAN); + //Point NAN_p(J_NAN, J_NAN); - if(std::isnan(intersectionPoint._x) || std::isnan(intersectionPoint._y)) - continue; + if(std::isnan(intersectionPoint._x) || std::isnan(intersectionPoint._y)) + continue; - Wall NewWall(intersectionPoint, bigWall.GetPoint2());// [IP -- P2] - Wall NewWall2(bigWall.GetPoint1(), intersectionPoint);// [IP -- P2] + Wall NewWall(intersectionPoint, bigWall.GetPoint2());// [IP -- P2] + Wall NewWall2(bigWall.GetPoint1(), intersectionPoint);// [IP -- P2] - WallPieces.push_back(NewWall); - WallPieces.push_back(NewWall2); + WallPieces.push_back(NewWall); + WallPieces.push_back(NewWall2); #if DEBUG - std::cout << "Add newwall: " << std::endl; - NewWall.WriteToErrorLog(); - NewWall2.WriteToErrorLog(); + std::cout << "Add newwall: " << std::endl; + NewWall.WriteToErrorLog(); + NewWall2.WriteToErrorLog(); #endif - } - }//other walls + } + }//other walls #if DEBUG - std::cout << "size " << WallPieces.size() << "\n"; - std::cout << "Leave collect\n--------" << std::endl; + std::cout << "size " << WallPieces.size() << "\n"; + std::cout << "Leave collect\n--------" << std::endl; #endif // getc(stdin); - return WallPieces; + return WallPieces; } bool Building::ReplaceBigWall(const std::shared_ptr& subroom, const Wall& bigWall, std::vector& WallPieces) const { #if DEBUG - Log->Write("INFO Replacing big line in Room %d | Subroom %d with:", subroom->GetRoomID(), subroom->GetSubRoomID()); - bigWall.WriteToErrorLog(); + Log->Write("INFO Replacing big line in Room %d | Subroom %d with:", subroom->GetRoomID(), subroom->GetSubRoomID()); + bigWall.WriteToErrorLog(); - // REMOVE BigLINE + // REMOVE BigLINE - std::cout << "\ns+ =" << subroom->GetAllWalls().size() << "\n"; + std::cout << "\ns+ =" << subroom->GetAllWalls().size() << "\n"; #endif - bool res = subroom->RemoveWall(bigWall); + bool res = subroom->RemoveWall(bigWall); #if DEBUG - std::cout << "s- =" << subroom->GetAllWalls().size() << "\n"; + std::cout << "s- =" << subroom->GetAllWalls().size() << "\n"; #endif - if(!res) { - Log->Write("ERROR: Correct fails. Could not remove wall: "); - bigWall.WriteToErrorLog(); - return false; - } - // ADD SOME LINE - res = AddWallToSubroom(subroom, WallPieces); - if(!res) { - Log->Write("ERROR: Correct fails. Could not add new wall piece"); - return false; - } - - return true; + if(!res) { + Log->Write("ERROR: Correct fails. Could not remove wall: "); + bigWall.WriteToErrorLog(); + return false; + } + // ADD SOME LINE + res = AddWallToSubroom(subroom, WallPieces); + if(!res) { + Log->Write("ERROR: Correct fails. Could not add new wall piece"); + return false; + } + + return true; } const string& Building::GetProjectFilename() const { - return _configuration->GetProjectFile(); + return _configuration->GetProjectFile(); } const string& Building::GetProjectRootDir() const { - return _configuration->GetProjectRootDir(); + return _configuration->GetProjectRootDir(); } const std::string& Building::GetGeometryFilename() const { - return _configuration->GetGeometryFile(); + return _configuration->GetGeometryFile(); } bool Building::AddWallToSubroom( - const std::shared_ptr & subroom, - std::vector WallPieces) const + const std::shared_ptr & subroom, + std::vector WallPieces) const { // CHOOSE WHICH PIECE SHOULD BE ADDED TO SUBROOM // this is a challngig function #if DEBUG - std::cout << "\n-----\nEnter add_wall with:\n"; - for(const auto & w : WallPieces) - w.WriteToErrorLog(); + std::cout << "\n-----\nEnter add_wall with:\n"; + for(const auto & w : WallPieces) + w.WriteToErrorLog(); #endif - auto walls = subroom->GetAllWalls(); - int maxCount = -1; - Wall choosenWall; - for(const auto & w : WallPieces) { - // std::cout <<"\n check wall: \n"; - // w.WriteToErrorLog(); - int count = 0; - for (const auto & checkWall: walls) { - if (checkWall==w) continue;// don't count big wall - if (w.ShareCommonPointWith(checkWall)) count++; - else - { // first use the cheap ShareCommonPointWith() before entering - // this else - Point interP; - if (w.IntersectionWith(checkWall, interP)) - { - if( (!std::isnan(interP._x)) && (!std::isnan(interP._y))) - count++; - } - } - } - auto transitions = subroom->GetAllTransitions(); - auto crossings = subroom->GetAllCrossings(); - //auto h = subroom.second->GetAllHlines(); - for (auto transition: transitions) - if (w.ShareCommonPointWith(*transition)) count++; - for (auto crossing: crossings) - if (w.ShareCommonPointWith(*crossing)) count++; - - if (count>=2) - { - subroom->AddWall(w); + auto walls = subroom->GetAllWalls(); + int maxCount = -1; + Wall choosenWall; + for(const auto & w : WallPieces) { + // std::cout <<"\n check wall: \n"; + // w.WriteToErrorLog(); + int count = 0; + for (const auto & checkWall: walls) { + if (checkWall==w) continue;// don't count big wall + if (w.ShareCommonPointWith(checkWall)) count++; + else + { // first use the cheap ShareCommonPointWith() before entering + // this else + Point interP; + if (w.IntersectionWith(checkWall, interP)) + { + if( (!std::isnan(interP._x)) && (!std::isnan(interP._y))) + count++; + } + } + } + auto transitions = subroom->GetAllTransitions(); + auto crossings = subroom->GetAllCrossings(); + //auto h = subroom.second->GetAllHlines(); + for (auto transition: transitions) + if (w.ShareCommonPointWith(*transition)) count++; + for (auto crossing: crossings) + if (w.ShareCommonPointWith(*crossing)) count++; + + if (count>=2) + { + subroom->AddWall(w); #if DEBUG - Log->Write("INFO: Wall candidate: "); - w.WriteToErrorLog(); + Log->Write("INFO: Wall candidate: "); + w.WriteToErrorLog(); #endif - if (count > maxCount) - { - maxCount = count; - choosenWall = w; - } + if (count > maxCount) + { + maxCount = count; + choosenWall = w; + } #if DEBUG - std::cout << "\n -- count= " << count << ", maxCount= " << maxCount << "\n"; + std::cout << "\n -- count= " << count << ", maxCount= " << maxCount << "\n"; #endif - } - }// WallPieces - if(maxCount<0) - return false; - else - { + } + }// WallPieces + if(maxCount<0) + return false; + else + { #if DEBUG - std::cout << KGRN << "Choosen Wall: " << RESET << std::endl; - choosenWall.WriteToErrorLog(); + std::cout << KGRN << "Choosen Wall: " << RESET << std::endl; + choosenWall.WriteToErrorLog(); #endif - subroom->AddWall(choosenWall); - return true; - } + subroom->AddWall(choosenWall); + return true; + } } void Building::WriteToErrorLog() const { - Log->Write("GEOMETRY: "); - for (int i = 0; iWriteToErrorLog(); - } - Log->Write("ROUTING: "); - - for (map::const_iterator iter = _crossings.begin(); - iter!=_crossings.end(); ++iter) { - iter->second->WriteToErrorLog(); - } - for (map::const_iterator iter = _transitions.begin(); - iter!=_transitions.end(); ++iter) { - iter->second->WriteToErrorLog(); - } - for (map::const_iterator iter = _hLines.begin(); - iter!=_hLines.end(); ++iter) { - iter->second->WriteToErrorLog(); - } - Log->Write("\n"); + Log->Write("GEOMETRY: "); + for (int i = 0; iWriteToErrorLog(); + } + Log->Write("ROUTING: "); + + for (map::const_iterator iter = _crossings.begin(); + iter!=_crossings.end(); ++iter) { + iter->second->WriteToErrorLog(); + } + for (map::const_iterator iter = _transitions.begin(); + iter!=_transitions.end(); ++iter) { + iter->second->WriteToErrorLog(); + } + for (map::const_iterator iter = _hLines.begin(); + iter!=_hLines.end(); ++iter) { + iter->second->WriteToErrorLog(); + } + Log->Write("\n"); } Room* Building::GetRoom(string caption) const { - for (const auto& it: _rooms) { - if (it.second->GetCaption()==caption) - return it.second.get(); - } - Log->Write("ERROR: Room not found with caption "+caption); - //return NULL; - exit(EXIT_FAILURE); + for (const auto& it: _rooms) { + if (it.second->GetCaption()==caption) + return it.second.get(); + } + Log->Write("ERROR: Room not found with caption "+caption); + //return NULL; + exit(EXIT_FAILURE); } bool Building::AddCrossing(Crossing* line) { - int IDRoom = line->GetRoom1()->GetID(); - int IDLine = line->GetUniqueID(); - int IDCrossing = 1000 * IDRoom + IDLine; - if (_crossings.count(IDCrossing) != 0) - { - char tmp[CLENGTH]; - sprintf(tmp, - "ERROR: Duplicate index for crossing found [%d] in Routing::AddCrossing()", - IDCrossing); - Log->Write(tmp); - exit(EXIT_FAILURE); - } - _crossings[IDCrossing] = line; - return true; + int IDRoom = line->GetRoom1()->GetID(); + int IDLine = line->GetUniqueID(); + int IDCrossing = 1000 * IDRoom + IDLine; + if (_crossings.count(IDCrossing) != 0) + { + char tmp[CLENGTH]; + sprintf(tmp, + "ERROR: Duplicate index for crossing found [%d] in Routing::AddCrossing()", + IDCrossing); + Log->Write(tmp); + exit(EXIT_FAILURE); + } + _crossings[IDCrossing] = line; + return true; +} +bool Building::RemoveTransition(Transition * line) +{ + // std::cout << "enter Building Remove Transitions with " << _transitions.size() << "\n"; + if (_transitions.count(line->GetID())!=0) { + _transitions.erase(line->GetID()); + // std::cout << " enter Nuilding Remove Transitions with " << _transitions.size() << "\n"; + return true; + } + // std::cout << "2 enter Nuilding Remove Transitions with " << _transitions.size() << "\n"; + return false; } - bool Building::AddTransition(Transition* line) { - if (_transitions.count(line->GetID())!=0) { - char tmp[CLENGTH]; - sprintf(tmp, + // std::cout << "building add transition "<< line->GetID()<< "\n"; + if (_transitions.count(line->GetID())!=0) { + char tmp[CLENGTH]; + sprintf(tmp, "ERROR: Duplicate index for transition found [%d] in Routing::AddTransition()", line->GetID()); - Log->Write(tmp); - exit(EXIT_FAILURE); - } - _transitions[line->GetID()] = line; + Log->Write(tmp); + exit(EXIT_FAILURE); + } + _transitions[line->GetID()] = line; - return true; + return true; } bool Building::AddHline(Hline* line) { - if (_hLines.count(line->GetID())!=0) { - // check if the lines are identical - Hline* ori = _hLines[line->GetID()]; - if (ori->operator==(*line)) { - Log->Write("INFO: \tSkipping identical hlines with ID [%d]", line->GetID()); - return false; - } - else { - Log->Write( - "ERROR: Duplicate index for hlines found [%d] in Routing::AddHline(). You have [%d] hlines", - line->GetID(), _hLines.size()); - exit(EXIT_FAILURE); - } - } - _hLines[line->GetID()] = line; - return true; + if (_hLines.count(line->GetID())!=0) { + // check if the lines are identical + Hline* ori = _hLines[line->GetID()]; + if (ori->operator==(*line)) { + Log->Write("INFO: \tSkipping identical hlines with ID [%d]", line->GetID()); + return false; + } + else { + Log->Write( + "ERROR: Duplicate index for hlines found [%d] in Routing::AddHline(). You have [%d] hlines", + line->GetID(), _hLines.size()); + exit(EXIT_FAILURE); + } + } + _hLines[line->GetID()] = line; + return true; } bool Building::AddGoal(Goal* goal) { - if (_goals.count(goal->GetId())!=0) { - Log->Write( - "ERROR: Duplicate index for goal found [%d] in Routing::AddGoal()", - goal->GetId()); - exit(EXIT_FAILURE); - } - _goals[goal->GetId()] = goal; + if (_goals.count(goal->GetId())!=0) { + Log->Write( + "ERROR: Duplicate index for goal found [%d] in Routing::AddGoal()", + goal->GetId()); + exit(EXIT_FAILURE); + } + _goals[goal->GetId()] = goal; + + return true; +} + +bool Building:: + + +AddTrainType(std::shared_ptr TT) +{ + if (_trainTypes.count(TT->type)!=0) { + Log->Write("WARNING: Duplicate type for train found [%s]",TT->type); + } + _trainTypes[TT->type] = TT; + return true; +} - return true; +bool Building::AddTrainTimeTable(std::shared_ptr TTT) +{ + if (_trainTimeTables.count(TTT->id)!=0) { + Log->Write("WARNING: Duplicate id for train time table found [%d]",TTT->id); + exit(EXIT_FAILURE); + } + _trainTimeTables[TTT->id] = TTT; + return true; } const map& Building::GetAllCrossings() const { - return _crossings; + return _crossings; } const map& Building::GetAllTransitions() const { - return _transitions; +// std::cout << "BUILDING " << _transitions. + return _transitions; } const map& Building::GetAllHlines() const { - return _hLines; + return _hLines; } -const map& Building::GetAllGoals() const +const std::map >& Building::GetTrainTypes() const +{ + return _trainTypes; +} + +const std::map >& Building::GetTrainTimeTables() const +{ + return _trainTimeTables; +} + +const std::map >& Building::GetPlatforms() const +{ + return _platforms; +} + +bool Building::AddPlatform(std::shared_ptr P) { - return _goals; + if (_platforms.count(P->id)!=0) { + Log->Write("WARNING: Duplicate platform found [%d]", P->id); + } + _platforms[P->id] = P; + return true; + } +const map& Building::GetAllGoals() const +{ + return _goals; +} Transition* Building::GetTransition(string caption) const { - //eventually - map::const_iterator itr; - for (itr = _transitions.begin(); itr!=_transitions.end(); ++itr) { - if (itr->second->GetCaption()==caption) - return itr->second; - } + //eventually + map::const_iterator itr; + for (itr = _transitions.begin(); itr!=_transitions.end(); ++itr) { + if (itr->second->GetCaption()==caption) + return itr->second; + } - Log->Write("WARNING: No Transition with Caption: "+caption); - exit(EXIT_FAILURE); + Log->Write("WARNING: No Transition with Caption: "+caption); + exit(EXIT_FAILURE); } Transition* Building::GetTransition(int ID) const //ar.graf: added const 2015-12-10 { - if (_transitions.count(ID)==1) { - return _transitions.at(ID); - } - else { - if (ID==-1) - return nullptr; - else { - Log->Write( - "ERROR: I could not find any transition with the 'ID' [%d]. You have defined [%d] transitions", - ID, _transitions.size()); - exit(EXIT_FAILURE); - } - } + if (_transitions.count(ID)==1) { + return _transitions.at(ID); + } + else { + if (ID==-1) + return nullptr; + else { + Log->Write( + "ERROR: I could not find any transition with the 'ID' [%d]. You have defined [%d] transitions", + ID, _transitions.size()); + exit(EXIT_FAILURE); + } + } } Crossing* Building::GetCrossing(int ID) const { - if (_crossings.count(ID)==1) { - return _crossings.at(ID); - } - else { - if (ID==-1) - return nullptr; - else { - Log->Write( - "ERROR: I could not find any crossing with the 'ID' [%d]. You have defined [%d] transitions", - ID, _crossings.size()); - exit(EXIT_FAILURE); - } - } + if (_crossings.count(ID)==1) { + return _crossings.at(ID); + } + else { + if (ID==-1) + return nullptr; + else { + Log->Write( + "ERROR: I could not find any crossing with the 'ID' [%d]. You have defined [%d] transitions", + ID, _crossings.size()); + exit(EXIT_FAILURE); + } + } } Goal* Building::GetFinalGoal(int ID) const { - if (_goals.count(ID)==1) { - return _goals.at(ID); - } - else { - if (ID==-1) - return nullptr; - else { - Log->Write( - "ERROR: I could not find any goal with the 'ID' [%d]. You have defined [%d] goals", - ID, _goals.size()); - exit(EXIT_FAILURE); - } - } + if (_goals.count(ID)==1) { + return _goals.at(ID); + } + else { + if (ID==-1) + return nullptr; + else { + Log->Write( + "ERROR: I could not find any goal with the 'ID' [%d]. You have defined [%d] goals", + ID, _goals.size()); + exit(EXIT_FAILURE); + } + } } Crossing* Building::GetTransOrCrossByName(string caption) const { - { - //eventually - map::const_iterator itr; - for (itr = _transitions.begin(); itr!=_transitions.end(); ++itr) { - if (itr->second->GetCaption()==caption) - return itr->second; - } - } - { - //finally the crossings - map::const_iterator itr; - for (itr = _crossings.begin(); itr!=_crossings.end(); ++itr) { - if (itr->second->GetCaption()==caption) - return itr->second; - } - } - - Log->Write("WARNING: No Transition or Crossing with Caption: "+caption); - return nullptr; + { + //eventually + map::const_iterator itr; + for (itr = _transitions.begin(); itr!=_transitions.end(); ++itr) { + if (itr->second->GetCaption()==caption) + return itr->second; + } + } + { + //finally the crossings + map::const_iterator itr; + for (itr = _crossings.begin(); itr!=_crossings.end(); ++itr) { + if (itr->second->GetCaption()==caption) + return itr->second; + } + } + + Log->Write("WARNING: No Transition or Crossing with Caption: "+caption); + return nullptr; } Hline* Building::GetTransOrCrossByUID(int id) const { - { - //eventually transitions - map::const_iterator itr; - for (itr = _transitions.begin(); itr!=_transitions.end(); ++itr) { - if (itr->second->GetUniqueID()==id) - return itr->second; - } - } - { - //then the crossings - map::const_iterator itr; - for (itr = _crossings.begin(); itr!=_crossings.end(); ++itr) { - if (itr->second->GetUniqueID()==id) - return itr->second; - } - } - { - //finally the hlines - for (auto itr = _hLines.begin(); itr!=_hLines.end(); ++itr) { - if (itr->second->GetUniqueID()==id) - return itr->second; - } - } - Log->Write("ERROR: No Transition or Crossing or hline with ID %d: ", id); - return nullptr; + { + //eventually transitions + map::const_iterator itr; + for (itr = _transitions.begin(); itr!=_transitions.end(); ++itr) { + if (itr->second->GetUniqueID()==id) + return itr->second; + } + } + { + //then the crossings + map::const_iterator itr; + for (itr = _crossings.begin(); itr!=_crossings.end(); ++itr) { + if (itr->second->GetUniqueID()==id) + return itr->second; + } + } + { + //finally the hlines + for (auto itr = _hLines.begin(); itr!=_hLines.end(); ++itr) { + if (itr->second->GetUniqueID()==id) + return itr->second; + } + } + Log->Write("ERROR: No Transition or Crossing or hline with ID %d: ", id); + return nullptr; } SubRoom* Building::GetSubRoomByUID(int uid) const { - for (auto&& itr_room: _rooms) { - for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { - if (itr_subroom.second->GetUID()==uid) - return itr_subroom.second.get(); - } - } - Log->Write("ERROR:\t No subroom exits with the unique id %d", uid); - return nullptr; + for (auto&& itr_room: _rooms) { + for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { + if (itr_subroom.second->GetUID()==uid) + return itr_subroom.second.get(); + } + } + Log->Write("ERROR:\t No subroom exits with the unique id %d", uid); + return nullptr; } + //bool Building::IsVisible(Line* l1, Line* l2, bool considerHlines) //{ // @@ -1077,83 +1485,83 @@ SubRoom* Building::GetSubRoomByUID(int uid) const //} bool Building::IsVisible(const Point& p1, const Point& p2, const std::vector& subrooms, - bool considerHlines) + bool considerHlines) { - //loop over all subrooms if none is provided - if (subrooms.empty()) { - for (auto&& itr_room: _rooms) { - for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { - if (!itr_subroom.second->IsVisible(p1, p2, considerHlines)) return false; - } - } - } - else { - for (auto&& sub: subrooms) { - if (sub && !sub->IsVisible(p1, p2, considerHlines)) return false; - } - } - - return true; + //loop over all subrooms if none is provided + if (subrooms.empty()) { + for (auto&& itr_room: _rooms) { + for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { + if (!itr_subroom.second->IsVisible(p1, p2, considerHlines)) return false; + } + } + } + else { + for (auto&& sub: subrooms) { + if (sub && !sub->IsVisible(p1, p2, considerHlines)) return false; + } + } + + return true; } bool Building::Triangulate() { - Log->Write("INFO:\tTriangulating the geometry"); - for (auto&& itr_room: _rooms) { - for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { - if (!itr_subroom.second->Triangulate()) - return false; - } - } - Log->Write("INFO:\tDone..."); - return true; + Log->Write("INFO:\tTriangulating the geometry"); + for (auto&& itr_room: _rooms) { + for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { + if (!itr_subroom.second->Triangulate()) + return false; + } + } + Log->Write("INFO:\tDone..."); + return true; } std::vector Building::GetBoundaryVertices() const { - double xMin=FLT_MAX; - double yMin=FLT_MAX; - double xMax=-FLT_MAX; - double yMax=-FLT_MAX; - for(auto&& itr_room: _rooms) - { - for(auto&& itr_subroom: itr_room.second->GetAllSubRooms()) - { - const std::vector vertices = itr_subroom.second->GetPolygon(); - - for (Point point:vertices) - { - if (point._x>xMax) - xMax=point._x; - else if (point._xyMax) - yMax=point._y; - else if (point._y{Point(xMin,yMin),Point(xMin,yMax),Point(xMax,yMax),Point(xMax,yMin)}; + double xMin=FLT_MAX; + double yMin=FLT_MAX; + double xMax=-FLT_MAX; + double yMax=-FLT_MAX; + for(auto&& itr_room: _rooms) + { + for(auto&& itr_subroom: itr_room.second->GetAllSubRooms()) + { + const std::vector vertices = itr_subroom.second->GetPolygon(); + + for (Point point:vertices) + { + if (point._x>xMax) + xMax=point._x; + else if (point._xyMax) + yMax=point._y; + else if (point._y{Point(xMin,yMin),Point(xMin,yMax),Point(xMax,yMax),Point(xMax,yMin)}; } bool Building::SanityCheck() { - Log->Write("INFO: \tChecking the geometry for artifacts: (Ignore Warnings, if ff_[...] router is used!)"); - bool status = true; + Log->Write("INFO: \tChecking the geometry for artifacts: (Ignore Warnings, if ff_[...] router is used!)"); + bool status = true; - for(auto&& itr_room: _rooms) - { - for(auto&& itr_subroom: itr_room.second->GetAllSubRooms()) - { - if (!itr_subroom.second->SanityCheck()) - status = false; - } - } + for(auto&& itr_room: _rooms) + { + for(auto&& itr_subroom: itr_room.second->GetAllSubRooms()) + { + if (!itr_subroom.second->SanityCheck()) + status = false; + } + } - Log->Write("INFO: \t...Done!!!\n"); - return status; + Log->Write("INFO: \t...Done!!!\n"); + return status; } #ifdef _SIMULATOR @@ -1161,103 +1569,103 @@ bool Building::SanityCheck() void Building::UpdateGrid() { // std::cout << Pedestrian::GetGlobalTime() <<":\t\tBuilding::UpdateGrid from: " << std::this_thread::get_id() <Update(_allPedestians); + _linkedCellGrid->Update(_allPedestians); } void Building::InitGrid() { - // first look for the geometry boundaries - double x_min = FLT_MAX; - double x_max = FLT_MIN; - double y_min = FLT_MAX; - double y_max = FLT_MIN; - - //finding the bounding of the grid - // and collect the pedestrians - for (auto&& itr_room: _rooms) { - for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { - for (auto&& wall:itr_subroom.second->GetAllWalls()) { - double x1 = wall.GetPoint1()._x; - double y1 = wall.GetPoint1()._y; - double x2 = wall.GetPoint2()._x; - double y2 = wall.GetPoint2()._y; - - double xmax = (x1>x2) ? x1 : x2; - double xmin = (x1>x2) ? x2 : x1; - double ymax = (y1>y2) ? y1 : y2; - double ymin = (y1>y2) ? y2 : y1; - - x_min = (xmin<=x_min) ? xmin : x_min; - x_max = (xmax>=x_max) ? xmax : x_max; - y_max = (ymax>=y_max) ? ymax : y_max; - y_min = (ymin<=y_min) ? ymin : y_min; - } - } - } - - double cellSize = _configuration->GetLinkedCellSize(); - //make the grid slightly larger. - x_min = x_min-1*cellSize; - x_max = x_max+1*cellSize; - y_min = y_min-1*cellSize; - y_max = y_max+1*cellSize; - - double boundaries[4] = {x_min, x_max, y_min, y_max}; - - //no algorithms - // the domain is made of a single cell - if (cellSize==-1) { - Log->Write("INFO: \tBrute Force will be used for neighborhoods query"); - if ((x_max-x_min)<(y_max-y_min)) { - cellSize = (y_max-y_min); - } - else { - cellSize = (x_max-x_min); - } - - } - else { - Log->Write("INFO: \tInitializing the grid with cell size: %f ", cellSize); - } - - //TODO: the number of pedestrian should be calculated using the capacity of the sources - //int nped= Pedestrian::GetAgentsCreated() + for src:sources src->GetMaxAgents() - - _linkedCellGrid = new LCGrid(boundaries, cellSize, Pedestrian::GetAgentsCreated()); - _linkedCellGrid->ShallowCopy(_allPedestians); - - Log->Write("INFO: \tDone with Initializing the grid "); + // first look for the geometry boundaries + double x_min = FLT_MAX; + double x_max = FLT_MIN; + double y_min = FLT_MAX; + double y_max = FLT_MIN; + + //finding the bounding of the grid + // and collect the pedestrians + for (auto&& itr_room: _rooms) { + for (auto&& itr_subroom: itr_room.second->GetAllSubRooms()) { + for (auto&& wall:itr_subroom.second->GetAllWalls()) { + double x1 = wall.GetPoint1()._x; + double y1 = wall.GetPoint1()._y; + double x2 = wall.GetPoint2()._x; + double y2 = wall.GetPoint2()._y; + + double xmax = (x1>x2) ? x1 : x2; + double xmin = (x1>x2) ? x2 : x1; + double ymax = (y1>y2) ? y1 : y2; + double ymin = (y1>y2) ? y2 : y1; + + x_min = (xmin<=x_min) ? xmin : x_min; + x_max = (xmax>=x_max) ? xmax : x_max; + y_max = (ymax>=y_max) ? ymax : y_max; + y_min = (ymin<=y_min) ? ymin : y_min; + } + } + } + + double cellSize = _configuration->GetLinkedCellSize(); + //make the grid slightly larger. + x_min = x_min-1*cellSize; + x_max = x_max+1*cellSize; + y_min = y_min-1*cellSize; + y_max = y_max+1*cellSize; + + double boundaries[4] = {x_min, x_max, y_min, y_max}; + + //no algorithms + // the domain is made of a single cell + if (cellSize==-1) { + Log->Write("INFO: \tBrute Force will be used for neighborhoods query"); + if ((x_max-x_min)<(y_max-y_min)) { + cellSize = (y_max-y_min); + } + else { + cellSize = (x_max-x_min); + } + + } + else { + Log->Write("INFO: \tInitializing the grid with cell size: %f ", cellSize); + } + + //TODO: the number of pedestrian should be calculated using the capacity of the sources + //int nped= Pedestrian::GetAgentsCreated() + for src:sources src->GetMaxAgents() + + _linkedCellGrid = new LCGrid(boundaries, cellSize, Pedestrian::GetAgentsCreated()); + _linkedCellGrid->ShallowCopy(_allPedestians); + + Log->Write("INFO: \tDone with Initializing the grid "); } void Building::DeletePedestrian(Pedestrian*& ped) { - vector::iterator it; - it = find(_allPedestians.begin(), _allPedestians.end(), ped); - if (it==_allPedestians.end()) { - Log->Write("\tERROR: \tPed not found with ID %d ", ped->GetID()); - // exit(EXIT_FAILURE); - return; - } - else { - // save the path history for this pedestrian before removing from the simulation - if (_savePathway) { - // string results; - string path = (*it)->GetPath(); - vector brokenpaths; - StringExplode(path, ">", &brokenpaths); - for (unsigned int i = 0; i tags; - StringExplode(brokenpaths[i], ":", &tags); - string room = _rooms[atoi(tags[0].c_str())]->GetCaption(); - string trans = GetTransition(atoi(tags[1].c_str()))->GetCaption(); - //ignore crossings/hlines - if (trans!="") - _pathWayStream << room << " " << trans << endl; - } - - } - } - //update the stats before deleting + vector::iterator it; + it = find(_allPedestians.begin(), _allPedestians.end(), ped); + if (it==_allPedestians.end()) { + Log->Write("\tERROR: \tPed not found with ID %d ", ped->GetID()); + // exit(EXIT_FAILURE); + return; + } + else { + // save the path history for this pedestrian before removing from the simulation + if (_savePathway) { + // string results; + string path = (*it)->GetPath(); + vector brokenpaths; + StringExplode(path, ">", &brokenpaths); + for (unsigned int i = 0; i tags; + StringExplode(brokenpaths[i], ":", &tags); + string room = _rooms[atoi(tags[0].c_str())]->GetCaption(); + string trans = GetTransition(atoi(tags[1].c_str()))->GetCaption(); + //ignore crossings/hlines + if (trans!="") + _pathWayStream << room << " " << trans << endl; + } + + } + } + //update the stats before deleting // Transition* trans =GetTransitionByUID(ped->GetExitIndex()); // if(trans) // { @@ -1267,233 +1675,233 @@ void Building::DeletePedestrian(Pedestrian*& ped) // // he will remain in the simulation in that case // //if(trans->IsOpen()==false) return; // } - _allPedestians.erase(it); - delete ped; + _allPedestians.erase(it); + delete ped; } const vector& Building::GetAllPedestrians() const { - return _allPedestians; + return _allPedestians; } void Building::AddPedestrian(Pedestrian* ped) { - for (unsigned int p = 0; p<_allPedestians.size(); p++) { - Pedestrian* ped1 = _allPedestians[p]; - if (ped->GetID()==ped1->GetID()) { - cout << "Pedestrian " << ped->GetID() << " already in the room." << endl; - return; - } - } - _allPedestians.push_back(ped); + for (unsigned int p = 0; p<_allPedestians.size(); p++) { + Pedestrian* ped1 = _allPedestians[p]; + if (ped->GetID()==ped1->GetID()) { + cout << "Pedestrian " << ped->GetID() << " already in the room." << endl; + return; + } + } + _allPedestians.push_back(ped); } void Building::GetPedestrians(int room, int subroom, std::vector& peds) const { - //for(unsigned int p = 0;p<_allPedestians.size();p++){ - // Pedestrian* ped=_allPedestians[p]; + //for(unsigned int p = 0;p<_allPedestians.size();p++){ + // Pedestrian* ped=_allPedestians[p]; - for (auto&& ped : _allPedestians) { - if ((room==ped->GetRoomID()) && (subroom==ped->GetSubRoomID())) { - peds.push_back(ped); - } - } + for (auto&& ped : _allPedestians) { + if ((room==ped->GetRoomID()) && (subroom==ped->GetSubRoomID())) { + peds.push_back(ped); + } + } } //obsolete void Building::InitSavePedPathway(const string& filename) { - _pathWayStream.open(filename.c_str()); - _savePathway = true; - - if (_pathWayStream.is_open()) { - Log->Write("#INFO:\tsaving pedestrian paths to [ "+filename+" ]"); - _pathWayStream << "##pedestrian ways" << endl; - _pathWayStream << "#nomenclature roomid caption" << endl; - // for (unsigned int r=0;r< pRooms.size();r++){ - // Room* room= GetRoom(r); - // const vector& goals=room->GetAllTransitionsIDs(); - // - // for(unsigned int g=0;gGetGoal(exitid)->GetCaption(); - // PpathWayStream<Write("#INFO:\t Unable to open [ "+filename+" ]"); - Log->Write("#INFO:\t saving to stdout"); - - } + _pathWayStream.open(filename.c_str()); + _savePathway = true; + + if (_pathWayStream.is_open()) { + Log->Write("#INFO:\tsaving pedestrian paths to [ "+filename+" ]"); + _pathWayStream << "##pedestrian ways" << endl; + _pathWayStream << "#nomenclature roomid caption" << endl; + // for (unsigned int r=0;r< pRooms.size();r++){ + // Room* room= GetRoom(r); + // const vector& goals=room->GetAllTransitionsIDs(); + // + // for(unsigned int g=0;gGetGoal(exitid)->GetCaption(); + // PpathWayStream<Write("#INFO:\t Unable to open [ "+filename+" ]"); + Log->Write("#INFO:\t saving to stdout"); + + } } void Building::StringExplode(string str, string separator, - vector* results) + vector* results) { - size_t found; - found = str.find_first_of(separator); - while (found!=string::npos) { - if (found>0) { - results->push_back(str.substr(0, found)); - } - str = str.substr(found+1); - found = str.find_first_of(separator); - } - if (str.length()>0) { - results->push_back(str); - } + size_t found; + found = str.find_first_of(separator); + while (found!=string::npos) { + if (found>0) { + results->push_back(str.substr(0, found)); + } + str = str.substr(found+1); + found = str.find_first_of(separator); + } + if (str.length()>0) { + results->push_back(str); + } } Pedestrian* Building::GetPedestrian(int pedID) const { - for (unsigned int p = 0; p<_allPedestians.size(); p++) { - Pedestrian* ped = _allPedestians[p]; - if (ped->GetID()==pedID) { - return ped; - } - } + for (unsigned int p = 0; p<_allPedestians.size(); p++) { + Pedestrian* ped = _allPedestians[p]; + if (ped->GetID()==pedID) { + return ped; + } + } - return nullptr; + return nullptr; } Transition* Building::GetTransitionByUID(int uid) const { - for (auto&& trans: _transitions) { - if (trans.second->GetUniqueID()==uid) - return trans.second; - } - return nullptr; + for (auto&& trans: _transitions) { + if (trans.second->GetUniqueID()==uid) + return trans.second; + } + return nullptr; } Crossing* Building::GetCrossingByUID(int uid) const { - for (auto&& cross : _crossings) { - if (cross.second->GetUniqueID() == uid) - return cross.second; - } - return nullptr; + for (auto&& cross : _crossings) { + if (cross.second->GetUniqueID() == uid) + return cross.second; + } + return nullptr; } bool Building::SaveGeometry(const std::string& filename) const { - std::stringstream geometry; - - //write the header - geometry << "" << endl; - geometry << "" << endl << endl; - - //write the rooms - geometry << "" << endl; - for (auto&& itroom : _rooms) { - auto&& room = itroom.second; - geometry << "\tGetID() << "\" caption =\"" << room->GetCaption() << "\">" << endl; - for (auto&& itr_sub : room->GetAllSubRooms()) { - auto&& sub = itr_sub.second; - const double* plane = sub->GetPlaneEquation(); - geometry << "\t\tGetSubRoomID() - << "\" caption=\"dummy_caption" - << "\" class=\"" << sub->GetType() - << "\" A_x=\"" << plane[0] - << "\" B_y=\"" << plane[1] - << "\" C_z=\"" << plane[2] << "\">" << endl; - - for (auto&& wall : sub->GetAllWalls()) { - const Point& p1 = wall.GetPoint1(); - const Point& p2 = wall.GetPoint2(); - - geometry << "\t\t\t" << endl - << "\t\t\t\t" << endl - << "\t\t\t\t" << endl - << "\t\t\t" << endl; - } - - if (sub->GetType()=="stair") { - const Point& up = ((Stair*) sub.get())->GetUp(); - const Point& down = ((Stair*) sub.get())->GetDown(); - geometry << "\t\t\t" << endl; - geometry << "\t\t\t" << endl; - } - - geometry << "\t\t" << endl; - } - - //write the crossings - geometry << "\t\t" << endl; - for (auto const& mapcross : _crossings) { - Crossing* cross = mapcross.second; - - //only write the crossings in this rooms - if (cross->GetRoom1()->GetID()!=room->GetID()) continue; - - const Point& p1 = cross->GetPoint1(); - const Point& p2 = cross->GetPoint2(); - - geometry << "\tGetID() - << "\" subroom1_id=\"" << cross->GetSubRoom1()->GetSubRoomID() - << "\" subroom2_id=\"" << cross->GetSubRoom2()->GetSubRoomID() << "\">" << endl; - - geometry << "\t\t" << endl - << "\t\t" << endl - << "\t" << endl; - } - geometry << "\t\t" << endl; - geometry << "\t" << endl; - } - - geometry << "" << endl; - - //write the transitions - geometry << "" << endl; - - for (auto const& maptrans : _transitions) { - Transition* trans = maptrans.second; - const Point& p1 = trans->GetPoint1(); - const Point& p2 = trans->GetPoint2(); - int room2_id = -1; - int subroom2_id = -1; - if (trans->GetRoom2()) { - room2_id = trans->GetRoom2()->GetID(); - subroom2_id = trans->GetSubRoom2()->GetSubRoomID(); - } - - geometry << "\tGetID() - << "\" caption=\"" << trans->GetCaption() - << "\" type=\"" << trans->GetType() - << "\" room1_id=\"" << trans->GetRoom1()->GetID() - << "\" subroom1_id=\"" << trans->GetSubRoom1()->GetSubRoomID() - << "\" room2_id=\"" << room2_id - << "\" subroom2_id=\"" << subroom2_id << "\">" << endl; - - geometry << "\t\t" << endl - << "\t\t" << endl - << "\t" << endl; - - } - - geometry << "" << endl; - geometry << "" << endl; - //write the routing file - - //cout<Write("INFO:\tfile saved to %s", filename.c_str()); - } - else { - Log->Write("ERROR:\tunable to save the geometry to %s", filename.c_str()); - return false; - } - - return true; + std::stringstream geometry; + + //write the header + geometry << "" << endl; + geometry << "" << endl << endl; + + //write the rooms + geometry << "" << endl; + for (auto&& itroom : _rooms) { + auto&& room = itroom.second; + geometry << "\tGetID() << "\" caption =\"" << room->GetCaption() << "\">" << endl; + for (auto&& itr_sub : room->GetAllSubRooms()) { + auto&& sub = itr_sub.second; + const double* plane = sub->GetPlaneEquation(); + geometry << "\t\tGetSubRoomID() + << "\" caption=\"dummy_caption" + << "\" class=\"" << sub->GetType() + << "\" A_x=\"" << plane[0] + << "\" B_y=\"" << plane[1] + << "\" C_z=\"" << plane[2] << "\">" << endl; + + for (auto&& wall : sub->GetAllWalls()) { + const Point& p1 = wall.GetPoint1(); + const Point& p2 = wall.GetPoint2(); + + geometry << "\t\t\t" << endl + << "\t\t\t\t" << endl + << "\t\t\t\t" << endl + << "\t\t\t" << endl; + } + + if (sub->GetType()=="stair") { + const Point& up = ((Stair*) sub.get())->GetUp(); + const Point& down = ((Stair*) sub.get())->GetDown(); + geometry << "\t\t\t" << endl; + geometry << "\t\t\t" << endl; + } + + geometry << "\t\t" << endl; + } + + //write the crossings + geometry << "\t\t" << endl; + for (auto const& mapcross : _crossings) { + Crossing* cross = mapcross.second; + + //only write the crossings in this rooms + if (cross->GetRoom1()->GetID()!=room->GetID()) continue; + + const Point& p1 = cross->GetPoint1(); + const Point& p2 = cross->GetPoint2(); + + geometry << "\tGetID() + << "\" subroom1_id=\"" << cross->GetSubRoom1()->GetSubRoomID() + << "\" subroom2_id=\"" << cross->GetSubRoom2()->GetSubRoomID() << "\">" << endl; + + geometry << "\t\t" << endl + << "\t\t" << endl + << "\t" << endl; + } + geometry << "\t\t" << endl; + geometry << "\t" << endl; + } + + geometry << "" << endl; + + //write the transitions + geometry << "" << endl; + + for (auto const& maptrans : _transitions) { + Transition* trans = maptrans.second; + const Point& p1 = trans->GetPoint1(); + const Point& p2 = trans->GetPoint2(); + int room2_id = -1; + int subroom2_id = -1; + if (trans->GetRoom2()) { + room2_id = trans->GetRoom2()->GetID(); + subroom2_id = trans->GetSubRoom2()->GetSubRoomID(); + } + + geometry << "\tGetID() + << "\" caption=\"" << trans->GetCaption() + << "\" type=\"" << trans->GetType() + << "\" room1_id=\"" << trans->GetRoom1()->GetID() + << "\" subroom1_id=\"" << trans->GetSubRoom1()->GetSubRoomID() + << "\" room2_id=\"" << room2_id + << "\" subroom2_id=\"" << subroom2_id << "\">" << endl; + + geometry << "\t\t" << endl + << "\t\t" << endl + << "\t" << endl; + + } + + geometry << "" << endl; + geometry << "" << endl; + //write the routing file + + //cout<Write("INFO:\tfile saved to %s", filename.c_str()); + } + else { + Log->Write("ERROR:\tunable to save the geometry to %s", filename.c_str()); + return false; + } + + return true; } #endif // _SIMULATOR diff --git a/geometry/Building.h b/geometry/Building.h index c6b1c54dedf424d7c3b729023dd8641b78fb5bcd..80fa9c55d314d28c01c7188b80f34bf8e3bd7889 100644 --- a/geometry/Building.h +++ b/geometry/Building.h @@ -45,6 +45,43 @@ #include "Goal.h" #include "../general/Configuration.h" +typedef std::pair PointWall; + +// train schedules: Trains get deleted and added. + +struct Platform +{ + int id; + int rid; + int sid; + std::map > tracks; +}; + +struct TrainTimeTable +{ + int id; + std::string type; + int rid; // room id + int sid; // subroom id + double tin; // arrival time + double tout; //leaving time + Point pstart; // track start + Point pend; // track end + Point tstart; // train start + Point tend; // train end + int pid; // Platform id + bool arrival; + bool departure; +}; +struct TrainType +{ + std::string type; + int nmax; // agents_max + float len; //length + std::vector doors; +}; + + class RoutingEngine; class Pedestrian; @@ -74,8 +111,9 @@ private: std::map _transitions; std::map _hLines; std::map _goals; - std::map> _sr2wa; - + std::map > _trainTypes; + std::map > _trainTimeTables; + std::map > _platforms; /// pedestrians pathway bool _savePathway; std::ofstream _pathWayStream; @@ -83,10 +121,13 @@ private: public: /// constructor Building(); + 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(); @@ -176,7 +217,7 @@ public: */ Transition* GetTransitionByUID(int uid) const; - Crossing* GetCrossingByUID(int uid) const; + Crossing* GetCrossingByUID(int uid) const; //TOD0: rename later to GetGoal Goal* GetFinalGoal(int id) const; @@ -208,15 +249,33 @@ public: const std::map& GetAllHlines() const; const std::map& GetAllGoals() const; + // --------------- Trains interface + const std::map >& GetTrainTypes() const; + + const std::map >& GetTrainTimeTables() const; + + const std::map >& GetPlatforms() const; + const std::vector GetTrackWalls(Point TrackStart, Point TrackEnd, int & room_id, int & subroom_id) const; + const std::vector > GetIntersectionPoints(const std::vector doors, const std::vector) const; + + // ------------------------------------ bool AddCrossing(Crossing* line); + bool RemoveTransition(Transition * line); + bool AddTransition(Transition* line); bool AddHline(Hline* line); bool AddGoal(Goal* goal); + bool AddTrainType(std::shared_ptr TT); + + bool AddTrainTimeTable(std::shared_ptr TTT); + + bool AddPlatform(std::shared_ptr P); + const std::string& GetProjectRootDir() const; const std::string& GetProjectFilename() const; @@ -258,9 +317,9 @@ public: private: - bool InitInsideGoals(); - - void StringExplode(std::string str, std::string separator, std::vector* results); + bool InitInsideGoals(); + bool InitPlatforms(); + void StringExplode(std::string str, std::string separator, std::vector* results); /** @defgroup auto-correct-geometry * functions used to auto-correct the geometry. * Main function is correct() @@ -349,7 +408,7 @@ private: * @param subroom * @return bool */ -bool RemoveOverlappingDoors( + bool RemoveOverlappingDoors( const std::shared_ptr& subroom) const; /** @} */ // end of group diff --git a/geometry/Crossing.cpp b/geometry/Crossing.cpp index ade0a3c16b5cf89c5c791af854e3bd4bc5d8930f..b30c1328ceb8bb03f8074ca9682035d199a1fdd8 100644 --- a/geometry/Crossing.cpp +++ b/geometry/Crossing.cpp @@ -334,9 +334,9 @@ DoorState Crossing::GetState() const return _state; } -void Crossing::SetState(DoorState _state) +void Crossing::SetState(DoorState state) { - Crossing::_state = _state; + Crossing::_state = state; } std::string Crossing::toString() const @@ -355,6 +355,9 @@ std::string Crossing::toString() const case DoorState::TEMP_CLOSE: tmp << " temp_close"; break; + case DoorState::Error: + tmp << " Error"; + break; } return tmp.str(); diff --git a/geometry/Line.cpp b/geometry/Line.cpp index 01d39c340f2771e4968cf9d2b7542d7144d6a261..d2bf8a3ff86421591bc559c0b088e32137f21907 100644 --- a/geometry/Line.cpp +++ b/geometry/Line.cpp @@ -201,6 +201,16 @@ Point Line::LotPoint(const Point& p) const return f; } +// return true if point the orthogonal projection of p on Line segment is on the +// line segment. +bool Line::isBetween(const Point& p) const +{ + const Point& t = _point1-_point2; + double lambda = (p-_point2).ScalarProduct(t)/t.ScalarProduct(t); + return (lambda>0) && (lambda <1); +} + + /* Punkt auf der Linie mit kürzestem Abstand zu p * In der Regel Lotfußpunkt, Ist der Lotfußpunkt nicht im Segment * wird der entsprechende Eckpunkt der Line genommen @@ -273,6 +283,12 @@ bool Line::operator!=(const Line& l) const return (!(*this==l)); } +// this function is necessary to use std::set and is basically the same as != +bool Line::operator<(const Line& l) const +{ + return (!(*this==l)); +} + double Line::GetLength() const { @@ -350,7 +366,8 @@ int Line::IntersectionWith(const Point& p1, const Point& p2, Point& p3) const double t = (_point1-p1).CrossProduct(s)/(r.CrossProduct(s)); double u = (_point1-p1).CrossProduct(r)/(r.CrossProduct(s)); - if (0>t || t>1) { + + if (-0.05>t || t>1) { return LineIntersectType::NO_INTERSECTION; } @@ -448,6 +465,7 @@ int Line::WichSide(const Point& pt) bool Line::ShareCommonPointWith(const Line& line, Point& P) const { + if (line.GetPoint1()==_point1 || line.GetPoint2()==_point1) { P = _point1; return true; @@ -472,6 +490,23 @@ bool Line::HasEndPoint(const Point& point) const return _point2==point; } +bool Line::NearlyHasEndPoint(const Point& point) const +{ + // std::cout << _point1.toString() << "\n"; + // std::cout << _point2.toString() << "\n"; + // std::cout << point.toString() << "\n"; + + + // std::cout << "--> " << (_point1-point).Norm() << "\n"; + // std::cout << "--> " << (_point2-point).Norm() << "\n"; + // std::cout << "<-- " << J_EPS_DIST << "\n"; + + + if ((_point1-point).Norm() <= J_EPS_DIST) return true; + return ((_point2-point).Norm() <= J_EPS_DIST); +} + + bool Line::IntersectionWithCircle(const Point& centre, double radius /*cm for pedestrians*/) { diff --git a/geometry/Line.h b/geometry/Line.h index 57c2221548ef485052b85dbb4a868fade538e991..d57a9ac3c23dd7b54f60128236a239f73296e0f9 100644 --- a/geometry/Line.h +++ b/geometry/Line.h @@ -134,6 +134,7 @@ public: bool IsInLineSegment(const Point& p) const; bool NearlyInLineSegment(const Point& p) const; + bool NearlyHasEndPoint(const Point& point) const; /** * @return the distance from the line to the point p @@ -161,6 +162,12 @@ public: */ bool Overlapp(const Line& l) const; + /** + * return true if point the orthogonal projection of p on Line segment is on the + * line segment. + */ + bool isBetween(const Point& p) const; + /** * @return true if both segments are equal. The end points must be in the range of J_EPS. * @see Macro.h @@ -173,6 +180,15 @@ public: */ bool operator!=(const Line& l) const; + + /** + * @return true if this < l segments are not equal. The end points must be in the range of J_EPS. + * @see Macro.h + */ + bool operator<(const Line& l) const; + + + /** * @see http://alienryderflex.com/intersect/ * @see http://social.msdn.microsoft.com/Forums/en-US/csharpgeneral/thread/e5993847-c7a9-46ec-8edc-bfb86bd689e3/ diff --git a/geometry/SubRoom.cpp b/geometry/SubRoom.cpp index 650be4ee1d7ca824d6292aeebaa0adfaa3d0e943..9c67964f0697e88e9a33826c7304f864da7e7cd0 100644 --- a/geometry/SubRoom.cpp +++ b/geometry/SubRoom.cpp @@ -247,9 +247,23 @@ bool SubRoom::AddCrossing(Crossing* line) _goalIDs.push_back(line->GetUniqueID()); return true; } - +// return true is walls was erased, otherwise false. +bool SubRoom::RemoveTransition(Transition * t) +{ + auto it = std::find(_transitions.begin(), _transitions.end(), t); + if (it != _transitions.end()) { + // std::cout << "subroom remove transition "<< t->GetID() << ", " << t->GetUniqueID()<< "\n"; + _transitions.erase(it); + RemoveGoalID(t->GetUniqueID()); + // std::cout << "enter Remove Transitions with " << _transitions.size() << "\n"; + return true; + } + // std::cout << "2 enter Remove Transitions with " << _transitions.size() << "\n"; + return false; +} bool SubRoom::AddTransition(Transition* line) { + // std::cout << "subroom addtransition "<< line->GetID() << ", " << line->GetUniqueID()<< "\n"; _transitions.push_back(line); _goalIDs.push_back(line->GetUniqueID()); return true; diff --git a/geometry/SubRoom.h b/geometry/SubRoom.h index 3ab392ba793e8aa37da9dc29ea04b3c8dcd48f71..d46847e38acfd621bdcc3d0d7ec24b31f8059a0b 100644 --- a/geometry/SubRoom.h +++ b/geometry/SubRoom.h @@ -304,6 +304,7 @@ public: //navigation bool AddCrossing(Crossing* line); bool AddTransition(Transition* line); + bool RemoveTransition(Transition * t); bool AddHline(Hline* line); void AddNeighbor(SubRoom* sub); diff --git a/math/VelocityModel.cpp b/math/VelocityModel.cpp index 590c4d5786d31424ed19d8c0cb95c2edb05d6d3d..ddec72d148f42c13ab173f5a9a6558a3d5722831 100644 --- a/math/VelocityModel.cpp +++ b/math/VelocityModel.cpp @@ -212,7 +212,7 @@ void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building* for (int i = 0; i < size; i++) { - + Pedestrian* ped1 = neighbours[i]; // if (ped->GetID() == 71) { // std::cout << "Velocity Model debug ped1: " << ped1->GetID() << "\t" << ped1->GetPos().toString() <GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 30 + ped->GetPremovementTime() && + if(ped->GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 10000 + ped->GetPremovementTime() && std::max(ped->GetMeanVelOverRecTime(), ped->GetV().Norm()) < 0.01 && size == 0 ) // size length of peds neighbour vector { - Log->Write("WARNING:\tped %d with vmean %f has been deleted in room [%i]/[%i] after time %f s (current=%f\n", ped->GetID(), ped->GetMeanVelOverRecTime(), ped->GetRoomID(), ped->GetSubRoomID(), ped->GetGlobalTime(), current); + Log->Write("WARNING:\tped %d with vmean %f has been deleted in room [%i]/[%i] after time %f s (current=%f\n", ped->GetID(), ped->GetMeanVelOverRecTime(), ped->GetRoomID(), ped->GetSubRoomID(), ped->GetGlobalTime(), current); Log->incrementDeletedAgents(); #pragma omp critical(VelocityModel_ComputeNextTimeStep_pedsToRemove) pedsToRemove.push_back(ped); @@ -352,9 +352,19 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const Point target; if(_direction && ped->GetExitLine()) target = _direction->GetTarget(room, ped); // target is where the ped wants to be after the next timestep - else { + else { //@todo: we need a model for waiting pedestrians std::cout << ped->GetID() << " VelocityModel::e0 Ped has no navline.\n"; - exit(EXIT_FAILURE); + //exit(EXIT_FAILURE); + // set random destination + std::mt19937 mt(ped->GetBuilding()->GetConfig()->GetSeed()); + std::uniform_real_distribution dist(0, 1.0); + double random_x = dist(mt); + double random_y = dist(mt); + Point P1 = Point(ped->GetPos()._x - random_x, ped->GetPos()._y - random_y); + Point P2 = Point(ped->GetPos()._x + random_x, ped->GetPos()._y + random_y); + const NavLine L = Line(P1, P2); + ped->SetExitLine((const NavLine *)&L); + target = P1; } Point desired_direction; const Point pos = ped->GetPos(); diff --git a/routing/DirectionStrategy.cpp b/routing/DirectionStrategy.cpp index 7104a9ea5604952f40f7950b279a523c7e826797..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; @@ -837,3 +839,55 @@ DirectionSubLocalFloorfieldTripsVoronoi::~DirectionSubLocalFloorfieldTripsVorono } } +// 12 +Point DirectionTrain::GetTarget(Room* room, Pedestrian* ped) const +{ + + Point p1 = ped->GetExitLine()->GetPoint1(); + Point p2 = ped->GetExitLine()->GetPoint2(); + Line ExitLine = Line(p1, p2, 0); + auto TrainTypes = ped->GetBuilding()->GetTrainTypes(); + auto TrainTimeTables = ped->GetBuilding()->GetTrainTimeTables(); + auto now = ped->GetGlobalTime(); + string type_delme=""; + // std::cout << ">>> Enter with ped at " << ped->GetPos().toString().c_str() << "\n"; + for(auto && t: TrainTimeTables) + { + if(ped->GetRoomID() != t.second->rid) continue; + + if( (now>=t.second->tin) && (now<=t.second->tout) ) + { + auto doors = TrainTypes[t.second->type]->doors; + int i=-1, imin=0; + double dist_min = 10000; + for(auto door: doors) + { + i++; + const Point & d1 = door.GetPoint1(); + const Point & d2 = door.GetPoint2(); + const Point & c = (d1+d2)*0.5; + + double dist = (ped->GetPos()-c).Norm(); + // std::cout << "door id: " << door.GetID()<< " dist: " << dist<< "\n"; + + if(dist <= dist_min) + { + dist_min = dist; + imin=i; + type_delme=t.second->type; + // std::cout << " > imin " << imin << " mindist " << dist_min << "\n"; + } + }// doors + p1 = doors[imin].GetPoint1(); + p2 = doors[imin].GetPoint2(); + // std::cout << "\n>>> train: now " << now << ", type: " << type_delme.c_str() << "\n"; + // std::cout << ">>> p1=" << p1.toString().c_str() << ". p2=" << p2.toString().c_str()<< "\n"; + // std::cout << ">>> ped at " << ped->GetPos().toString().c_str() << "\n"; + // getc(stdin); + + }// if time in + } + + + return (p1+ p2)*0.5; +} diff --git a/routing/DirectionStrategy.h b/routing/DirectionStrategy.h index bfb4c687bd359f04982ef0695e8b7c7e20df949e..09e273b1abe2197beeba7a0554e54b9fdf96fa72 100644 --- a/routing/DirectionStrategy.h +++ b/routing/DirectionStrategy.h @@ -51,7 +51,7 @@ public: virtual Point GetTarget(Room* room, Pedestrian* ped) const = 0; virtual double GetDistance2Wall(Pedestrian* ped) const; virtual double GetDistance2Target(Pedestrian* ped, int UID); - + }; @@ -65,6 +65,11 @@ public: virtual Point GetTarget(Room* room, Pedestrian* ped) const; }; +class DirectionTrain : public DirectionStrategy { +public: + virtual Point GetTarget(Room* room, Pedestrian* ped) const; +}; + class DirectionMinSeperationShorterLine : public DirectionStrategy { public: virtual Point GetTarget(Room* room, Pedestrian* ped) const; @@ -204,4 +209,3 @@ protected: #endif /* _DIRECTIONSTRATEGY_H */ - diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp index be5e5d0491012edd15051a5f4bc00c700c38c374..653628923ec876ea68cde4951a4a1ddb2e6056fe 100644 --- a/routing/ff_router/UnivFFviaFM.cpp +++ b/routing/ff_router/UnivFFviaFM.cpp @@ -1651,7 +1651,7 @@ double UnivFFviaFM::getCostToDestination(const int destID, const Point& position } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) { key = key + _grid->GetiMax(); } else { - Log->Write("ERROR:\t In getCostToDestination(3 args)"); + // Log->Write("ERROR:\t In getCostToDestination(3 args)"); } } if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) { @@ -1689,7 +1689,7 @@ double UnivFFviaFM::getCostToDestination(const int destID, const Point& position } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) { key = key + _grid->GetiMax(); } else { - Log->Write("ERROR:\t In getCostToDestination(2 args)"); + // Log->Write("ERROR:\t In getCostToDestination(2 args)"); } } if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) { @@ -1817,7 +1817,7 @@ void UnivFFviaFM::getDirectionToUID(int destID, long int key, Point& direction){ } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) { key = key + _grid->GetiMax(); } else { - Log->Write("ERROR:\t In getDirectionToUID (3 args)"); + // Log->Write("ERROR:\t In getDirectionToUID (3 args)"); } } if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) { diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp index a71235d6733ecda4f2f0aa3a403b2e09b864f401..ec6dafb0089ece8a70d68268c2c970dfca765f65 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(); @@ -299,7 +299,7 @@ bool FFRouter::Init(Building* building) auto iter = _locffviafm.begin(); std::advance(iter, i); int roomNr = iter->first; - iter->second->writeFF("ffrouterOfRoom" + std::to_string(roomNr) + ".vtk", _allDoorUIDs); + iter->second->writeFF("ffrouterRoom_" + std::to_string(roomNr) + "_t_"+ std::to_string(Pedestrian::GetGlobalTime()) + ".vtk", _allDoorUIDs); } } @@ -322,6 +322,44 @@ bool FFRouter::ReInit() _distMatrix.clear(); _pathsMatrix.clear(); + // Geommetry may change dure to trains or whatever happens in the future + //get all door UIDs + _allDoorUIDs.clear(); + _TransByUID.clear(); + _ExitsByUID.clear(); + _CroTrByUID.clear(); + auto& allTrans = _building->GetAllTransitions(); + auto& allCross = _building->GetAllCrossings(); + std::vector> roomAndCroTrVector; + roomAndCroTrVector.clear(); + for (auto& pair:allTrans) { + //TODO if (pair.second->IsOpen()) { + if (!pair.second->IsClose()) { + _allDoorUIDs.emplace_back(pair.second->GetUniqueID()); + _CroTrByUID.insert(std::make_pair(pair.second->GetUniqueID(), pair.second)); + if (pair.second->IsExit()) { + _ExitsByUID.insert(std::make_pair(pair.second->GetUniqueID(), pair.second)); + } + Room* room1 = pair.second->GetRoom1(); + if (room1) roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID())); + Room* room2 = pair.second->GetRoom2(); + if (room2) roomAndCroTrVector.emplace_back(std::make_pair(room2->GetID(), pair.second->GetUniqueID())); + } + } + for (auto& pair:allCross) { + //TODO if (pair.second->IsOpen()) { + if (!pair.second->IsClose()) { + _allDoorUIDs.emplace_back(pair.second->GetUniqueID()); + _CroTrByUID.insert(std::make_pair(pair.second->GetUniqueID(), pair.second)); + Room* room1 = pair.second->GetRoom1(); + if (room1) roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID())); + } + } + //make unique + std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end()); + _allDoorUIDs.erase( std::unique(_allDoorUIDs.begin(),_allDoorUIDs.end()), _allDoorUIDs.end()); + +// alldoorUIDs reinit //init, yet no distances, only create map entries for(auto& id1 : _allDoorUIDs) { for(auto& id2 : _allDoorUIDs){ @@ -644,4 +682,4 @@ void FFRouter::SetRecalc(double t) { void FFRouter::Update(){ this->ReInit(); -} \ No newline at end of file +} diff --git a/routing/ff_router_trips/UnivFFviaFMTrips.cpp b/routing/ff_router_trips/UnivFFviaFMTrips.cpp index 8da06ea4757bce0ff269d22fb14958b2a091d3df..6d1a499b5ba6ea1b6c67f2b1b187cd7aee9231d3 100644 --- a/routing/ff_router_trips/UnivFFviaFMTrips.cpp +++ b/routing/ff_router_trips/UnivFFviaFMTrips.cpp @@ -1664,7 +1664,7 @@ double UnivFFviaFMTrips::getCostToDestination(const int destID, const Point& pos } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) { key = key + _grid->GetiMax(); } else { - Log->Write("ERROR:\t In getCostToDestination(3 args)"); + // Log->Write("ERROR:\t In getCostToDestination(3 args)"); } } if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) { @@ -1702,7 +1702,7 @@ double UnivFFviaFMTrips::getCostToDestination(const int destID, const Point& pos } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) { key = key + _grid->GetiMax(); } else { - Log->Write("ERROR:\t In getCostToDestination(2 args)"); + // Log->Write("ERROR:\t In getCostToDestination(2 args)"); } } if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) { @@ -1834,7 +1834,7 @@ void UnivFFviaFMTrips::getDirectionToUID(int destID, long int key, Point& direct } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) { key = key + _grid->GetiMax(); } else { - Log->Write("ERROR:\t In getDirectionToUID (3 args)"); + // Log->Write("ERROR:\t In getDirectionToUID (3 args)"); } } if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) { diff --git a/routing/global_shortest/GlobalRouter.cpp b/routing/global_shortest/GlobalRouter.cpp index f34480eee80daae1536e760c7da5e45f2df66b00..2287a6cce42eef857a79fafc4930ab44e2016375 100644 --- a/routing/global_shortest/GlobalRouter.cpp +++ b/routing/global_shortest/GlobalRouter.cpp @@ -890,7 +890,7 @@ int GlobalRouter::GetBestDefaultRandomExit(Pedestrian* ped) } else { - if (_building->GetRoom(ped->GetRoomID())->GetCaption() != "outside") + if (_building->GetRoom(ped->GetRoomID())->GetCaption() != "outside" && relevantAPs.size()>0) { //Log->Write( // @@ -941,8 +941,9 @@ void GlobalRouter::GetRelevantRoutesTofinalDestination(Pedestrian *ped, vectorIsClosed()) - relevantAPS.push_back(ap); + if(ap) + if(!ap->IsClosed()) + relevantAPS.push_back(ap); } } } @@ -1388,7 +1389,7 @@ string GlobalRouter::GetRoutingInfoFile() //wronf //router section nav_line_file=e->FirstChild("parameters")->FirstChildElement("navigation_lines")->Attribute("file"); - + TiXmlElement* para =e->FirstChild("parameters")->FirstChildElement("navigation_mesh"); if (para) { @@ -1491,7 +1492,7 @@ bool GlobalRouter::LoadRoutingInfos(const std::string &filename) if(_building->AddHline(h)) { subroom->AddHline(h); - HlineCount++; + HlineCount++; //h is freed in building } else @@ -1619,7 +1620,7 @@ double GlobalRouter::MinAngle(const Point& p1, const Point& p2, const Point& p3) if(fabs(alpha+beta+gamma-M_PI) vec = { alpha, beta, gamma }; + std::vector vec = { alpha, beta, gamma }; return *std::min_element(vec.begin(), vec.end()) * (180.0 / M_PI); } else