getCost../getDirection.. added

routing/ff_router/UnivFFviaFM.cpp

@@ -30,6 +30,7 @@ UnivFFviaFM::UnivFFviaFM(Room* roomArg, Configuration* const confArg, double hx,
 
      _configuration = confArg;
      _scope = FF_ROOM_SCALE;
+     _room = roomArg->GetID();
      std::vector<Line> lines;
      std::map<int, Line> tmpDoors;
 
@@ -68,7 +69,6 @@ UnivFFviaFM::UnivFFviaFM(Room* roomArg, Configuration* const confArg, double hx,
 
      //create(lines, tmpDoors, wantedDoors, FF_HOMO_SPEED, hx, wallAvoid, useWallAvoid);
      create(lines, tmpDoors, wantedDoors, FF_WALL_AVOID, hx, wallAvoid, useWallAvoid);
-     //writeFF("UnivFFRoom.vtk", this->getKnownDoorUIDs());
 }
 
 UnivFFviaFM::UnivFFviaFM(SubRoom* sr, Configuration* const conf, double hx, double wallAvoid, bool useWallAvoid)
@@ -80,6 +80,7 @@ UnivFFviaFM::UnivFFviaFM(SubRoom* subRoomArg, Configuration* const confArg, doub
      //then call other constructor including the mode
      _configuration = confArg;
      _scope = FF_SUBROOM_SCALE;
+     _room = subRoomArg->GetRoomID();
      std::vector<Line> lines;
      std::map<int, Line> tmpDoors;
 
@@ -110,7 +111,6 @@ UnivFFviaFM::UnivFFviaFM(SubRoom* subRoomArg, Configuration* const confArg, doub
      }
 
      create(lines, tmpDoors, wantedDoors, FF_HOMO_SPEED, hx, wallAvoid, useWallAvoid);
-     writeFF("UnivFFSubroom.vtk", this->getKnownDoorUIDs());
 }
 
 void UnivFFviaFM::create(std::vector<Line>& walls, std::map<int, Line>& doors, std::vector<int> targetUIDs, int mode,
@@ -573,6 +573,9 @@ void UnivFFviaFM::calcCost(const long int key, double* cost, Point* dir, const d
      if (col == DBL_MAX) { //one sided update with row
           cost[key] = onesidedCalc(row, _grid->Gethx()/speed[key]);
           //flag[key] = FM_SINGLE;
+          if (!dir) {
+               return;
+          }
           if (pointsRight) {
                dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
                dir[key]._y = (0.);
@@ -587,6 +590,9 @@ void UnivFFviaFM::calcCost(const long int key, double* cost, Point* dir, const d
      if (row == DBL_MAX) { //one sided update with col
           cost[key] = onesidedCalc(col, _grid->Gethy()/speed[key]);
           //flag[key] = FM_SINGLE;
+          if (!dir) {
+               return;
+          }
           if (pointsUp) {
                dir[key]._x = (0.);
                dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
@@ -603,6 +609,9 @@ void UnivFFviaFM::calcCost(const long int key, double* cost, Point* dir, const d
      if (precheck >= 0) {
           cost[key] = precheck;
           //flag[key] = FM_DOUBLE;
+          if (!dir) {
+               return;
+          }
           if (pointsUp && pointsRight) {
                dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
                dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
@@ -756,6 +765,9 @@ void UnivFFviaFM::calcDist(const long int key, double* cost, Point* dir, const d
      if (col == DBL_MAX) { //one sided update with row
           cost[key] = onesidedCalc(row, _grid->Gethx()/speed[key]);
           //flag[key] = FM_SINGLE;
+          if (!dir) {
+               return;
+          }
           if (pointsRight) {
                dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
                dir[key]._y = (0.);
@@ -770,6 +782,9 @@ void UnivFFviaFM::calcDist(const long int key, double* cost, Point* dir, const d
      if (row == DBL_MAX) { //one sided update with col
           cost[key] = onesidedCalc(col, _grid->Gethy()/speed[key]);
           //flag[key] = FM_SINGLE;
+          if (!dir) {
+               return;
+          }
           if (pointsUp) {
                dir[key]._x = (0.);
                dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
@@ -786,6 +801,9 @@ void UnivFFviaFM::calcDist(const long int key, double* cost, Point* dir, const d
      if (precheck >= 0) {
           cost[key] = precheck;
           //flag[key] = FM_DOUBLE;
+          if (!dir) {
+               return;
+          }
           if (pointsUp && pointsRight) {
                dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
                dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
@@ -824,18 +842,22 @@ inline double UnivFFviaFM::twosidedCalc(double x, double y, double hDivF) { //on
      return -2.; //this line should never execute
 } //twosidedCalc
 
-void UnivFFviaFM::addTarget(const int uid) {
+void UnivFFviaFM::addTarget(const int uid, double* costarrayDBL, Point* gradarrayPt) {
+     if (_doors.count(uid) == 0) {
+          Log->Write("ERROR: \tCould not find door with uid %d in Room %d", uid, _room);
+          return;
+     }
      Line tempTargetLine = Line(_doors[uid]);
      Point tempCenterPoint = Point(tempTargetLine.GetCentre());
 
      //this allocation must be on shared heap! to be accessible by any thread later (should be shared in openmp)
-     double* newArrayDBL = new double[_nPoints];
+     double* newArrayDBL = (costarrayDBL)? costarrayDBL : new double[_nPoints];
      Point* newArrayPt = nullptr;
      if (_user == DISTANCE_AND_DIRECTIONS_USED) {
-          newArrayPt = new Point[_nPoints];
+          newArrayPt = (gradarrayPt)? gradarrayPt : new Point[_nPoints];
      }
 
-     if (_costFieldWithKey[uid])
+     if (_costFieldWithKey[uid] && !costarrayDBL)
           delete[] _costFieldWithKey[uid];
      _costFieldWithKey[uid] = newArrayDBL;
 
@@ -848,7 +870,7 @@ void UnivFFviaFM::addTarget(const int uid) {
           }
      }
 
-     if (_directionFieldWithKey[uid])
+     if (_directionFieldWithKey[uid] && !gradarrayPt)
           delete[] _directionFieldWithKey[uid];
      if (newArrayPt)
           _directionFieldWithKey[uid] = newArrayPt;
@@ -869,16 +891,65 @@ void UnivFFviaFM::addTarget(const int uid) {
      _uids.emplace_back(uid);
 }
 
+void UnivFFviaFM::addTarget(const int uid, Line* door, double* costarray, Point* gradarray){
+     if (_doors.count(uid) == 0) {
+          _doors.emplace(std::make_pair(uid, *door));
+     }
+     addTarget(uid, costarray, gradarray);
+}
+
 void UnivFFviaFM::addAllTargets() {
      for (auto uidmap : _doors) {
           addTarget(uidmap.first);
      }
 }
 
+void UnivFFviaFM::addAllTargetsParallel() {
+     //free old memory
+     for (auto memoryDBL : _costFieldWithKey) {
+          if (memoryDBL.first == 0) continue;          //do not free distancemap
+          if (memoryDBL.second) delete[](memoryDBL.second);
+     }
+     for (auto memoryPt : _directionFieldWithKey) {
+          if (memoryPt.first == 0) continue;           //do not free walldirectionmap
+          if (memoryPt.second) delete[](memoryPt.second);
+     }
+     //allocate new memory
+     for (auto uidmap : _doors) {
+          _costFieldWithKey[uidmap.first] = new double[_nPoints];
+          if (_user == DISTANCE_MEASUREMENTS_ONLY) {
+               _directionFieldWithKey[uidmap.first] = nullptr;
+          }
+          if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+               _directionFieldWithKey[uidmap.first] = new Point[_nPoints];
+          }
+     }
+
+     //parallel region
+#pragma omp parallel
+     {
+#pragma omp for
+          for (unsigned int i = 0; i < _doors.size(); ++i) {
+               auto doorPair = _doors.begin();
+               std::advance(doorPair, i);
+               addTarget(doorPair->first, _costFieldWithKey[doorPair->first], _directionFieldWithKey[doorPair->first]);
+          }
+     };
+}
+
 std::vector<int> UnivFFviaFM::getKnownDoorUIDs(){
      return _uids;
 }
 
+void UnivFFviaFM::setUser(int userArg) {
+     _user=userArg;
+}
+
+void UnivFFviaFM::setMode(int modeArg) {
+     _mode=modeArg;
+}
+
+
 void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID) {
     Log->Write("INFO: \tWrite Floorfield to file");
     Log->Write(filename);
@@ -939,6 +1010,26 @@ void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID
     if (!targetID.empty()) {
          for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
               Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
+
+              if (_costFieldWithKey.count(targetID[iTarget]) == 0) {
+                   continue;
+              }
+              double *costarray = _costFieldWithKey[targetID[iTarget]];
+
+              std::string name = _building->GetTransOrCrossByUID(targetID[iTarget])->GetCaption() + "-" +
+                                                                                          std::to_string(targetID[iTarget]);
+              std::replace(name.begin(), name.end(), ' ', '_');
+
+              if (!costarray) {
+                   continue;
+              }
+
+              file << "SCALARS CostTarget" << name << " float 1" << std::endl;
+              file << "LOOKUP_TABLE default" << std::endl;
+              for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+                   file << costarray[i] << std::endl;
+              }
+
               if (_directionFieldWithKey.count(targetID[iTarget]) == 0) {
                    continue;
               }
@@ -948,21 +1039,111 @@ void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID
                    continue;
               }
 
-              std::string name = _building->GetTransOrCrossByUID(targetID[iTarget])->GetCaption() + "-" +
-                                 std::to_string(targetID[iTarget]);
-              std::replace(name.begin(), name.end(), ' ', '_');
+
               file << "VECTORS GradientTarget" << name << " float" << std::endl;
               for (int i = 0; i < _grid->GetnPoints(); ++i) {
                    file << gradarray[i]._x << " " << gradarray[i]._y << " 0.0" << std::endl;
               }
 
-              double *costarray = _costFieldWithKey[targetID[iTarget]];
-              file << "SCALARS CostTarget" << name << " float 1" << std::endl;
-              file << "LOOKUP_TABLE default" << std::endl;
-              for (long int i = 0; i < _grid->GetnPoints(); ++i) {
-                   file << costarray[i] << std::endl;
-              }
+
          }
     }
     file.close();
-}
\ No newline at end of file
+}
+
+//@todo: @ar.graf: 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) {
+     assert(_grid->includesPoint(position));
+     if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
+          return _costFieldWithKey[destID][_grid->getKeyAtPoint(position)];
+     } else if (_directCalculation && _doors.count(destID) > 0) {
+          _costFieldWithKey[destID] = new double[_nPoints];
+          if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+               _directionFieldWithKey[destID] = new Point[_nPoints];
+          } else {
+               _directionFieldWithKey[destID] = nullptr;
+          }
+
+          addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
+          getCostToDestination(destID, position, mode);
+     }
+     return 0.;
+}
+
+double UnivFFviaFM::getCostToDestination(const int destID, const Point& position) {
+     assert(_grid->includesPoint(position));
+     if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
+          return _costFieldWithKey[destID][_grid->getKeyAtPoint(position)];
+     } else if (_directCalculation && _doors.count(destID) > 0) {
+          _costFieldWithKey[destID] = new double[_nPoints];
+          if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+               _directionFieldWithKey[destID] = new Point[_nPoints];
+          } else {
+               _directionFieldWithKey[destID] = nullptr;
+          }
+
+          addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
+          getCostToDestination(destID, position);
+     }
+     return 0.;
+}
+
+RectGrid* UnivFFviaFM::getGrid(){
+     return _grid;
+}
+
+void UnivFFviaFM::getDirectionToUID(int destID, const long int key, Point& direction, int mode){
+     assert(key > 0 && key < _nPoints);
+     if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) {
+          direction = _directionFieldWithKey[destID][key];
+     } else if (_directCalculation && _doors.count(destID) > 0) {
+          //free memory if needed
+          if (_costFieldWithKey.count(destID) == 1 && _costFieldWithKey[destID]) {
+               delete[] _costFieldWithKey[destID];
+          }
+          //allocate memory
+          _costFieldWithKey[destID] = new double[_nPoints];
+          if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+               _directionFieldWithKey[destID] = new Point[_nPoints];
+          } else {
+               _directionFieldWithKey[destID] = nullptr;
+          }
+
+          //calculate destID's fields and call function
+          addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
+          getDirectionToUID(destID, key, direction, mode);
+     }
+     return;
+}
+
+void UnivFFviaFM::getDirectionToUID(int destID, const long int key, Point& direction){
+     assert(key > 0 && key < _nPoints);
+     if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) {
+          direction = _directionFieldWithKey[destID][key];
+     } else if (_directCalculation && _doors.count(destID) > 0) {
+          //free memory if needed
+          if (_costFieldWithKey.count(destID) == 1 && _costFieldWithKey[destID]) {
+               delete[] _costFieldWithKey[destID];
+          }
+          //allocate memory
+          _costFieldWithKey[destID] = new double[_nPoints];
+          if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+               _directionFieldWithKey[destID] = new Point[_nPoints];
+          } else {
+               _directionFieldWithKey[destID] = nullptr;
+          }
+
+          //calculate destID's fields and call function
+          addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
+          getDirectionToUID(destID, key, direction);
+     }
+     return;
+}
+
+/* Log:
+ * todo:
+ *   - implement error treatment: extend fctns to throw errors and handle them
+ *   - error treatment will be advantageous, if calculation of FFs can be postponed
+ *                                           to be done in Simulation::RunBody, where
+ *                                           all cores are available
+ *   - (WIP) fill subroom* array with correct values
\ No newline at end of file

routing/ff_router/UnivFFviaFM.h

@@ -91,16 +91,19 @@ public:
      UnivFFviaFM(UnivFFviaFM&){};
      ~UnivFFviaFM(){};
 
-     void addTarget(const int uid, Line* door);
-     void addTarget(const int uid);
+     void addTarget(const int uid, Line* door, double* costarray = nullptr, Point* gradarray = nullptr);
+     void addTarget(const int uid, double* costarray = nullptr, Point* gradarray = nullptr);
      void addAllTargets();
+     void addAllTargetsParallel();
      std::vector<int> getKnownDoorUIDs();
-
-     double getCostToDestination(const int destID, const Point& position, int mode) {return 0.;};
-     double getCostToDestination(const int destID, const Point& position) {return 0.;};
-     RectGrid* getGrid(){return nullptr;};
-     virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode){};
-     void getDirectionToUID(int destID, const long int key, Point& direction){};
+     void setUser(int userArg);
+     void setMode(int modeArg);
+
+     double getCostToDestination(const int destID, const Point& position, int mode);
+     double getCostToDestination(const int destID, const Point& position);
+     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);
      void writeFF(const std::string&, std::vector<int> targetID);
 
      void createRectGrid(std::vector<Line>& walls, std::map<int, Line>& doors, double spacing);
@@ -128,10 +131,12 @@ public:
 private:
      Building* _building = nullptr;
      Configuration* _configuration = nullptr;
+     int _room = -1;                              //not set
      int _mode = LINESEGMENT;                     //default
      int _user = DISTANCE_AND_DIRECTIONS_USED;    //default
      int _speedmode = FF_HOMO_SPEED;              //default
      int _scope = 0;                              //not set / unknown
+     bool _directCalculation = true;
      RectGrid* _grid = nullptr;
      long int _nPoints = 0;
      std::vector<double*> _speedFieldSelector;

routing/ff_router/ffRouter.cpp

@@ -187,13 +187,15 @@ bool FFRouter::Init(Building* building)
 //               } else {
 //                    locffptr = new LocalFloorfieldViaFM(pairRoomIt->second.get(), building, 0.125, 0.125, 0.0, false);
 //               }
-               locffptr->addAllTargets();
+               locffptr->setUser(DISTANCE_MEASUREMENTS_ONLY);
+               locffptr->setMode(CENTERPOINT);
+               locffptr->addAllTargetsParallel();
                locffptr->writeFF("UnivFF.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));
           }
-      return true;  //@todo: ar.graf: remove this!!
+
 
           // nowait, because the parallel region ends directly afterwards
 //#pragma omp for nowait