diff --git a/CMakeLists.txt b/CMakeLists.txt index 97aa645aaeed5034f489df1811b16b8371b21f60..c0793e6959870dcb01cb546dc23c74859143958b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,7 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Intel") message(STATUS "Compiling with Intel settings") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -w") - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -O0 -w -tcheck") + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -Og -w -tcheck") elseif () endif () set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${warnings}") @@ -463,6 +463,7 @@ set(header_files tinyxml/tinystr.h general/ArgumentParser.h + general/Configuration.h general/Macros.h general/randomnumbergenerator.h diff --git a/IO/IniFileParser.cpp b/IO/IniFileParser.cpp index 1a3a854292bde52a8dfb1b511b5ec4d39f665cf9..a6ec04f420cdff46de12cc6007d51e8791bfbb1c 100644 --- a/IO/IniFileParser.cpp +++ b/IO/IniFileParser.cpp @@ -1485,6 +1485,7 @@ bool IniFileParser::ParseStrategyNodeToObject(const TiXmlNode& strategyNode) Log->Write("ERROR: \tExit Strategy 7 is not supported any longer. Please refer to www.jupedsim.org"); Log->Write("WARNING: \tChanging Exit-Strategy to #9 (Floorfields with targets within subroom)"); pExitStrategy = 9; + _exit_strat_number = 9; _exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionSubLocalFloorfield()); break; case 8: @@ -1511,6 +1512,7 @@ bool IniFileParser::ParseStrategyNodeToObject(const TiXmlNode& strategyNode) return false; } Log->Write("INFO: \texit_crossing_strategy < %d >", pExitStrategy); + _config->set_exit_strat(_exit_strat_number); } return true; } diff --git a/general/Configuration.h b/general/Configuration.h index cb27882daab59cc30530897a83141c1d8a7b3db2..9c26c3ea7b055b00bffecc90006c2dfc216d2812 100644 --- a/general/Configuration.h +++ b/general/Configuration.h @@ -118,6 +118,7 @@ public: //ff router _has_specific_goals = false; _write_VTK_files = false; + _exit_strat = 9; //for random numbers _rdGenerator=RandomNumberGenerator(); @@ -272,6 +273,10 @@ public: bool get_write_VTK_files() const {return _write_VTK_files;} + void set_exit_strat(int e_strat) {_exit_strat = e_strat;} + + int get_exit_strat() const {return _exit_strat;} + const std::string& GetHostname() const { return _hostname; }; void SetHostname(std::string hostname) { _hostname = hostname; }; @@ -383,6 +388,8 @@ private: bool _has_specific_goals; bool _write_VTK_files; + int _exit_strat; + std::string _hostname; std::string _trajectoriesFile; std::string _errorLogFile; diff --git a/math/VelocityModel.cpp b/math/VelocityModel.cpp index fbd51e37eddc35040cabb7f9cf884679e063269e..0e892420e62345309321a4b5e0268997244d2700 100644 --- a/math/VelocityModel.cpp +++ b/math/VelocityModel.cpp @@ -308,19 +308,18 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const if ( (dynamic_cast<DirectionFloorfield*>(_direction.get())) || (dynamic_cast<DirectionLocalFloorfield*>(_direction.get())) || (dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get())) ) { - if (dist > 50*J_EPS_GOAL) { + if (dist > 5*J_EPS_GOAL) { desired_direction = target - pos; //ped->GetV0(target); } else { desired_direction = lastE0; ped->SetLastE0(lastE0); //keep old vector (revert set operation done 9 lines above) } - } - else if (dist > J_EPS_GOAL) { + } else if (dist > J_EPS_GOAL) { desired_direction = ped->GetV0(target); } else { ped->SetSmoothTurning(); desired_direction = ped->GetV0(); - } + } return desired_direction; } diff --git a/routing/DirectionStrategy.cpp b/routing/DirectionStrategy.cpp index 6e7ae0c67224b05d82fd323aa03e820573b4993e..f3f02a30cc7194719e538902f8f8f5da5179d4fe 100644 --- a/routing/DirectionStrategy.cpp +++ b/routing/DirectionStrategy.cpp @@ -354,9 +354,9 @@ Point DirectionLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const } #endif floorfield->getDirectionToUID(ped->GetExitIndex(), ped->GetPos(), p); - if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0) { - p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()); - } +// if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0) { +// p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()); +// } return (p + ped->GetPos()); #if DEBUG @@ -443,9 +443,9 @@ Point DirectionSubLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const } #endif floorfield->getDirectionToUID(ped->GetExitIndex(), ped->GetPos(),p); - if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0){ - p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()); - } +// if (floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()) < 1.0){ +// p = p * floorfield->getCostToDestination(ped->GetExitIndex(), ped->GetPos()); +// } return (p + ped->GetPos()); #if DEBUG diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp index dd9327aaa3a6d2f857e6f1cba00fe924ec31144c..b8729babe33ad4f669cc7d924365366442d2007b 100644 --- a/routing/ff_router/UnivFFviaFM.cpp +++ b/routing/ff_router/UnivFFviaFM.cpp @@ -515,6 +515,193 @@ void UnivFFviaFM::createPedSpeed(Pedestrian *const *pedsArg, int nsize, int mode } } +void UnivFFviaFM::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 { + 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 { + 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 { + 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 { + Log->Write("ERROR:\t in finalizingTargetLine"); + } + } + } +} + void UnivFFviaFM::drawLinesOnGrid(std::map<int, Line>& doors, int *const grid) { for (auto&& doorPair : doors) { int tempUID = doorPair.first; @@ -1195,6 +1382,21 @@ void UnivFFviaFM::addTarget(const int uid, double* costarrayDBL, Point* gradarra 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->getKeyAtPoint(trial) >= 0) && (_gridCode[_grid->getKeyAtPoint(trial)] == INSIDE)) { + finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector); + } else if ((_grid->getKeyAtPoint(trial2) >= 0) && (_gridCode[_grid->getKeyAtPoint(trial2)] == INSIDE)) { + passvector = passvector * -1.0; + finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector); + } else { + Log->Write("ERROR:\t in addTarget: calling finalizeTargetLine"); + } + } #pragma omp critical(_uids) _uids.emplace_back(uid); } @@ -1419,18 +1621,19 @@ void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID //mode is argument, which should not be needed, the info is stored in members like speedmode, ... double UnivFFviaFM::getCostToDestination(const int destID, const Point& position, int mode) { -// if (!_grid->includesPoint(position)) { -// Log->Write("Was ist denn hier los?"); -// } - assert(_grid->includesPoint(position)); + 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 ((_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) { + if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) { key = key+1; - } else if ((_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)"); } @@ -1461,10 +1664,14 @@ double UnivFFviaFM::getCostToDestination(const int destID, const Point& 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 ((_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) { + if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) { key = key+1; - } else if ((_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)"); } @@ -1531,6 +1738,21 @@ RectGrid* UnivFFviaFM::getGrid(){ void UnivFFviaFM::getDirectionToUID(int destID, const 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) { @@ -1561,6 +1783,21 @@ void UnivFFviaFM::getDirectionToUID(int destID, const long int key, Point& direc void UnivFFviaFM::getDirectionToUID(int destID, const long int key, Point& direction){ 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 (3 args)"); + } + } if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) { direction = _directionFieldWithKey[destID][key]; } else if (_directCalculation && _doors.count(destID) > 0) { diff --git a/routing/ff_router/UnivFFviaFM.h b/routing/ff_router/UnivFFviaFM.h index 6313dbde95f4d2be60b707f2a86dbbdbc5c62502..b5ab3ccf102fc6c81e4ac664345a051e028f2695 100644 --- a/routing/ff_router/UnivFFviaFM.h +++ b/routing/ff_router/UnivFFviaFM.h @@ -123,6 +123,7 @@ public: 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> diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp index 57661402d39a1e317800ea55dfea17c9c8b7e935..6ca94d4ef17fee35ec46bbef140cea79e8c78bcb 100644 --- a/routing/ff_router/ffRouter.cpp +++ b/routing/ff_router/ffRouter.cpp @@ -64,6 +64,7 @@ FFRouter::FFRouter(int id, RoutingStrategy s, bool hasSpecificGoals, Configurati _hasSpecificGoals = hasSpecificGoals; _globalFF = nullptr; _targetWithinSubroom = true; //depending on exit_strat 8 => false, depending on exit_strat 9 => true; + _targetWithinSubroom = (_config->get_exit_strat() == 9); if (s == ROUTING_FF_QUICKEST) { _mode = quickest; _recalc_interval = _config->get_recalc_interval();