From c58f05d5746086337e4bf09df017b91d370a38ce Mon Sep 17 00:00:00 2001
From: Arne Graf <ar.graf.cst@gmail.com>
Date: Fri, 4 May 2018 16:00:19 +0200
Subject: [PATCH] fixed the ff_router bug: standing on lines
---
math/VelocityModel.cpp | 1 +
routing/ff_router/UnivFFviaFM.cpp | 24 ++++++++++++++++++------
routing/ff_router/ffRouter.cpp | 8 ++++++--
3 files changed, 25 insertions(+), 8 deletions(-)
diff --git a/math/VelocityModel.cpp b/math/VelocityModel.cpp
index a96cc841..fd13d8b5 100644
--- a/math/VelocityModel.cpp
+++ b/math/VelocityModel.cpp
@@ -313,6 +313,7 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const
desired_direction = lastE0;
ped->SetLastE0(lastE0);
Log->Write("%f %f", desired_direction._x, desired_direction._y);
+ //_direction->GetTarget(room, ped);
}
// if (dist > 1*J_EPS_GOAL) {
// desired_direction = target - pos; //ped->GetV0(target);
diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp
index f67ba5cb..0008ce5f 100644
--- a/routing/ff_router/UnivFFviaFM.cpp
+++ b/routing/ff_router/UnivFFviaFM.cpp
@@ -583,7 +583,8 @@ void UnivFFviaFM::finalizeTargetLine(const int uid, const Line& line, Point* tar
} else if (_gridCode[jDot*iMax + iDot] == WALL) {
//do nothing
} else {
- Log->Write("ERROR:\t in finalizingTargetLine");
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
}
for (i=0; iDot < xe; ++i) {
++iDot;
@@ -621,7 +622,8 @@ void UnivFFviaFM::finalizeTargetLine(const int uid, const Line& line, Point* tar
} else if (_gridCode[jDot*iMax + iDot] == WALL) {
//do nothing
} else {
- Log->Write("ERROR:\t in finalizingTargetLine");
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
}
}
} else {
@@ -658,7 +660,8 @@ void UnivFFviaFM::finalizeTargetLine(const int uid, const Line& line, Point* tar
} else if (_gridCode[jDot*iMax + iDot] == WALL) {
//do nothing
} else {
- Log->Write("ERROR:\t in finalizingTargetLine");
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
}
for(i=0; jDot<ye; ++i) {
++jDot;
@@ -696,7 +699,8 @@ void UnivFFviaFM::finalizeTargetLine(const int uid, const Line& line, Point* tar
} else if (_gridCode[jDot*iMax + iDot] == WALL) {
//do nothing
} else {
- Log->Write("ERROR:\t in finalizingTargetLine");
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
}
}
}
@@ -1388,14 +1392,22 @@ void UnivFFviaFM::addTarget(const int uid, double* costarrayDBL, Point* gradarra
Point passvector = tempTargetLine.NormalVec();
Point trial = tempTargetLine.GetCentre() - passvector * 0.25;
Point trial2 = tempTargetLine.GetCentre() + passvector * 0.25;
- if ((_grid->getKeyAtPoint(trial) >= 0) && (_gridCode[_grid->getKeyAtPoint(trial)] == INSIDE)) {
+ if ((_grid->includesPoint(trial)) && (_gridCode[_grid->getKeyAtPoint(trial)] == INSIDE)) {
finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector);
- } else if ((_grid->getKeyAtPoint(trial2) >= 0) && (_gridCode[_grid->getKeyAtPoint(trial2)] == INSIDE)) {
+ finalizeTargetLine(uid, tempTargetLine, newArrayPt, passvector);
+ } else if ((_grid->includesPoint(trial2)) && (_gridCode[_grid->getKeyAtPoint(trial2)] == INSIDE)) {
passvector = passvector * -1.0;
finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector);
+ finalizeTargetLine(uid, tempTargetLine, newArrayPt, passvector);
+
} else {
Log->Write("ERROR:\t in addTarget: calling finalizeTargetLine");
}
+// for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+// if ((_gridCode[i] != OUTSIDE) && (_gridCode[i] != WALL) && (newArrayPt[i] == Point(0.0, 0.0) )) {
+// Log->Write("Mist");
+// }
+// }
}
#pragma omp critical(_uids)
_uids.emplace_back(uid);
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
index 9190bd1e..067c28f0 100644
--- a/routing/ff_router/ffRouter.cpp
+++ b/routing/ff_router/ffRouter.cpp
@@ -549,8 +549,12 @@ int FFRouter::FindExit(Pedestrian* p)
if ((_distMatrix.at(key) + locDistToDoor) < minDist) {
minDist = _distMatrix.at(key) + locDistToDoor;
bestDoor = key.first; //doorUID
- if (locDistToDoor == 0.) {
- bestDoor = _pathsMatrix[key]; //@todo: @ar.graf: check this hack
+ //if (locDistToDoor == 0.) {
+ if (true) {
+ auto subroomDoors = _building->GetSubRoomByUID(p->GetSubRoomUID())->GetAllGoalIDs();
+ if (std::find(subroomDoors.begin(), subroomDoors.end(), _pathsMatrix[key]) != subroomDoors.end()) {
+ bestDoor = _pathsMatrix[key]; //@todo: @ar.graf: check this hack
+ }
}
bestFinalDoor = key.second;
}
--
GitLab