new fkt of ffRouter/UnivFF: getDistanceBetweenDoors; debug vtk/Matrix enabled

routing/ff_router/UnivFFviaFM.cpp

@@ -1439,7 +1439,7 @@ double UnivFFviaFM::getCostToDestination(const int destID, const Point& position
           }
 
           addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
-          getCostToDestination(destID, position);
+          return getCostToDestination(destID, position);
      } else if (!_directCalculation && _doors.count(destID) > 0) {
 //omp critical
 #pragma omp critical(UnivFFviaFM_toDo)
@@ -1448,6 +1448,40 @@ double UnivFFviaFM::getCostToDestination(const int destID, const Point& position
      return DBL_MAX;
 }
 
+double UnivFFviaFM::getDistanceBetweenDoors(const int door1_ID, const int door2_ID) {
+    assert(_doors.count(door1_ID) != 0);
+    assert(_doors.count(door2_ID) != 0);
+
+    if (_costFieldWithKey.count(door1_ID)==1 && _costFieldWithKey[door1_ID]) {
+        long int key = _grid->getKeyAtPoint(_doors.at(door2_ID).GetCentre());
+        if (_gridCode[key] != door2_ID) {
+            //bresenham line (treppenstruktur) at middle and calculated centre of line are on different keys
+            //find a key that belongs to door (must be one left or right and second one below or above)
+            if (_gridCode[key+1] == door2_ID) {
+                key = key+1;
+            } else {
+                key = key-1;
+            }
+        }
+        return _costFieldWithKey[door1_ID][key];
+    } else if (_directCalculation && _doors.count(door1_ID) > 0) {
+        _costFieldWithKey[door1_ID] = new double[_nPoints];
+        if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+            _directionFieldWithKey[door1_ID] = new Point[_nPoints];
+        } else {
+            _directionFieldWithKey[door1_ID] = nullptr;
+        }
+
+        addTarget(door1_ID, _costFieldWithKey[door1_ID], _directionFieldWithKey[door1_ID]);
+        return getDistanceBetweenDoors(door1_ID, door2_ID);
+    } else if (!_directCalculation && _doors.count(door1_ID) > 0) {
+//omp critical
+#pragma omp critical(UnivFFviaFM_toDo)
+        _toDo.emplace_back(door1_ID);
+    }
+    return DBL_MAX;
+}
+
 RectGrid* UnivFFviaFM::getGrid(){
      return _grid;
 }

routing/ff_router/UnivFFviaFM.h

@@ -107,6 +107,7 @@ public:
 
      double getCostToDestination(const int destID, const Point& position, int mode);
      double getCostToDestination(const int destID, const Point& position);
+     double getDistanceBetweenDoors(const int door1_ID, const int door2_ID);
      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);

routing/ff_router/ffRouter.cpp

@@ -230,8 +230,11 @@ bool FFRouter::Init(Building* building)
                }
 
                UnivFFviaFM* locffptr = _locffviafm[rctIt->first];
-               double tempDistance = locffptr->getCostToDestination(rctIt->second,
-                                                                    _CroTrByUID.at(otherDoor.second)->GetCentre());
+              Point test = _CroTrByUID.at(otherDoor.second)->GetCentre();
+              Point test2 = test + Point(0.0 , 0.1);
+               //double tempDistance = locffptr->getCostToDestination(rctIt->second,
+               //                                                     _CroTrByUID.at(otherDoor.second)->GetCentre());
+               double tempDistance = locffptr->getDistanceBetweenDoors(rctIt->second, otherDoor.second);
 
                if (tempDistance < locffptr->getGrid()->Gethx()) {
                     //Log->Write("WARNING:\tDistance of doors %d and %d is too small: %f",*otherDoor, *innerPtr, tempDistance);
@@ -319,21 +322,21 @@ bool FFRouter::Init(Building* building)
           }
      }
 
-//     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() << std::endl;
-////          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() << std::endl;
+//          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;
 }