From 23d09e20a0aed5e216e9a8bcc8b725141a8b7134 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Schr=C3=B6dter?= <t.schroedter@fz-juelich.de>
Date: Sun, 3 Feb 2019 16:46:24 +0100
Subject: [PATCH] FF now navigates from waiting area to final goal
---
geometry/WaitingArea.cpp | 15 ++++++++++++++-
geometry/WaitingArea.h | 2 ++
routing/ff_router_trips/ffRouterTrips.cpp | 10 ++++++----
3 files changed, 22 insertions(+), 5 deletions(-)
diff --git a/geometry/WaitingArea.cpp b/geometry/WaitingArea.cpp
index e82d277d..653e3991 100644
--- a/geometry/WaitingArea.cpp
+++ b/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
diff --git a/geometry/WaitingArea.h b/geometry/WaitingArea.h
index 3070ad45..390211de 100644
--- a/geometry/WaitingArea.h
+++ b/geometry/WaitingArea.h
@@ -66,6 +66,8 @@ public:
std::vector<Line> getAllLines();
+ int GetNextGoal();
+
private:
void updateProbabilities();
bool checkProbabilities();
diff --git a/routing/ff_router_trips/ffRouterTrips.cpp b/routing/ff_router_trips/ffRouterTrips.cpp
index e16e4eb1..bcbe2685 100644
--- a/routing/ff_router_trips/ffRouterTrips.cpp
+++ b/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);
}
--
GitLab