diff --git a/routing/DirectionStrategy.cpp b/routing/DirectionStrategy.cpp
index 96cee01fa97d6bf567fe3850c3b9410744c9708c..979066e54acdcece2aecb2a3e5d43148791c3f89 100644
--- a/routing/DirectionStrategy.cpp
+++ b/routing/DirectionStrategy.cpp
@@ -33,6 +33,7 @@
 #include "../pedestrian/Pedestrian.h"
 #include "../geometry/SubRoom.h"
 //#include "../geometry/Wall.h"
+#include "../routing/ff_router/UnivFFviaFM.h"
 #include "../routing/ff_router/FloorfieldViaFM.h"
 #include "../routing/ff_router/LocalFloorfieldViaFM.h"
 #include "../routing/ff_router/ffRouter.h"
@@ -346,8 +347,11 @@ Point DirectionLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const
 #endif // DEBUG
 
      Point p;
-     _locffviafm.at(room->GetID())->getDirectionToDestination(ped, p);
-     p = p.Normalized();     // @todo: argraf : scale with costvalue: " * ffviafm->getCostToTransition(ped->GetTransitionID(), ped->GetPos()) "
+     UnivFFviaFM* floorfield = _locffviafm.at(room->GetID());
+     floorfield->getDirectionToUID(ped->GetExitIndex(), ped->GetPos(), p);
+     if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0) {
+          p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos());
+     }
      return (p + ped->GetPos());
 
 #if DEBUG
@@ -373,24 +377,28 @@ double DirectionLocalFloorfield::GetDistance2Wall(Pedestrian* ped) const
 }
 
 void DirectionLocalFloorfield::Init(Building* buildingArg, double stepsize,
-                                    double threshold, bool useDistancMap) {
-     _hx = stepsize;
-     _hy = stepsize;
+                                    double threshold, bool useDistanceMap) {
+     _stepsize = stepsize;
      _building = buildingArg;
      _wallAvoidDistance = threshold;
-     _useDistancefield = useDistancMap;
-
-
-    std::chrono::time_point<std::chrono::system_clock> start, end;
-    start = std::chrono::system_clock::now();
-    Log->Write("INFO: \tCalling Constructor of LocFloorfieldViaFM in DirectionLocalFloorfield::Init(...)");
-#pragma omp parallel for schedule(dynamic)
-     for (unsigned int i = 0; i < _building->GetAllRooms().size(); ++i) {
-     //for (auto& roomPair : building->GetAllRooms()) {
-         auto roomPairIt = _building->GetAllRooms().begin();
-         std::advance(roomPairIt, i);
-         _locffviafm[(*roomPairIt).first] = new LocalFloorfieldViaFM((*roomPairIt).second.get(), _building,
-                 _hx, _hy, _wallAvoidDistance, _useDistancefield);
+     _useDistancefield = useDistanceMap;
+
+
+     std::chrono::time_point<std::chrono::system_clock> start, end;
+     start = std::chrono::system_clock::now();
+     Log->Write("INFO: \tCalling Constructor of LocFloorfieldViaFM in DirectionLocalFloorfield::Init(...)");
+
+     for (auto& roomPair : _building->GetAllRooms()) {
+         UnivFFviaFM* newfield =  new UnivFFviaFM(roomPair.second.get(), _building, stepsize, _wallAvoidDistance, _useDistancefield);
+         _locffviafm[roomPair.first] = newfield;
+          newfield->setUser(DISTANCE_AND_DIRECTIONS_USED);
+          newfield->setMode(LINESEGMENT);
+          if (useDistanceMap) {
+               newfield->setSpeedMode(FF_WALL_AVOID);
+          } else {
+               newfield->setSpeedMode(FF_HOMO_SPEED);
+          }
+         newfield->addAllTargetsParallel();
      }
      end = std::chrono::system_clock::now();
      std::chrono::duration<double> elapsed_seconds = end-start;
@@ -398,34 +406,16 @@ void DirectionLocalFloorfield::Init(Building* buildingArg, double stepsize,
      _initDone = true;
 }
 
-
-
-void DirectionLocalFloorfield::CalcFloorfield(int room, int destUID) {
-     Point dummy;
-     _locffviafm.at(room)->getDirectionToUID(destUID, 0, dummy);
-}
-
-void DirectionLocalFloorfield::writeFF(int room, std::vector<int> targets) {
-     std::string lfilename = "floorfield" + std::to_string(room) + ".vtk";
-    std::chrono::time_point<std::chrono::system_clock> start, end;
-    start = std::chrono::system_clock::now();
-     _locffviafm[room]->writeFF(lfilename, targets);
-    end = std::chrono::system_clock::now();
-    std::chrono::duration<double> elapsed_seconds = end-start;
-    Log->Write("INFO: \twriting %s took %f s ", lfilename.c_str(), elapsed_seconds.count());
-}
-
 DirectionLocalFloorfield::DirectionLocalFloorfield() {
-     //locffviafm = nullptr;
      _initDone = false;
 }
 
 DirectionLocalFloorfield::~DirectionLocalFloorfield() {
-     //if (locffviafm) {
      for (auto pair : _locffviafm) {
-          delete pair.second;
+          if (pair.second) {
+               delete pair.second;
+          }
      }
-     //}
 }
 
 
@@ -438,18 +428,16 @@ Point DirectionSubLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const
 #endif // DEBUG
 
      Point p;
-     _locffviafm.at(ped->GetSubRoomUID())->getDirectionToDestination(ped, p);
-
-     p = p.Normalized();     // @todo: argraf : scale with costvalue: " * ffviafm->getCostToTransition(ped->GetTransitionID(), ped->GetPos()) "
+     UnivFFviaFM* floorfield = _locffviafm.at(ped->GetSubRoomUID());
+     floorfield->getDirectionToUID(ped->GetExitIndex(), ped->GetPos(),p);
+     if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0){
+          p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos());
+     }
      return (p + ped->GetPos());
 
 #if DEBUG
      }
 #endif // DEBUG
-
-     //this should not execute:
-     //std::cerr << "Failure in DirectionFloorfield::GetTarget!!" << std::endl;
-     //exit(EXIT_FAILURE);
 }
 
 Point DirectionSubLocalFloorfield::GetDir2Wall(Pedestrian* ped) const
@@ -466,68 +454,54 @@ double DirectionSubLocalFloorfield::GetDistance2Wall(Pedestrian* ped) const
 }
 
 void DirectionSubLocalFloorfield::Init(Building* buildingArg, double stepsize,
-      double threshold, bool useDistancMap) {
-     _hx = stepsize;
-     _hy = stepsize;
+                                       double threshold, bool useDistanceMap) {
+     _stepsize = stepsize;
      _building = buildingArg;
      _wallAvoidDistance = threshold;
-     _useDistancefield = useDistancMap;
-
-          std::chrono::time_point<std::chrono::system_clock> start, end;
-          start = std::chrono::system_clock::now();
-          Log->Write("INFO: \tCalling Construtor of SubLocFloorfieldViaFM");
-          std::vector<std::pair<int, int>> subAndTarget;
-          subAndTarget.clear();
-          std::vector<int> subUIDs;
-          subUIDs.clear();
-          for (auto& roomPair : _building->GetAllRooms()) {
-#pragma omp parallel for
-               for (unsigned int i = 0; i < roomPair.second->GetAllSubRooms().size(); ++i) {
-                    auto subroomIt = roomPair.second->GetAllSubRooms().begin();
-                    std::advance(subroomIt, i);
-                    int subUID = subroomIt->second->GetUID();
-#pragma omp critical(subUIDs)
-                    subUIDs.emplace_back(subUID);
-                    Log->Write("Creating SubLocFF at key: %d", subUID);
-                    _locffviafm[subUID] = new SubLocalFloorfieldViaFM(
-                              subroomIt->second.get(), _building,
-                              _hx, _hy, _wallAvoidDistance, _useDistancefield);
-                    auto targets = subroomIt->second->GetAllGoalIDs();
-                    for (auto targetUID : targets) {
-                         subAndTarget.emplace_back(std::make_pair(subUID, targetUID));
-                    }
-               }
+     _useDistancefield = useDistanceMap;
 
+     std::chrono::time_point<std::chrono::system_clock> start, end;
+     start = std::chrono::system_clock::now();
+     Log->Write("INFO: \tCalling Construtor of SubLocFloorfieldViaFM");
+
+     for (auto& roomPair : _building->GetAllRooms()) {
+          for (auto& subPair : roomPair.second->GetAllSubRooms()) {
+               int subUID = subPair.second->GetUID();
+               UnivFFviaFM* floorfield = new UnivFFviaFM(subPair.second.get(), _building, stepsize, _wallAvoidDistance, _useDistancefield);
+               _locffviafm[subUID] = floorfield;
+               floorfield->setUser(DISTANCE_AND_DIRECTIONS_USED);
+               floorfield->setMode(LINESEGMENT);
+               if (useDistanceMap) {
+                    floorfield->setSpeedMode(FF_WALL_AVOID);
+               } else {
+                    floorfield->setSpeedMode(FF_HOMO_SPEED);
+               }
+               floorfield->addAllTargetsParallel();
           }
-#pragma omp parallel for
-          for (unsigned int i = 0; i < subAndTarget.size(); ++i) {
-               auto pairIT = subAndTarget.begin();
-               std::advance(pairIT, i);
-               Point dummy;
-               _locffviafm[pairIT->first]->getDirectionToUID(pairIT->second, 0, dummy);
-          }
-          end = std::chrono::system_clock::now();
-          std::chrono::duration<double> elapsed_seconds = end-start;
-          Log->Write("INFO: \tTaken time: " + std::to_string(elapsed_seconds.count()));
+     }
+
+     end = std::chrono::system_clock::now();
+     std::chrono::duration<double> elapsed_seconds = end-start;
+     Log->Write("INFO: \tTaken time: " + std::to_string(elapsed_seconds.count()));
 
      _initDone = true;
 
      //write floorfields to file, one file per subroom //ar.graf: [SWITCH writevtk ON/OFF]
-     for(unsigned int i = 0; i < subUIDs.size(); ++i) {
-          std::vector<int> targets = {};
-          targets.clear();
-          int subroomUID = subUIDs[i];
-          if (subroomUID != 26) continue;
-
-          for (auto pair : subAndTarget) {
-               if (pair.first == subroomUID) {
-                    targets.emplace_back(pair.second);
-               }
-          }
-          std::string filename1 = "floorfield" + std::to_string(subroomUID) + ".vtk";
-          if (targets.size() > 0)
-          _locffviafm[subroomUID]->writeFF(filename1, targets);
-     }
+//     for(unsigned int i = 0; i < subUIDs.size(); ++i) {
+//          std::vector<int> targets = {};
+//          targets.clear();
+//          int subroomUID = subUIDs[i];
+//          //if (subroomUID != 26) continue;
+//
+//          for (auto pair : subAndTarget) {
+//               if (pair.first == subroomUID) {
+//                    targets.emplace_back(pair.second);
+//               }
+//          }
+//          std::string filename1 = "floorfield" + std::to_string(subroomUID) + ".vtk";
+//          if (targets.size() > 0)
+//               _locffviafm[subroomUID]->writeFF(filename1, targets);
+//     }
 }
 
 DirectionSubLocalFloorfield::DirectionSubLocalFloorfield() {
diff --git a/routing/DirectionStrategy.h b/routing/DirectionStrategy.h
index fb3a5d09bf17aec80627c0e0b8e86c265933cef4..8f20cd07f551d2c8fb2dfbce17ceb5f9c790e726 100644
--- a/routing/DirectionStrategy.h
+++ b/routing/DirectionStrategy.h
@@ -36,6 +36,7 @@ class Room;
 class Building;
 class Pedestrian;
 class Point;
+class UnivFFviaFM;
 class FloorfieldViaFM;
 class LocalFloorfieldViaFM;
 class SubLocalFloorfieldViaFM;
@@ -95,23 +96,17 @@ class DirectionLocalFloorfield : public DirectionStrategy {
 public:
      DirectionLocalFloorfield();
      void Init(Building* building, double stepsize, double threshold,
-               bool useDistancMap);
+               bool useDistanceMap);
      ~DirectionLocalFloorfield();
-     //void Init();
      virtual Point GetTarget(Room* room, Pedestrian* ped) const;
      virtual Point GetDir2Wall(Pedestrian* ped) const;
      virtual double GetDistance2Wall(Pedestrian* ped) const;
-     virtual void CalcFloorfield(int room, int destUID);
-     virtual void writeFF(int room, std::vector<int> targets);
-
-     //void WriteAll(std::string filename);
 
 protected:
-     std::map<int, LocalFloorfieldViaFM*> _locffviafm;
+     std::map<int, UnivFFviaFM*> _locffviafm;
      bool _initDone;
-     const Building* _building;
-     double _hx;
-     double _hy;
+     Building* _building;
+     double _stepsize;
      double _wallAvoidDistance;
      bool _useDistancefield;
      std::string _filename;
@@ -121,21 +116,17 @@ class DirectionSubLocalFloorfield : public DirectionStrategy {
 public:
      DirectionSubLocalFloorfield();
      void Init(Building* building, double stepsize, double threshold,
-           bool useDistancMap);
+           bool useDistanceMap);
      ~DirectionSubLocalFloorfield();
-     //void Init();
      virtual Point GetTarget(Room* room, Pedestrian* ped) const;
      virtual Point GetDir2Wall(Pedestrian* ped) const;
      virtual double GetDistance2Wall(Pedestrian* ped) const;
 
-     //void WriteAll(std::string filename);
-
 protected:
-     std::map<int, SubLocalFloorfieldViaFM*> _locffviafm;
+     std::map<int, UnivFFviaFM*> _locffviafm;
      bool _initDone;
-     const Building* _building;
-     double _hx;
-     double _hy;
+     Building* _building;
+     double _stepsize;
      double _wallAvoidDistance;
      bool _useDistancefield;
      std::string _filename;
diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp
index a6e75530abdeb3b88266a9e65ecbee44c1886c4c..c54a1c72cca3f7c8cef96eebc268e58f7b93c50c 100644
--- a/routing/ff_router/UnivFFviaFM.cpp
+++ b/routing/ff_router/UnivFFviaFM.cpp
@@ -1300,6 +1300,14 @@ void UnivFFviaFM::getDirectionToUID(int destID, const long int key, Point& direc
      return;
 }
 
+void UnivFFviaFM::getDirectionToUID(int destID, const Point& pos, Point& direction, int mode) {
+     getDirectionToUID(destID, _grid->getKeyAtPoint(pos), direction, mode);
+}
+
+void UnivFFviaFM::getDirectionToUID(int destID, const Point& pos,Point& direction) {
+     getDirectionToUID(destID, _grid->getKeyAtPoint(pos), direction);
+}
+
 double UnivFFviaFM::getDistance2WallAt(const Point &pos) {
      if (_useWallDistances || (_speedmode == FF_WALL_AVOID)) {
           if (_costFieldWithKey[0]) {
diff --git a/routing/ff_router/UnivFFviaFM.h b/routing/ff_router/UnivFFviaFM.h
index 7ed3e697830d16ce3fee39dc5a4fe02ba48b7e7e..8bcb80a8f5d6cec405c3f524d58da9ac585a6492 100644
--- a/routing/ff_router/UnivFFviaFM.h
+++ b/routing/ff_router/UnivFFviaFM.h
@@ -107,6 +107,8 @@ public:
      RectGrid* getGrid();
      virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
      void getDirectionToUID(int destID, const long int key, Point& direction);
+     virtual void getDirectionToUID(int destID, const Point& pos, Point& direction, int mode);
+     void getDirectionToUID(int destID, const Point& pos, Point& direction);
      double getDistance2WallAt(const Point& pos);
      void getDir2WallAt(const Point& pos, Point& p);
 
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
index fdee8e08ad34375e6c5e52d79f3440c27c49ae61..4249c21d115a2ce3f41b72cbe7c4ebbe19ab432b 100644
--- a/routing/ff_router/ffRouter.cpp
+++ b/routing/ff_router/ffRouter.cpp
@@ -168,80 +168,78 @@ bool FFRouter::Init(Building* building)
      //type of allRooms: const std::map<int, std::unique_ptr<Room> >&
      const std::map<int, std::shared_ptr<Room> >& allRooms = _building->GetAllRooms();
 
-//#pragma omp parallel
-     {
-//#pragma omp for
-          for (unsigned int i = 0; i < allRooms.size(); ++i) {
+
+     for (unsigned int i = 0; i < allRooms.size(); ++i) {
 
 #ifdef DEBUG
-               std::cerr << "Creating Floorfield for Room: " << pair.first << std::endl;
+          std::cerr << "Creating Floorfield for Room: " << pair.first << std::endl;
 #endif
 
-               auto pairRoomIt = allRooms.begin();
-               std::advance(pairRoomIt, i);
-               UnivFFviaFM *locffptr = nullptr;
-               locffptr = new UnivFFviaFM(pairRoomIt->second.get(), building, 0.125, 0.0, false);
+          auto pairRoomIt = allRooms.begin();
+          std::advance(pairRoomIt, i);
+          UnivFFviaFM *locffptr = nullptr;
+          locffptr = new UnivFFviaFM(pairRoomIt->second.get(), building, 0.125, 0.0, false);
 //               Log->Write("INFO: \tusing %s in ffRouter::Init", _useCentrePointDistance ? "CentrePointLocalFFViaFm" : "LocalFloorfieldViaFM");
 //               if (_useCentrePointDistance) {
 //                    locffptr = new CentrePointLocalFFViaFM(pairRoomIt->second.get(), building, 0.125, 0.125, 0.0, false);
 //               } else {
 //                    locffptr = new LocalFloorfieldViaFM(pairRoomIt->second.get(), building, 0.125, 0.125, 0.0, false);
 //               }
-               locffptr->setUser(DISTANCE_AND_DIRECTIONS_USED);
-               locffptr->setMode(CENTERPOINT);
-               locffptr->setSpeedMode(FF_HOMO_SPEED);
-               locffptr->addAllTargetsParallel();
-               locffptr->writeFF("UnivFF"+std::to_string(pairRoomIt->first)+".vtk", locffptr->getKnownDoorUIDs());
-               Log->Write("INFO: \tAdding distances in Room %d to matrix", (*pairRoomIt).first);
+          locffptr->setUser(DISTANCE_AND_DIRECTIONS_USED);
+          locffptr->setMode(CENTERPOINT);
+          locffptr->setSpeedMode(FF_HOMO_SPEED);
+          locffptr->addAllTargetsParallel();
+          //locffptr->writeFF("UnivFF"+std::to_string(pairRoomIt->first)+".vtk", locffptr->getKnownDoorUIDs());
+          Log->Write("INFO: \tAdding distances in Room %d to matrix", (*pairRoomIt).first);
 //#pragma omp critical(_locffviafm)
-               _locffviafm.insert(std::make_pair((*pairRoomIt).first, locffptr));
-          }
+          _locffviafm.insert(std::make_pair((*pairRoomIt).first, locffptr));
+     }
 
 
-          // nowait, because the parallel region ends directly afterwards
+     // nowait, because the parallel region ends directly afterwards
 //#pragma omp for nowait
-          for (unsigned int i = 0; i < roomAndCroTrVector.size(); ++i) {
-               auto rctIt = roomAndCroTrVector.begin();
-               std::advance(rctIt, i);
-
-               ////loop over upper triangular matrice (i,j) and write to (j,i) as well
-               for (auto otherDoor : roomAndCroTrVector) {
-                    if (otherDoor.first != rctIt->first) continue; // we only want doors with one room in common
-                    if (otherDoor.second <= rctIt->second) continue; // calculate every path only once
-                    // if we exclude otherDoor.second == rctIt->second, the program loops forever
-
-                    //if the door is closed, then don't calc distances
-                    //if (!_CroTrByUID.at(*otherDoor)->IsOpen()) {
-                    //     continue;
-                    //}
-
-                    // if the two doors are not within the same subroom, do not consider (ar.graf)
-                    // should fix problems of oscillation caused by doorgaps in the distancegraph
-                    int thisUID1 = (_CroTrByUID.at(rctIt->second)->GetSubRoom1()) ? _CroTrByUID.at(rctIt->second)->GetSubRoom1()->GetUID() : -1 ;
-                    int thisUID2 = (_CroTrByUID.at(rctIt->second)->GetSubRoom2()) ? _CroTrByUID.at(rctIt->second)->GetSubRoom2()->GetUID() : -2 ;
-                    int otherUID1 = (_CroTrByUID.at(otherDoor.second)->GetSubRoom1()) ? _CroTrByUID.at(otherDoor.second)->GetSubRoom1()->GetUID() : -3 ;
-                    int otherUID2 = (_CroTrByUID.at(otherDoor.second)->GetSubRoom2()) ? _CroTrByUID.at(otherDoor.second)->GetSubRoom2()->GetUID() : -4 ;
-
-                    if (
+     for (unsigned int i = 0; i < roomAndCroTrVector.size(); ++i) {
+          auto rctIt = roomAndCroTrVector.begin();
+          std::advance(rctIt, i);
+
+          ////loop over upper triangular matrice (i,j) and write to (j,i) as well
+          for (auto otherDoor : roomAndCroTrVector) {
+               if (otherDoor.first != rctIt->first) continue; // we only want doors with one room in common
+               if (otherDoor.second <= rctIt->second) continue; // calculate every path only once
+               // if we exclude otherDoor.second == rctIt->second, the program loops forever
+
+               //if the door is closed, then don't calc distances
+               //if (!_CroTrByUID.at(*otherDoor)->IsOpen()) {
+               //     continue;
+               //}
+
+               // if the two doors are not within the same subroom, do not consider (ar.graf)
+               // should fix problems of oscillation caused by doorgaps in the distancegraph
+               int thisUID1 = (_CroTrByUID.at(rctIt->second)->GetSubRoom1()) ? _CroTrByUID.at(rctIt->second)->GetSubRoom1()->GetUID() : -1 ;
+               int thisUID2 = (_CroTrByUID.at(rctIt->second)->GetSubRoom2()) ? _CroTrByUID.at(rctIt->second)->GetSubRoom2()->GetUID() : -2 ;
+               int otherUID1 = (_CroTrByUID.at(otherDoor.second)->GetSubRoom1()) ? _CroTrByUID.at(otherDoor.second)->GetSubRoom1()->GetUID() : -3 ;
+               int otherUID2 = (_CroTrByUID.at(otherDoor.second)->GetSubRoom2()) ? _CroTrByUID.at(otherDoor.second)->GetSubRoom2()->GetUID() : -4 ;
+
+               if (
                          (thisUID1 != otherUID1) &&
                          (thisUID1 != otherUID2) &&
                          (thisUID2 != otherUID1) &&
                          (thisUID2 != otherUID2)      ) {
-                         continue;
-                    }
+                    continue;
+               }
 
-                    UnivFFviaFM* locffptr = _locffviafm[rctIt->first];
-                    double tempDistance = locffptr->getCostToDestination(rctIt->second,
-                                                                         _CroTrByUID.at(otherDoor.second)->GetCentre());
+               UnivFFviaFM* locffptr = _locffviafm[rctIt->first];
+               double tempDistance = locffptr->getCostToDestination(rctIt->second,
+                                                                    _CroTrByUID.at(otherDoor.second)->GetCentre());
 
-                    if (tempDistance < locffptr->getGrid()->Gethx()) {
-                         //Log->Write("WARNING:\tDistance of doors %d and %d is too small: %f",*otherDoor, *innerPtr, tempDistance);
-                         //Log->Write("^^^^^^^^\tIf there are scattered subrooms, which are not connected, this is ok.");
-                         continue;
-                    }
+               if (tempDistance < locffptr->getGrid()->Gethx()) {
+                    //Log->Write("WARNING:\tDistance of doors %d and %d is too small: %f",*otherDoor, *innerPtr, tempDistance);
+                    //Log->Write("^^^^^^^^\tIf there are scattered subrooms, which are not connected, this is ok.");
+                    continue;
+               }
 //
-                    std::pair<int, int> key_ij = std::make_pair(otherDoor.second, rctIt->second);
-                    std::pair<int, int> key_ji = std::make_pair(rctIt->second, otherDoor.second);
+               std::pair<int, int> key_ij = std::make_pair(otherDoor.second, rctIt->second);
+               std::pair<int, int> key_ji = std::make_pair(rctIt->second, otherDoor.second);
 //                    SubRoom *subroom = nullptr;
 //
 //                    auto cr1 = _CroTrByUID[rctIt->second];
@@ -284,11 +282,11 @@ bool FFRouter::Init(Building* building)
 //                    }
 
 //#pragma omp critical(_distMatrix)
-                    if (_distMatrix.at(key_ij) > tempDistance) {
-                         _distMatrix.erase(key_ij);
-                         _distMatrix.erase(key_ji);
-                         _distMatrix.insert(std::make_pair(key_ij, tempDistance));
-                         _distMatrix.insert(std::make_pair(key_ji, tempDistance));
+               if (_distMatrix.at(key_ij) > tempDistance) {
+                    _distMatrix.erase(key_ij);
+                    _distMatrix.erase(key_ji);
+                    _distMatrix.insert(std::make_pair(key_ij, tempDistance));
+                    _distMatrix.insert(std::make_pair(key_ji, tempDistance));
 //#pragma omp critical(_subroomMatrix)
 //                         {
 //                              _subroomMatrix.erase(key_ij);
@@ -297,11 +295,10 @@ bool FFRouter::Init(Building* building)
 //                              _subroomMatrix.insert(std::make_pair(key_ji, subroom));
 //                         }
 
-                    }
-                    //}
-               } // otherDoor
-          } // roomAndCroTrVector
-     } // omp parallel
+               }
+               //}
+          } // otherDoor
+     } // roomAndCroTrVector
 
      FloydWarshall();
 
@@ -319,21 +316,21 @@ bool FFRouter::Init(Building* building)
 //          iter->second->writeFF("testFF" + std::to_string(roomNr) + ".vtk", _allDoorUIDs);
 //     }
 
-     std::ofstream matrixfile;
-     matrixfile.open("Matrix.txt");
-
-     for (auto mapItem : _distMatrix) {
-          matrixfile << mapItem.first.first << " to " << mapItem.first.second << " : " << mapItem.second << "\t via \t" << _pathsMatrix[mapItem.first];
-          matrixfile << "\t" << _CroTrByUID.at(mapItem.first.first)->GetID() << " to " << _CroTrByUID.at(mapItem.first.second)->GetID() << "\t via \t";
-          matrixfile << _CroTrByUID.at(_pathsMatrix[mapItem.first])->GetID();
-//          auto sub = _subroomMatrix.at(mapItem.first);
-//          if (sub) {
-//               matrixfile << std::string("\tSubroom: UID ") << sub->GetUID() << " (room: " << sub->GetRoomID() << " subroom ID: " << sub->GetSubRoomID() << ")" << std::endl;
-//          } else {
-//               matrixfile << std::string("\tSubroom is nullptr") << std::endl;
-//          }
-     }
-     matrixfile.close();
+//     std::ofstream matrixfile;
+//     matrixfile.open("Matrix.txt");
+//
+//     for (auto mapItem : _distMatrix) {
+//          matrixfile << mapItem.first.first << " to " << mapItem.first.second << " : " << mapItem.second << "\t via \t" << _pathsMatrix[mapItem.first];
+//          matrixfile << "\t" << _CroTrByUID.at(mapItem.first.first)->GetID() << " to " << _CroTrByUID.at(mapItem.first.second)->GetID() << "\t via \t";
+//          matrixfile << _CroTrByUID.at(_pathsMatrix[mapItem.first])->GetID();
+////          auto sub = _subroomMatrix.at(mapItem.first);
+////          if (sub) {
+////               matrixfile << std::string("\tSubroom: UID ") << sub->GetUID() << " (room: " << sub->GetRoomID() << " subroom ID: " << sub->GetSubRoomID() << ")" << std::endl;
+////          } else {
+////               matrixfile << std::string("\tSubroom is nullptr") << std::endl;
+////          }
+//     }
+//     matrixfile.close();
      Log->Write("INFO: \tFF Router Init done.");
      return true;
 }