diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp
index 17913cdd1cdbb0f56894932594a3bf5c6fa862b4..2a86c419c6b84c4a040685872e792e6fcd69b61a 100644
--- a/routing/ff_router/UnivFFviaFM.cpp
+++ b/routing/ff_router/UnivFFviaFM.cpp
@@ -1358,6 +1358,9 @@ void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID
//mode is argument, which should not be needed, the info is stored in members like speedmode, ...
double UnivFFviaFM::getCostToDestination(const int destID, const Point& position, int mode) {
+// if (!_grid->includesPoint(position)) {
+// Log->Write("Was ist denn hier los?");
+// }
assert(_grid->includesPoint(position));
if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
return _costFieldWithKey[destID][_grid->getKeyAtPoint(position)];
@@ -1370,7 +1373,7 @@ double UnivFFviaFM::getCostToDestination(const int destID, const Point& position
}
addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
- getCostToDestination(destID, position, mode);
+ return getCostToDestination(destID, position, mode);
} else if (!_directCalculation && _doors.count(destID) > 0) {
//omp critical
#pragma omp critical(UnivFFviaFM_toDo)
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
index 580da024400398ee9465fd02a0fe2e0934f8c709..d2d2c1dac4b08ee9928ce28464dabc4318ed3cb6 100644
--- a/routing/ff_router/ffRouter.cpp
+++ b/routing/ff_router/ffRouter.cpp
@@ -456,59 +456,77 @@ int FFRouter::FindExit(Pedestrian* p)
if (_building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->IsInSubRoom(p->GetPos())) {
//ped is in the subroom, according to its member attribs
} else {
- // find next crossing / transition and set new room to the OTHER
- //candidates of current room (ID) (provided by Room)
- for (auto transUID : _building->GetRoom(p->GetRoomID())->GetAllTransitionsIDs()) {
- if ((_CroTrByUID.count(transUID) != 0) && (_CroTrByUID[transUID]->IsOpen())) {
- DoorUIDsOfRoom.emplace_back(transUID);
- }
- }
- for (auto &subIPair : _building->GetRoom(p->GetRoomID())->GetAllSubRooms()) {
- for (auto &crossI : subIPair.second->GetAllCrossings()) {
- if (crossI->IsOpen()) {
- DoorUIDsOfRoom.emplace_back(crossI->GetUniqueID());
+ bool located = false;
+ SubRoom* oldSubRoom = _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID());
+ for (auto& room : _building->GetAllRooms()) {
+ if (located) {break;}
+ for (auto& subroom : room.second->GetAllSubRooms()) {
+ if (subroom.second->IsInSubRoom(p->GetPos()) && subroom.second->IsDirectlyConnectedWith(oldSubRoom)) {
+ //maybe room on wrong floor
+ p->SetRoomID(room.second->GetID(), room.second->GetCaption());
+ p->SetSubRoomID(subroom.second->GetSubRoomID());
+ p->SetSubRoomUID(subroom.second->GetUID());
+ located = true;
+ break;
}
}
}
- //find closest Door
- int UIDofLinePedStandsOn;
- double UIDsDistance = DBL_MAX;
- for (auto dooruid : DoorUIDsOfRoom) {
- if (UIDsDistance > _locffviafm[p->GetRoomID()]->getCostToDestination(dooruid, p->GetPos(), _mode)) {
- UIDofLinePedStandsOn = dooruid;
- UIDsDistance = _locffviafm[p->GetRoomID()]->getCostToDestination(dooruid, p->GetPos(), _mode);
- }
- }
-
- //to that doors, find the (room/subroom) tuples of both sides of the door and then choose the one, that is not
- //equal to "pedTupel" (which is false information).
- //@todo: @ar.graf: if no second tupel exists, and pedTupel is false, the agent must be outside, right?
- std::pair<int, int> pedTupel = std::make_pair(p->GetRoomID(), p->GetSubRoomID());
-
- std::pair<int, int> roomNsub1 = std::make_pair(
- _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetRoomID(),
- _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetSubRoomID());
- std::pair<int, int> roomNsub2;
- if (_building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom2()) {
- roomNsub2 = std::make_pair(
- _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom2()->GetRoomID(),
- _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom2()->GetSubRoomID());
- } else { // if no second exists, then door leads to outside and nothing should change, which we do by roomNsub1 == roomNsub2
- roomNsub2 = std::make_pair(
- _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetRoomID(),
- _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetSubRoomID());
- }
- if (pedTupel == roomNsub1) {
- p->SetRoomID(roomNsub2.first, _building->GetRoom(roomNsub2.first)->GetCaption());
- p->SetSubRoomID(roomNsub2.second);
- p->SetSubRoomUID(_building->GetRoom(roomNsub2.first)->GetSubRoom(roomNsub2.second)->GetUID());
- } else {
- p->SetRoomID(roomNsub1.first, _building->GetRoom(roomNsub1.first)->GetCaption());
- p->SetSubRoomID(roomNsub1.second);
- p->SetSubRoomUID(_building->GetRoom(roomNsub1.first)->GetSubRoom(roomNsub1.second)->GetUID());
+ if (!located) { //ped is outside
+ return -1;
}
-
- //Log->Write("ERROR: \tffRouter cannot handle incorrect room/subroom attribs of pedestrian %d!!", p->GetID());
+// // find next crossing / transition and set new room to the OTHER
+// //candidates of current room (ID) (provided by Room)
+// for (auto transUID : _building->GetRoom(p->GetRoomID())->GetAllTransitionsIDs()) {
+// if ((_CroTrByUID.count(transUID) != 0) && (_CroTrByUID[transUID]->IsOpen())) {
+// DoorUIDsOfRoom.emplace_back(transUID);
+// }
+// }
+// for (auto &subIPair : _building->GetRoom(p->GetRoomID())->GetAllSubRooms()) {
+// for (auto &crossI : subIPair.second->GetAllCrossings()) {
+// if (crossI->IsOpen()) {
+// DoorUIDsOfRoom.emplace_back(crossI->GetUniqueID());
+// }
+// }
+// }
+// //find closest Door
+// int UIDofLinePedStandsOn;
+// double UIDsDistance = DBL_MAX;
+// for (auto dooruid : DoorUIDsOfRoom) {
+// if (UIDsDistance > _locffviafm[p->GetRoomID()]->getCostToDestination(dooruid, p->GetPos(), _mode)) {
+// UIDofLinePedStandsOn = dooruid;
+// UIDsDistance = _locffviafm[p->GetRoomID()]->getCostToDestination(dooruid, p->GetPos(), _mode);
+// }
+// }
+//
+// //to that doors, find the (room/subroom) tuples of both sides of the door and then choose the one, that is not
+// //equal to "pedTupel" (which is false information).
+// //@todo: @ar.graf: if no second tupel exists, and pedTupel is false, the agent must be outside, right?
+// std::pair<int, int> pedTupel = std::make_pair(p->GetRoomID(), p->GetSubRoomID());
+//
+// std::pair<int, int> roomNsub1 = std::make_pair(
+// _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetRoomID(),
+// _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetSubRoomID());
+// std::pair<int, int> roomNsub2;
+// if (_building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom2()) {
+// roomNsub2 = std::make_pair(
+// _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom2()->GetRoomID(),
+// _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom2()->GetSubRoomID());
+// } else { // if no second exists, then door leads to outside and nothing should change, which we do by roomNsub1 == roomNsub2
+// roomNsub2 = std::make_pair(
+// _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetRoomID(),
+// _building->GetTransOrCrossByUID(UIDofLinePedStandsOn)->GetSubRoom1()->GetSubRoomID());
+// }
+// if (pedTupel == roomNsub1) {
+// p->SetRoomID(roomNsub2.first, _building->GetRoom(roomNsub2.first)->GetCaption());
+// p->SetSubRoomID(roomNsub2.second);
+// p->SetSubRoomUID(_building->GetRoom(roomNsub2.first)->GetSubRoom(roomNsub2.second)->GetUID());
+// } else {
+// p->SetRoomID(roomNsub1.first, _building->GetRoom(roomNsub1.first)->GetCaption());
+// p->SetSubRoomID(roomNsub1.second);
+// p->SetSubRoomUID(_building->GetRoom(roomNsub1.first)->GetSubRoom(roomNsub1.second)->GetUID());
+// }
+//
+// //Log->Write("ERROR: \tffRouter cannot handle incorrect room/subroom attribs of pedestrian %d!!", p->GetID());
}
DoorUIDsOfRoom.clear();
if (!_targetWithinSubroom) {