First Working train schedule

- Works only for model=3 + ff_quickest + strategy 8
- Later will be extended to other models

IO/GeoFileParser.cpp

@@ -470,7 +470,7 @@ bool GeoFileParser::parseDoorNode(TiXmlElement * xDoor, int id, Building* buildi
           building->GetTransition(id)->TempClose();
           break;
      case DoorState::Error:
-          Log->Write("WARNING:\t Unknown door state: <%s>. open or close. Default: open",
+          Log->Write("WARNING:\t Unknown door state: <%s>. open, close or temp_close. Default: open",
                     stateStr.c_str());
           building->GetTransition(id)->Open();
           break;
@@ -926,6 +926,7 @@ std::shared_ptr<TrainTimeTable> GeoFileParser::parseTrainTimeTableNode(TiXmlElem
 
      float arrival_time = xmltof(e->Attribute("arrival_time"), -1);
      float departure_time = xmltof(e->Attribute("departure_time"), -1);
+     // @todo: check these values for correctness e.g. arrival < departure
      Log->Write("INFO:\tTrain time table:");
      Log->Write("INFO:\t   id: %d", id);
      Log->Write("INFO:\t   type: %s", type.c_str());
@@ -953,6 +954,8 @@ std::shared_ptr<TrainTimeTable> GeoFileParser::parseTrainTimeTableNode(TiXmlElem
                     train_start,
                     train_end,
                     platform_id,
+                    false,
+                    false,
                     });
 
      return trainTimeTab;

Simulation.cpp

@@ -575,19 +575,36 @@ double Simulation::RunBody(double maxSimTime)
             //here we could place router-tasks (calc new maps) that can use multiple cores AND we have 't'
             //update quickestRouter
             if (_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST)) {
-                FFRouter* ffrouter = dynamic_cast<FFRouter*>(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST));
                 if(geometryChanged)
                 {
-                     ffrouter->Init(_building.get());
-                     std::cout << KBLU << " Init router in simulation\n" << RESET;
-                }
+                     FFRouter* ffrouter2 = dynamic_cast<FFRouter*>(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST));
+                     ffrouter2->Init(_building.get());
+                     // debug
+                     fs::path f("tmp_"+std::to_string(t)+"_"+_config->GetGeometryFile());
+                     std::string filename = f.string();
+                     std::cout << "\n Write geometry --> " << filename.c_str() << "\n";
+                     _building->SaveGeometry(filename);
+                     //
+                     double _deltaH = _building->GetConfig()->get_deltaH();
+                     double _wallAvoidDistance = _building->GetConfig()->get_wall_avoid_distance();
+                     bool _useWallAvoidance = _building->GetConfig()->get_use_wall_avoidance();
+
+                     if(auto dirlocff = dynamic_cast<DirectionLocalFloorfield*>(_building->GetConfig()->get_dirStrategy())){
+                          Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ...");
+                          dirlocff->Init(_building.get(), _deltaH, _wallAvoidDistance, _useWallAvoidance);
+                          Log->Write("INFO:\t Init DirectionLOCALFloorfield done");
+                     }
 
 
-                if (ffrouter->MustReInit()) {
-                    ffrouter->ReInit();
-                    ffrouter->SetRecalc(t);
+                     std::cout << KBLU << " Init router in simulation\n" << RESET;
+                }
+                else{
+                     FFRouter* ffrouter = dynamic_cast<FFRouter*>(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST));
+                     if (ffrouter->MustReInit()) {
+                          ffrouter->ReInit();
+                          ffrouter->SetRecalc(t);
+                     }
                 }
-
             }
 
             // here the used routers are update, when needed due to external changes
@@ -703,18 +720,24 @@ bool Simulation::WriteTrajectories(std::string trajectoryName)
 * add doors
 * set _routingEngine->setNeedUpdate(true);
 */
-bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::string trainType, Point TrackStart, Point TrackEnd)
+bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::shared_ptr<TrainTimeTable> tab)
 {
-      std::cout << "enter with train " << trainType.c_str() << "\n";
-      std::cout<< KBLU << "Enter correctGeometry: Building Has " << building->GetAllTransitions().size() << " Transitions\n" << RESET;
-      //auto platforms = building->GetPlatforms();
-      SubRoom * subroom;
-      int room_id, subroom_id;
-      auto mytrack = building->GetTrackWalls(TrackStart, TrackEnd, room_id, subroom_id);
-      std::cout << "room: " << room_id << " subroom_id " << subroom_id << "\n" ;
-      Room* room = building->GetRoom(room_id);
-      subroom = room->GetSubRoom(subroom_id);//todo safety check
-      int transition_id = 10000;
+     //auto platforms = building->GetPlatforms();
+     int trainId = tab->id;
+     std::string trainType = tab->type;
+     Point TrackStart = tab->pstart;
+     Point TrackEnd = tab->pend;
+     SubRoom * subroom;
+     int room_id, subroom_id;
+     auto mytrack = building->GetTrackWalls(TrackStart, TrackEnd, room_id, subroom_id);
+     Room* room = building->GetRoom(room_id);
+     subroom = room->GetSubRoom(subroom_id);//todo safety check
+     int transition_id = 10000;
+
+     std::cout << "enter with train " << trainType.c_str() << "\n";
+     std::cout<< KBLU << "Enter correctGeometry: Building Has " << building->GetAllTransitions().size() << " Transitions\n" << RESET;
+     std::cout << "room: " << room_id << " subroom_id " << subroom_id << "\n" ;
+
 
       if(mytrack.empty() || subroom == nullptr)
             return false;
@@ -730,7 +753,7 @@ bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::string
       subroom->GetUID();
       auto walls = subroom->GetAllWalls();
       // debugging
-      std::cout << "------\n";
+      // std::cout << "------\n";
       for(auto pw: pws)
       {
             auto pw1 = pw.first;
@@ -739,9 +762,9 @@ bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::string
             auto w1 = pw1.second;
             auto p2 = pw2.first;
             auto w2 = pw2.second;
-            std::cout << "p1 " << p1.toString() << ", wall: " << w1.toString() << "\n";
-            std::cout << "p2 " << p2.toString() << ", wall: " << w2.toString() << "\n";
-            std::cout << "------\n";
+            // std::cout << "p1 " << p1.toString() << ", wall: " << w1.toString() << "\n";
+            // std::cout << "p2 " << p2.toString() << ", wall: " << w2.toString() << "\n";
+            // std::cout << "------\n";
             // case 1
             Point P;
             if(w1 == w2)
@@ -758,10 +781,6 @@ bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::string
                   e->SetSubRoom1(subroom);
                   subroom->AddTransition(e);// danger area
                   building->AddTransition(e);// danger area
-                  std::cout << "added transition\n";
-                  std::cout << "open:  " << e->IsOpen() << "\n" ;
-                  std::cout << "Close:  " << e->IsClose() << "\n";
-                  std::cout << "TempClose:  " << e->IsTempClose() << "\n" ;
                   std::cout<< KGRN << "Transition added. Building Has " << building->GetAllTransitions().size() << " Transitions\n" << RESET;
                   double dist_pt1 = (w1.GetPoint1() - e->GetPoint1()).NormSquare();
                   double dist_pt2 = (w1.GetPoint1() - e->GetPoint2()).NormSquare();
@@ -781,10 +800,10 @@ bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::string
                   Wall NewWall(w1.GetPoint1(), A);
                   Wall NewWall1(w1.GetPoint2(), B);
                   // add new lines to be controled against overlap with exits
-                  building->TempAddedWalls.push_back(NewWall);
-                  building->TempAddedWalls.push_back(NewWall1);
-                  building->TempAddedDoors.push_back(*e);
-                  building->TempRemovedWalls.push_back(w1);
+                  building->TempAddedWalls[trainId].push_back(NewWall);
+                  building->TempAddedWalls[trainId].push_back(NewWall1);
+                  building->TempAddedDoors[trainId].push_back(*e);
+                  building->TempRemovedWalls[trainId].push_back(w1);
                   subroom->AddWall(NewWall);
                   subroom->AddWall(NewWall1);
                   subroom->RemoveWall(w1);
@@ -963,31 +982,40 @@ int Simulation::GetMaxSimTime() const{
 // return true is changes are made to the geometry
 bool Simulation::TrainTraffic()
 {
-     // here open transition that should be closed
-     //        TODO fix, opens door everytime...
      bool trainHere = false;
+     bool trainLeave = false;
      std::string trainType = "";
      Point trackStart, trackEnd;
+     int trainId = 0;
      auto now = Pedestrian::GetGlobalTime();
-     static int once =1;
      for(auto && tab: TrainTimeTables)
      {
-          if( (now>=tab.second->tin) && (now<=tab.second->tout) )
+          trainType = tab.second->type;
+          trainId = tab.second->id;
+          trackStart = tab.second->pstart;
+          trackEnd = tab.second->pend;
+          if(!tab.second->arrival && (now >= tab.second->tin) && (now <= tab.second->tout))
           {
                trainHere = true;
-               trainType = tab.second->type;
-               trackStart = tab.second->pstart;
-               trackEnd = tab.second->pend;
-               continue;
+               TrainTimeTables.at(trainId)->arrival = true;
+               std::cout << KRED << "Arrival: TRAIN " << trainType << " at time: " << now << "\n" << RESET;
+               correctGeometry(_building, tab.second);
+
+          }
+          else if(tab.second->arrival && now >= tab.second->tout)
+          {
+               std::cout <<KGRN << "Departure: TRAIN " << trainType << " at time: " << now  << "\n" << RESET;
+
+               _building->resetGeometry(tab.second);
+               trainLeave = true;
+               TrainTimeTables.at(trainId)->arrival = false;
+
           }
      }
 
      // todo: correctgeometry on arrival of a train. Reset it on departure of train.
-     if(trainHere && once)
+     if(trainHere || trainLeave)
      {
-          correctGeometry(_building, trainType, trackStart, trackEnd);
-          std::cout << KRED << ">> update router\n" << RESET;
-          once=0;
           return true;
      }
 

Simulation.h

@@ -192,7 +192,7 @@ public:
      int GetMaxSimTime() const;
      void  incrementCountTraj();
 
-     bool correctGeometry(std::shared_ptr<Building> building,  std::string trainType, Point TrackStart, Point TrackEnd);
+     bool correctGeometry(std::shared_ptr<Building> building,  std::shared_ptr<TrainTimeTable>);
      bool WriteTrajectories(std::string trajName);
      bool TrainTraffic();
 

geometry/Building.cpp

@@ -410,7 +410,7 @@ const std::vector<Wall> Building::GetTrackWalls(Point TrackStart, Point TrackEnd
             {
                   track_id=track.first;
                   int commonPoints = 0;
-                  std::cout << "\t track " << track.first << "\n";
+                  // std::cout << "\t track " << track.first << "\n";
                   auto walls = track.second;
                   for(auto wall: walls)
                   {
@@ -439,13 +439,15 @@ const std::vector<Wall> Building::GetTrackWalls(Point TrackStart, Point TrackEnd
             room_id = _platforms.at(platform_id)->rid;
             subroom_id = _platforms.at(platform_id)->sid;
             mytrack = _platforms.at(platform_id)->tracks[track_id];
-            std::cout << "track has walls:  " << mytrack.size() << "\n";
-            std::cout << "platform " << platform_id << " track " << track_id << "\n";
-            std::cout << "room " << room_id << " subroom " << subroom_id << "\n";
+            // std::cout << "track has walls:  " << mytrack.size() << "\n";
+            // std::cout << "platform " << platform_id << " track " << track_id << "\n";
+            // std::cout << "room " << room_id << " subroom " << subroom_id << "\n";
       }
       else
       {
-            std::cout << "could not find any track! \n";
+            std::cout << "could not find any track! Exit.\n";
+            exit(-1);
+
       }
       return mytrack;
 }
@@ -490,107 +492,144 @@ const std::vector<std::pair<PointWall, PointWall > > Building::GetIntersectionPo
                         /* std::cout << "intersection at :" << interPoint2.toString() << "\n"; */
                   }
             } // tracks
-            std::cout << "door: " << door.toString() << ", intersections: " <<  nintersections << "\n";
+            // std::cout << "door: " << door.toString() << ", intersections: " <<  nintersections << "\n";
             if(nintersections == 2)
                   pws.push_back(std::make_pair(pw1, pw2));
 
       }// doors
       return pws;
 }
-bool Building::resetTempVectors()
+// reset changes made by trainTimeTable[id]
+bool Building::resetGeometry(std::shared_ptr<TrainTimeTable> tab)
 {
+     // this function is composed of three copy/pasted blocks.
       int room_id, subroom_id;
       SubRoom * subroom;
       // remove temp added walls
-      for(auto wall: TempAddedWalls)
+      for(auto id_wall: TempAddedWalls)
       {
-            for (auto platform: _platforms)
-            {
-                  auto tracks =  platform.second->tracks;
-                  room_id = platform.second->rid;
-                  subroom_id = platform.second->sid;
-                  for(auto r: GetAllRooms())
-                  {
-                        if(r.first != room_id)
-                              continue;
-                        subroom = r.second->GetSubRoom(subroom_id);
-                  }
-
-                  for (auto track : tracks)
-                  {
-                        auto walls = track.second;
-                        for(auto trackWall : walls)
-                        {
-                              if (trackWall == wall)
-                              {
-                                    std::cout<< "todo\n";
+           int i = id_wall.first;
+           if(i != tab->id) continue;
+           auto tempWalls = id_wall.second;
+           for(auto wall: tempWalls)
+           {
+                for (auto platform: _platforms)
+                {
+                     auto tracks =  platform.second->tracks;
+                     room_id = platform.second->rid;
+                     subroom_id = platform.second->sid;
+                     for(auto r: GetAllRooms())
+                     {
+                          if(r.first != room_id)
+                               continue;
+                          subroom = r.second->GetSubRoom(subroom_id);
+                     }
+
+                     for (auto track : tracks)
+                     {
+                          auto walls = track.second;
+                          for(auto trackWall : walls)
+                          {
+                               if (trackWall == wall)
+                               {
                                     subroom->RemoveWall(wall);
-                              }
-                        }
-                  }
-            }
+                                    auto it = std::find(tempWalls.begin(), tempWalls.end(), wall);
+                                          if (it != tempWalls.end())
+                                          {
+                                                // std::cout<< KGRN << "delete wall ..." << RESET <<std::endl;
+                                                // wallPiece.WriteToErrorLog();
+                                               tempWalls.erase(it);
+                                          }
+                               }
+                          }
+                     }
+                }
+           }
       }
+
 /*       // add remove walls */
-      for(auto wall: TempRemovedWalls)
+      for(auto id_wall: TempRemovedWalls)
       {
-            for (auto platform: _platforms)
-            {
-                  auto tracks =  platform.second->tracks;
-                  room_id = platform.second->rid;
-                  subroom_id = platform.second->sid;
-                  for(auto r: GetAllRooms())
-                  {
-                        if(r.first != room_id)
-                              continue;
-                        subroom = r.second->GetSubRoom(subroom_id);
-                  }
-                  for (auto track : tracks)
-                  {
-                       auto walls = track.second;
-                       for(auto trackWall : walls)
-                       {
-                             if (trackWall == wall)
-                             {
-                                   subroom->AddWall(wall);
-                                   std::cout<< "todo\n";
-                             }
-                       }
-                  }
-            }
+           int i = id_wall.first;
+           if(i != tab->id) continue;
+           auto tempWalls = id_wall.second;
+
+           for(auto wall: tempWalls)
+           {
+                for (auto platform: _platforms)
+                {
+                     auto tracks =  platform.second->tracks;
+                     room_id = platform.second->rid;
+                     subroom_id = platform.second->sid;
+                     for(auto r: GetAllRooms())
+                     {
+                          if(r.first != room_id)
+                               continue;
+                          subroom = r.second->GetSubRoom(subroom_id);
+                     }
+                     for (auto track : tracks)
+                     {
+                          auto walls = track.second;
+                          for(auto trackWall : walls)
+                          {
+                               if (trackWall == wall)
+                               {
+                                    subroom->AddWall(wall);
+                                    auto it = std::find(tempWalls.begin(), tempWalls.end(), wall);
+                                    if (it != tempWalls.end())
+                                    {
+                                         // std::cout<< KGRN << "delete wall ..." << RESET <<std::endl;
+                                         // wallPiece.WriteToErrorLog();
+                                         tempWalls.erase(it);
+                                    }
+                               }
+                          }
+                     }
+                }
+           }
       }
 
 /*       // remove added doors */
-      for(auto door: TempAddedDoors)
+      for(auto id_wall: TempAddedDoors)
       {
-            for (auto platform: _platforms)
-            {
-                  auto tracks =  platform.second->tracks;
-                  room_id = platform.second->rid;
-                  subroom_id = platform.second->sid;
-                  for(auto r: GetAllRooms())
-                  {                        if(r.first != room_id)
-                              continue;
-                        subroom = r.second->GetSubRoom(subroom_id);
-                  }
-
-                  for (auto track : tracks)
-                  {
-                        auto walls = track.second;
-                        for(auto trackWall : walls)
-                        {
-                              if (trackWall == door)
-                              {
+           int i = id_wall.first;
+           if(i != tab->id) continue;
+           auto tempWalls = id_wall.second;
+
+           for(auto door: tempWalls)
+           {
+                for (auto platform: _platforms)
+                {
+                     auto tracks =  platform.second->tracks;
+                     room_id = platform.second->rid;
+                     subroom_id = platform.second->sid;
+                     for(auto r: GetAllRooms())
+                     {                        if(r.first != room_id)
+                               continue;
+                          subroom = r.second->GetSubRoom(subroom_id);
+                     }
+
+                     for (auto track : tracks)
+                     {
+                          auto walls = track.second;
+                          for(auto trackWall : walls)
+                          {
+                               if (trackWall == door)
+                               {
                                     subroom->RemoveTransition(&door);
-                                    std::cout<< "todo\n";
-                              }
-                        }
-                  }
-            }
+                                    auto it = std::find(tempWalls.begin(), tempWalls.end(), door);
+                                    if (it != tempWalls.end())
+                                    {
+                                         // std::cout<< KGRN << "delete wall ..." << RESET <<std::endl;
+                                         // wallPiece.WriteToErrorLog();
+                                         tempWalls.erase(it);
+                                    }
+                               }
+                          }
+                     }
+                }
+           }
       }
-
-      TempAddedWalls.clear();
-      TempRemovedWalls.clear();
-      TempAddedDoors.clear();
       return true;
 }
 bool Building::InitPlatforms()

geometry/Building.h

@@ -70,6 +70,8 @@ struct TrainTimeTable
      Point tstart; // train start
      Point tend; // train end
      int pid; // Platform id
+     bool arrival;
+     bool departure;
 };
 struct TrainType
 {
@@ -119,15 +121,13 @@ private:
 public:
      /// constructor
      Building();
-     std::vector<Wall> TempAddedWalls;
-     std::vector<Wall> TempRemovedWalls;
-     std::vector<Transition> TempAddedDoors;
-     bool resetTempVectors();
-
+     std::map<int, std::vector<Wall> > TempAddedWalls; // map to trainTimeTable
+     std::map<int, std::vector<Wall> > TempRemovedWalls;
+     std::map<int, std::vector<Transition> > TempAddedDoors;
 
 //    Building(const std::string &, const std::string &, RoutingEngine &, PedDistributor &, double);
      Building(Configuration* config, PedDistributor& pedDistributor);
-
+     bool resetGeometry(std::shared_ptr<TrainTimeTable> tab);
      /// destructor
      virtual ~Building();
 
@@ -406,7 +406,7 @@ private:
       * @param subroom
       * @return bool
       */
-bool RemoveOverlappingDoors(
+     bool RemoveOverlappingDoors(
           const std::shared_ptr<SubRoom>& subroom) const;
      /** @} */ // end of group
 

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].