Commit 8139b85c authored by Arne Graf's avatar Arne Graf

finalized FF with transition vector; enabled _targetWithinSubroom mechanic; fixed bresenham inacc

parent 703d4514
Pipeline #9178 failed with stages
in 13 seconds
......@@ -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
......
......@@ -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;
}
......
......@@ -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;
......
......@@ -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;
}
......
......@@ -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
......
......@@ -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) {
......
......@@ -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>
......
......@@ -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();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment