FF now navigates from waiting area to final goal

geometry/WaitingArea.cpp

@@ -101,5 +101,18 @@ void WaitingArea::setWaitingTime(double waitingTime)
 
 std::vector<Line> WaitingArea::getAllLines()
 {
+}
+
+int WaitingArea::GetNextGoal()
+{
+     double random = std::rand()/RAND_MAX;
+     double cumProb = 0.;
+
+     for (auto& nextGoal : nextGoals){
+          cumProb += nextGoal.second;
+          if (random <= cumProb ){
+               return nextGoal.first;
+          }
+     }
 
 }
\ No newline at end of file

geometry/WaitingArea.h

@@ -66,6 +66,8 @@ public:
 
     std::vector<Line> getAllLines();
 
+    int GetNextGoal();
+
 private:
     void updateProbabilities();
     bool checkProbabilities();

routing/ff_router_trips/ffRouterTrips.cpp

@@ -487,10 +487,12 @@ int FFRouterTrips::FindExit(Pedestrian* ped)
      // yes: set next goal and return findExit(p)
      if (subroom->IsInSubRoom(goal->GetCentroid())){
           std::cout << "Ped and Goal in same subroom: " << subroom->IsInSubRoom(goal->GetCentroid()) << std::endl;
-          int bestDoor = 31;
-          ped->SetExitIndex(bestDoor);
-          ped->SetExitLine(_CroTrByUID.at(bestDoor));
-
+          std::cout << "Ped Final Destination before: " << ped->GetFinalDestination() << std::endl;
+          if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
+               ped->SetFinalDestination(wa->GetNextGoal());
+          }
+          std::cout << "Ped Final Destination after: " << ped->GetFinalDestination() << std::endl;
+          ret = FindExit1(ped);
      }else{
           ret = FindExit1(ped);
      }