diff --git a/math/VelocityModel.cpp b/math/VelocityModel.cpp
index a96cc84124a9f5b541f7a46996022984ee832adf..fd13d8b5aea8a2fc8eaa495929f2533cfba2a555 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 f67ba5cb6f02221c69e96c6a52526916d7b164c6..0008ce5f9e262b81994dbf51aa83ccd8802bd0e8 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 9190bd1e379c1599cddc8ad1ae00e567f2878c6c..067c28f0dc292027030ce172b290e751a89b247b 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;
                     }