FF navigates people inside waiting area. people are waiting inside and after...

FF navigates people inside waiting area. people are waiting inside and after time going to next goal

IO/GeoFileParser.cpp

@@ -423,7 +423,7 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
 
                Goal* wa = new WaitingArea();
                WaitingArea* waitingArea = static_cast<WaitingArea*>(wa);
-               waitingArea->SetIsFinalGoal(1);
+               waitingArea->SetIsFinalGoal(0);
                waitingArea->SetId(id);
                waitingArea->SetCaption(caption);
                waitingArea->setOpen(open);
@@ -471,7 +471,7 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
                     }
                }
 
-               if (!wa->ConvertLineToPoly()){
+               if (!waitingArea->ConvertLineToPoly()){
                     std::cout << "wa convertLineToPoly!";
                     return false;
                }
@@ -490,6 +490,8 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
 
                building->AddGoal(wa);
                _configuration->GetRoutingEngine()->AddFinalDestinationID(wa->GetId());
+
+
                for (auto &itrGoal : building->GetAllGoals()) {
                     std::cout << "Goal ID: " << itrGoal.second->GetId() << std::endl;
                }

Simulation.cpp

@@ -36,6 +36,8 @@
 #include "math/GradientModel.h"
 #include "pedestrian/AgentsQueue.h"
 #include "pedestrian/AgentsSourcesManager.h"
+#include "geometry/WaitingArea.h"
+
 #ifdef _OPENMP
 
 #else
@@ -318,7 +320,7 @@ void Simulation::UpdateRoutesAndLocations()
                pedsToRemove.insert(ped);
           } else if ((ped->GetFinalDestination() != FINAL_DEST_OUT)
                     && (goals.at(ped->GetFinalDestination())->Contains(
-                              ped->GetPos()))) {
+                              ped->GetPos()))&& (goals.at(ped->GetFinalDestination())->GetIsFinalGoal())) {
 #pragma omp critical(Simulation_Update_pedsToRemove)
                pedsToRemove.insert(ped);
           }

geometry/Building.cpp

@@ -363,12 +363,44 @@ bool Building::InitGeometry()
           if (s2) s2->AddNeighbor(s1);
      }
 
-
+     InitInsideGoals();
      Log->Write("INFO: \tInit Geometry successful!!!\n");
 
      return true;
 }
 
+bool Building::InitInsideGoals()
+{
+     for (auto& goalItr : _goals){
+          Goal* goal = goalItr.second;
+          for (auto& roomItr : _rooms){
+               Room* room = roomItr.second.get();
+               for (auto& subRoomItr : room->GetAllSubRooms()){
+                    SubRoom* subRoom = subRoomItr.second.get();
+                    if (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);
+
+                         for (auto& cross : subRoom->GetAllCrossings()){
+                              std::cout << "Crossing Subroom: " << cross->GetUniqueID() << std::endl;
+                         }
+
+                         for (auto& cross: _crossings){
+                              std::cout << "Crossing Building: " << cross.second->GetUniqueID() << std::endl;
+                         }
+
+                    }
+               }
+          }
+     }
+
+     return true;
+}
 bool Building::correct() const {
      auto t_start = std::chrono::high_resolution_clock::now();
      Log->Write("INFO:\tenter correct ...");

geometry/Building.h

@@ -187,6 +187,7 @@ public:
      // convenience methods
      bool InitGeometry();
 
+
      void InitGrid();
 
      //void InitRoomsAndSubroomsMap();
@@ -254,7 +255,10 @@ public:
      std::vector<Point> GetBoundaryVertices() const;
 
 private:
-     void StringExplode(std::string str, std::string separator, std::vector<std::string>* results);
+
+    bool InitInsideGoals();
+
+    void StringExplode(std::string str, std::string separator, std::vector<std::string>* results);
      /** @defgroup auto-correct-geometry
       * functions used to auto-correct the geometry.
       * Main function is correct()

geometry/Goal.cpp

@@ -29,7 +29,7 @@
 #include "Point.h"
 #include "Goal.h"
 #include "Wall.h"
-
+#include "Crossing.h"
 
 using namespace std;
 
@@ -41,6 +41,7 @@ Goal::Goal()
      _isFinalGoal=0;
      _walls = vector<Wall > ();
      _poly = vector<Point > ();
+     _crossing = Crossing();
 }
 
 Goal::~Goal()
@@ -221,7 +222,29 @@ bool Goal::ConvertLineToPoly()
           return false;
      }
      _poly = tmpPoly;
-     ComputeControid();
+     CreateBoostPoly();
+     ComputeCentroid();
+
+     //compute dummy crossing in the middle
+     Point point1, point2, tmp, diff, diff1, diff2;
+     if (_poly.size() %2 == 0){
+          point1 = _poly[0];
+          tmp = _poly[_poly.size()/2];
+          diff = point1 - tmp;
+
+          point1 = tmp +  diff * 0.95;
+          point2 = tmp +  diff * 0.05;
+
+          _crossing.SetPoint1(point1);
+          _crossing.SetPoint2(point2);
+     }else{
+          _crossing.SetPoint1(_poly[0]);
+          Line line(_poly[_poly.size()/2], _poly[(_poly.size()/2)+1], 0);
+          _crossing.SetPoint2(line.GetCentre());
+     }
+
+
+     std::cout << "Crossing goal: " << _crossing.GetUniqueID() << _crossing.toString() << std::endl;
      return true;
 }
 
@@ -230,7 +253,7 @@ const Point& Goal::GetCentroid() const
      return _centroid;
 }
 
-void  Goal::ComputeControid()
+void  Goal::ComputeCentroid()
 {
 
      double px=0,py=0;
@@ -271,3 +294,52 @@ void  Goal::ComputeControid()
      _centroid._x=px;
      _centroid._y=py;
 }
+
+Crossing& Goal::GetCentreCrossing()
+{
+     return _crossing;
+}
+
+//bool Goal::IsInsideGoal(Pedestrian* ped) const
+//{
+//     return IsInsideGoal(ped->GetPos());
+//}
+
+bool Goal::IsInsideGoal(const Point& point) const
+{
+     return boost::geometry::within(point, _boostPoly);
+}
+
+bool Goal::CreateBoostPoly() {
+     std::vector<Point> copyPts;
+     copyPts.insert(copyPts.begin(), _poly.begin(), _poly.end());
+
+     if(!IsClockwise()){
+          std::reverse(copyPts.begin(), copyPts.end());
+     }
+
+     boost::geometry::assign_points(_boostPoly, _poly);
+
+     return true;
+}
+
+bool Goal::IsClockwise()
+{
+     //http://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order
+     if(_poly.size()<3) {
+          Log->Write("ERROR:\tYou need at least 3 vertices to check for orientation. Obstacle ID [%d]",_id);
+          return false;
+          //exit(EXIT_FAILURE);
+     }
+     double sum = 0;
+     for (unsigned int i = 0; i < _poly.size() - 1; ++i) {
+          Point a = _poly[i];
+          Point b = _poly[i+1];
+          sum += (b._x - a._x) * (b._y + a._y);
+     }
+     Point first = _poly[0];
+     Point last = _poly[_poly.size()-1];
+     sum += (first._x - last._x) * (first._y + last._y);
+
+     return (sum > 0.);
+}

geometry/Goal.h

@@ -33,11 +33,17 @@
 #include <vector>
 
 #include "Point.h"
+#include "Crossing.h"
+#include <boost/polygon/polygon.hpp>
+#include <boost/geometry.hpp>
 
 //forward declarations
 class Wall;
-//class Point;
+class Point;
+//class Pedestrian;
 
+namespace bg = boost::geometry;
+typedef bg::model::polygon<Point, false, false> polygon_type;
 
 class Goal {
 
@@ -48,6 +54,10 @@ protected:
      std::string _caption;
      std::vector<Wall> _walls;
      std::vector<Point> _poly;
+     Crossing _crossing;
+
+     polygon_type _boostPoly;
+     bool _inside;
 
 public:
      Goal();
@@ -112,7 +122,7 @@ public:
       * @return the centroid of the subroom
       * @see http://en.wikipedia.org/wiki/Centroid
       */
-     void ComputeControid() ;
+     void ComputeCentroid() ;
 
      /**
       * @return the centroid of the goal
@@ -125,8 +135,17 @@ public:
       */
      std::string Write();
 
+     Crossing& GetCentreCrossing();
+
+//     bool IsInsideGoal(Pedestrian* ped) const;
+
+     bool IsInsideGoal(const Point& point) const;
+     
 private:
-     int WhichQuad(const Point& vertex, const Point& hitPos) const;
+    bool IsClockwise();
+    bool CreateBoostPoly();
+
+    int WhichQuad(const Point& vertex, const Point& hitPos) const;
 
      // x-Koordinate der Linie von einer Eccke zur nächsten
      double Xintercept(const Point& point1, const Point& point2,

geometry/WaitingArea.cpp

@@ -115,4 +115,40 @@ int WaitingArea::GetNextGoal()
           }
      }
 
-}
\ No newline at end of file
+}
+
+void WaitingArea::addPed()
+{
+     numPed++;
+}
+
+void WaitingArea::removePed()
+{
+     numPed--;
+}
+
+void WaitingArea::startTimer(double time)
+{
+     startTime = time;
+     std::cout << "Timer started at " << startTime << std::endl;
+}
+
+bool WaitingArea::isWaiting(double time)
+{
+     if ((numPed >= minNumPed) && (startTime < 0. )){
+          startTimer(time);
+     }
+
+     if ((time < startTime + waitingTime) || (startTime < 0. )){
+          std::cout << "Waiting ..." << std::endl;
+          return true;
+     }else{
+          std::cout << "Waiting over!" << std::endl;
+          return false;
+     }
+}
+
+int WaitingArea::getNumPed()
+{
+     return numPed;
+}

geometry/WaitingArea.h

@@ -28,6 +28,9 @@ protected:
 
     double waitingTime = -1;
 
+    double startTime = -1;
+
+    int numPed = 0;
 protected:
 
     /**
@@ -68,6 +71,12 @@ public:
 
     int GetNextGoal();
 
+    void addPed();
+    void removePed();
+    void startTimer(double time);
+    int  getNumPed();
+    bool isWaiting(double time);
+
 private:
     void updateProbabilities();
     bool checkProbabilities();

routing/ff_router_trips/FloorfieldViaFMTrips.cpp

@@ -368,38 +368,42 @@ void FloorfieldViaFMTrips::createMapEntryInLineToGoalID(const int goalID, bool i
             long int dummykey;
 
             if (isInside) {
-                for (const auto& loccross : crossings) {
-                    std::cout << "Check crossing ID " << loccross.second->GetID() << " " << loccross.second->toString() << std::endl;
-                    if ( !loccross.second->IsOpen()) {
-                        continue;
-                    }
-                    dummykey = _grid->getKeyAtPoint(loccross.second->GetCentre());
-                    if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
-                        UID_of_MIN3 = UID_of_MIN2;
-                        cost_of_MIN3 = cost_of_MIN2;
-
-                        UID_of_MIN2 = UID_of_MIN;
-                        cost_of_MIN2 = cost_of_MIN;
-
-                        UID_of_MIN = loccross.second->GetUniqueID();
-                        cost_of_MIN = localcostptr[dummykey];
-                        std::cout << "Closer Line found: " << UID_of_MIN << std::endl;
-                        continue;
-                    }
-                    if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
-                        UID_of_MIN3 = UID_of_MIN2;
-                        cost_of_MIN3 = cost_of_MIN2;
-
-                        UID_of_MIN2 = loccross.second->GetUniqueID();
-                        cost_of_MIN2 = localcostptr[dummykey];
-                        continue;
-                    }
-                    if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
-                        UID_of_MIN3 = loccross.second->GetUniqueID();
-                        cost_of_MIN3 = localcostptr[dummykey];
-                        continue;
-                    }
-                }
+//                for (const auto& loccross : crossings) {
+//                    std::cout << "Check crossing ID " << loccross.second->GetID() << " " << loccross.second->toString() << std::endl;
+//                    if ( !loccross.second->IsOpen()) {
+//                        continue;
+//                    }
+//                    dummykey = _grid->getKeyAtPoint(loccross.second->GetCentre());
+//                    if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+//                        UID_of_MIN3 = UID_of_MIN2;
+//                        cost_of_MIN3 = cost_of_MIN2;
+//
+//                        UID_of_MIN2 = UID_of_MIN;
+//                        cost_of_MIN2 = cost_of_MIN;
+//
+//                        UID_of_MIN = loccross.second->GetUniqueID();
+//                        cost_of_MIN = localcostptr[dummykey];
+//                        std::cout << "Closer Line found: " << UID_of_MIN << " " << cost_of_MIN << std::endl;
+//                        continue;
+//                    }
+//                    if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+//                        UID_of_MIN3 = UID_of_MIN2;
+//                        cost_of_MIN3 = cost_of_MIN2;
+//
+//                        UID_of_MIN2 = loccross.second->GetUniqueID();
+//                        cost_of_MIN2 = localcostptr[dummykey];
+//                        continue;
+//                    }
+//                    if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+//                        UID_of_MIN3 = loccross.second->GetUniqueID();
+//                        cost_of_MIN3 = localcostptr[dummykey];
+//                        continue;
+//                    }
+//
+//                    UID_of_MIN = 33;
+//
+//                }
+                UID_of_MIN = allgoals.at(goalID)->GetCentreCrossing().GetUniqueID();
             }
             else {
                 for (const auto& loctrans : transitions) {

routing/ff_router_trips/UnivFFviaFMTrips.cpp

@@ -114,20 +114,20 @@ UnivFFviaFMTrips::UnivFFviaFMTrips(Room* roomArg, Configuration* const confArg,
           }
 
 //          //save all waiting area walls in tmpdoors
-          for (auto& goalMap : goals){
-               Goal* goal = goalMap.second;
-               if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
-                    std::cout << "Testing for subroom " << subRoomPtr->GetSubRoomID() << std::endl;
-                    if ((subRoomPtr->IsInSubRoom(wa->GetCentroid())) && (wa->isOpen())){
-                         for (const Wall wall : wa->GetAllWalls()){
-                              int uid = wall.GetUniqueID();
-                              if (tmpDoors.count(uid) == 0) {
-                                   tmpDoors.emplace(std::make_pair(uid, (Line) wall));
-                              }
-                         }
-                    }
-               }
-          }
+//          for (auto& goalMap : goals){
+//               Goal* goal = goalMap.second;
+//               if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
+//                    std::cout << "Testing for subroom " << subRoomPtr->GetSubRoomID() << std::endl;
+//                    if ((subRoomPtr->IsInSubRoom(wa->GetCentroid())) && (wa->isOpen())){
+//                         for (const Wall wall : wa->GetAllWalls()){
+//                              int uid = wall.GetUniqueID();
+//                              if (tmpDoors.count(uid) == 0) {
+//                                   tmpDoors.emplace(std::make_pair(uid, (Line) wall));
+//                              }
+//                         }
+//                    }
+//               }
+//          }
 
           for (auto& door : tmpDoors){
                std::cout << "uid: " << door.first << " door: " << door.second.toString() << std::endl;

routing/ff_router_trips/ffRouterTrips.cpp

@@ -116,6 +116,11 @@ bool FFRouterTrips::Init(Building* building)
                goalIDs.emplace_back(itrGoal.first);
           }
           _goalToLineUIDmap = _globalFF->getGoalToLineUIDmap();
+
+          for (auto& tmp : _goalToLineUIDmap) {
+               std::cout << "Goal: " << tmp.first << " Crossing: " << tmp.second << std::endl;
+          }
+
           _goalToLineUIDmap2 = _globalFF->getGoalToLineUIDmap2();
           _goalToLineUIDmap3 = _globalFF->getGoalToLineUIDmap3();
           _globalFF->writeGoalFF("goal.vtk", goalIDs);
@@ -153,34 +158,34 @@ bool FFRouterTrips::Init(Building* building)
                if (room1) roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID()));
           }
      }
-     for (auto& goalMap : allGoals) {
-          Goal* goal = goalMap.second;
-          if (WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
-               if (wa->isOpen()){
-                    int roomID;
-                    for (auto& room : _building->GetAllRooms()){
-                         for (auto& subroom : room.second->GetAllSubRooms()){
-                              if (subroom.second->IsInSubRoom(wa->GetCentroid())){
-                                   roomID = room.second->GetID();
-                              }
-                         }
-                    }
-
-                    for (const Wall wall : wa->GetAllWalls()) {
-                         int uid = wall.GetUniqueID();
-                         if (std::find(_allDoorUIDs.begin(), _allDoorUIDs.end(), uid) == _allDoorUIDs.end()) {
-                              _allDoorUIDs.emplace_back(uid);
-                              Crossing cross;
-                              cross.SetPoint1(wall.GetPoint1());
-                              cross.SetPoint2(wall.GetPoint2());
-                              cross.SetCaption(wa->GetCaption());
-                              _CroTrByUID.insert(std::make_pair(uid, &cross));
-                              roomAndCroTrVector.emplace_back(roomID, uid);
-                         }
-                    }
-               }
-          }
-     }
+//     for (auto& goalMap : allGoals) {
+//          Goal* goal = goalMap.second;
+//          if (WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
+//               if (wa->isOpen()){
+//                    int roomID;
+//                    for (auto& room : _building->GetAllRooms()){
+//                         for (auto& subroom : room.second->GetAllSubRooms()){
+//                              if (subroom.second->IsInSubRoom(wa->GetCentroid())){
+//                                   roomID = room.second->GetID();
+//                              }
+//                         }
+//                    }
+//
+//                    for (const Wall wall : wa->GetAllWalls()) {
+//                         int uid = wall.GetUniqueID();
+//                         if (std::find(_allDoorUIDs.begin(), _allDoorUIDs.end(), uid) == _allDoorUIDs.end()) {
+//                              _allDoorUIDs.emplace_back(uid);
+//                              Crossing cross;
+//                              cross.SetPoint1(wall.GetPoint1());
+//                              cross.SetPoint2(wall.GetPoint2());
+//                              cross.SetCaption(wa->GetCaption());
+//                              _CroTrByUID.insert(std::make_pair(uid, &cross));
+//                              roomAndCroTrVector.emplace_back(roomID, uid);
+//                         }
+//                    }
+//               }
+//          }
+//     }
 
      //make unique
      std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end());
@@ -485,11 +490,18 @@ int FFRouterTrips::FindExit(Pedestrian* ped)
 
      // Check if current position is already waiting area
      // yes: set next goal and return findExit(p)
-     if (subroom->IsInSubRoom(goal->GetCentroid())){
+     if (goal->IsInsideGoal(ped->GetPos())){
           std::cout << "Ped and Goal in same subroom: " << subroom->IsInSubRoom(goal->GetCentroid()) << std::endl;
           std::cout << "Ped Final Destination before: " << ped->GetFinalDestination() << std::endl;
           if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
-               ped->SetFinalDestination(wa->GetNextGoal());
+               //take the current time from the pedestrian
+               double t = Pedestrian::GetGlobalTime();
+
+               wa->addPed();
+
+               if (!wa->isWaiting(t)){
+                    ped->SetFinalDestination(wa->GetNextGoal());
+               }
           }
           std::cout << "Ped Final Destination after: " << ped->GetFinalDestination() << std::endl;
           ret = FindExit1(ped);