From 3d913f772c309291bbf61b7949dd55376e1c5807 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Schr=C3=B6dter?= <t.schroedter@fz-juelich.de>
Date: Wed, 30 Jan 2019 09:11:18 +0100
Subject: [PATCH] FF now navigates ped to subroom, which contains a waiting
area
---
geometry/Building.cpp | 1 +
.../ff_router_trips/FloorfieldViaFMTrips.cpp | 1821 ++++++++++++++++
.../ff_router_trips/FloorfieldViaFMTrips.h | 226 ++
routing/ff_router_trips/UnivFFviaFMTrips.cpp | 1916 +++++++++++++++++
routing/ff_router_trips/UnivFFviaFMTrips.h | 165 ++
routing/ff_router_trips/ffRouterTrips.cpp | 38 +-
routing/trips_router/TripsRouter.cpp | 9 +-
7 files changed, 4165 insertions(+), 11 deletions(-)
create mode 100644 routing/ff_router_trips/FloorfieldViaFMTrips.cpp
create mode 100644 routing/ff_router_trips/FloorfieldViaFMTrips.h
create mode 100644 routing/ff_router_trips/UnivFFviaFMTrips.cpp
create mode 100644 routing/ff_router_trips/UnivFFviaFMTrips.h
diff --git a/geometry/Building.cpp b/geometry/Building.cpp
index 5a975e94..3d819302 100644
--- a/geometry/Building.cpp
+++ b/geometry/Building.cpp
@@ -363,6 +363,7 @@ bool Building::InitGeometry()
if (s2) s2->AddNeighbor(s1);
}
+
Log->Write("INFO: \tInit Geometry successful!!!\n");
return true;
diff --git a/routing/ff_router_trips/FloorfieldViaFMTrips.cpp b/routing/ff_router_trips/FloorfieldViaFMTrips.cpp
new file mode 100644
index 00000000..2ab078e1
--- /dev/null
+++ b/routing/ff_router_trips/FloorfieldViaFMTrips.cpp
@@ -0,0 +1,1821 @@
+/**
+ * \file FloorfieldViaFMTrips.cpp
+ * \date Mar 05, 2015
+ * \version N/A (v0.6)
+ * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ *
+ * \section License
+ * This file is part of JuPedSim.
+ *
+ * JuPedSim is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * JuPedSim is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \section Description
+ * Implementation of classes for ...
+ *
+ *
+ **/
+#define TESTING
+#define GEO_UP_SCALE 1
+#include "FloorfieldViaFMTrips.h"
+
+#ifdef _OPENMP
+#else
+#define omp_get_thread_num() 0
+#define omp_get_max_threads() 1
+#endif
+
+FloorfieldViaFMTrips::FloorfieldViaFMTrips()
+{
+ //ctor (very ugly)
+ //std::cerr << "The defaultconsturctor FloorfieldViaFMTrips should not be called!" << std::endl;
+}
+
+FloorfieldViaFMTrips::~FloorfieldViaFMTrips()
+{
+ //dtor
+ delete _grid;
+ if (_gcode) delete[] _gcode;
+ if (_subrooms) delete[] _subrooms;
+ if (_dist2Wall) delete[] _dist2Wall;
+ if (_speedInitial) delete[] _speedInitial;
+ if (_modifiedspeed) delete[] _modifiedspeed;
+ if (_densityspeed) delete[] _densityspeed;
+ //if (cost) delete[] cost;
+ //if (neggrad) delete[] neggrad;
+ if (_dirToWall) delete[] _dirToWall;
+ //if (trialfield) delete[] trialfield;
+ for ( const auto& goalid : _goalcostmap) {
+ if (goalid.second) delete[] goalid.second;
+ }
+ for ( const auto& id : _costmap) {
+ //if (id.first == -1) continue;
+ if (id.second) delete[] id.second;
+ if (_neggradmap.at(id.first)) delete[] _neggradmap.at(id.first);
+ //map will be deleted by itself
+ }
+
+}
+
+FloorfieldViaFMTrips::FloorfieldViaFMTrips(const Building* const buildingArg, const double hxArg, const double hyArg,
+ const double wallAvoidDistance, const bool useDistancefield, const bool onlyRoomsWithExits) {
+ Log->Write("FloorfieldViaFMTrips::FloorfieldViaFMTrips(...)");
+ //ctor
+ //_threshold = -1; //negative value means: ignore threshold
+ _threshold = wallAvoidDistance;
+ _building = buildingArg;
+ _useDistanceToWall = useDistancefield;
+
+ if (hxArg != hyArg) {
+ //std::cerr << "ERROR: hx != hy <=========";
+ Log->Write("WARNING: \tFloor field: stepsize hx differs from hy! Taking hx = %d for both.", hxArg);
+ }
+ //parse building and create list of walls/obstacles (find xmin xmax, ymin, ymax, and add border?)
+ //Log->Write("INFO: \tStart Parsing: Building");
+ if (onlyRoomsWithExits) {
+ parseBuildingForExits(buildingArg, hxArg, hyArg);
+ } else {
+ parseBuilding(buildingArg, hxArg, hyArg);
+ }
+ //Log->Write("INFO: \tFinished Parsing: Building");
+ //testoutput("AALineScan.vtk", "AALineScan.txt", dist2Wall);
+
+ prepareForDistanceFieldCalculation(onlyRoomsWithExits);
+ //Log->Write("INFO: \tGrid initialized: Walls");
+
+ calculateDistanceField(-1.); //negative threshold is ignored, so all distances get calculated. this is important since distances is used for slowdown/redirect
+ //Log->Write("INFO: \tGrid initialized: Walldistances");
+
+ setSpeed(useDistancefield); //use distance2Wall
+ //Log->Write("INFO: \tGrid initialized: Speed");
+
+ calculateFloorfield(_exitsFromScope, _cost, _neggrad);
+ //writing FF-file disabled, we will not revive it ( @todo: argraf )
+}
+
+FloorfieldViaFMTrips::FloorfieldViaFMTrips(const FloorfieldViaFMTrips &other) :
+_building(other._building)
+
+{
+ _grid = other.getGrid();
+ long int otherNumOfPoints = _grid->GetnPoints();
+
+ _wall.clear();
+ _wall.reserve(other._wall.size());
+ std::copy(other._wall.begin(), other._wall.end(), _wall.begin());
+
+ _exitsFromScope.clear();
+ _exitsFromScope.reserve(other._exitsFromScope.size());
+ std::copy(other._exitsFromScope.begin(), other._exitsFromScope.end(), _exitsFromScope.begin());
+
+ _numOfExits = other._numOfExits;
+
+ _gcode = new int[otherNumOfPoints];
+ std::copy(other._gcode, other._gcode + otherNumOfPoints, _gcode);
+
+ _dist2Wall = new double[otherNumOfPoints];
+ std::copy(other._dist2Wall, other._dist2Wall + otherNumOfPoints, _dist2Wall);
+
+ _speedInitial = new double[otherNumOfPoints];
+ std::copy(other._speedInitial, other._speedInitial + otherNumOfPoints, _speedInitial);
+
+ _modifiedspeed = new double[otherNumOfPoints];
+ std::copy(other._modifiedspeed, other._modifiedspeed + otherNumOfPoints, _modifiedspeed);
+
+ _densityspeed = new double[otherNumOfPoints];
+ std::copy(other._densityspeed, other._densityspeed + otherNumOfPoints, _densityspeed);
+
+ _cost = new double[otherNumOfPoints];
+ std::copy(other._cost, other._cost + otherNumOfPoints, _cost);
+
+ _neggrad = new Point[otherNumOfPoints]; //gradients
+ std::copy(other._neggrad, other._neggrad + otherNumOfPoints, _neggrad);
+
+ _dirToWall = new Point[otherNumOfPoints];
+ std::copy(other._dirToWall, other._dirToWall + otherNumOfPoints, _dirToWall);
+
+ _threshold = other._threshold;
+ _useDistanceToWall = other._useDistanceToWall;
+}
+
+FloorfieldViaFMTrips::FloorfieldViaFMTrips(const std::string& filename) {
+
+
+ Log->Write("ERROR: \tReading FF from file not supported!!");
+ Log->Write(filename);
+
+}
+
+//void FloorfieldViaFMTrips::getDirectionAt(const Point& position, Point& direction){
+// long int key = grid->getKeyAtPoint(position);
+// direction._x = (neggrad[key]._x);
+// direction._y = (neggrad[key]._y);
+//}
+
+void FloorfieldViaFMTrips::getDirectionToDestination(Pedestrian* ped, Point& direction){
+ const Point& position = ped->GetPos();
+ int destID = ped->GetExitIndex();
+ long int key = _grid->getKeyAtPoint(position);
+ getDirectionToUID(destID, key, direction);
+ if (direction._x == DBL_MAX && direction._y == DBL_MAX) {
+ // This can be interpreted differently by the different operational models.
+ // The inaccuracy introduced by this is negligible. --f.mack
+ direction._x = 0;
+ direction._y = 0;
+ }
+}
+
+void FloorfieldViaFMTrips::getDirectionToUID(int destID, const long int key, Point &direction) {
+ getDirectionToUID(destID, key, direction, global_shortest);
+}
+
+void FloorfieldViaFMTrips::getDirectionToUID(int destID, const long int key, Point& direction, int mode) {
+ //what if goal == -1, meaning closest exit... is GetExitIndex then -1? NO... ExitIndex is UID, given by router
+ //if (ped->GetFinalDestination() == -1) /*go to closest exit*/ destID != -1;
+
+ if ((key < 0) || (key >= _grid->GetnPoints())) { // @todo: ar.graf: this check in a #ifdef-block?
+ Log->Write("ERROR: \t Floorfield tried to access a key out of grid!");
+ direction._x = 0.;
+ direction._y = 0.;
+ return;
+ }
+ Point* localneggradptr = nullptr;
+ double* localcostptr = nullptr;
+ {
+ if (_neggradmap.count(destID) == 0) {
+ //Log->Write("FF for destID %d does not exist (key is %d)", destID, key);
+ //check, if distID is in this grid
+ Hline* destLine = _building->GetTransOrCrossByUID(destID);
+ Point A = destLine->GetPoint1();
+ Point B = destLine->GetPoint2();
+ if (!(_grid->includesPoint(A)) || !(_grid->includesPoint(B))) {
+ Log->Write("ERROR: \t Destination ID %d is not in grid!", destID);
+ direction._x = direction._y = 0.;
+ //return;
+ }
+ }
+ localneggradptr = (_neggradmap.count(destID) == 0) ? nullptr : _neggradmap.at(destID);
+ localcostptr = (_costmap.count(destID) == 0) ? nullptr : _costmap.at(destID);
+ if (localneggradptr == nullptr) {
+ bool isBeingCalculated;
+#pragma omp critical(floorfieldsBeingCalculated)
+ {
+ if (!(isBeingCalculated = _floorfieldsBeingCalculated.count(destID) > 0)) {
+ _floorfieldsBeingCalculated.insert(destID);
+ }
+ }
+ if (isBeingCalculated) {
+ // we do not want to wait until the other calculation has finished, so we return immediately
+ // the values are corrected in getDirectionToDestination(), and getCostToDestination doesn't care about the direction
+ direction._x = DBL_MAX;
+ direction._y = DBL_MAX;
+ return;
+ }
+
+ //create floorfield (remove mapentry with nullptr, allocate memory, add mapentry, create ff)
+ localcostptr = new double[_grid->GetnPoints()];
+ localneggradptr = new Point[_grid->GetnPoints()];
+#pragma omp critical(neggradmap)
+ _neggradmap.erase(destID);
+#pragma omp critical(neggradmap)
+ _neggradmap.emplace(destID, localneggradptr);
+#pragma omp critical(costmap)
+ _costmap.erase(destID);
+#pragma omp critical(costmap)
+ _costmap.emplace(destID, localcostptr);
+
+ //create ff (prepare Trial-mechanic, then calc)
+// for (long int i = 0; i < grid->GetnPoints(); ++i) {
+// //set TrialPTripstr to fieldelements
+// trialfield[i].cost = localcostptr + i;
+// trialfield[i].neggrad = localneggradptr + i;
+// trialfield[i].father = nullptr;
+// trialfield[i].child = nullptr;
+// }
+// clearAndPrepareForFloorfieldReCalc(localcostptr);
+ std::vector<Line> localline = {Line((Line) *(_building->GetTransOrCrossByUID(destID)))};
+// setNewGoalAfterTheClear(localcostptr, localline);
+ //Log->Write("Starting FF for UID %d (ID %d)", destID, dynamic_cast<Crossing*>(building->GetTransOrCrossByUID(destID))->GetID());
+ //std::cerr << "\rW\tO\tR\tK\tI\tN\tG";
+ if (mode == quickest) {
+ calculateFloorfield(localline, localcostptr, localneggradptr, _densityspeed);
+ } else {
+ calculateFloorfield(localline, localcostptr, localneggradptr, _modifiedspeed);
+ }
+#pragma omp critical(floorfieldsBeingCalculated)
+ {
+ if (_floorfieldsBeingCalculated.count(destID) != 1) {
+ Log->Write("ERROR: FloorfieldViaFMTrips::getDirectionToUID: key %d was calculating FF for destID %d, but it was removed from floorfieldsBeingCalculated meanwhile", key, destID);
+ }
+ _floorfieldsBeingCalculated.erase(destID);
+ }
+ //Log->Write("Ending FF for UID %d", destID);
+ //std::cerr << "\r W\t O\t R\t K\t I\t N\t G";
+ }
+ }
+ direction._x = (localneggradptr[key]._x);
+ direction._y = (localneggradptr[key]._y);
+}
+
+void FloorfieldViaFMTrips::createMapEntryInLineToGoalID(const int goalID, bool isInside)
+{
+ Point* localneggradptr;
+ double* localcostptr;
+ if (goalID < 0) {
+ Log->Write("WARNING: \t goalID was negative in FloorfieldViaFMTrips::createMapEntryInLineToGoalID");
+ return;
+ }
+ if (!_building->GetFinalGoal(goalID)) {
+ Log->Write("WARNING: \t goalID was unknown in FloorfieldViaFMTrips::createMapEntryInLineToGoalID");
+ return;
+ }
+
+ // The scope of this critical section can probably be reduced (maybe use a GoalsBeingCalculated similar to FloorfieldViaFMTrips::getDirectionToUID)
+#pragma omp critical(FloorfieldViaFMTrips_maps)
+ {
+ if (_goalcostmap.count(goalID) == 0) { //no entry for goalcostmap, so we need to calc FF
+ _goalcostmap.emplace(goalID, nullptr);
+ _goalneggradmap.emplace(goalID, nullptr);
+ _goalToLineUIDmap.emplace(goalID, -1);
+ _goalToLineUIDmap2.emplace(goalID, -1);
+ _goalToLineUIDmap3.emplace(goalID, -1);
+ }
+ localneggradptr = _goalneggradmap.at(goalID);
+ localcostptr = _goalcostmap.at(goalID);
+ if (localneggradptr == nullptr) {
+ //create floorfield (remove mapentry with nullptr, allocate memory, add mapentry, create ff)
+ localcostptr = new double[_grid->GetnPoints()];
+ localneggradptr = new Point[_grid->GetnPoints()];
+ _goalneggradmap.erase(goalID);
+ //goalneggradmap.emplace(goalID, localneggradptr);
+ _goalcostmap.erase(goalID);
+ //goalcostmap.emplace(goalID, localcostptr);
+ //create ff (prepare Trial-mechanic, then calc)
+// for (long int i = 0; i < grid->GetnPoints(); ++i) {
+// //set TrialPTripstr to fieldelements
+// trialfield[i].cost = localcostptr + i;
+// trialfield[i].neggrad = localneggradptr + i;
+// trialfield[i].father = nullptr;
+// trialfield[i].child = nullptr;
+// }
+// clearAndPrepareForFloorfieldReCalc(localcostptr);
+
+ //get all lines/walls of goalID
+ vector<Line> localline;
+ const std::map<int, Goal*>& allgoals = _building->GetAllGoals();
+ vector<Wall> localwalls = allgoals.at(goalID)->GetAllWalls();
+
+ double xMin = _grid->GetxMin();
+ double xMax = _grid->GetxMax();
+
+ double yMin = _grid->GetyMin();
+ double yMax = _grid->GetyMax();
+
+ for (const auto& iwall:localwalls) {
+ const Point& a = iwall.GetPoint1();
+ const Point& b = iwall.GetPoint2();
+ if (
+ (a._x>=xMin) && (a._x<=xMax)
+ && (a._y>=yMin) && (a._y<=yMax)
+ && (b._x>=xMin) && (b._x<=xMax)
+ && (b._y>=yMin) && (b._y<=yMax)
+ ) {
+ localline.emplace_back(Line((Line) iwall));
+ std::cout << "(Line) iwall : " << ((Line) iwall).toString() << std::endl;
+
+ }
+ else {
+ std::cerr << "GOAL " << goalID << " includes point out of grid!" << std::endl;
+ std::cerr << "Point: " << a._x << ", " << a._y << std::endl;
+ std::cerr << "Point: " << b._x << ", " << b._y << std::endl;
+ }
+ }
+
+// setNewGoalAfterTheClear(localcostptr, localline);
+
+ //performance-measurement:
+ //auto start = std::chrono::steady_clock::now();
+ calculateFloorfield(localline, localcostptr, localneggradptr);
+
+ //performance-measurement:
+ //auto end = std::chrono::steady_clock::now();
+ //auto diff = end - start;
+ //std::cerr << std::chrono::duration_cast<std::chrono::seconds>(end - start).count() << std::endl;
+ //std::cerr << "new GOALfield " << goalID << " : " << localline[0].GetPoint1().GetX() << " " << localline[0].GetPoint1().GetY() << " " << localline[0].GetPoint2().GetX() << " " << localline[0].GetPoint2().GetY() << std::endl;
+ //Log->Write("new GOALfield " + std::to_string(goalID) + " : " + std::to_string(localline[0].GetPoint1().GetX()));
+ //Log->Write("new GOALfield " + std::to_string(goalID) + " : " + std::to_string( std::chrono::duration_cast<std::chrono::seconds>(end - start).count() ) + " " + std::to_string(localline.size()) );
+ //find closest door and add to cheatmap "goalToLineUID" map
+
+ const std::map<int, Transition*>& transitions = _building->GetAllTransitions();
+ const std::map<int, Crossing*>& crossings = _building->GetAllCrossings();
+
+ int UID_of_MIN = -1;
+ int UID_of_MIN2 = -1;
+ int UID_of_MIN3 = -1;
+ double cost_of_MIN = DBL_MAX;
+ double cost_of_MIN2 = DBL_MAX;
+ double cost_of_MIN3 = DBL_MAX;
+ long int dummykey;
+
+ if (isInside) {
+ for (const auto& loccross : crossings) {
+ std::cout << "Check crossing ID " << loccross.second->GetID() << " " << loccross.second->toString() << std::endl;
+ if ( !loccross.second->IsOpen()) {
+ continue;
+ }
+ dummykey = _grid->getKeyAtPoint(loccross.second->GetCentre());
+ if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+ UID_of_MIN3 = UID_of_MIN2;
+ cost_of_MIN3 = cost_of_MIN2;
+
+ UID_of_MIN2 = UID_of_MIN;
+ cost_of_MIN2 = cost_of_MIN;
+
+ UID_of_MIN = loccross.second->GetUniqueID();
+ cost_of_MIN = localcostptr[dummykey];
+ std::cout << "Closer Line found: " << UID_of_MIN << std::endl;
+ continue;
+ }
+ if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+ UID_of_MIN3 = UID_of_MIN2;
+ cost_of_MIN3 = cost_of_MIN2;
+
+ UID_of_MIN2 = loccross.second->GetUniqueID();
+ cost_of_MIN2 = localcostptr[dummykey];
+ continue;
+ }
+ if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+ UID_of_MIN3 = loccross.second->GetUniqueID();
+ cost_of_MIN3 = localcostptr[dummykey];
+ continue;
+ }
+ }
+ }
+ else {
+ for (const auto& loctrans : transitions) {
+ if (!loctrans.second->IsExit() || !loctrans.second->IsOpen()) {
+ continue;
+ }
+ dummykey = _grid->getKeyAtPoint(loctrans.second->GetCentre());
+ if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+ UID_of_MIN3 = UID_of_MIN2;
+ cost_of_MIN3 = cost_of_MIN2;
+
+ UID_of_MIN2 = UID_of_MIN;
+ cost_of_MIN2 = cost_of_MIN;
+
+ UID_of_MIN = loctrans.second->GetUniqueID();
+ cost_of_MIN = localcostptr[dummykey];
+ //std::cerr << std::endl << "Closer Line found: " << UID_of_MIN ;
+ continue;
+ }
+ if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+ UID_of_MIN3 = UID_of_MIN2;
+ cost_of_MIN3 = cost_of_MIN2;
+
+ UID_of_MIN2 = loctrans.second->GetUniqueID();
+ cost_of_MIN2 = localcostptr[dummykey];
+ continue;
+ }
+ if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
+ UID_of_MIN3 = loctrans.second->GetUniqueID();
+ cost_of_MIN3 = localcostptr[dummykey];
+ continue;
+ }
+ }
+ }
+
+ _goalToLineUIDmap.erase(goalID);
+ _goalToLineUIDmap.emplace(goalID, UID_of_MIN);
+ _goalToLineUIDmap2.erase(goalID);
+ _goalToLineUIDmap2.emplace(goalID, UID_of_MIN2);
+ _goalToLineUIDmap3.erase(goalID);
+ _goalToLineUIDmap3.emplace(goalID, UID_of_MIN3);
+ delete[] localcostptr;
+ delete[] localneggradptr;
+ }
+ }
+}
+
+double FloorfieldViaFMTrips::getCostToDestination(const int destID, const Point& position) {
+ return getCostToDestination(destID, position, global_shortest);
+}
+
+double FloorfieldViaFMTrips::getCostToDestination(const int destID, const Point& position, int mode) {
+ if ((_costmap.count(destID) == 0) || (_costmap.at(destID) == nullptr)) {
+ Point dummy;
+ getDirectionToUID(destID, 0, dummy, mode); //this call induces the floorfieldcalculation
+ }
+ if ((_costmap.count(destID) == 0) || (_costmap.at(destID) == nullptr)) {
+ Log->Write("ERROR: \tDestinationUID %d is invalid / out of grid.", destID);
+ return DBL_MAX;
+ }
+ if (_grid->getKeyAtPoint(position) == -1) { //position is out of grid
+ return -7;
+ }
+ return (_costmap.at(destID))[_grid->getKeyAtPoint(position)];
+}
+
+void FloorfieldViaFMTrips::getDir2WallAt(const Point& position, Point& direction){
+ long int key = _grid->getKeyAtPoint(position);
+ //debug assert
+ if (key < 0) {
+ Log->Write("ERROR: \tgetDir2WallAt error");
+ } else {
+ direction._x = (_dirToWall[key]._x);
+ direction._y = (_dirToWall[key]._y);
+ }
+}
+
+double FloorfieldViaFMTrips::getDistance2WallAt(const Point& position) {
+ long int key = _grid->getKeyAtPoint(position);
+ //debug assert
+ if (key < 0) {
+ Log->Write("ERROR: \tgetDistance2WallAt error");
+ return 1.;
+ } else {
+ return _dist2Wall[key];
+ }
+}
+
+//int FloorfieldViaFMTrips::getSubroomUIDAt(const Point &position) {
+// long int key = grid->getKeyAtPoint(position);
+// return subroomUID[key];
+//}
+
+/*!
+ * \brief Parsing geo-info but conflicts in multi-floor-buildings OBSOLETE
+ *
+ * When parsing a building with multiple floors, the projection of all floors onto the x-/y- plane leads to wrong
+ * results.
+ *
+ * \param[in] buildingArg provides the handle to all information
+ * \param[out] stepSizeX/-Y discretization of the grid, which will be created here
+ * \return member attributes of FloorfieldViaFMTrips class are allocated and initialized
+ * \sa Macros.h, RectGrid.h
+ * \note
+ * \warning
+ */
+void FloorfieldViaFMTrips::parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY) {
+ _building = buildingArg;
+ //init min/max before parsing
+ double xMin = DBL_MAX;
+ double xMax = -DBL_MAX;
+ double yMin = xMin;
+ double yMax = xMax;
+
+ if (stepSizeX != stepSizeY) Log->Write("ERROR: \tStepsizes in x- and y-direction must be identical!");
+
+ _costmap.clear();
+ _neggradmap.clear();
+ _wall.clear();
+ _exitsFromScope.clear();
+
+ //create a list of walls
+ const std::map<int, Transition*>& allTransitions = buildingArg->GetAllTransitions();
+ for (auto& trans : allTransitions) {
+ if (
+ trans.second->IsExit() && trans.second->IsOpen()
+ )
+ {
+ _exitsFromScope.emplace_back(Line ( (Line) *(trans.second)));
+ }
+ //populate both maps: costmap, neggradmap. These are the lookup maps for floorfields to specific transitions
+ _costmap.emplace(trans.second->GetUniqueID(), nullptr);
+ _neggradmap.emplace(trans.second->GetUniqueID(), nullptr);
+ }
+ _numOfExits = (unsigned int) _exitsFromScope.size();
+ for (auto& trans : allTransitions) {
+ if (!trans.second->IsOpen()) {
+ _wall.emplace_back(Line ( (Line) *(trans.second)));
+ }
+
+ }
+ for (const auto& itRoom : buildingArg->GetAllRooms()) {
+ for (const auto& itSubroom : itRoom.second->GetAllSubRooms()) {
+ std::vector<Obstacle*> allObstacles = itSubroom.second->GetAllObstacles();
+ for (std::vector<Obstacle*>::iterator itObstacles = allObstacles.begin(); itObstacles != allObstacles.end(); ++itObstacles) {
+
+ std::vector<Wall> allObsWalls = (*itObstacles)->GetAllWalls();
+ for (std::vector<Wall>::iterator itObsWall = allObsWalls.begin(); itObsWall != allObsWalls.end(); ++itObsWall) {
+ _wall.emplace_back(Line( (Line) *itObsWall));
+ // xMin xMax
+ if ((*itObsWall).GetPoint1()._x < xMin) xMin = (*itObsWall).GetPoint1()._x;
+ if ((*itObsWall).GetPoint2()._x < xMin) xMin = (*itObsWall).GetPoint2()._x;
+ if ((*itObsWall).GetPoint1()._x > xMax) xMax = (*itObsWall).GetPoint1()._x;
+ if ((*itObsWall).GetPoint2()._x > xMax) xMax = (*itObsWall).GetPoint2()._x;
+
+ // yMin yMax
+ if ((*itObsWall).GetPoint1()._y < yMin) yMin = (*itObsWall).GetPoint1()._y;
+ if ((*itObsWall).GetPoint2()._y < yMin) yMin = (*itObsWall).GetPoint2()._y;
+ if ((*itObsWall).GetPoint1()._y > yMax) yMax = (*itObsWall).GetPoint1()._y;
+ if ((*itObsWall).GetPoint2()._y > yMax) yMax = (*itObsWall).GetPoint2()._y;
+ }
+ }
+
+ std::vector<Wall> allWalls = itSubroom.second->GetAllWalls();
+ for (std::vector<Wall>::iterator itWall = allWalls.begin(); itWall != allWalls.end(); ++itWall) {
+ _wall.emplace_back( Line( (Line) *itWall));
+
+ // xMin xMax
+ if ((*itWall).GetPoint1()._x < xMin) xMin = (*itWall).GetPoint1()._x;
+ if ((*itWall).GetPoint2()._x < xMin) xMin = (*itWall).GetPoint2()._x;
+ if ((*itWall).GetPoint1()._x > xMax) xMax = (*itWall).GetPoint1()._x;
+ if ((*itWall).GetPoint2()._x > xMax) xMax = (*itWall).GetPoint2()._x;
+
+ // yMin yMax
+ if ((*itWall).GetPoint1()._y < yMin) yMin = (*itWall).GetPoint1()._y;
+ if ((*itWall).GetPoint2()._y < yMin) yMin = (*itWall).GetPoint2()._y;
+ if ((*itWall).GetPoint1()._y > yMax) yMax = (*itWall).GetPoint1()._y;
+ if ((*itWall).GetPoint2()._y > yMax) yMax = (*itWall).GetPoint2()._y;
+ }
+
+ const vector<Crossing*>& allCrossings = itSubroom.second->GetAllCrossings();
+ for (Crossing* crossPtr : allCrossings) {
+ if (!crossPtr->IsOpen()) {
+ _wall.emplace_back( Line( (Line) *crossPtr));
+
+ if (crossPtr->GetPoint1()._x < xMin) xMin = crossPtr->GetPoint1()._x;
+ if (crossPtr->GetPoint2()._x < xMin) xMin = crossPtr->GetPoint2()._x;
+ if (crossPtr->GetPoint1()._x > xMax) xMax = crossPtr->GetPoint1()._x;
+ if (crossPtr->GetPoint2()._x > xMax) xMax = crossPtr->GetPoint2()._x;
+
+ if (crossPtr->GetPoint1()._y < yMin) yMin = crossPtr->GetPoint1()._y;
+ if (crossPtr->GetPoint2()._y < yMin) yMin = crossPtr->GetPoint2()._y;
+ if (crossPtr->GetPoint1()._y > yMax) yMax = crossPtr->GetPoint1()._y;
+ if (crossPtr->GetPoint2()._y > yMax) yMax = crossPtr->GetPoint2()._y;
+ }
+ }
+ }
+ }
+
+ //all goals
+ const std::map<int, Goal*>& allgoals = buildingArg->GetAllGoals();
+ for (auto eachgoal:allgoals) {
+ for (auto& eachwall:eachgoal.second->GetAllWalls() ) {
+ if (eachwall.GetPoint1()._x < xMin) xMin = eachwall.GetPoint1()._x;
+ if (eachwall.GetPoint2()._x < xMin) xMin = eachwall.GetPoint2()._x;
+ if (eachwall.GetPoint1()._x > xMax) xMax = eachwall.GetPoint1()._x;
+ if (eachwall.GetPoint2()._x > xMax) xMax = eachwall.GetPoint2()._x;
+
+ if (eachwall.GetPoint1()._y < yMin) yMin = eachwall.GetPoint1()._y;
+ if (eachwall.GetPoint2()._y < yMin) yMin = eachwall.GetPoint2()._y;
+ if (eachwall.GetPoint1()._y > yMax) yMax = eachwall.GetPoint1()._y;
+ if (eachwall.GetPoint2()._y > yMax) yMax = eachwall.GetPoint2()._y;
+ }
+ _goalcostmap.emplace(eachgoal.second->GetId(), nullptr);
+ _goalneggradmap.emplace(eachgoal.second->GetId(), nullptr);
+ }
+
+ //create Rect Grid
+ _grid = new RectGrid();
+ _grid->setBoundaries(xMin, yMin, xMax, yMax);
+ _grid->setSpacing(stepSizeX, stepSizeX);
+ _grid->createGrid();
+
+ //create arrays
+ _subrooms = new SubRoom*[_grid->GetnPoints()]();
+ _gcode = new int[_grid->GetnPoints()]; //gcode:
+ // enum GridCode { //used in floor fields
+ // WALL = 0,
+ // INSIDE,
+ // OUTSIDE,
+ // OPEN_CROSSING,
+ // OPEN_TRANSITION,
+ // CLOSED_CROSSING,
+ // CLOSED_TRANSITION
+ // };
+ _dist2Wall = new double[_grid->GetnPoints()];
+ _speedInitial = new double[_grid->GetnPoints()];
+ _modifiedspeed = new double[_grid->GetnPoints()];
+ _densityspeed = new double[_grid->GetnPoints()];
+ _cost = new double[_grid->GetnPoints()];
+ _neggrad = new Point[_grid->GetnPoints()];
+ _dirToWall = new Point[_grid->GetnPoints()];
+
+ _costmap.emplace(-1 , _cost); // enable default ff (closest exit)
+ _neggradmap.emplace(-1, _neggrad);
+
+ //init grid with -3 as unknown distance to any wall
+ for(long int i = 0; i < _grid->GetnPoints(); ++i) {
+ _dist2Wall[i] = -3.;
+ _cost[i] = -2.;
+ _gcode[i] = OUTSIDE;
+ //flag[i] = FM_UNKNOWN; //unknown
+ }
+ drawLinesOnGrid<double>(_wall, _dist2Wall, 0.);
+ drawLinesOnGrid<double>(_wall, _cost, -7.);
+ drawLinesOnGrid<int>(_wall, _gcode, WALL);
+ drawLinesOnGrid<int>(_exitsFromScope, _gcode, OPEN_TRANSITION);
+}
+
+/*!
+ * \brief Parsing geo-info ONLY considering rooms with EXITS to the outside to avoid conflicts in multi-floor-buildings
+ *
+ * When parsing a building with multiple floors, the projection of all floors onto the x-/y- plane leads to wrong
+ * results. We then decided, to consider each floor separately and each staircase separately.
+ * We still need to match the ouside goals to an exit (transition), so that the router can guide agents to that goals.
+ * Next problem was in buildings, where exits to the outside would now be on different rooms. This is why we create this
+ * function. We want to create one floorfield for all rooms, that lead outside. Reason: We want the router to lead agents
+ * to the maybe second or third best exit-door, which might be in a different room.
+ *
+ * \param[in] buildingArg provides the handle to all information
+ * \param[out] stepSizeX/-Y discretization of the grid, which will be created here
+ * \return member attributes of FloorfieldViaFMTrips class are allocated and initialized
+ * \sa Macros.h, RectGrid.h
+ * \note
+ * \warning
+ */
+void FloorfieldViaFMTrips::parseBuildingForExits(const Building* const buildingArg, const double stepSizeX, const double stepSizeY) {
+ _building = buildingArg;
+ //init min/max before parsing
+ double xMin = DBL_MAX;
+ double xMax = -DBL_MAX;
+ double yMin = xMin;
+ double yMax = xMax;
+
+ if (stepSizeX != stepSizeY) Log->Write("ERROR: \tStepsizes in x- and y-direction must be identical!");
+
+ _costmap.clear();
+ _neggradmap.clear();
+ _wall.clear();
+ _exitsFromScope.clear();
+
+ std::vector<int> exitRoomIDs;
+ exitRoomIDs.clear();
+
+ //create a list of walls
+ const std::map<int, Transition*>& allTransitions = buildingArg->GetAllTransitions();
+ for (auto& trans : allTransitions) {
+ if (
+ trans.second->IsExit() && trans.second->IsOpen()
+ )
+ {
+ _exitsFromScope.emplace_back(Line ( (Line) *(trans.second)));
+ int roomID = -1;
+ if (trans.second->GetRoom1()) {
+ roomID = trans.second->GetRoom1()->GetID();
+ }
+ if (trans.second->GetRoom2()) {
+ roomID = trans.second->GetRoom2()->GetID();
+ }
+ if (std::find(exitRoomIDs.begin(), exitRoomIDs.end(), roomID) == exitRoomIDs.end()) {
+ exitRoomIDs.emplace_back(roomID);
+ }
+ }
+ //populate both maps: costmap, neggradmap. These are the lookup maps for floorfields to specific transitions
+ _costmap.emplace(trans.second->GetUniqueID(), nullptr);
+ _neggradmap.emplace(trans.second->GetUniqueID(), nullptr);
+ }
+ _numOfExits = (unsigned int) _exitsFromScope.size();
+ for (auto& trans : allTransitions) {
+ if (!trans.second->IsOpen()) {
+ _wall.emplace_back(Line ( (Line) *(trans.second)));
+ }
+
+ }
+ for (const auto& itRoom : buildingArg->GetAllRooms()) {
+ if (std::find(exitRoomIDs.begin(), exitRoomIDs.end(), itRoom.second->GetID()) == exitRoomIDs.end()) { //room with no exit
+ continue;
+ }
+ for (const auto& itSubroom : itRoom.second->GetAllSubRooms()) {
+ std::vector<Obstacle*> allObstacles = itSubroom.second->GetAllObstacles();
+ for (std::vector<Obstacle*>::iterator itObstacles = allObstacles.begin(); itObstacles != allObstacles.end(); ++itObstacles) {
+
+ std::vector<Wall> allObsWalls = (*itObstacles)->GetAllWalls();
+ for (std::vector<Wall>::iterator itObsWall = allObsWalls.begin(); itObsWall != allObsWalls.end(); ++itObsWall) {
+ _wall.emplace_back(Line( (Line) *itObsWall));
+ // xMin xMax
+ if ((*itObsWall).GetPoint1()._x < xMin) xMin = (*itObsWall).GetPoint1()._x;
+ if ((*itObsWall).GetPoint2()._x < xMin) xMin = (*itObsWall).GetPoint2()._x;
+ if ((*itObsWall).GetPoint1()._x > xMax) xMax = (*itObsWall).GetPoint1()._x;
+ if ((*itObsWall).GetPoint2()._x > xMax) xMax = (*itObsWall).GetPoint2()._x;
+
+ // yMin yMax
+ if ((*itObsWall).GetPoint1()._y < yMin) yMin = (*itObsWall).GetPoint1()._y;
+ if ((*itObsWall).GetPoint2()._y < yMin) yMin = (*itObsWall).GetPoint2()._y;
+ if ((*itObsWall).GetPoint1()._y > yMax) yMax = (*itObsWall).GetPoint1()._y;
+ if ((*itObsWall).GetPoint2()._y > yMax) yMax = (*itObsWall).GetPoint2()._y;
+ }
+ }
+
+ std::vector<Wall> allWalls = itSubroom.second->GetAllWalls();
+ for (std::vector<Wall>::iterator itWall = allWalls.begin(); itWall != allWalls.end(); ++itWall) {
+ _wall.emplace_back( Line( (Line) *itWall));
+
+ // xMin xMax
+ if ((*itWall).GetPoint1()._x < xMin) xMin = (*itWall).GetPoint1()._x;
+ if ((*itWall).GetPoint2()._x < xMin) xMin = (*itWall).GetPoint2()._x;
+ if ((*itWall).GetPoint1()._x > xMax) xMax = (*itWall).GetPoint1()._x;
+ if ((*itWall).GetPoint2()._x > xMax) xMax = (*itWall).GetPoint2()._x;
+
+ // yMin yMax
+ if ((*itWall).GetPoint1()._y < yMin) yMin = (*itWall).GetPoint1()._y;
+ if ((*itWall).GetPoint2()._y < yMin) yMin = (*itWall).GetPoint2()._y;
+ if ((*itWall).GetPoint1()._y > yMax) yMax = (*itWall).GetPoint1()._y;
+ if ((*itWall).GetPoint2()._y > yMax) yMax = (*itWall).GetPoint2()._y;
+ }
+ const vector<Crossing*>& allCrossings = itSubroom.second->GetAllCrossings();
+ for (Crossing* crossPtr : allCrossings) {
+ if (!crossPtr->IsOpen()) {
+ _wall.emplace_back( Line( (Line) *crossPtr));
+
+ if (crossPtr->GetPoint1()._x < xMin) xMin = crossPtr->GetPoint1()._x;
+ if (crossPtr->GetPoint2()._x < xMin) xMin = crossPtr->GetPoint2()._x;
+ if (crossPtr->GetPoint1()._x > xMax) xMax = crossPtr->GetPoint1()._x;
+ if (crossPtr->GetPoint2()._x > xMax) xMax = crossPtr->GetPoint2()._x;
+
+ if (crossPtr->GetPoint1()._y < yMin) yMin = crossPtr->GetPoint1()._y;
+ if (crossPtr->GetPoint2()._y < yMin) yMin = crossPtr->GetPoint2()._y;
+ if (crossPtr->GetPoint1()._y > yMax) yMax = crossPtr->GetPoint1()._y;
+ if (crossPtr->GetPoint2()._y > yMax) yMax = crossPtr->GetPoint2()._y;
+ }
+ }
+ }
+ }
+
+ //all goals
+ const std::map<int, Goal*>& allgoals = buildingArg->GetAllGoals();
+ for (auto eachgoal:allgoals) {
+ for (auto& eachwall:eachgoal.second->GetAllWalls() ) {
+ if (eachwall.GetPoint1()._x < xMin) xMin = eachwall.GetPoint1()._x;
+ if (eachwall.GetPoint2()._x < xMin) xMin = eachwall.GetPoint2()._x;
+ if (eachwall.GetPoint1()._x > xMax) xMax = eachwall.GetPoint1()._x;
+ if (eachwall.GetPoint2()._x > xMax) xMax = eachwall.GetPoint2()._x;
+
+ if (eachwall.GetPoint1()._y < yMin) yMin = eachwall.GetPoint1()._y;
+ if (eachwall.GetPoint2()._y < yMin) yMin = eachwall.GetPoint2()._y;
+ if (eachwall.GetPoint1()._y > yMax) yMax = eachwall.GetPoint1()._y;
+ if (eachwall.GetPoint2()._y > yMax) yMax = eachwall.GetPoint2()._y;
+ }
+ //goalcostmap.emplace(eachgoal.second->GetId(), nullptr);
+ //goalneggradmap.emplace(eachgoal.second->GetId(), nullptr);
+ }
+
+ //create Rect Grid
+ _grid = new RectGrid();
+ _grid->setBoundaries(xMin, yMin, xMax, yMax);
+ _grid->setSpacing(stepSizeX, stepSizeX);
+ _grid->createGrid();
+
+ //create arrays
+ _gcode = new int[_grid->GetnPoints()]; //see Macros.h: enum GridCode {...}
+ _subrooms = new SubRoom*[_grid->GetnPoints()]();
+ _dist2Wall = new double[_grid->GetnPoints()];
+ _speedInitial = new double[_grid->GetnPoints()];
+ _modifiedspeed = new double[_grid->GetnPoints()];
+ _cost = new double[_grid->GetnPoints()];
+ _neggrad = new Point[_grid->GetnPoints()];
+ _dirToWall = new Point[_grid->GetnPoints()];
+
+ _costmap.emplace(-1 , _cost); // enable default ff (closest exit)
+ _neggradmap.emplace(-1, _neggrad);
+
+ //init grid with -3 as unknown distance to any wall
+ for(long int i = 0; i < _grid->GetnPoints(); ++i) {
+ _dist2Wall[i] = -3.;
+ _cost[i] = -2.;
+ _gcode[i] = OUTSIDE; //unknown
+ _subrooms[i] = nullptr;
+ }
+ drawLinesOnGrid<double>(_wall, _dist2Wall, 0.);
+ drawLinesOnGrid<double>(_wall, _cost, -7.);
+ drawLinesOnGrid<int>(_wall, _gcode, WALL);
+ drawLinesOnGrid<int>(_exitsFromScope, _gcode, OPEN_TRANSITION);
+}
+
+//this function must only be used BEFORE calculateDistanceField(), because we set trialfield[].cost = dist2Wall AND we init dist2Wall with "-3"
+void FloorfieldViaFMTrips::prepareForDistanceFieldCalculation(const bool onlyRoomsWithExits) { //onlyRoomsWithExits means, outside points must be considered
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+
+ switch (_gcode[i]) {
+ //wall, closed_cross/_trans are all coded with "WALL" after prasing building
+ case WALL:
+ _speedInitial[i] = .001;
+ _cost[i] = -7.;
+ _dist2Wall[i] = 0.;
+ break;
+ case CLOSED_CROSSING:
+ _speedInitial[i] = .001;
+ _cost[i] = -7.;
+ _dist2Wall[i] = 0.;
+ break;
+ case CLOSED_TRANSITION:
+ _speedInitial[i] = .001;
+ _cost[i] = -7.;
+ _dist2Wall[i] = 0.;
+ break;
+ //open transitions are marked
+ case OPEN_TRANSITION:
+ _speedInitial[i] = 1.;
+ _cost[i] = 0.;
+ _neggrad[i]._x = (0.); //must be changed to costarray/neggradarray?
+ _neggrad[i]._y = (0.); //we can leave it, if we agree on cost/neggrad being
+ _dirToWall[i]._x = (0.); //default floorfield using all exits and have the
+ _dirToWall[i]._y = (0.); //array mechanic on top
+ break;
+ //open crossings are not marked at all after parsing building
+ case OPEN_CROSSING:
+ _speedInitial[i] = 1.;
+ _cost[i] = -2.;
+ break;
+ //after parsing, none is INSIDE, but for style reasons, I want switch cases to show all values
+ case INSIDE:
+ _speedInitial[i] = 1.;
+ _cost[i] = -2.;
+ break;
+ //this is the main thing in this loop, we want to find and mark inside points (and it is costly!!)
+ case OUTSIDE:
+ {
+ SubRoom* subroom = isInside(i);
+ if (subroom || onlyRoomsWithExits) {
+ _speedInitial[i] = 1.;
+ _cost[i] = -2.;
+ _gcode[i] = INSIDE;
+ _subrooms[i] = subroom;
+ }
+ break;
+ }
+ } //switch
+ } //for loop (points)
+ // drawLinesOnGrid(exits, cost, 0.); //already mark targets/exits in cost array (for floorfieldcalc and crossout (LocalFF))
+}
+
+void FloorfieldViaFMTrips::deleteAllFFs() {
+ for (size_t i = 0; i < _costmap.size(); ++i) {
+ auto costIter = _costmap.begin();
+ auto negIter = _neggradmap.begin();
+ std::advance(costIter, (_costmap.size() - (i+1)));
+ std::advance(negIter, (_neggradmap.size() - (i+1)));
+
+ if (costIter->second) delete[] costIter->second;
+ if (negIter->second) delete[] negIter->second;
+
+ costIter->second = nullptr;
+ negIter->second = nullptr;
+ }
+}
+
+template <typename T>
+void FloorfieldViaFMTrips::drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value) { //no init, plz init elsewhere
+// i~x; j~y;
+//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
+//src in answer of "Avi"; adapted to fit this application
+
+// //init with inside value:
+// long int indexMax = grid->GetnPoints();
+// for (long int i = 0; i < indexMax; ++i) {
+// target[i] = inside;
+// }
+
+ //grid handeling local vars:
+ long int iMax = _grid->GetiMax();
+
+ long int iStart, iEnd;
+ long int jStart, jEnd;
+ long int iDot, jDot;
+ long int key;
+ long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
+
+ for (auto& line : wallArg) {
+ key = _grid->getKeyAtPoint(line.GetPoint1());
+ iStart = (long) _grid->get_i_fromKey(key);
+ jStart = (long) _grid->get_j_fromKey(key);
+
+ key = _grid->getKeyAtPoint(line.GetPoint2());
+ iEnd = (long) _grid->get_i_fromKey(key);
+ jEnd = (long) _grid->get_j_fromKey(key);
+
+ deltaX = (int) (iEnd - iStart);
+ deltaY = (int) (jEnd - jStart);
+ deltaX1 = abs( (int) (iEnd - iStart));
+ deltaY1 = abs( (int) (jEnd - jStart));
+
+ px = 2*deltaY1 - deltaX1;
+ py = 2*deltaX1 - deltaY1;
+
+ if(deltaY1<=deltaX1) {
+ if(deltaX>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ xe = iEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ xe = iStart;
+ }
+ if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ for (i=0; iDot < xe; ++i) {
+ ++iDot;
+ if(px<0) {
+ px+=2*deltaY1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++jDot;
+ } else {
+ --jDot;
+ }
+ px+=2*(deltaY1-deltaX1);
+ }
+ if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ }
+ } else {
+ if(deltaY>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ ye = jEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ ye = jStart;
+ }
+ if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ for(i=0; jDot<ye; ++i) {
+ ++jDot;
+ if (py<=0) {
+ py+=2*deltaX1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++iDot;
+ } else {
+ --iDot;
+ }
+ py+=2*(deltaX1-deltaY1);
+ }
+ if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ }
+ }
+ } //loop over all walls
+
+} //drawLinesOnGrid
+
+void FloorfieldViaFMTrips::setSpeed(bool useDistance2WallArg) {
+ if (useDistance2WallArg && (_threshold > 0)) {
+ double temp; //needed to only slowdown band of threshold. outside of band modifiedspeed should be 1
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ temp = (_dist2Wall[i] < _threshold) ? _dist2Wall[i] : _threshold;
+ _modifiedspeed[i] = 0.001 + 0.999 * (temp/_threshold); //linear ramp from wall (0.001) to thresholddistance (1.000)
+ }
+ } else {
+ if (useDistance2WallArg) { //favor middle of hallways/rooms
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if (_threshold == 0.) { //special case if user set (use_wall_avoidance = true, wall_avoid_distance = 0.0) and thus wants a plain floorfield
+ _modifiedspeed[i] = _speedInitial[i];
+ } else { //this is the regular case for "favor middle of hallways/rooms
+ _modifiedspeed[i] = 0.001 + 0.999 * (_dist2Wall[i]/10); // @todo: ar.graf (10 ist ein hardgecodeter wert.. sollte ggf. angepasst werden)
+ }
+ }
+ } else { //do not use Distance2Wall
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ _modifiedspeed[i] = _speedInitial[i];
+ }
+ }
+ }
+ if (_densityspeed) {
+ std::copy(_modifiedspeed, _modifiedspeed + _grid->GetnPoints(), _densityspeed);
+ }
+}
+
+void FloorfieldViaFMTrips::setSpeedThruPeds(Pedestrian* const * pedsArg, int nsize, int modechoice, double radius) {
+
+ long int delta = radius / _grid->Gethx();
+ long int posIndex = 0;
+ long int pos_i = 0;
+ long int pos_j = 0;
+ long int i_start = 0;
+ long int j_start = 0;
+ long int i_end = 0;
+ long int j_end = 0;
+ double indexDistance = 0.0;
+
+ if (nsize == 0) {
+ Log->Write("WARNING: \tSetSpeedThruPeds: nsize is ZERO");
+ } else {
+ Log->Write("INFO: \t\tNumber of Peds used in setSpeedThruPeds: %d",nsize);
+ }
+
+ if ((modechoice == quickest) && (!_densityspeed)) {
+ _densityspeed = new double[_grid->GetnPoints()];
+ }
+ //std::copy(modifiedspeed, modifiedspeed+(grid->GetnPoints()), densityspeed);
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ _densityspeed[i] = _speedInitial[i];
+ }
+
+ for (int i = 0; i < nsize; ++i) {
+ //the following check is not 3D proof, we require the caller of this function to provide a list with "valid"
+ //pedestrian-pointer
+ if (!_grid->includesPoint(pedsArg[i]->GetPos())) {
+ continue;
+ }
+ /*this value defines the jam-speed threshold*/
+// if (pedsArg[i]->GetEllipse().GetV().Norm() > 0.8*pedsArg[i]->GetEllipse().GetV0()) {
+// continue;
+// }
+ posIndex = _grid->getKeyAtPoint(pedsArg[i]->GetPos());
+ pos_i = _grid->get_i_fromKey(posIndex);
+ pos_j = _grid->get_j_fromKey(posIndex);
+
+ i_start = ((pos_i - delta) < 0) ? 0 : (pos_i - delta);
+ i_end = ((pos_i + delta) >= _grid->GetiMax()) ? _grid->GetiMax()-1 : (pos_i + delta);
+
+ j_start = ((pos_j - delta) < 0) ? 0 : (pos_j - delta);
+ j_end = ((pos_j + delta) >= _grid->GetjMax()) ? _grid->GetjMax()-1 : (pos_j + delta);
+
+ for (long int curr_i = i_start; curr_i < i_end; ++curr_i) {
+ for (long int curr_j = j_start; curr_j < j_end; ++curr_j) {
+ //indexDistance holds the square
+ indexDistance = ( (curr_i - pos_i)*(curr_i - pos_i) + (curr_j - pos_j)*(curr_j - pos_j) );
+ //now using indexDistance to store the (speed) reduction value
+ //indexDistance = (delta*delta) - (indexDistance);
+ //if (indexDistance < 0) { indexDistance = 0.;}
+ //scale to [0 .. 1]
+ //indexDistance = indexDistance/(delta*delta);
+
+ //densityspeed[curr_j*grid->GetiMax() + curr_i] = (indexDistance > (delta*delta)) ? densityspeed[curr_j*grid->GetiMax() + curr_i] : .001;
+ if (indexDistance < (delta*delta)) {
+ //std::cout << "c h a n g i n g ";
+ _densityspeed[curr_j*_grid->GetiMax() + curr_i] = 0.07;
+ }
+ }
+ }
+ }
+
+}
+
+void FloorfieldViaFMTrips::calculateFloorfield(std::vector<Line>& targetlines, double* costarray, Point* neggradarray) {
+ calculateFloorfield(targetlines, costarray, neggradarray, _modifiedspeed);
+}
+
+void FloorfieldViaFMTrips::calculateFloorfield(std::vector<Line>& targetlines, double* costarray, Point* neggradarray, double* speedarray) {
+
+ std::priority_queue<TrialPTrips, std::vector<TrialPTrips>, std::greater<TrialPTrips>> trialqueue;
+ int* flag = new int[_grid->GetnPoints()];
+
+ //re-init memory
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if ((_gcode[i] == INSIDE) || (_gcode[i] == OPEN_TRANSITION) || (_gcode[i] == OPEN_CROSSING)) {
+ flag[i] = FM_UNKNOWN; //inside
+ } else {
+ flag[i] = FM_OUTSIDE; //wall, outside or closed
+ }
+
+ costarray[i] = -7.;
+ }
+ drawLinesOnGrid<double>(targetlines, costarray, 0.);
+ drawLinesOnGrid<int>(targetlines, flag, FM_FINAL);
+
+ //init narrowband
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if (flag[i] == FM_FINAL) {
+ TrialPTrips newTrialPTrips = TrialPTrips(i, costarray, speedarray, flag, neggradarray);
+ checkNeighborsAndAddToNarrowband(trialqueue, newTrialPTrips,
+ [&] (TrialPTrips TrialPTripsArg) {this->calcFloorfield(TrialPTripsArg);});
+ }
+ }
+
+ // initial narrowband done, now loop (while not empty: remove smallest, add neighbors of removed)
+ while (!trialqueue.empty()) {
+ long int keyOfSmallest = trialqueue.top().key;
+ flag[keyOfSmallest] = FM_FINAL;
+ trialqueue.pop();
+ TrialPTrips newTrialPTrips = TrialPTrips(keyOfSmallest, costarray, speedarray, flag, neggradarray);
+ checkNeighborsAndAddToNarrowband(trialqueue, newTrialPTrips,
+ [&] (TrialPTrips TrialPTripsArg) { this->calcFloorfield(TrialPTripsArg);});
+ }
+ delete[] flag;
+}
+
+void FloorfieldViaFMTrips::calculateDistanceField(const double thresholdArg) { //if threshold negative, then ignore it
+
+ std::priority_queue<TrialPTrips, std::vector<TrialPTrips>, std::greater<TrialPTrips>> trialqueue;
+ int* flag = new int[_grid->GetnPoints()];
+#ifdef TESTING
+ //sanity check (fields <> 0)
+ if (flag == nullptr) return;
+ if (_dist2Wall == 0) return;
+ if (_speedInitial == 0) return;
+ if (_cost == 0) return;
+ if (_neggrad == 0) return;
+#endif //TESTING
+
+ //re-init memory
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if ((_gcode[i] == INSIDE) || (_gcode[i] == OPEN_TRANSITION) || (_gcode[i] == OPEN_CROSSING)) {
+ flag[i] = FM_UNKNOWN; //inside
+ } else if (_gcode[i] == OUTSIDE) {
+ flag[i] = FM_OUTSIDE; //outside
+ } else {
+ flag[i] = FM_FINAL; //wall or closed
+ }
+ }
+
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ //if ((dist2Wall[i] == 0.) && (flag[i] == -7.)) {
+ if ((_gcode[i] == WALL) || (_gcode[i] == CLOSED_TRANSITION) || (_gcode[i] == CLOSED_CROSSING)) {
+ TrialPTrips newP = TrialPTrips(i, _dist2Wall, _speedInitial, flag, _dirToWall);
+ checkNeighborsAndAddToNarrowband(trialqueue, newP,
+ [&] (TrialPTrips TrialPTripsArg) { this->calcDist2Wall(TrialPTripsArg);} );
+ }
+ }
+
+ while (!trialqueue.empty()) {
+ long int keyOfSmallest = trialqueue.top().key;
+ flag[keyOfSmallest] = FM_FINAL;
+ trialqueue.pop();
+ if ((thresholdArg > 0) && (_dist2Wall[keyOfSmallest] > thresholdArg)) { //set rest of nearfield and rest of unknown to this max value:
+
+ //rest of nearfield
+ while (!trialqueue.empty()) {
+ long int currKey = trialqueue.top().key;
+ _dist2Wall[currKey] = _dist2Wall[keyOfSmallest];
+ flag[currKey] = FM_FINAL;
+ _dirToWall[currKey]._x = (0.);
+ _dirToWall[currKey]._y = (0.);
+ trialqueue.pop();
+ }
+
+ //rest of unknown
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if (flag[i] == FM_UNKNOWN) {
+ flag[i] = FM_FINAL;
+ _dist2Wall[i] = _dist2Wall[keyOfSmallest];
+ _dirToWall[i]._x = (0.);
+ _dirToWall[i]._y = (0.);
+ }
+ }
+ } else {
+ //Log->Write(std::to_string(debugcounter++) + " " + std::to_string(grid->GetnPoints()));
+ TrialPTrips smallestP = TrialPTrips(keyOfSmallest, _dist2Wall, _speedInitial, flag, _dirToWall);
+ checkNeighborsAndAddToNarrowband(trialqueue, smallestP,
+ [&] (TrialPTrips TrialPTripsArg) { this->calcDist2Wall(TrialPTripsArg);} );
+ }
+ }
+ delete[] flag;
+} //calculateDistancField
+
+void FloorfieldViaFMTrips::checkNeighborsAndAddToNarrowband(std::priority_queue<TrialPTrips, std::vector<TrialPTrips>, std::greater<TrialPTrips>>& trialqueue, TrialPTrips keyP,
+ std::function<void (TrialPTrips)> calc) {
+ int* flag = keyP.flag;
+
+ directNeighbor dNeigh = _grid->getNeighbors(keyP.key);
+
+ //check for valid neigh
+ long int aux = dNeigh.key[0];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in prepareForDistanceFieldCalc
+ if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
+ flag[aux] = FM_ADDED; // added to trial but not calculated
+ TrialPTrips currNeighP = TrialPTrips(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
+ calc(currNeighP);
+ trialqueue.emplace(currNeighP);
+ }
+ aux = dNeigh.key[1];
+ if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
+ flag[aux] = FM_ADDED; // added to trial but not calculated
+ TrialPTrips currNeighP = TrialPTrips(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
+ calc(currNeighP);
+ trialqueue.emplace(currNeighP);
+ }
+ aux = dNeigh.key[2];
+ if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
+ flag[aux] = FM_ADDED; // added to trial but not calculated
+ TrialPTrips currNeighP = TrialPTrips(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
+ calc(currNeighP);
+ trialqueue.emplace(currNeighP);
+ }
+ aux = dNeigh.key[3];
+ if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
+ flag[aux] = FM_ADDED; // added to trial but not calculated
+ TrialPTrips currNeighP = TrialPTrips(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
+ calc(currNeighP);
+ trialqueue.emplace(currNeighP);
+ }
+}
+
+void FloorfieldViaFMTrips::calcDist2Wall(TrialPTrips newPoint) {
+ double row;
+ double col;
+ long int aux;
+ bool pointsUp;
+ bool pointsRight;
+
+ int* flag = newPoint.flag;
+ double* cost = newPoint.cost;
+ long int key = newPoint.key;
+
+ row = 100000.;
+ col = 100000.;
+ //aux = -1;
+
+ directNeighbor dNeigh = _grid->getNeighbors(key);
+
+ aux = dNeigh.key[0];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in resetGoalAndCosts
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
+ (_gcode[aux] != OUTSIDE)) //gridpoint holds a calculated value
+ {
+ row = cost[aux];
+ pointsRight = true;
+ if (row < 0) {
+ std::cerr << "hier ist was schief " << row << " " << aux << " " << flag[aux] << std::endl;
+ row = 100000;
+ }
+ }
+ aux = dNeigh.key[2];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
+ (_gcode[aux] != OUTSIDE) &&
+ (cost[aux] < row)) //calculated value promises smaller cost
+ {
+ row = cost[aux];
+ pointsRight = false;
+ }
+
+ aux = dNeigh.key[1];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in parseBuilding after linescan call
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
+ (_gcode[aux] != OUTSIDE))
+ {
+ col = cost[aux];
+ pointsUp = true;
+ if (col < 0) {
+ std::cerr << "hier ist was schief " << col << " " << aux << " " << flag[aux] << std::endl;
+ col = 100000;
+ }
+ }
+ aux = dNeigh.key[3];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
+ (_gcode[aux] != OUTSIDE) &&
+ (cost[aux] < col)) //calculated value promises smaller cost
+ {
+ col = cost[aux];
+ pointsUp = false;
+ }
+ if (col == 100000.) { //one sided update with row
+ cost[key] = onesidedCalc(row, _grid->Gethx());
+ flag[key] = FM_SINGLE;
+ if (pointsRight) {
+ _dirToWall[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ _dirToWall[key]._y = (0.);
+ } else {
+ _dirToWall[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ _dirToWall[key]._y = (0.);
+ }
+ _dirToWall[key] = _dirToWall[key].Normalized(); //@todo: ar.graf: what yields better performance? scale every point here or scale each read value? more points or more calls to any element of dir2Wall
+ return;
+ }
+
+ if (row == 100000.) { //one sided update with col
+ cost[key] = onesidedCalc(col, _grid->Gethy());
+ flag[key] = FM_SINGLE;
+ if (pointsUp) {
+ _dirToWall[key]._x = (0.);
+ _dirToWall[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ } else {
+ _dirToWall[key]._x = (0.);
+ _dirToWall[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ _dirToWall[key] = _dirToWall[key].Normalized();
+ return;
+ }
+
+ //two sided update
+ double precheck = twosidedCalc(row, col, _grid->Gethx());
+ if (precheck >= 0) {
+ cost[key] = precheck;
+ flag[key] = FM_DOUBLE;
+ if (pointsUp && pointsRight) {
+ _dirToWall[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ _dirToWall[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ }
+ if (pointsUp && !pointsRight) {
+ _dirToWall[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ _dirToWall[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ }
+ if (!pointsUp && pointsRight) {
+ _dirToWall[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ _dirToWall[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ if (!pointsUp && !pointsRight) {
+ _dirToWall[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ _dirToWall[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ } else {
+ std::cerr << "else in twosided Dist " << std::endl;
+ }
+ _dirToWall[key] = _dirToWall[key].Normalized();
+}
+
+void FloorfieldViaFMTrips::calcFloorfield(TrialPTrips newPoint) {
+ double row;
+ double col;
+ long int aux;
+ bool pointsUp = false;
+ bool pointsRight = false;
+
+ int* flag = newPoint.flag;
+ double* cost = newPoint.cost;
+ long int key = newPoint.key;
+ double* speed = newPoint.speed;
+ Point* neggrad = newPoint.neggrad;
+
+ row = DBL_MAX;
+ col = DBL_MAX;
+ //aux = -1;
+
+ directNeighbor dNeigh = _grid->getNeighbors(key);
+
+ aux = dNeigh.key[0];
+ //hint: trialfield[i].cost = costarray + i; <<< set in calculateFloorfield
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) &&
+ //((gcode[aux] == INSIDE) || (gcode[aux] == OPEN_CROSSING) || (gcode[aux] == OPEN_TRANSITION)) ) //gridpoint holds a calculated value
+ (flag[aux] != FM_OUTSIDE))
+ {
+ row = cost[aux];
+ pointsRight = true;
+ }
+ aux = dNeigh.key[2];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) &&
+ (flag[aux] != FM_OUTSIDE) && //gridpoint holds a calculated value
+ (cost[aux] < row)) //calculated value promises smaller cost
+ {
+ row = cost[aux];
+ pointsRight = false;
+ }
+
+ aux = dNeigh.key[1];
+ //hint: trialfield[i].cost = cost + i; <<< set in calculateFloorfield
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) &&
+ (flag[aux] != FM_OUTSIDE)) //gridpoint holds a calculated value
+ {
+ col = cost[aux];
+ pointsUp = true;
+ }
+ aux = dNeigh.key[3];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (flag[aux] != FM_UNKNOWN) &&
+ (flag[aux] != FM_OUTSIDE) && //gridpoint holds a calculated value
+ (cost[aux] < col)) //calculated value promises smaller cost
+ {
+ col = cost[aux];
+ pointsUp = false;
+ }
+ if ((col == DBL_MAX) && (row == DBL_MAX)) {
+ std::cerr << "Issue 175 in FloorfieldViaFMTrips: invalid combination of row,col (both on max)" <<std::endl;
+ return;
+ }
+ if (col == DBL_MAX) { //one sided update with row
+ cost[key] = onesidedCalc(row, _grid->Gethx()/speed[key]);
+ flag[key] = FM_SINGLE;
+ if (pointsRight && (dNeigh.key[0] != -2)) {
+ neggrad[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ neggrad[key]._y = (0.);
+ } else if (dNeigh.key[2] != -2) {
+ neggrad[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ neggrad[key]._y = (0.);
+ }
+ return;
+ }
+
+ if (row == DBL_MAX) { //one sided update with col
+ cost[key] = onesidedCalc(col, _grid->Gethy()/speed[key]);
+ flag[key] = FM_SINGLE;
+ if ((pointsUp) && (dNeigh.key[1] != -2)) {
+ neggrad[key]._x = (0.);
+ neggrad[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ } else if (dNeigh.key[3] != -2){
+ neggrad[key]._x = (0.);
+ neggrad[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ return;
+ }
+
+ //two sided update
+ double precheck = twosidedCalc(row, col, _grid->Gethx()/speed[key]);
+// if (precheck > 10000) {
+// Log->Write("ERROR \t\t\t is in twosided");
+// }
+ if (precheck >= 0) {
+ cost[key] = precheck;
+ flag[key] = FM_DOUBLE;
+ if (pointsUp && pointsRight) {
+ neggrad[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ neggrad[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ }
+ if (pointsUp && !pointsRight) {
+ neggrad[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ neggrad[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ }
+ if (!pointsUp && pointsRight) {
+ neggrad[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ neggrad[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ if (!pointsUp && !pointsRight) {
+ neggrad[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ neggrad[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ } else {
+ std::cerr << "else in twosided Floor " << precheck << " " << row << " " << col << std::endl;
+ }
+}
+
+inline double FloorfieldViaFMTrips::onesidedCalc(double xy, double hDivF) {
+ //if ( (xy+hDivF) > 10000) std::cerr << "error in onesided " << xy << std::endl;
+ return xy + hDivF;
+}
+
+inline double FloorfieldViaFMTrips::twosidedCalc(double x, double y, double hDivF) { //on error return -2
+ double determinante = (2*hDivF*hDivF - (x-y)*(x-y));
+ if (determinante >= 0) {
+ return (x + y + sqrt(determinante))/2;
+ } else {
+ return (x < y) ? (x + hDivF) : (y + hDivF);
+ }
+ std::cerr << "error in two-sided 2!!!!!!!!!!!!!!!!!!!!!!! o_O??" << std::endl;
+ return -2.; //this line should never execute
+} //twosidedCalc
+
+void FloorfieldViaFMTrips::testoutput(const char* filename1, const char* filename2, const double* target) {
+//security debug check
+ std::ofstream file;
+ std::ofstream file2;
+ int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
+ int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
+ int numTotal = numX * numY;
+ //std::cerr << numTotal << " numTotal" << std::endl;
+ //std::cerr << grid->GetnPoints() << " grid" << std::endl;
+ file.open(filename1);
+ file2.open(filename2);
+ file << "# vtk DataFile Version 3.0" << std::endl;
+ file << "Testdata: Fast Marching: Test: " << std::endl;
+ file << "ASCII" << std::endl;
+ file << "DATASET STRUCTURED_POINTS" << std::endl;
+ file << "DIMENSIONS " <<
+ std::to_string(_grid->GetiMax()) <<
+ " " <<
+ std::to_string(_grid->GetjMax()) <<
+ " 1" << std::endl;
+ file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
+ file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
+ file << "POINT_DATA " << std::to_string(numTotal) << std::endl;
+ file << "SCALARS Cost float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << target[i] << std::endl;
+ Point iPoint = _grid->getPointFromKey(i);
+ file2 << iPoint._x /*- grid->GetxMin()*/ << " " << iPoint._y /*- grid->GetyMin()*/ << " " << target[i] << std::endl;
+ }
+
+ if (target == _cost) {
+ file << "VECTORS Gradient float" << std::endl;
+ for (int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _neggrad[i]._x << " " << _neggrad[i]._y << " 0.0" << std::endl;
+ }
+ }
+
+ file.close();
+ file2.close();
+
+ //std::cerr << "INFO: \tFile closed: " << filename1 << std::endl;
+}
+
+void FloorfieldViaFMTrips::writeFF(const std::string& filename, std::vector<int> targetID) {
+ Log->Write("INFO: \tWrite Floorfield to file");
+ Log->Write(filename);
+ std::ofstream file;
+
+ Log->Write("FloorfieldViaFMTrips::writeFF(): writing to file %s: There are %d targets.", filename.c_str(), targetID.size());
+
+ int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
+ int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
+ int numTotal = numX * numY;
+ //std::cerr << numTotal << " numTotal" << std::endl;
+ //std::cerr << grid->GetnPoints() << " grid" << std::endl;
+ file.open(filename);
+
+ file << "# vtk DataFile Version 3.0" << std::endl;
+ file << "Testdata: Fast Marching: Test: " << std::endl;
+ file << "ASCII" << std::endl;
+ file << "DATASET STRUCTURED_POINTS" << std::endl;
+ file << "DIMENSIONS " <<
+ std::to_string(_grid->GetiMax()) <<
+ " " <<
+ std::to_string(_grid->GetjMax()) <<
+ " 1" << std::endl;
+ file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
+ file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
+ file << "POINT_DATA " << std::to_string(numTotal) << std::endl;
+ file << "SCALARS Dist2Wall float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _dist2Wall[i] << std::endl; //@todo: change target to all dist2wall
+ //Point iPoint = grid->getPointFromKey(i);
+ //file2 << iPoint._x /*- grid->GetxMin()*/ << " " << iPoint._y /*- grid->GetyMin()*/ << " " << target[i] << std::endl;
+ }
+
+ file << "VECTORS Dir2Wall float" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _dirToWall[i]._x << " " << _dirToWall[i]._y << " 0.0" << std::endl;
+ }
+
+ file << "SCALARS SubroomPtr float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if (_subrooms[i]) {
+ file << _subrooms[i]->GetUID() << std::endl;
+ } else {
+ file << 0.0 << std::endl;
+ }
+ }
+
+ for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
+ Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
+ if (_neggradmap.count(targetID[iTarget]) == 0) {
+ continue;
+ }
+
+ Point *gradarray = _neggradmap[targetID[iTarget]];
+ if (gradarray == nullptr) {
+ 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 = _costmap[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 << "SCALARS GCode float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _gcode[i] << std::endl;
+ }
+ file.close();
+}
+
+void FloorfieldViaFMTrips::writeGoalFF(const std::string& filename, std::vector<int> targetID) {
+ Log->Write("INFO: \tWrite Floorfield to file");
+ Log->Write(filename);
+ std::ofstream file;
+
+ int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
+ int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
+ int numTotal = numX * numY;
+ //std::cerr << numTotal << " numTotal" << std::endl;
+ //std::cerr << grid->GetnPoints() << " grid" << std::endl;
+ file.open(filename);
+
+ file << "# vtk DataFile Version 3.0" << std::endl;
+ file << "Testdata: Fast Marching: Test: " << std::endl;
+ file << "ASCII" << std::endl;
+ file << "DATASET STRUCTURED_POINTS" << std::endl;
+ file << "DIMENSIONS " <<
+ std::to_string(_grid->GetiMax()) <<
+ " " <<
+ std::to_string(_grid->GetjMax()) <<
+ " 1" << std::endl;
+ file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
+ file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
+ file << "POINT_DATA " << std::to_string(numTotal) << std::endl;
+ //file << "SCALARS Dist2Wall float 1" << std::endl;
+ //file << "LOOKUP_TABLE default" << std::endl;
+ //for (long int i = 0; i < grid->GetnPoints(); ++i) {
+ // file << dist2Wall[i] << std::endl; //@todo: change target to all dist2wall
+ //Point iPoint = grid->getPointFromKey(i);
+ //file2 << iPoint._x /*- grid->GetxMin()*/ << " " << iPoint._y /*- grid->GetyMin()*/ << " " << target[i] << std::endl;
+ //}
+
+ //file << "VECTORS Dir2Wall float" << std::endl;
+ //for (long int i = 0; i < grid->GetnPoints(); ++i) {
+ // file << dirToWall[i]._x << " " << dirToWall[i]._y << " 0.0" << std::endl;
+ //}
+
+ for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
+ if (_goalneggradmap.count(targetID[iTarget]) == 0) {
+ continue;
+ }
+
+ Point *gradarray = _goalneggradmap[targetID[iTarget]];
+ if (gradarray == nullptr) {
+ continue;
+ }
+
+ file << "VECTORS GradientTarget" << targetID[iTarget] << " float" << std::endl;
+ for (int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << gradarray[i]._x << " " << gradarray[i]._y << " 0.0" << std::endl;
+ }
+
+ double *costarray = _goalcostmap[targetID[iTarget]];
+ file << "SCALARS CostTarget" << targetID[iTarget] << " 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 << "SCALARS GCode float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _gcode[i] << std::endl;
+ }
+ file.close();
+}
+
+SubRoom* FloorfieldViaFMTrips::isInside(const long int key) {
+// Point probe = _grid->getPointFromKey(key);
+
+// const std::map<int, std::shared_ptr<Room>>& roomMap = _building->GetAllRooms();
+//
+// for (auto& roomPair : roomMap) {
+//
+// Room* roomPtr = roomPair.second.get();
+// const std::map<int, std::shared_ptr<SubRoom>>& subRoomMap = roomPtr->GetAllSubRooms();
+//
+// for (auto& subRoomPair : subRoomMap) {
+//
+// SubRoom* subRoomPtr = subRoomPair.second.get();
+//
+// if (subRoomPtr->IsInSubRoom(probe)) {
+// return subRoomPtr;
+// }
+// }
+
+ //FloorfieldViaFMTrips does not support isInside because of unresolved problem of ambiguous projection of xy plane in multi-
+ //storage buildings
+
+ return nullptr;
+}
+
+void CentrePointFFViaFMTrips::getDirectionToUID(int destID, const long int key, Point& direction, int mode) {
+ //what if goal == -1, meaning closest exit... is GetExitIndex then -1? NO... ExitIndex is UID, given by router
+ //if (ped->GetFinalDestination() == -1) /*go to closest exit*/ destID != -1;
+
+ if ((key < 0) || (key >= _grid->GetnPoints())) { // @todo: ar.graf: this check in a #ifdef-block?
+ Log->Write("ERROR: \t Floorfield tried to access a key out of grid!");
+ direction._x = 0.;
+ direction._y = 0.;
+ return;
+ }
+ Point* localneggradptr = nullptr;
+ double* localcostptr = nullptr;
+ {
+ if (_neggradmap.count(destID) == 0) {
+ //Log->Write("FF for destID %d does not exist (key is %d)", destID, key);
+ //check, if distID is in this grid
+ Hline* destLine = _building->GetTransOrCrossByUID(destID);
+ Point A = destLine->GetCentre();
+ //Point B = destLine->GetPoint2();
+ if (!(_grid->includesPoint(A))) {
+ Log->Write("ERROR: \t Destination ID %d is not in grid!", destID);
+ direction._x = direction._y = 0.;
+ //return;
+ }
+ }
+ localneggradptr = (_neggradmap.count(destID) == 0) ? nullptr : _neggradmap.at(destID);
+ localcostptr = (_costmap.count(destID) == 0) ? nullptr : _costmap.at(destID);
+ if (localneggradptr == nullptr) {
+ bool isBeingCalculated;
+#pragma omp critical(floorfieldsBeingCalculated)
+ {
+ if (!(isBeingCalculated = _floorfieldsBeingCalculated.count(destID) > 0)) {
+ _floorfieldsBeingCalculated.insert(destID);
+ }
+ }
+ if (isBeingCalculated) {
+ // we do not want to wait until the other calculation has finished, so we return immediately
+ // the values are corrected in getDirectionToDestination(), and getCostToDestination doesn't care about the direction
+ direction._x = DBL_MAX;
+ direction._y = DBL_MAX;
+ return;
+ }
+
+ //create floorfield (remove mapentry with nullptr, allocate memory, add mapentry, create ff)
+ localcostptr = new double[_grid->GetnPoints()];
+ localneggradptr = new Point[_grid->GetnPoints()];
+#pragma omp critical(neggradmap)
+ _neggradmap.erase(destID);
+#pragma omp critical(neggradmap)
+ _neggradmap.emplace(destID, localneggradptr);
+#pragma omp critical(costmap)
+ _costmap.erase(destID);
+#pragma omp critical(costmap)
+ _costmap.emplace(destID, localcostptr);
+
+
+ //create ff (prepare Trial-mechanic, then calc)
+// for (long int i = 0; i < grid->GetnPoints(); ++i) {
+// //set TrialPTripstr to fieldelements
+// trialfield[i].cost = localcostptr + i;
+// trialfield[i].neggrad = localneggradptr + i;
+// trialfield[i].father = nullptr;
+// trialfield[i].child = nullptr;
+// }
+// clearAndPrepareForFloorfieldReCalc(localcostptr);
+ //std::vector<Line> localline = {Line((Line) *(building->GetTransOrCrossByUID(destID)))};
+ Point centre = _building->GetTransOrCrossByUID(destID)->GetCentre();
+ std::vector<Line> localline = {Line(centre, centre, 0)}; // only one point
+// setNewGoalAfterTheClear(localcostptr, localline);
+ //Log->Write("Starting FF for UID %d (ID %d)", destID, dynamic_cast<Crossing*>(building->GetTransOrCrossByUID(destID))->GetID());
+ //std::cerr << "\rW\tO\tR\tK\tI\tN\tG";
+ if (mode == quickest) {
+ calculateFloorfield(localline, localcostptr, localneggradptr, _densityspeed);
+ } else {
+ calculateFloorfield(localline, localcostptr, localneggradptr, _modifiedspeed);
+ }
+ //when using CentralPoint, the rest of destID Line would not be calculated. We set that line to zero in the lines below.
+ //this way, the ffrouter understands, if a pedestrian reached the line, but is next to central point.
+ Point a = _building->GetTransOrCrossByUID(destID)->GetPoint1();
+ Point b = _building->GetTransOrCrossByUID(destID)->GetPoint2();
+ localline.emplace_back(Line(a, b, 0));
+ drawLinesOnGrid<double>(localline, localcostptr, 0.);
+#pragma omp critical(floorfieldsBeingCalculated)
+ {
+ if (_floorfieldsBeingCalculated.count(destID) != 1) {
+ Log->Write("ERROR: FloorfieldViaFMTrips::getDirectionToUID: key %d was calculating FF for destID %d, but it was removed from floorfieldsBeingCalculated meanwhile", key, destID);
+ }
+ _floorfieldsBeingCalculated.erase(destID);
+ }
+ //Log->Write("Ending FF for UID %d", destID);
+ //std::cerr << "\r W\t O\t R\t K\t I\t N\t G";
+ }
+ }
+ direction._x = (localneggradptr[key]._x);
+ direction._y = (localneggradptr[key]._y);
+}
+
+SubRoom* FloorfieldViaFMTrips::GetSubroom(Pedestrian* ped) {
+ long int key = _grid->getKeyAtPoint(ped->GetPos());
+ assert(key >= 0 && key <= _grid->GetnPoints());
+ return _subrooms[key];
+}
\ No newline at end of file
diff --git a/routing/ff_router_trips/FloorfieldViaFMTrips.h b/routing/ff_router_trips/FloorfieldViaFMTrips.h
new file mode 100644
index 00000000..cd39fdf7
--- /dev/null
+++ b/routing/ff_router_trips/FloorfieldViaFMTrips.h
@@ -0,0 +1,226 @@
+/**
+ * \file FloorfieldViaFMTrips.h
+ * \date Mar 05, 2015
+ * \version N/A (v0.6)
+ * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ *
+ * \section License
+ * This file is part of JuPedSim.
+ *
+ * JuPedSim is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * JuPedSim is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \section Description
+ * Implementation of classes for ...
+ *
+ *
+ **/
+
+ //remark:
+ //refac the code to use enums instead of integer values where integer values code sth
+ //was considered, but enum classes do not implicitly cast to int
+ //rather use makros/masks like in plain C? or just makros (defines)?
+ //this would make it easier to read
+
+#ifndef FloorfieldViaFMTrips_H
+#define FloorfieldViaFMTrips_H
+
+#include <vector>
+#include <unordered_set>
+#include <cmath>
+#include <functional>
+#include "mesh/RectGrid.h"
+#include "../../geometry/Wall.h"
+#include "../../geometry/Point.h"
+#include "../../geometry/Building.h"
+#include "../../geometry/SubRoom.h" //check: should Room.h include SubRoom.h??
+#include "./mesh/Trial.h"
+#include "../../pedestrian/Pedestrian.h"
+
+//maybe put following in macros.h
+#define LOWSPEED 0.001
+
+class TrialPTrips
+{
+public:
+ long int key;
+ int* flag;
+ double* cost;
+ double* speed;
+ Point* neggrad;
+
+ TrialPTrips() {
+ key = 0;
+ flag = nullptr;
+ cost = nullptr;
+ speed = nullptr;
+ neggrad = nullptr;
+ }
+
+ TrialPTrips(long int keyArg, double* t, double* f, int* flagArg, Point* neggradArg) {
+ key = keyArg;
+ cost = t;
+ speed = f;
+ flag = flagArg;
+ neggrad = neggradArg;
+ }
+
+ ~TrialPTrips(){}
+
+ bool operator <(const TrialPTrips& rhs) const
+ {
+ return this->cost[this->key] < rhs.cost[rhs.key];
+ }
+
+ bool operator >(const TrialPTrips& rhs) const
+ {
+ return this->cost[this->key] > rhs.cost[rhs.key];
+ }
+
+ bool operator ==(const TrialPTrips& rhs) const
+ {
+ return this->cost[this->key] == rhs.cost[rhs.key];
+ }
+};
+
+class FloorfieldViaFMTrips
+{
+public:
+ FloorfieldViaFMTrips();
+ FloorfieldViaFMTrips(const std::string&);
+ FloorfieldViaFMTrips(const Building* const buildingArg, const double hxArg, const double hyArg,
+ const double wallAvoidDistance, const bool useDistancefield, const bool onlyRoomsWithExits);
+ //FloorfieldViaFMTrips(const FloorfieldViaFMTrips* const refFM);
+ virtual ~FloorfieldViaFMTrips();
+ FloorfieldViaFMTrips(const FloorfieldViaFMTrips& other); //will not make a copy; only takes geometry info
+ //FloorfieldViaFMTrips& operator=(const FloorfieldViaFMTrips& other);
+
+ //void getDirectionAt(const Point& position, Point& direction); //obsolete
+ //void getDirectionToDestination (const int destID, const Point& position, Point& direction); //obsolete
+ void getDirectionToUID(int destID, const long int key, Point& direction); // shall not be used any more, therefore not virtual
+ virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
+ //void getDirectionToUIDParallel(int destID, const long int key, Point& direction);
+ virtual void getDirectionToDestination (Pedestrian* ped, Point& direction);
+ //void getDirectionToFinalDestination(Pedestrian* ped, Point& direction); //this is router buissness! problem in multi-storage buildings
+
+ void createMapEntryInLineToGoalID(const int goalID, bool isInside);
+
+ double getCostToDestination(const int destID, const Point& position);
+ double getCostToDestination(const int destID, const Point& position, int mode);
+ //double getCostToDestinationParallel(const int destID, const Point& position);
+
+ void getDir2WallAt(const Point& position, Point& direction);
+ double getDistance2WallAt(const Point& position);
+
+ int getSubroomUIDAt(const Point& position);
+
+ void parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
+ void parseBuildingForExits(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
+ void prepareForDistanceFieldCalculation(const bool withExits);
+ template <typename T>
+ void drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value);
+ void setSpeed(bool useDistance2WallArg);
+ void setSpeedThruPeds(Pedestrian* const* pedsArg, int nPeds, int modechoice, double radius);
+ void deleteAllFFs();
+ void clearAndPrepareForFloorfieldReCalc(double* costarray);
+ void setNewGoalAfterTheClear(double* costarray, std::vector<Line>& GoalWallArg);
+ void calculateFloorfield(std::vector<Line>& wallArg, double* costarray, Point* neggradarray); //make private
+ void calculateFloorfield(std::vector<Line>& wallArg, double* costarray, Point* neggradarray, double* speedarray);
+ void calculateDistanceField(const double thresholdArg); //make private
+
+ void checkNeighborsAndAddToNarrowband(std::priority_queue<TrialPTrips, std::vector<TrialPTrips>, std::greater<TrialPTrips> >& trialfield, TrialPTrips key,
+ std::function<void (TrialPTrips)> calc);
+
+ void calcDist2Wall(TrialPTrips);
+ void calcFloorfield(TrialPTrips);
+ //void (*checkNeighborsAndCalc)(const long int key);
+
+ inline double onesidedCalc(double xy, double hDivF);
+ inline double twosidedCalc(double x, double y, double hDivF);
+
+ void testoutput(const char*, const char*, const double*);
+ void writeFF(const std::string&, std::vector<int> targetID);
+ void writeGoalFF(const std::string&, std::vector<int> targetID);
+
+ virtual SubRoom* isInside(const long int key);
+ SubRoom* GetSubroom(Pedestrian* p);
+
+ std::map<int, int> getGoalToLineUIDmap() const
+ {
+ return _goalToLineUIDmap;
+ }
+
+ std::map<int, int> getGoalToLineUIDmap2() const
+ {
+ return _goalToLineUIDmap2;
+ }
+
+ std::map<int, int> getGoalToLineUIDmap3() const
+ {
+ return _goalToLineUIDmap3;
+ }
+
+ RectGrid* getGrid() const
+ {
+ return _grid;
+ }
+
+#ifdef TESTING
+ void setGrid(RectGrid* gridArg) {_grid = gridArg;}
+#endif //TESTING
+
+protected:
+ RectGrid* _grid = nullptr;
+ std::vector<Line> _wall;
+ std::vector<Line> _exitsFromScope;
+ unsigned int _numOfExits;
+
+ const Building* _building;
+
+ //GridPoint Data in independant arrays (shared primary key)
+ // changed to threadsafe creation when needed: int* flag;
+ int* _gcode = nullptr; //gridcode (see Macros.h)
+ SubRoom* * _subrooms = nullptr; // this is an array (first asterisk) of pointers (second asterisk)
+ double* _dist2Wall = nullptr;
+ double* _speedInitial = nullptr;
+ double* _modifiedspeed = nullptr;
+ double* _densityspeed = nullptr;
+ double* _cost = nullptr;
+ //long int* secKey; //secondary key to address ... not used yet
+ Point* _neggrad = nullptr; //gradients
+ Point* _dirToWall = nullptr;
+ // changed to threadsafe creation when needed: Trial* trialfield;
+
+ std::map<int, double*> _goalcostmap;
+ std::map<int, int> _goalToLineUIDmap; //key is the goalID and value is the UID of closest transition -> it maps goal to LineUID
+ std::map<int, int> _goalToLineUIDmap2;
+ std::map<int, int> _goalToLineUIDmap3;
+ std::map<int, Point*> _goalneggradmap;
+ std::map<int, double*> _costmap;
+ std::map<int, Point*> _neggradmap;
+ // use an unordered_set for faster access (it is accessed within a critical region)
+ std::unordered_set<int> _floorfieldsBeingCalculated;
+ bool maps_deleted = false; // @todo f.mack remove
+
+ double _threshold;
+ bool _useDistanceToWall;
+};
+
+// very similar to FloorfieldViaFMTrips, but the calculation of floorfields starts at the center of the door only, not on the whole line
+// this happens in order to avoid "door hopping" (for a pedestrian, it is shorter to go to a nearby door and skip half the door width)
+class CentrePointFFViaFMTrips : public virtual FloorfieldViaFMTrips {
+public:
+ virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
+};
+
+#endif // FloorfieldViaFMTrips_H
diff --git a/routing/ff_router_trips/UnivFFviaFMTrips.cpp b/routing/ff_router_trips/UnivFFviaFMTrips.cpp
new file mode 100644
index 00000000..7a99aec6
--- /dev/null
+++ b/routing/ff_router_trips/UnivFFviaFMTrips.cpp
@@ -0,0 +1,1916 @@
+//
+// Created by arne on 5/9/17.
+//
+#define NOMINMAX
+#include <unordered_set>
+#include "UnivFFviaFMTrips.h"
+#include "../../geometry/Line.h"
+#include "../../geometry/Building.h"
+#include "../../geometry/SubRoom.h"
+#include "../../pedestrian/Pedestrian.h"
+#include "mesh/RectGrid.h"
+#include "../../geometry/Goal.h"
+#include "../../geometry/WaitingArea.h"
+
+UnivFFviaFMTrips::~UnivFFviaFMTrips() {
+ if (_grid) delete _grid;
+ //size_t speedsize = _speedFieldSelector.size();
+ for (auto speedPtr : _speedFieldSelector) {
+ if (speedPtr) delete[] speedPtr;
+ }
+ if (_gridCode) delete[] _gridCode;
+ if (_subrooms) delete[] _subrooms;
+
+ std::map<int, double*>::reverse_iterator delIterCost;
+
+ for (delIterCost = _costFieldWithKey.rbegin();
+ delIterCost != _costFieldWithKey.rend();
+ ++delIterCost) {
+ delete[] delIterCost->second;
+ }
+ std::map<int, Point*>::reverse_iterator delIterDir;
+ for (delIterDir = _directionFieldWithKey.rbegin();
+ delIterDir != _directionFieldWithKey.rend();
+ ++delIterDir) {
+ delete[] delIterDir->second;
+ }
+}
+
+UnivFFviaFMTrips::UnivFFviaFMTrips(Room* r, Building* b, double hx, double wallAvoid, bool useWallDistances)
+ : UnivFFviaFMTrips(r, b->GetConfig(), hx, wallAvoid, useWallDistances, b->GetAllGoals()) {
+ _building = b;
+}
+
+UnivFFviaFMTrips::UnivFFviaFMTrips(SubRoom* sr, Building* b, double hx, double wallAvoid, bool useWallDistances)
+ : UnivFFviaFMTrips(sr, b->GetConfig(), hx, wallAvoid, useWallDistances, b->GetAllGoals()) {
+ _building = b;
+}
+
+UnivFFviaFMTrips::UnivFFviaFMTrips(Room* r, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances, std::map<int, Goal*> goals)
+ : UnivFFviaFMTrips(r, conf, hx, wallAvoid, useWallDistances, std::vector<int>(), goals){
+}
+
+UnivFFviaFMTrips::UnivFFviaFMTrips(Room* roomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors, std::map<int, Goal*> goals) {
+ //build the vector with walls(wall or obstacle), the map with <UID, Door(Cross or Trans)>, the vector with targets(UIDs)
+ //then call other constructor including the mode
+ Log->Write("UnivFFviaFMTrips::UnivFFviaFMTrips(Room* roomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors)\n");
+ _configuration = confArg;
+ _scope = FF_ROOM_SCALE;
+ _room = roomArg->GetID();
+ std::vector<Line> lines;
+ std::map<int, Line> tmpDoors;
+ Line anyDoor = Line{};
+
+ // Loop over all subrooms of roomArg
+ for (auto& subroomMap : roomArg->GetAllSubRooms()) {
+ SubRoom* subRoomPtr = subroomMap.second.get();
+
+ // Save all walls of subroom in lines
+ std::vector<Wall> walls = std::vector<Wall>(subRoomPtr->GetAllWalls());
+ for (auto& wall : walls) {
+ lines.emplace_back((Line)wall);
+ }
+
+ // save all obstacles of subroom in lines
+ std::vector<Obstacle*> tmpObsPtrVec = subRoomPtr->GetAllObstacles();
+ for (Obstacle* ptrObs : tmpObsPtrVec) {
+ const std::vector<Wall> obsWalls = ptrObs->GetAllWalls();
+ for (auto& owall : obsWalls) {
+ lines.emplace_back((Line)owall);
+ }
+ }
+
+ // save all transisitions/crossings in tmpDoor
+ const std::vector<Crossing*> tmpCross = subRoomPtr->GetAllCrossings();
+ const std::vector<Transition*> tmpTrans = subRoomPtr->GetAllTransitions();
+
+ int uidNotConst = 0;
+ bool isOpen = false;
+ for (auto& cross : tmpCross) {
+ uidNotConst = cross->GetUniqueID();
+ isOpen = cross->IsOpen();
+ if (!isOpen) {
+ //will be added twice! is it a problem?
+ lines.emplace_back((Line)*cross);
+ } else {
+ anyDoor = Line{*cross};
+ if (tmpDoors.count(uidNotConst) == 0) {
+ tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *cross));
+ }
+ }
+ }
+ for (auto& trans : tmpTrans) {
+ uidNotConst = trans->GetUniqueID();
+ isOpen = trans->IsOpen();
+ if (!isOpen) {
+ //will be added twice! is it a problem?
+ lines.emplace_back((Line)*trans);
+ } else {
+ anyDoor = Line{*trans};
+ if (tmpDoors.count(uidNotConst) == 0) {
+ tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
+ }
+ }
+ }
+
+// //save all waiting area walls in tmpdoors
+ for (auto& goalMap : goals){
+ Goal* goal = goalMap.second;
+ if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
+ std::cout << "Testing for subroom " << subRoomPtr->GetSubRoomID() << std::endl;
+ if ((subRoomPtr->IsInSubRoom(wa->GetCentroid())) && (wa->isOpen())){
+ for (const Wall wall : wa->GetAllWalls()){
+ int uid = wall.GetUniqueID();
+ if (tmpDoors.count(uid) == 0) {
+ tmpDoors.emplace(std::make_pair(uid, (Line) wall));
+ }
+ }
+ }
+ }
+ }
+
+ for (auto& door : tmpDoors){
+ std::cout << "uid: " << door.first << " door: " << door.second.toString() << std::endl;
+ }
+
+ //find insidePoint and save it, together with UID
+ Point normalVec = anyDoor.NormalVec();
+ double length = normalVec.Norm();
+ Point midPoint = anyDoor.GetCentre();
+ Point candidate01 = midPoint + (normalVec * 0.25);
+ Point candidate02 = midPoint - (normalVec * 0.25);
+ if (subRoomPtr->IsInSubRoom(candidate01)) {
+ _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomPtr, candidate01));
+ } else {
+ //candidate = candidate - (normalVec * 0.25);
+ if (subRoomPtr->IsInSubRoom(candidate02)) {
+ _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomPtr, candidate02));
+ } else {
+ Log->Write("ERROR:\t In UnivFF InsidePoint Analysis");
+ bool a = subRoomPtr->IsInSubRoom(candidate01);
+ bool b = subRoomPtr->IsInSubRoom(candidate02);
+ a = b && a; //ignore this line. only to have a codeline after initialization of bools (to place a breakpoint)
+ }
+ }
+ //_subroomUIDtoSubRoomPtr.emplace(std::make_pair(subRoomPtr->GetUID(), subRoomPtr));
+ }
+ //this will interpret "useWallDistances" as best as possible. Users should clearify with "setSpeedMode" before calling "AddTarget"
+ if (useWallDistances) {
+ create(lines, tmpDoors, wantedDoors, FF_WALL_AVOID, hx, wallAvoid, useWallDistances);
+ } else {
+ create(lines, tmpDoors, wantedDoors, FF_HOMO_SPEED, hx, wallAvoid, useWallDistances);
+ }
+}
+
+UnivFFviaFMTrips::UnivFFviaFMTrips(SubRoom* sr, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances, std::map<int, Goal*> goals)
+ : UnivFFviaFMTrips(sr, conf, hx, wallAvoid, useWallDistances, std::vector<int>(), goals){
+}
+
+UnivFFviaFMTrips::UnivFFviaFMTrips(SubRoom* subRoomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors, std::map<int, Goal*> goals) {
+ //build the vector with walls(wall or obstacle), the map with <UID, Door(Cross or Trans)>, the vector with targets(UIDs)
+ //then call other constructor including the mode
+
+ Log->Write("UnivFFviaFMTrips::UnivFFviaFMTrips(SubRoom* subRoomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors))");
+
+ _configuration = confArg;
+ _scope = FF_SUBROOM_SCALE;
+ _room = subRoomArg->GetRoomID();
+ std::vector<Line> lines;
+ std::map<int, Line> tmpDoors;
+
+ std::vector<Wall> walls = std::vector<Wall> (subRoomArg->GetAllWalls());
+ for (auto& wall : walls) {
+ lines.emplace_back((Line)wall);
+ }
+
+ std::vector<Obstacle*> tmpObsPtrVec = subRoomArg->GetAllObstacles();
+ for (Obstacle* ptrObs : tmpObsPtrVec) {
+ const std::vector<Wall> obsWalls = ptrObs->GetAllWalls();
+ for (auto& owall : obsWalls) {
+ lines.emplace_back((Line)owall);
+ }
+ }
+
+ const std::vector<Crossing*> tmpCross = subRoomArg->GetAllCrossings();
+ const std::vector<Transition*> tmpTrans = subRoomArg->GetAllTransitions();
+
+ int uidNotConst = 0;
+ bool isOpen = false;
+ for (auto& cross : tmpCross) {
+ uidNotConst = cross->GetUniqueID();
+ isOpen = cross->IsOpen();
+ if (!isOpen) {
+ lines.emplace_back((Line)*cross);
+ } else {
+ tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *cross));
+ }
+ }
+ for (auto& trans : tmpTrans) {
+ uidNotConst = trans->GetUniqueID();
+ isOpen = trans->IsOpen();
+ if (!isOpen) {
+ lines.emplace_back((Line)*trans);
+ } else {
+ tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
+ }
+ }
+
+ _building->GetAllGoals();
+ //find insidePoint and save it, together with UID
+ Line anyDoor = Line{(--tmpDoors.end())->second};
+ Point normalVec = anyDoor.NormalVec();
+ double length = normalVec.Norm();
+ Point midPoint = anyDoor.GetCentre();
+ Point candidate01 = midPoint + (normalVec * 0.25);
+ Point candidate02 = midPoint - (normalVec * 0.25);
+ if (subRoomArg->IsInSubRoom(candidate01)) {
+ _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomArg, candidate01));
+ } else {
+ //candidate = candidate - (normalVec * 0.25);
+ if (subRoomArg->IsInSubRoom(candidate02)) {
+ _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomArg, candidate02));
+ } else {
+ Log->Write("ERROR:\t In UnivFF InsidePoint Analysis");
+ bool a = subRoomArg->IsInSubRoom(candidate01);
+ bool b = subRoomArg->IsInSubRoom(candidate02);
+ a = b && a;
+ }
+ }
+
+ //_subroomUIDtoSubRoomPtr.emplace(std::make_pair(subRoomArg->GetUID(), subRoomArg));
+
+ //this will interpret "useWallDistances" as best as possible. Users should clearify with "setSpeedMode" before calling "AddTarget"
+ if (useWallDistances) {
+ create(lines, tmpDoors, wantedDoors, FF_WALL_AVOID, hx, wallAvoid, useWallDistances);
+ } else {
+ create(lines, tmpDoors, wantedDoors, FF_HOMO_SPEED, hx, wallAvoid, useWallDistances);
+ }
+}
+
+void UnivFFviaFMTrips::create(std::vector<Line>& walls, std::map<int, Line>& doors, std::vector<int> targetUIDs, int mode,
+ double spacing, double wallAvoid, bool useWallDistances) {
+
+ _wallAvoidDistance = wallAvoid;
+ _useWallDistances = useWallDistances;
+ _speedmode = mode;
+
+ //find circumscribing rectangle (x_min/max, y_min/max) //create RectGrid
+ createRectGrid(walls, doors, spacing);
+ _nPoints = _grid->GetnPoints();
+
+ //allocate _gridCode and _speedFieldSelector and initialize them ("draw" walls and doors)
+ _gridCode = new int[_nPoints];
+ processGeometry(walls, doors);
+ _speedFieldSelector.emplace(_speedFieldSelector.begin()+INITIAL_SPEED, new double[_nPoints]);
+ std::fill(_speedFieldSelector[INITIAL_SPEED], _speedFieldSelector[INITIAL_SPEED]+_nPoints, 1.0);
+
+ _speedFieldSelector.emplace(_speedFieldSelector.begin()+REDU_WALL_SPEED, nullptr);
+ _speedFieldSelector.emplace(_speedFieldSelector.begin()+PED_SPEED, nullptr);
+
+ //mark Inside areas (dont write outside areas)
+ for (auto subRoomPointPair : _subRoomPtrTOinsidePoint) {
+ markSubroom(subRoomPointPair.second, subRoomPointPair.first);
+ }
+
+ //allocate _modifiedSpeed
+ if ((_speedmode == FF_WALL_AVOID) || (useWallDistances)) {
+ double* cost_alias_walldistance = new double[_nPoints];
+ _costFieldWithKey[0] = cost_alias_walldistance;
+ Point* gradient_alias_walldirection = new Point[_nPoints];
+ _directionFieldWithKey[0] = gradient_alias_walldirection;
+
+ //create wall distance field
+ //init costarray
+ for (int i = 0; i < _nPoints; ++i) {
+ if (_gridCode[i] == WALL) {
+ cost_alias_walldistance[i] = magicnum(WALL_ON_COSTARRAY);
+ } else {
+ cost_alias_walldistance[i] = magicnum(UNKNOWN_COST);
+ }
+ }
+ drawLinesOnWall(walls, cost_alias_walldistance, magicnum(TARGET_REGION));
+ calcDF(cost_alias_walldistance, gradient_alias_walldirection, _speedFieldSelector[INITIAL_SPEED]);
+ //_uids.emplace_back(0);
+
+ double* temp_reduWallSpeed = new double[_nPoints];
+ if (_speedFieldSelector[REDU_WALL_SPEED]) { //free memory before overwriting
+ delete[] _speedFieldSelector[REDU_WALL_SPEED];
+ }
+ _speedFieldSelector[REDU_WALL_SPEED] = temp_reduWallSpeed;
+ //init _reducedWallSpeed by using distance field
+ createReduWallSpeed(temp_reduWallSpeed);
+ }
+
+ // parallel call
+ if (!targetUIDs.empty()) {
+ addTargetsParallel(targetUIDs);
+ }
+}
+
+void UnivFFviaFMTrips::createRectGrid(std::vector<Line>& walls, std::map<int, Line>& doors, double spacing) {
+ double x_min = DBL_MAX; double x_max = DBL_MIN;
+ double y_min = DBL_MAX; double y_max = DBL_MIN;
+
+ for(auto& wall : walls) {
+ if (wall.GetPoint1()._x < x_min) x_min = wall.GetPoint1()._x;
+ if (wall.GetPoint1()._y < y_min) y_min = wall.GetPoint1()._y;
+ if (wall.GetPoint2()._x < x_min) x_min = wall.GetPoint2()._x;
+ if (wall.GetPoint2()._y < y_min) y_min = wall.GetPoint2()._y;
+
+ if (wall.GetPoint1()._x > x_max) x_max = wall.GetPoint1()._x;
+ if (wall.GetPoint1()._y > y_max) y_max = wall.GetPoint1()._y;
+ if (wall.GetPoint2()._x > x_max) x_max = wall.GetPoint2()._x;
+ if (wall.GetPoint2()._y > y_max) y_max = wall.GetPoint2()._y;
+ }
+
+ for(auto& doorPair:doors) {
+ Line& door = doorPair.second;
+ if (door.GetPoint1()._x < x_min) x_min = door.GetPoint1()._x;
+ if (door.GetPoint1()._y < y_min) y_min = door.GetPoint1()._y;
+ if (door.GetPoint2()._x < x_min) x_min = door.GetPoint2()._x;
+ if (door.GetPoint2()._y < y_min) y_min = door.GetPoint2()._y;
+
+ if (door.GetPoint1()._x > x_max) x_max = door.GetPoint1()._x;
+ if (door.GetPoint1()._y > y_max) y_max = door.GetPoint1()._y;
+ if (door.GetPoint2()._x > x_max) x_max = door.GetPoint2()._x;
+ if (door.GetPoint2()._y > y_max) y_max = door.GetPoint2()._y;
+ }
+
+ //create Rect Grid
+ _grid = new RectGrid();
+ _grid->setBoundaries(x_min, y_min, x_max, y_max);
+ _grid->setSpacing(spacing, spacing);
+ _grid->createGrid();
+}
+
+void UnivFFviaFMTrips::processGeometry(std::vector<Line>&walls, std::map<int, Line>& doors) {
+ for (int i = 0; i < _nPoints; ++i) {
+ _gridCode[i] = OUTSIDE;
+ }
+
+ for (auto mapentry : doors) {
+ _doors.insert(mapentry);
+ }
+ //_doors = doors;
+
+ drawLinesOnGrid<int>(walls, _gridCode, WALL);
+ drawLinesOnGrid(doors, _gridCode); //UIDs of doors will be drawn on _gridCode
+}
+
+void UnivFFviaFMTrips::markSubroom(const Point& insidePoint, SubRoom* const value) {
+ //value must not be nullptr. it would lead to infinite loop
+ if (!value) return;
+
+ if(!_grid->includesPoint(insidePoint)) return;
+
+ //alloc mem if needed
+ if (!_subrooms) {
+ _subrooms = new SubRoom*[_nPoints];
+ for (long i = 0; i < _nPoints; ++i) {
+ _subrooms[i] = nullptr;
+ }
+ }
+
+ //init start
+ _subrooms[_grid->getKeyAtPoint(insidePoint)] = value;
+ _gridCode[_grid->getKeyAtPoint(insidePoint)] = INSIDE;
+
+ std::unordered_set<long> wavefront;
+ wavefront.reserve(_nPoints);
+
+ directNeighbor _neigh = _grid->getNeighbors(_grid->getKeyAtPoint(insidePoint));
+ long aux = _neigh.key[0];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ aux = _neigh.key[1];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ aux = _neigh.key[2];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ aux = _neigh.key[3];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ while(!wavefront.empty()) {
+ long current = *(wavefront.begin());
+ wavefront.erase(current);
+ _gridCode[current] = INSIDE;
+
+ _neigh = _grid->getNeighbors(current);
+ aux = _neigh.key[0];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ aux = _neigh.key[1];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ aux = _neigh.key[2];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+
+ aux = _neigh.key[3];
+ if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
+ wavefront.insert(aux);
+ _subrooms[aux] = value;
+ }
+ }
+}
+
+void UnivFFviaFMTrips::createReduWallSpeed(double* reduWallSpeed){
+ double factor = 1/_wallAvoidDistance;
+ double* wallDstAlias = _costFieldWithKey[0];
+
+ for (long int i = 0; i < _nPoints; ++i) {
+ if (wallDstAlias[i] > 0.) {
+ reduWallSpeed[i] = (wallDstAlias[i] > _wallAvoidDistance) ? 1.0 : (factor * wallDstAlias[i]);
+ }
+ }
+}
+
+void UnivFFviaFMTrips::recreateAllForQuickest() {
+ //allocate if neccessary (should not be!)
+ for (int doorUID : _uids) {
+ if (!_costFieldWithKey[doorUID]) {
+ _costFieldWithKey[doorUID] = new double[_nPoints];
+ }
+ if (_user == DISTANCE_MEASUREMENTS_ONLY) {
+ if (_directionFieldWithKey.count(doorUID) != 0 && _directionFieldWithKey[doorUID]){
+ delete[] _directionFieldWithKey[doorUID];
+ }
+ _directionFieldWithKey[doorUID] = nullptr;
+ }
+ if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+ //check if not in map OR (in map but) nullptr)
+ if ((_directionFieldWithKey.count(doorUID) == 0) || (!_directionFieldWithKey[doorUID])) {
+ _directionFieldWithKey[doorUID] = new Point[_nPoints];
+ }
+ }
+ }
+
+ //parallel region
+#pragma omp parallel
+ {
+#pragma omp for
+ for (signed int i = 0; i < _doors.size(); ++i) {
+ auto doorPair = _doors.begin();
+ std::advance(doorPair, i);
+ addTarget(doorPair->first, _costFieldWithKey[doorPair->first], _directionFieldWithKey[doorPair->first]);
+ }
+ };
+}
+
+void UnivFFviaFMTrips::createPedSpeed(Pedestrian *const *pedsArg, int nsize, int modechoice, double radius) {
+ long int delta = radius / _grid->Gethx();
+ long int posIndex = 0;
+ long int pos_i = 0;
+ long int pos_j = 0;
+ long int i_start = 0;
+ long int j_start = 0;
+ long int i_end = 0;
+ long int j_end = 0;
+ long int iStride = _grid->GetiMax();
+ double indexDistance = 0.0;
+ double newWaveSpeed = 0.0;
+
+// if (nsize == 0) {
+// Log->Write("WARNING: \tcreatePedSpeed: nsize is ZERO");
+// } else {
+// Log->Write("INFO: \t\tNumber of Peds used in createPedSpeed: %d",nsize);
+// }
+
+ if ((modechoice == quickest) && (!_speedFieldSelector[PED_SPEED])) {
+ _speedFieldSelector[PED_SPEED] = new double[_grid->GetnPoints()];
+ }
+
+ //we assume, that this function is only used by router and is not using REDU_WALL_SPEED
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ _speedFieldSelector[PED_SPEED][i] = _speedFieldSelector[INITIAL_SPEED][i];
+ }
+ if (_speedFieldSelector[REDU_WALL_SPEED] && _mode == LINESEGMENT) { //indicates, that the direction strat is using it
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ _speedFieldSelector[PED_SPEED][i] = _speedFieldSelector[REDU_WALL_SPEED][i];
+ }
+ }
+
+ for (int i = 0; i < nsize; ++i) {
+ //the following check is not 3D proof, we require the caller of this function to provide a list with "valid"
+ //pedestrian-pointer
+ if (!_grid->includesPoint(pedsArg[i]->GetPos())) {
+ continue;
+ }
+
+ newWaveSpeed = pedsArg[i]->GetEllipse().GetV().Norm()/pedsArg[i]->GetEllipse().GetV0(); // (current speed)/(desired speed)
+ newWaveSpeed *= 0.8;
+ if (newWaveSpeed < 0.1) {
+ newWaveSpeed = 0.1;
+ }
+ posIndex = _grid->getKeyAtPoint(pedsArg[i]->GetPos());
+ pos_i = _grid->get_i_fromKey(posIndex);
+ pos_j = _grid->get_j_fromKey(posIndex);
+
+ i_start = ((pos_i - delta) < 0) ? 0 : (pos_i - delta);
+ i_end = ((pos_i + delta) >= iStride) ? iStride-1 : (pos_i + delta);
+
+ j_start = ((pos_j - delta) < 0) ? 0 : (pos_j - delta);
+ j_end = ((pos_j + delta) >= _grid->GetjMax()) ? _grid->GetjMax()-1 : (pos_j + delta);
+
+ for (long int curr_i = i_start; curr_i < i_end; ++curr_i) {
+ for (long int curr_j = j_start; curr_j < j_end; ++curr_j) {
+ //indexDistance holds the square
+ indexDistance = ( (curr_i - pos_i)*(curr_i - pos_i) + (curr_j - pos_j)*(curr_j - pos_j) );
+
+ if (indexDistance < (delta*delta)) {
+ //_speedFieldSelector[PED_SPEED][curr_j*iStride + curr_i] = 0.2;
+ _speedFieldSelector[PED_SPEED][curr_j*iStride + curr_i] = std::min(newWaveSpeed, _speedFieldSelector[PED_SPEED][curr_j*iStride + curr_i]);
+ }
+ }
+ }
+ }
+}
+
+void UnivFFviaFMTrips::finalizeTargetLine(const int uid, const Line& line, Point* target, Point& value) {
+ // i~x; j~y;
+//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
+//src in answer of "Avi"; adapted to fit this application
+
+ //grid handeling local vars:
+ long int iMax = _grid->GetiMax();
+
+ long int iStart, iEnd;
+ long int jStart, jEnd;
+ long int iDot, jDot;
+ long int key;
+ long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
+
+ long int goodneighbor;
+ directNeighbor neigh;
+
+
+ key = _grid->getKeyAtPoint(line.GetPoint1());
+ iStart = (long) _grid->get_i_fromKey(key);
+ jStart = (long) _grid->get_j_fromKey(key);
+
+ key = _grid->getKeyAtPoint(line.GetPoint2());
+ iEnd = (long) _grid->get_i_fromKey(key);
+ jEnd = (long) _grid->get_j_fromKey(key);
+
+ deltaX = (int) (iEnd - iStart);
+ deltaY = (int) (jEnd - jStart);
+ deltaX1 = abs( (int) (iEnd - iStart));
+ deltaY1 = abs( (int) (jEnd - jStart));
+
+ px = 2*deltaY1 - deltaX1;
+ py = 2*deltaX1 - deltaY1;
+
+ if(deltaY1<=deltaX1) {
+ if(deltaX>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ xe = iEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ xe = iStart;
+ }
+ if (_gridCode[jDot*iMax + iDot] == uid) {
+ /* //find a good neighborvalue
+ neigh = _grid->getNeighbors(jDot*iMax + iDot);
+ if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
+ goodneighbor = neigh.key[0];
+ } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
+ goodneighbor = neigh.key[1];
+ } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
+ goodneighbor = neigh.key[2];
+ } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
+ goodneighbor = neigh.key[3];
+ } else {
+ //ERROR - should have an inside neighbor
+ Log->Write("ERROR:\t In finalizeTargetLine");
+ }
+ //write the value on targetline
+ if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
+ //ERROR - should have a true vector
+ Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
+ }*/
+ target[jDot * iMax + iDot] = value;
+ } else if (_gridCode[jDot*iMax + iDot] == WALL) {
+ //do nothing
+ } else {
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
+ }
+ for (i=0; iDot < xe; ++i) {
+ ++iDot;
+ if (px < 0) {
+ px += 2 * deltaY1;
+ } else {
+ if ((deltaX < 0 && deltaY < 0) || (deltaX > 0 && deltaY > 0)) {
+ ++jDot;
+ } else {
+ --jDot;
+ }
+ px += 2 * (deltaY1 - deltaX1);
+ }
+ if (_gridCode[jDot * iMax + iDot] == uid) {
+ /*//find a good neighborvalue
+ neigh = _grid->getNeighbors(jDot*iMax + iDot);
+ if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
+ goodneighbor = neigh.key[0];
+ } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
+ goodneighbor = neigh.key[1];
+ } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
+ goodneighbor = neigh.key[2];
+ } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
+ goodneighbor = neigh.key[3];
+ } else {
+ //ERROR - should have an inside neighbor
+ Log->Write("ERROR:\t In finalizeTargetLine");
+ }
+ //write the value on targetline
+ if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
+ //ERROR - should have a true vector
+ Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
+ }*/
+ target[jDot * iMax + iDot] = value;
+ } else if (_gridCode[jDot*iMax + iDot] == WALL) {
+ //do nothing
+ } else {
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
+ }
+ }
+ } else {
+ if(deltaY>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ ye = jEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ ye = jStart;
+ }
+ if (_gridCode[jDot*iMax + iDot] == uid) {
+ /*//find a good neighborvalue
+ neigh = _grid->getNeighbors(jDot*iMax + iDot);
+ if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
+ goodneighbor = neigh.key[0];
+ } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
+ goodneighbor = neigh.key[1];
+ } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
+ goodneighbor = neigh.key[2];
+ } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
+ goodneighbor = neigh.key[3];
+ } else {
+ //ERROR - should have an inside neighbor
+ Log->Write("ERROR:\t In finalizeTargetLine");
+ }
+ //write the value on targetline
+ if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
+ //ERROR - should have a true vector
+ Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
+ }*/
+ target[jDot * iMax + iDot] = value;
+ } else if (_gridCode[jDot*iMax + iDot] == WALL) {
+ //do nothing
+ } else {
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
+ }
+ for(i=0; jDot<ye; ++i) {
+ ++jDot;
+ if (py<=0) {
+ py+=2*deltaX1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++iDot;
+ } else {
+ --iDot;
+ }
+ py+=2*(deltaX1-deltaY1);
+ }
+ if (_gridCode[jDot*iMax + iDot] == uid) {
+ /*//find a good neighborvalue
+ neigh = _grid->getNeighbors(jDot*iMax + iDot);
+ if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
+ goodneighbor = neigh.key[0];
+ } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
+ goodneighbor = neigh.key[1];
+ } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
+ goodneighbor = neigh.key[2];
+ } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
+ goodneighbor = neigh.key[3];
+ } else {
+ //ERROR - should have an inside neighbor
+ Log->Write("ERROR:\t In finalizeTargetLine");
+ }
+ //write the value on targetline
+ if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
+ //ERROR - should have a true vector
+ Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
+ }*/
+ target[jDot * iMax + iDot] = value;
+ } else if (_gridCode[jDot*iMax + iDot] == WALL) {
+ //do nothing
+ } else {
+ target[jDot * iMax + iDot] = value;
+ //Log->Write("ERROR:\t in finalizingTargetLine");
+ }
+ }
+ }
+}
+
+void UnivFFviaFMTrips::drawLinesOnGrid(std::map<int, Line>& doors, int *const grid) {
+ for (auto&& doorPair : doors) {
+ int tempUID = doorPair.first;
+ Line tempDoorLine = Line(doorPair.second);
+ drawLinesOnGrid(tempDoorLine, grid, tempUID);
+ }
+}
+
+template <typename T>
+void UnivFFviaFMTrips::drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value) { //no init, plz init elsewhere
+
+ for (auto& line : wallArg) {
+ drawLinesOnGrid(line, target, value);
+ } //loop over all walls
+
+} //drawLinesOnGrid
+
+template <typename T>
+void UnivFFviaFMTrips::drawLinesOnGrid(Line& line, T* const target, const T value) { //no init, plz init elsewhere
+// i~x; j~y;
+//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
+//src in answer of "Avi"; adapted to fit this application
+
+ //grid handeling local vars:
+ long int iMax = _grid->GetiMax();
+
+ long int iStart, iEnd;
+ long int jStart, jEnd;
+ long int iDot, jDot;
+ long int key;
+ long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
+
+
+ key = _grid->getKeyAtPoint(line.GetPoint1());
+ iStart = (long) _grid->get_i_fromKey(key);
+ jStart = (long) _grid->get_j_fromKey(key);
+
+ key = _grid->getKeyAtPoint(line.GetPoint2());
+ iEnd = (long) _grid->get_i_fromKey(key);
+ jEnd = (long) _grid->get_j_fromKey(key);
+
+ deltaX = (int) (iEnd - iStart);
+ deltaY = (int) (jEnd - jStart);
+ deltaX1 = abs( (int) (iEnd - iStart));
+ deltaY1 = abs( (int) (jEnd - jStart));
+
+ px = 2*deltaY1 - deltaX1;
+ py = 2*deltaX1 - deltaY1;
+
+ if(deltaY1<=deltaX1) {
+ if(deltaX>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ xe = iEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ xe = iStart;
+ }
+ if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ for (i=0; iDot < xe; ++i) {
+ ++iDot;
+ if(px<0) {
+ px+=2*deltaY1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++jDot;
+ } else {
+ --jDot;
+ }
+ px+=2*(deltaY1-deltaX1);
+ }
+ if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ }
+ } else {
+ if(deltaY>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ ye = jEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ ye = jStart;
+ }
+ if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ for(i=0; jDot<ye; ++i) {
+ ++jDot;
+ if (py<=0) {
+ py+=2*deltaX1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++iDot;
+ } else {
+ --iDot;
+ }
+ py+=2*(deltaX1-deltaY1);
+ }
+ if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ }
+ }
+} //drawLinesOnGrid
+
+template <typename T>
+void UnivFFviaFMTrips::drawLinesOnWall(std::vector<Line>& wallArg, T* const target, const T value) { //no init, plz init elsewhere
+
+ for (auto& line : wallArg) {
+ drawLinesOnWall(line, target, value);
+ } //loop over all walls
+
+} //drawLinesOnWall
+
+template <typename T>
+void UnivFFviaFMTrips::drawLinesOnWall(Line& line, T* const target, const T value) { //no init, plz init elsewhere
+// i~x; j~y;
+//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
+//src in answer of "Avi"; adapted to fit this application
+
+ //grid handeling local vars:
+ long int iMax = _grid->GetiMax();
+
+ long int iStart, iEnd;
+ long int jStart, jEnd;
+ long int iDot, jDot;
+ long int key;
+ long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
+
+
+ key = _grid->getKeyAtPoint(line.GetPoint1());
+ iStart = (long) _grid->get_i_fromKey(key);
+ jStart = (long) _grid->get_j_fromKey(key);
+
+ key = _grid->getKeyAtPoint(line.GetPoint2());
+ iEnd = (long) _grid->get_i_fromKey(key);
+ jEnd = (long) _grid->get_j_fromKey(key);
+
+ deltaX = (int) (iEnd - iStart);
+ deltaY = (int) (jEnd - jStart);
+ deltaX1 = abs( (int) (iEnd - iStart));
+ deltaY1 = abs( (int) (jEnd - jStart));
+
+ px = 2*deltaY1 - deltaX1;
+ py = 2*deltaX1 - deltaY1;
+
+ if(deltaY1<=deltaX1) {
+ if(deltaX>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ xe = iEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ xe = iStart;
+ }
+ if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ for (i=0; iDot < xe; ++i) {
+ ++iDot;
+ if(px<0) {
+ px+=2*deltaY1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++jDot;
+ } else {
+ --jDot;
+ }
+ px+=2*(deltaY1-deltaX1);
+ }
+ if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ }
+ } else {
+ if(deltaY>=0) {
+ iDot = iStart;
+ jDot = jStart;
+ ye = jEnd;
+ } else {
+ iDot = iEnd;
+ jDot = jEnd;
+ ye = jStart;
+ }
+ if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ for(i=0; jDot<ye; ++i) {
+ ++jDot;
+ if (py<=0) {
+ py+=2*deltaX1;
+ } else {
+ if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
+ ++iDot;
+ } else {
+ --iDot;
+ }
+ py+=2*(deltaX1-deltaY1);
+ }
+ if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
+ target[jDot * iMax + iDot] = value;
+ }
+ }
+ }
+} //drawLinesOnWall
+
+void UnivFFviaFMTrips::calcFF(double* costOutput, Point* directionOutput, const double *const speed) {
+ //CompareCost comp = CompareCost(costOutput);
+ std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield(costOutput); //pass the argument for the constr of CompareCost
+ //std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield2(comp); //pass the CompareCost object directly
+
+ directNeighbor local_neighbor = _grid->getNeighbors(0);
+ long int aux = 0;
+ //init trial field
+ for (long int i = 0; i < _nPoints; ++i) {
+ if (costOutput[i] == 0.0) {
+ //check for negative neighbours, calc that ones and add to queue trialfield
+ local_neighbor = _grid->getNeighbors(i);
+
+ //check for valid neigh
+ aux = local_neighbor.key[0];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[1];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[2];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[3];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ }
+ }
+
+ while(!trialfield.empty()) {
+ local_neighbor = _grid->getNeighbors(trialfield.top());
+ trialfield.pop();
+
+ //check for valid neigh
+ aux = local_neighbor.key[0];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[1];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[2];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[3];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcCost(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ }
+}
+
+void UnivFFviaFMTrips::calcCost(const long int key, double* cost, Point* dir, const double* const speed) {
+ //adapt from calcFloorfield
+ double row = DBL_MAX;
+ double col = DBL_MAX;
+ long int aux = -1; //will be set below
+ bool pointsUp = false;
+ bool pointsRight = false;
+
+ directNeighbor dNeigh = _grid->getNeighbors(key);
+
+ aux = dNeigh.key[0];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in resetGoalAndCosts
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
+ (_gridCode[aux] != WALL)) //gridpoint holds a calculated value
+ {
+ row = cost[aux];
+ pointsRight = true;
+ if (row < 0) {
+ std::cerr << "hier ist was schief " << row << " " << aux << " " << std::endl;
+ row = DBL_MAX;
+ }
+ }
+ aux = dNeigh.key[2];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
+ (_gridCode[aux] != WALL) &&
+ (cost[aux] < row)) //calculated value promises smaller cost
+ {
+ row = cost[aux];
+ pointsRight = false;
+ }
+
+ aux = dNeigh.key[1];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in parseBuilding after linescan call
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
+ (_gridCode[aux] != WALL))
+ {
+ col = cost[aux];
+ pointsUp = true;
+ if (col < 0) {
+ std::cerr << "hier ist was schief " << col << " " << aux << " " << std::endl;
+ col = DBL_MAX;
+ }
+ }
+ aux = dNeigh.key[3];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
+ (_gridCode[aux] != WALL) &&
+ (cost[aux] < col)) //calculated value promises smaller cost
+ {
+ col = cost[aux];
+ pointsUp = false;
+ }
+ 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.);
+ } else {
+ dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ dir[key]._y = (0.);
+ }
+ dir[key] = dir[key].Normalized();
+ return;
+ }
+
+ 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());
+ } else {
+ dir[key]._x = (0.);
+ dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ dir[key] = dir[key].Normalized();
+ return;
+ }
+
+ //two sided update
+ double precheck = twosidedCalc(row, col, _grid->Gethx()/speed[key]);
+ 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());
+ }
+ if (pointsUp && !pointsRight) {
+ dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ }
+ if (!pointsUp && pointsRight) {
+ dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ if (!pointsUp && !pointsRight) {
+ dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ } else {
+ std::cerr << "else in twosided Dist " << std::endl;
+ }
+ dir[key] = dir[key].Normalized();
+}
+
+void UnivFFviaFMTrips::calcDF(double* costOutput, Point* directionOutput, const double *const speed) {
+ //CompareCost comp = CompareCost(costOutput);
+ std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield(costOutput); //pass the argument for the constr of CompareCost
+ //std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield2(comp); //pass the CompareCost object directly
+
+ directNeighbor local_neighbor = _grid->getNeighbors(0);
+ long int aux = 0;
+ //init trial field
+ for (long int i = 0; i < _nPoints; ++i) {
+ if (costOutput[i] == 0.0) {
+ //check for negative neighbours, calc that ones and add to queue trialfield
+ local_neighbor = _grid->getNeighbors(i);
+
+ //check for valid neigh
+ aux = local_neighbor.key[0];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[1];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[2];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[3];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ }
+ }
+
+ while(!trialfield.empty()) {
+ local_neighbor = _grid->getNeighbors(trialfield.top());
+ trialfield.pop();
+
+ //check for valid neigh
+ aux = local_neighbor.key[0];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[1];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[2];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ aux = local_neighbor.key[3];
+ if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
+ calcDist(aux, costOutput, directionOutput, speed);
+ trialfield.emplace(aux);
+ //trialfield2.emplace(aux);
+ }
+ }
+}
+
+void UnivFFviaFMTrips::calcDist(const long int key, double* cost, Point* dir, const double* const speed) {
+ //adapt from calcFloorfield
+ double row = DBL_MAX;
+ double col = DBL_MAX;
+ long int aux = -1; //will be set below
+ bool pointsUp = false;
+ bool pointsRight = false;
+
+ directNeighbor dNeigh = _grid->getNeighbors(key);
+
+ aux = dNeigh.key[0];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in resetGoalAndCosts
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) //gridpoint holds a calculated value
+ ) //gridpoint holds a calculated value
+ {
+ row = cost[aux];
+ pointsRight = true;
+ if (row < 0) {
+ std::cerr << "hier ist was schief " << row << " " << aux << " " << std::endl;
+ row = DBL_MAX;
+ }
+ }
+ aux = dNeigh.key[2];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) //gridpoint holds a calculated value
+ &&
+ (cost[aux] < row)) //calculated value promises smaller cost
+ {
+ row = cost[aux];
+ pointsRight = false;
+ }
+
+ aux = dNeigh.key[1];
+ //hint: trialfield[i].cost = dist2Wall + i; <<< set in parseBuilding after linescan call
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) //gridpoint holds a calculated value
+ )
+ {
+ col = cost[aux];
+ pointsUp = true;
+ if (col < 0) {
+ std::cerr << "hier ist was schief " << col << " " << aux << " " << std::endl;
+ col = DBL_MAX;
+ }
+ }
+ aux = dNeigh.key[3];
+ if ((aux != -2) && //neighbor is a gridpoint
+ (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
+
+ (cost[aux] < col)) //calculated value promises smaller cost
+ {
+ col = cost[aux];
+ pointsUp = false;
+ }
+ 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.);
+ } else {
+ dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ dir[key]._y = (0.);
+ }
+ dir[key] = dir[key].Normalized();
+ return;
+ }
+
+ 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());
+ } else {
+ dir[key]._x = (0.);
+ dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ dir[key] = dir[key].Normalized();
+ return;
+ }
+
+ //two sided update
+ double precheck = twosidedCalc(row, col, _grid->Gethx()/speed[key]);
+ 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());
+ }
+ if (pointsUp && !pointsRight) {
+ dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
+ }
+ if (!pointsUp && pointsRight) {
+ dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
+ dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ if (!pointsUp && !pointsRight) {
+ dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
+ dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
+ }
+ } else {
+ std::cerr << "else in twosided Dist " << std::endl;
+ }
+ dir[key] = dir[key].Normalized();
+}
+
+inline double UnivFFviaFMTrips::onesidedCalc(double xy, double hDivF) {
+ //if ( (xy+hDivF) > 10000) std::cerr << "error in onesided " << xy << std::endl;
+ return xy + hDivF;
+}
+
+inline double UnivFFviaFMTrips::twosidedCalc(double x, double y, double hDivF) { //on error return -2
+ double determinante = (2*hDivF*hDivF - (x-y)*(x-y));
+ if (determinante >= 0) {
+ return (x + y + sqrt(determinante))/2;
+ } else {
+ return (x < y) ? (x + hDivF) : (y + hDivF);
+ }
+ std::cerr << "error in two-sided 2!!!!!!!!!!!!!!!!!!!!!!! o_O??" << std::endl;
+ return -2.; //this line should never execute
+} //twosidedCalc
+
+void UnivFFviaFMTrips::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());
+ if (_mode == LINESEGMENT) {
+ if (tempTargetLine.GetLength() > 0.6) { //shorten line from both Points to avoid targeting edges of real door
+ const Point &p1 = tempTargetLine.GetPoint1();
+ const Point &p2 = tempTargetLine.GetPoint2();
+ double length = tempTargetLine.GetLength();
+ double u = 0.2 / length;
+ tempTargetLine = Line(p1 + (p2 - p1) * u, p1 + (p2 - p1) * (1 - u), 0);
+ } else if (tempTargetLine.GetLength() > 0.2) {
+ const Point &p1 = tempTargetLine.GetPoint1();
+ const Point &p2 = tempTargetLine.GetPoint2();
+ double length = tempTargetLine.GetLength();
+ double u = 0.05 / length;
+ tempTargetLine = Line(p1 + (p2 - p1) * u, p1 + (p2 - p1) * (1 - u), 0);
+ }
+ }
+
+ //this allocation must be on shared heap! to be accessible by any thread later (should be shared in openmp)
+ double* newArrayDBL = (costarrayDBL)? costarrayDBL : new double[_nPoints];
+ Point* newArrayPt = nullptr;
+ if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+ newArrayPt = (gradarrayPt)? gradarrayPt : new Point[_nPoints];
+ }
+
+ if ((_costFieldWithKey[uid]) && (_costFieldWithKey[uid] != costarrayDBL))
+ delete[] _costFieldWithKey[uid];
+ _costFieldWithKey[uid] = newArrayDBL;
+
+ //init costarray
+ for (int i = 0; i < _nPoints; ++i) {
+ if (_gridCode[i] == WALL) {
+ newArrayDBL[i] = magicnum(WALL_ON_COSTARRAY);
+ } else {
+ newArrayDBL[i] = magicnum(UNKNOWN_COST);
+ }
+ }
+
+ if ((_directionFieldWithKey[uid]) && (_directionFieldWithKey[uid] != gradarrayPt))
+ delete[] _directionFieldWithKey[uid];
+ if (newArrayPt)
+ _directionFieldWithKey[uid] = newArrayPt;
+
+ //initialize start area
+ if (_mode == LINESEGMENT) {
+ drawLinesOnGrid(tempTargetLine, newArrayDBL, magicnum(TARGET_REGION));
+ }
+ if (_mode == CENTERPOINT) {
+ newArrayDBL[_grid->getKeyAtPoint(tempCenterPoint)] = magicnum(TARGET_REGION);
+ }
+
+ if (_speedmode == FF_WALL_AVOID) {
+ calcFF(newArrayDBL, newArrayPt, _speedFieldSelector[REDU_WALL_SPEED]);
+ } else if (_speedmode == FF_HOMO_SPEED) {
+ calcFF(newArrayDBL, newArrayPt, _speedFieldSelector[INITIAL_SPEED]);
+ } else if (_speedmode == FF_PED_SPEED) {
+ calcFF(newArrayDBL, newArrayPt, _speedFieldSelector[PED_SPEED]);
+ }
+
+ //the rest of the door must be initialized if centerpoint was used. else ff_router will have probs getting localDist
+ if (_mode == CENTERPOINT) {
+ drawLinesOnGrid(tempTargetLine, newArrayDBL, magicnum(TARGET_REGION));
+ }
+ //the directional field is yet undefined on the target line itself. we will use neighboring vector to help agents
+ //to cross the line
+ if (newArrayPt) {
+ Point passvector = tempTargetLine.NormalVec();
+ Point trial = tempTargetLine.GetCentre() - passvector * 0.25;
+ Point trial2 = tempTargetLine.GetCentre() + passvector * 0.25;
+ if ((_grid->includesPoint(trial)) && (_gridCode[_grid->getKeyAtPoint(trial)] == INSIDE)) {
+ finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector);
+ 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);
+}
+
+void UnivFFviaFMTrips::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 UnivFFviaFMTrips::addAllTargets() {
+ for (auto uidmap : _doors) {
+ addTarget(uidmap.first);
+ }
+}
+
+void UnivFFviaFMTrips::addAllTargetsParallel() {
+ //Reason: freeing and reallocating takes time. We do not use already allocated memory, because we do not know if it
+ // is shared memory. Maybe this is not neccessary - maybe reconsider. This way, it is safe. If this function
+ // is called from a parallel region, we all go to hell.
+ //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 (signed int i = 0; i < _doors.size(); ++i) {
+ auto doorPair = _doors.begin();
+ std::advance(doorPair, i);
+ addTarget(doorPair->first, _costFieldWithKey[doorPair->first], _directionFieldWithKey[doorPair->first]);
+ }
+ };
+}
+
+void UnivFFviaFMTrips::addTargetsParallel(std::vector<int> wantedDoors) {
+ //free old memory (but not the distancemap with key == 0)
+ for (int targetUID : wantedDoors) {
+ if ((targetUID != 0) && _costFieldWithKey.count(targetUID) && _costFieldWithKey[targetUID]) {
+ delete[] _costFieldWithKey[targetUID];
+ }
+ if ((targetUID != 0) && _directionFieldWithKey.count(targetUID) && _directionFieldWithKey[targetUID]) {
+ delete[] _directionFieldWithKey[targetUID];
+ }
+ }
+ //allocate new memory
+ for (int targetUID : wantedDoors) {
+ _costFieldWithKey[targetUID] = new double[_nPoints];
+ if (_user == DISTANCE_MEASUREMENTS_ONLY) {
+ _directionFieldWithKey[targetUID] = nullptr;
+ }
+ if (_user == DISTANCE_AND_DIRECTIONS_USED) {
+ _directionFieldWithKey[targetUID] = new Point[_nPoints];
+ }
+ }
+
+ //parallel region
+#pragma omp parallel
+ {
+#pragma omp for
+ for (signed int i = 0; i < wantedDoors.size(); ++i) {
+ auto doorUID = wantedDoors.begin();
+ std::advance(doorUID, i);
+ addTarget(*doorUID, _costFieldWithKey[*doorUID], _directionFieldWithKey[*doorUID]);
+ }
+ };
+}
+
+SubRoom** UnivFFviaFMTrips::getSubRoomFF(){
+ return _subrooms;
+}
+
+SubRoom* UnivFFviaFMTrips::getSubRoom(const Point& pos){
+ if (!_subrooms) return nullptr;
+ if (!_grid->includesPoint(pos)) return nullptr;
+
+ long key = _grid->getKeyAtPoint(pos);
+ return _subrooms[key];
+}
+
+std::vector<int> UnivFFviaFMTrips::getKnownDoorUIDs(){
+ return _uids;
+}
+
+void UnivFFviaFMTrips::setUser(int userArg) {
+ _user=userArg;
+}
+
+void UnivFFviaFMTrips::setMode(int modeArg) {
+ _mode=modeArg;
+}
+
+void UnivFFviaFMTrips::setSpeedMode(int speedModeArg) {
+ _speedmode = speedModeArg;
+ if (_speedmode == FF_PED_SPEED && !_speedFieldSelector[PED_SPEED]) {
+ _speedFieldSelector[PED_SPEED] = new double[_nPoints];
+ }
+}
+
+
+void UnivFFviaFMTrips::writeFF(const std::string& filename, std::vector<int> targetID) {
+ Log->Write("INFO: \tWrite Floorfield to file");
+ Log->Write(filename);
+ std::ofstream file;
+
+ Log->Write("FloorfieldViaFM::writeFF(): writing to file %s: There are %d targets.", filename.c_str(), targetID.size());
+
+ int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
+ int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
+ //int numTotal = numX * numY;
+ //std::cerr << numTotal << " numTotal" << std::endl;
+ //std::cerr << grid->GetnPoints() << " grid" << std::endl;
+ file.open(_configuration->GetProjectRootDir()+filename);
+
+ file << "# vtk DataFile Version 3.0" << std::endl;
+ file << "Testdata: Fast Marching: Test: " << std::endl;
+ file << "ASCII" << std::endl;
+ file << "DATASET STRUCTURED_POINTS" << std::endl;
+ file << "DIMENSIONS " <<
+ std::to_string(_grid->GetiMax()) <<
+ " " <<
+ std::to_string(_grid->GetjMax()) <<
+ " 1" << std::endl;
+ file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
+ file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
+ file << "POINT_DATA " << std::to_string(_grid->GetnPoints()) << std::endl;
+ file << "SCALARS GCode float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ if (!_gridCode) {
+ return;
+ }
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _gridCode[i] << std::endl;
+ }
+
+ if (_directionFieldWithKey[0]) {
+ file << "VECTORS Dir2Wall float" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _directionFieldWithKey[0][i]._x << " " << _directionFieldWithKey[0][i]._y << " 0.0" << std::endl;
+ }
+
+ file << "SCALARS Dist2Wall float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ file << _costFieldWithKey[0][i] << std::endl; //@todo: change target to all dist2wall
+ }
+ }
+
+ if (_subrooms) {
+ file << "SCALARS SubroomPtr float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if (_subrooms[i]) {
+ file << _subrooms[i]->GetUID() << std::endl;
+ } else {
+ file << 0.0 << std::endl;
+ }
+ }
+ }
+
+ if (!targetID.empty()) {
+ for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
+ if (_costFieldWithKey.count(targetID[iTarget]) == 0) {
+ continue;
+ }
+ double *costarray = _costFieldWithKey[targetID[iTarget]];
+
+ Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
+
+// std::string name = _building->GetTransOrCrossByUID(targetID[iTarget])->GetCaption() + "-" +
+// std::to_string(targetID[iTarget]);
+
+ std::string name = 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;
+ }
+
+ Point *gradarray = _directionFieldWithKey[targetID[iTarget]];
+ if (gradarray == nullptr) {
+ continue;
+ }
+
+
+ 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;
+ }
+
+
+ }
+ }
+ file.close();
+}
+
+//mode is argument, which should not be needed, the info is stored in members like speedmode, ...
+double UnivFFviaFMTrips::getCostToDestination(const int destID, const Point& position, int mode) {
+ assert(_grid->includesPoint(position));
+ long int key = _grid->getKeyAtPoint(position);
+ if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
+ //bresenham line (treppenstruktur) at middle and calculated centre of line are on different gridpoints
+ //find a key that belongs domain (must be one left or right and second one below or above)
+ if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
+ key = key+1;
+ } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
+ key = key - 1;
+ } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
+ key = key - _grid->GetiMax();
+ } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
+ key = key + _grid->GetiMax();
+ } else {
+ Log->Write("ERROR:\t In getCostToDestination(3 args)");
+ }
+ }
+ if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
+ return _costFieldWithKey[destID][key];
+ } 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]);
+ return getCostToDestination(destID, position, mode);
+ } else if (!_directCalculation && _doors.count(destID) > 0) {
+ //omp critical
+#pragma omp critical(UnivFFviaFMTrips_toDo)
+ _toDo.emplace_back(destID);
+ }
+ return DBL_MAX;
+}
+
+double UnivFFviaFMTrips::getCostToDestination(const int destID, const Point& position) {
+ assert(_grid->includesPoint(position));
+ long int key = _grid->getKeyAtPoint(position);
+ if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
+ //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
+ //find a key that belongs domain (must be one left or right and second one below or above)
+ if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
+ key = key+1;
+ } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
+ key = key - 1;
+ } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
+ key = key - _grid->GetiMax();
+ } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
+ key = key + _grid->GetiMax();
+ } else {
+ Log->Write("ERROR:\t In getCostToDestination(2 args)");
+ }
+ }
+ if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
+ return _costFieldWithKey[destID][key];
+ } 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]);
+ return getCostToDestination(destID, position);
+ } else if (!_directCalculation && _doors.count(destID) > 0) {
+//omp critical
+#pragma omp critical(UnivFFviaFMTrips_toDo)
+ _toDo.emplace_back(destID);
+ }
+ return DBL_MAX;
+}
+
+double UnivFFviaFMTrips::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) getKeyAtPoint yields gridpoint next to edge, although position is on edge
+ //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 if (_gridCode[key-1] == door2_ID){
+ key = key-1;
+ } else {
+ Log->Write("ERROR:\t In DistanceBetweenDoors");
+ }
+ }
+ 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(UnivFFviaFMTrips_toDo)
+ _toDo.emplace_back(door1_ID);
+ }
+ return DBL_MAX;
+}
+
+RectGrid* UnivFFviaFMTrips::getGrid(){
+ return _grid;
+}
+
+void UnivFFviaFMTrips::getDirectionToUID(int destID, long int key, Point& direction, int mode){
+ assert(key > 0 && key < _nPoints);
+ if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
+ //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
+ //find a key that belongs domain (must be one left or right and second one below or above)
+ if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
+ key = key+1;
+ } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
+ key = key - 1;
+ } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
+ key = key - _grid->GetiMax();
+ } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
+ key = key + _grid->GetiMax();
+ } else {
+ Log->Write("ERROR:\t In getDirectionToUID (4 args)");
+ }
+ }
+ 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);
+ } else if (!_directCalculation && _doors.count(destID) > 0) {
+//omp critical
+#pragma omp critical(UnivFFviaFMTrips_toDo)
+ _toDo.emplace_back(destID);
+ direction._x = 0.;
+ direction._y = 0.;
+ }
+ return;
+}
+
+void UnivFFviaFMTrips::getDirectionToUID(int destID, long int key, Point& direction){
+ //assert(key > 0 && key < _nPoints);
+ if(key <=0 || key>=_nPoints)
+ {
+ direction._x = 0.;
+ direction._y = 0.;
+ return;
+ }
+ if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
+ //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
+ //find a key that belongs domain (must be one left or right and second one below or above)
+ if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
+ key = key+1;
+ } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
+ key = key - 1;
+ } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
+ key = key - _grid->GetiMax();
+ } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
+ key = key + _grid->GetiMax();
+ } else {
+ Log->Write("ERROR:\t In getDirectionToUID (3 args)");
+ }
+ }
+ 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);
+ } else if (!_directCalculation && _doors.count(destID) > 0) {
+//omp critical
+#pragma omp critical(UnivFFviaFMTrips_toDo)
+ _toDo.emplace_back(destID);
+ direction._x = 0.;
+ direction._y = 0.;
+ }
+ return;
+}
+
+void UnivFFviaFMTrips::getDirectionToUID(int destID, const Point& pos, Point& direction, int mode) {
+ getDirectionToUID(destID, _grid->getKeyAtPoint(pos), direction, mode);
+}
+
+void UnivFFviaFMTrips::getDirectionToUID(int destID, const Point& pos,Point& direction) {
+ getDirectionToUID(destID, _grid->getKeyAtPoint(pos), direction);
+}
+
+double UnivFFviaFMTrips::getDistance2WallAt(const Point &pos) {
+ if (_useWallDistances || (_speedmode == FF_WALL_AVOID)) {
+ if (_costFieldWithKey[0]) {
+ return _costFieldWithKey[0][_grid->getKeyAtPoint(pos)];
+ }
+ }
+ return DBL_MAX;
+}
+
+void UnivFFviaFMTrips::getDir2WallAt(const Point &pos, Point &p) {
+ if (_useWallDistances || (_speedmode == FF_WALL_AVOID)) {
+ if (_directionFieldWithKey[0]) {
+ p = _directionFieldWithKey[0][_grid->getKeyAtPoint(pos)];
+ }
+ } else {
+ p = Point(0.0, 0.0);
+ }
+}
+
+/* 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
+ * */
diff --git a/routing/ff_router_trips/UnivFFviaFMTrips.h b/routing/ff_router_trips/UnivFFviaFMTrips.h
new file mode 100644
index 00000000..069e92b9
--- /dev/null
+++ b/routing/ff_router_trips/UnivFFviaFMTrips.h
@@ -0,0 +1,165 @@
+//
+// Created by arne on 5/9/17.
+//
+/**
+ * \file UnivFFviaFMTrips.h
+ * \date May 09, 2017
+ * \version N/A (v0.8.x)
+ * \copyright <2017-2020> Forschungszentrum Jülich GmbH. All rights reserved.
+ *
+ * \section License
+ * This file is part of JuPedSim.
+ *
+ * JuPedSim is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * JuPedSim is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \section Description
+ * Implementation of classes for a reworked floorfield. A floorfield in general
+ * yields a cost field to a specific goal and a correlated vectorfield for the
+ * optimal path in terms of the cost value.
+ *
+ * Rework focused on a cleaner structure and less inheritance (no diamond) and
+ * less workarounds.
+ *
+ *
+ **/
+#ifndef JPSCORE_UnivFFviaFMTrips_H
+#define JPSCORE_UnivFFviaFMTrips_H
+
+#include <string>
+#include <vector>
+#include <map>
+#include <float.h>
+#include "../../general/Macros.h"
+
+class Pedestrian;
+class Room;
+class SubRoom;
+class Building;
+class Configuration;
+class Point;
+class RectGrid;
+class Line;
+class Goal;
+
+
+class CompareCost { //this class is used in std::priority_queue in UnivFFviaFMTrips::calcFF
+public:
+ CompareCost(double* costarray) : _costarray(costarray) {}
+ bool operator() (const int a, const int b) const {
+ return _costarray[a] > _costarray[b];
+ }
+
+private:
+ double* _costarray = nullptr;
+};
+
+class UnivFFviaFMTrips {
+public:
+ UnivFFviaFMTrips(Room* a, Building* b, double c, double e, bool f);
+ UnivFFviaFMTrips(SubRoom* a, Building* b, double c, double e, bool f);
+ UnivFFviaFMTrips(Room* a, Configuration* const b, double hx, double wallAvoid, bool useWallDistances, std::map<int, Goal*> goals);
+ UnivFFviaFMTrips(Room* a, Configuration* const b, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors, std::map<int, Goal*> goals);
+ UnivFFviaFMTrips(SubRoom* sr, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances, std::map<int, Goal*> goals);
+ UnivFFviaFMTrips(SubRoom* subRoomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors, std::map<int, Goal*> goals);
+ void create(std::vector<Line>& walls, std::map<int, Line>& doors, std::vector<int> targetUIDs, int mode,
+ double spacing, double wallAvoidDist, bool useWallDistances);
+ void recreateAllForQuickest();
+ UnivFFviaFMTrips() {};
+ UnivFFviaFMTrips(UnivFFviaFMTrips&){};
+ virtual ~UnivFFviaFMTrips();
+
+ 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();
+ void addTargetsParallel(std::vector<int> wantedDoors);
+ std::vector<int> getKnownDoorUIDs();
+ void setUser(int userArg);
+ void setMode(int modeArg);
+ void setSpeedMode(int speedModeArg);
+ SubRoom** getSubRoomFF();
+ SubRoom* getSubRoom(const Point& pos);
+
+ 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, long int key, Point& direction, int mode);
+ void getDirectionToUID(int destID, 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);
+
+ void writeFF(const std::string&, std::vector<int> targetID);
+
+ void createRectGrid(std::vector<Line>& walls, std::map<int, Line>& doors, double spacing);
+ void processGeometry(std::vector<Line>&walls, std::map<int, Line>& doors);
+ void markSubroom(const Point& insidePoint, SubRoom* const value);
+ void createReduWallSpeed(double* reduWallSpeed);
+ void createPedSpeed(Pedestrian* const * pedsArg, int nsize, int modechoice, double radius);
+ void finalizeTargetLine(const int uid, const Line& tempTargetLine, Point* newArrayPt, Point& passvector);
+
+ void drawLinesOnGrid(std::map<int, Line>& doors, int *const grid);
+ template <typename T>
+ void drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value);
+ template <typename T>
+ void drawLinesOnGrid(Line& line, T* const target, const T value);
+
+ template <typename T>
+ void drawLinesOnWall(std::vector<Line>& wallArg, T* const target, const T value);
+ template <typename T>
+ void drawLinesOnWall(Line& line, T* const target, const T value);
+
+ void calcFF(double*, Point*, const double* const);
+ void calcCost(const long int key, double* cost, Point* dir, const double* const speed);
+ void calcDF(double*, Point*, const double* const);
+ void calcDist(const long int key, double* cost, Point* dir, const double* const speed);
+ inline double onesidedCalc(double xy, double hDivF);
+ inline double twosidedCalc(double x, double y, double hDivF);
+
+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;
+ int* _gridCode = nullptr;
+ SubRoom* * _subrooms = nullptr; // this is an array (first asterisk) of pointers (second asterisk)
+
+ double _wallAvoidDistance = 0.;
+ bool _useWallDistances = false; //could be used in DirectionStrategy even if mode _speedmode is FF_HOMO_SPEED
+
+ //the following maps are responsible for dealloc the arrays
+ std::map<int, double*> _costFieldWithKey;
+ std::map<int, Point*> _directionFieldWithKey;
+
+ std::vector<int> _uids;
+ std::map<int, Line> _doors;
+ std::vector<int> _toDo;
+
+ std::map<int, Point> _subroomUIDtoInsidePoint;
+ std::map<int, SubRoom*> _subroomUIDtoSubRoomPtr;
+ std::map<SubRoom*, Point> _subRoomPtrTOinsidePoint;
+
+};
+
+
+#endif //JPSCORE_UnivFFviaFMTrips_H
diff --git a/routing/ff_router_trips/ffRouterTrips.cpp b/routing/ff_router_trips/ffRouterTrips.cpp
index 36faf374..e16e4eb1 100644
--- a/routing/ff_router_trips/ffRouterTrips.cpp
+++ b/routing/ff_router_trips/ffRouterTrips.cpp
@@ -108,7 +108,11 @@ bool FFRouterTrips::Init(Building* building)
std::cout << std::endl;
for (auto &itrGoal : building->GetAllGoals()) {
std::cout << "Goal ID: " << itrGoal.second->GetId() << std::endl;
- _globalFF->createMapEntryInLineToGoalID(itrGoal.first);
+ if(WaitingArea* wa = dynamic_cast<WaitingArea*>(itrGoal.second)) {
+ _globalFF->createMapEntryInLineToGoalID(itrGoal.first, true);
+ }else{
+ _globalFF->createMapEntryInLineToGoalID(itrGoal.first, false);
+ }
goalIDs.emplace_back(itrGoal.first);
}
_goalToLineUIDmap = _globalFF->getGoalToLineUIDmap();
@@ -470,10 +474,27 @@ int FFRouterTrips::FindExit(Pedestrian* ped)
// std::cout << "ExitLine: " << ped->GetExitLine() << std::endl;
std::cout << "ExitIndex: " << ped->GetExitIndex() << std::endl << std::endl;
+ for (auto& goal : _goalToLineUIDmap){
+ std::cout << goal.first << " -> " << goal.second << std::endl;
+ }
+
+ SubRoom* subroom = _building->GetSubRoomByUID(ped->GetSubRoomUID());
+ Goal* goal = _building->GetFinalGoal(ped->GetFinalDestination());
+
+ int ret;
// Check if current position is already waiting area
// yes: set next goal and return findExit(p)
- int ret = FindExit1(ped);
+ if (subroom->IsInSubRoom(goal->GetCentroid())){
+ std::cout << "Ped and Goal in same subroom: " << subroom->IsInSubRoom(goal->GetCentroid()) << std::endl;
+ int bestDoor = 31;
+ ped->SetExitIndex(bestDoor);
+ ped->SetExitLine(_CroTrByUID.at(bestDoor));
+
+ }else{
+ ret = FindExit1(ped);
+ }
+
std::cout << "Ped[" << ped->GetID() << "] in (" << ped->GetRoomID() << ", " << ped->GetSubRoomID()
<< "/" << ped->GetSubRoomUID() << "): " << std::endl;
@@ -527,6 +548,7 @@ int FFRouterTrips::FindExit1(Pedestrian* p)
}
}
+
std::vector<int> DoorUIDsOfRoom;
DoorUIDsOfRoom.clear();
if (_building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->IsInSubRoom(p->GetPos())) {
@@ -624,12 +646,12 @@ int FFRouterTrips::FindExit1(Pedestrian* p)
}
//at this point, bestDoor is either a crossing or a transition
- if ((!_targetWithinSubroom) && (_CroTrByUID.count(bestDoor) != 0)) {
- while (!_CroTrByUID[bestDoor]->IsTransition()) {
- std::pair<int, int> key = std::make_pair(bestDoor, bestFinalDoor);
- bestDoor = _pathsMatrix[key];
- }
- }
+// if ((!_targetWithinSubroom) && (_CroTrByUID.count(bestDoor) != 0)) {
+// while (!_CroTrByUID[bestDoor]->IsTransition()) {
+// std::pair<int, int> key = std::make_pair(bestDoor, bestFinalDoor);
+// bestDoor = _pathsMatrix[key];
+// }
+// }
//#pragma omp critical(finalDoors)
// _finalDoors.emplace(std::make_pair(p->GetID(), bestFinalDoor));
diff --git a/routing/trips_router/TripsRouter.cpp b/routing/trips_router/TripsRouter.cpp
index 776f3da0..f28b0aff 100644
--- a/routing/trips_router/TripsRouter.cpp
+++ b/routing/trips_router/TripsRouter.cpp
@@ -23,6 +23,11 @@ TripsRouter::~TripsRouter()
bool TripsRouter::Init(Building* building)
{
+ Log->Write("TripsRouter::Init");
+
+
+ // Create Floorfield for each subroom
+ // If goal inside room, start wave from each edge to the outside
return true;
}
@@ -36,8 +41,6 @@ int TripsRouter::FindExit(Pedestrian* p)
// Check if current position is already waiting area
// yes: set next goal and return findExit(p)
- p->SetExitIndex(17);
- p->SetExitLine(0);
- return 17;
+ return 0;
}
--
GitLab