Commit 59e8e882 authored by f.mack's avatar f.mack

fetched files f.mack modified

parent 88cb9470
This diff is collapsed.
......@@ -39,6 +39,7 @@ class IniFileParser {
public:
IniFileParser(Configuration* config);
~IniFileParser(){};
bool Parse(std::string iniFile);
......@@ -57,7 +58,7 @@ private:
bool ParseRoutingStrategies(TiXmlNode* routingNode, TiXmlNode* agentDistri);
bool ParseFfRouterOps(TiXmlNode* routingNode);
bool ParseFfRouterOps(TiXmlNode* routingNode, RoutingStrategy s);
bool ParseCogMapOpts(TiXmlNode* routingNode);
......
......@@ -97,6 +97,15 @@ public:
_showStatistics = false;
_fileFormat = FORMAT_XML_PLAIN;
_agentsParameters = std::map<int, std::shared_ptr<AgentsParameters> >();
// ---------- floorfield
_deltaH = 0.0625;
_wall_avoid_distance = 0.4;
_use_wall_avoidance = true;
// ---------- gradientmodel
_slow_down_distance = 0.2;
//ff router quickest
_recalc_interval = 5;
}
int GetSolver() const { return _solver; };
......@@ -212,6 +221,26 @@ public:
void SetDistEffMaxWall(double distEffMaxWall) { _distEffMaxWall = distEffMaxWall; };
double get_deltaH() const { return _deltaH; }
void set_deltaH(double _deltaH) { Configuration::_deltaH = _deltaH; }
double get_wall_avoid_distance() const { return _wall_avoid_distance; }
void set_wall_avoid_distance(double _wall_avoid_distance) { Configuration::_wall_avoid_distance = _wall_avoid_distance; }
bool get_use_wall_avoidance() const { return _use_wall_avoidance; }
void set_use_wall_avoidance(bool _use_wall_avoidance) { Configuration::_use_wall_avoidance = _use_wall_avoidance; }
double get_slow_down_distance() const { return _slow_down_distance; }
void set_slow_down_distance(double _slow_down_distance) { Configuration::_slow_down_distance = _slow_down_distance; }
double get_recalc_interval() const { return _recalc_interval; }
void set_recalc_interval(double _recalc_interval) { Configuration::_recalc_interval = _recalc_interval; }
const std::string& GetHostname() const { return _hostname; };
void SetHostname(std::string hostname) { _hostname = hostname; };
......@@ -304,6 +333,16 @@ private:
double _maxFWall;
double _distEffMaxPed;
double _distEffMaxWall;
//floorfield
double _deltaH;
double _wall_avoid_distance;
bool _use_wall_avoidance;
//gradientmodel
double _slow_down_distance;
//ff router quickest
double _recalc_interval;
std::string _hostname;
std::string _trajectoriesFile;
std::string _errorLogFile;
......
......@@ -338,6 +338,12 @@ void GradientModel::ComputeNextTimeStep(double current, double deltaT, Building*
movDirection = oldMov - diff;
movDirection = (movDirection.Norm() > 1.) ? movDirection.Normalized() : movDirection;
if (Pedestrian::GetGlobalTime() == 10) {
Log->Write("exit: %d", ped->GetExitIndex());
Log->Write("toTarget: %f, %f", toTarget._x, toTarget._y);
Log->Write("movDirection: %f, %f", movDirection._x, movDirection._y);
}
//redirect near wall mechanics:
Point dir2Wall = Point{0., 0.};
double distance2Wall = -1.;
......@@ -356,18 +362,29 @@ void GradientModel::ComputeNextTimeStep(double current, double deltaT, Building*
double dotProduct = 0;
double antiClippingFactor = 1;
if (distance2Wall < _slowDownDistance) {
Log->Write("distance2Wall: %f, _slowDownDistance: %f", distance2Wall, _slowDownDistance);
dotProduct = movDirection.ScalarProduct(dir2Wall);
if ((dotProduct > 0) && (distance2Wall < .5 * _slowDownDistance)) { //acute angle && really close to wall
movDirection = movDirection - (dir2Wall*dotProduct); //remove walldirection from movDirection
++(*redircnt);
}
antiClippingFactor = ( 1 - .5*(dotProduct + fabs(dotProduct)) );
Log->Write("antiClippingFactor: %f", antiClippingFactor);
++(*slowcnt);
}
movDirection = movDirection * (antiClippingFactor * ped->GetV0Norm() * deltaT);
Point pos_neu = ped->GetPos() + movDirection;
if (Pedestrian::GetGlobalTime() == 10 || Pedestrian::GetGlobalTime() == 3) {
Log->Write("dir2Wall: %f, %f", dir2Wall._x, dir2Wall._y);
Log->Write("dotProduct: %f", dotProduct);
Log->Write("V0Norm: %f", ped->GetV0Norm());
Log->Write("deltaT: %f", deltaT);
Log->Write("final movDirection: %f, %f", movDirection._x, movDirection._y);
Log->Write("oldPos: %f, %f", ped->GetPos()._x, ped->GetPos()._y);
Log->Write("pos_neu: %f, %f", pos_neu._x, pos_neu._y);
}
ped->SetPos(pos_neu);
ped->SetV(movDirection/deltaT);
......
......@@ -368,7 +368,10 @@ Point DirectionLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const
// filename));
// locffviafm[room->GetID()]->getDirectionToDestination(ped, p);
// }
if (Pedestrian::GetGlobalTime() == 10) Log->Write("DirectionLocalFloorfield::GetTarget(): p == %f, %f", p._x, p._y);
p = p.Normalized(); // @todo: argraf : scale with costvalue: " * ffviafm->getCostToTransition(ped->GetTransitionID(), ped->GetPos()) "
if (Pedestrian::GetGlobalTime() == 10) Log->Write("DirectionLocalFloorfield::GetTarget(): p.Normalized == %f, %f", p._x, p._y);
if (Pedestrian::GetGlobalTime() == 10) Log->Write("DirectionLocalFloorfield::GetTarget(): ped->GetPos() == %f, %f", ped->GetPos()._x, ped->GetPos()._y);
return (p + ped->GetPos());
#if DEBUG
......@@ -385,6 +388,10 @@ Point DirectionLocalFloorfield::GetDir2Wall(Pedestrian* ped) const
Point p;
int roomID = ped->GetRoomID();
locffviafm.at(roomID)->getDir2WallAt(ped->GetPos(), p);
if (Pedestrian::GetGlobalTime() == 10) {
Log->Write("DirectionLocalFloorfield::GetDir2Wall was called for roomID %d, subroomID %d", roomID, ped->GetSubRoomID());
Log->Write("at %f, %f, the result is %f, %f", ped->GetPos()._x, ped->GetPos()._y, p._x, p._y);
}
return p;
}
......@@ -422,8 +429,8 @@ void DirectionLocalFloorfield::Init(Building* buildingArg, double stepsize,
//for (auto& roomPair : building->GetAllRooms()) {
auto roomPairIt = building->GetAllRooms().begin();
std::advance(roomPairIt, i);
locffviafm[(*roomPairIt).first] = new LocalFloorfieldViaFM((*roomPairIt).second.get(), building, hx, hy,
wallAvoidDistance, useDistancefield);
locffviafm[(*roomPairIt).first] = new LocalFloorfieldViaFM((*roomPairIt).second.get(), building,
hx, hy, wallAvoidDistance, useDistancefield);
}
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end-start;
......@@ -470,8 +477,9 @@ void DirectionLocalFloorfield::Init(Building* buildingArg, double stepsize,
targets.emplace_back(pair.second);
}
}
//@todo: ar.graf: plz make switch in ini file and dependand on ini write yes/no (issue 208)
std::string lfilename = "floorfield" + std::to_string(roomNr) + ".vtk";
locffviafm[roomNr]->writeFF(lfilename, targets);
//locffviafm[roomNr]->writeFF(lfilename, targets);
}
*/
......@@ -538,14 +546,14 @@ Point DirectionSubLocalFloorfield::GetTarget(Room* room, Pedestrian* ped) const
Point DirectionSubLocalFloorfield::GetDir2Wall(Pedestrian* ped) const
{
Point p;
int key = ped->GetSubRoomID();
int key = ped->GetSubRoomUID();
locffviafm.at(key)->getDir2WallAt(ped->GetPos(), p);
return p;
}
double DirectionSubLocalFloorfield::GetDistance2Wall(Pedestrian* ped) const
{
return locffviafm.at(ped->GetSubRoomID())->getDistance2WallAt(ped->GetPos());
return locffviafm.at(ped->GetSubRoomUID())->getDistance2WallAt(ped->GetPos());
}
void DirectionSubLocalFloorfield::Init(Building* buildingArg, double stepsize,
......@@ -585,8 +593,9 @@ void DirectionSubLocalFloorfield::Init(Building* buildingArg, double stepsize,
#pragma omp critical
subUIDs.emplace_back(subUID);
Log->Write("Creating SubLocFF at key: %d", subUID);
locffviafm[subUID] = new SubLocalFloorfieldViaFM(subroomIt->second.get(), building, hx, hy,
wallAvoidDistance, useDistancefield);
locffviafm[subUID] = new SubLocalFloorfieldViaFM(
subroomIt->second.get(), building,
hx, hy, wallAvoidDistance, useDistancefield);
auto targets = subroomIt->second->GetAllGoalIDs();
for (auto targetUID : targets) {
subAndTarget.emplace_back(std::make_pair(subUID, targetUID));
......@@ -625,8 +634,9 @@ void DirectionSubLocalFloorfield::Init(Building* buildingArg, double stepsize,
targets.emplace_back(pair.second);
}
}
std::string filename1 = "floorfield" + std::to_string(subroomUID) + ".vtk";
locffviafm[subroomUID]->writeFF(filename1, targets);
//@todo: ar.graf: make ini file switch if files are written (yes/no) (Issue 208)
std::string filename = "floorfield" + std::to_string(subroomUID) + ".vtk";
//locffviafm[subroomUID]->writeFF(filename, targets);
}
}
......
This diff is collapsed.
......@@ -65,6 +65,7 @@ public:
//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);
void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
//void getDirectionToUIDParallel(int destID, const long int key, Point& direction);
void getDirectionToDestination (Pedestrian* ped, Point& direction);
//void getDirectionToFinalDestination(Pedestrian* ped, Point& direction); //this is router buissness! problem in multi-storage buildings
......@@ -72,6 +73,7 @@ public:
void createMapEntryInLineToGoalID(const int goalID);
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);
......@@ -85,7 +87,8 @@ public:
void drawLinesOnGrid(std::vector<Line>& wallArg, double* const target, const double dbl2draw);
void drawLinesOnGrid(std::vector<Line>& wallArg, int* const target, const int int2draw);
void setSpeed(bool useDistance2WallArg);
void setSpeedFromLCGrid(double* newspeed);
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
......@@ -104,6 +107,7 @@ public:
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 int isInside(const long int key);
......@@ -139,10 +143,6 @@ protected:
const Building* building;
//stuff to handle wrapper grid (unused, cause RectGrid handles offset)
//double offsetX;
//double offsetY;
//GridPoint Data in independant arrays (shared primary key)
// changed to threadsafe creation when needed: int* flag; //flag:( 0 = unknown, 1 = singel, 2 = double, 3 = final, 4 = added to trial but not calculated, -7 = outside)
int* gcode = nullptr; //gridcode (see Macros.h)
......
......@@ -8,8 +8,7 @@ LocalFloorfieldViaFM::LocalFloorfieldViaFM(){};
LocalFloorfieldViaFM::LocalFloorfieldViaFM(const Room* const roomArg,
const Building* buildingArg,
const double hxArg, const double hyArg,
const double wallAvoidDistance, const bool useDistancefield
) {
const double wallAvoidDistance, const bool useDistancefield) {
//ctor
threshold = -1; //negative value means: ignore threshold
threshold = wallAvoidDistance;
......@@ -148,6 +147,7 @@ void LocalFloorfieldViaFM::parseRoom(const Room* const roomArg,
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()];
......@@ -579,6 +579,7 @@ void SubLocalFloorfieldViaFM::parseRoom(const SubRoom* const roomArg,
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()];
......
......@@ -39,8 +39,10 @@
class LocalFloorfieldViaFM : public FloorfieldViaFM{
public:
LocalFloorfieldViaFM();
LocalFloorfieldViaFM(const Room* const room, const Building* buildingArg, const double hxArg,
const double hyArg, const double wallAvoidDistance, const bool useDistancefield);
LocalFloorfieldViaFM(const Room* const room, const Building* buildingArg,
const double hxArg, const double hyArg,
const double wallAvoidDistance,
const bool useDistancefield);
void parseRoom(const Room* const roomArg, const double hxArg, const double hyArg);
void getDirectionToDestination (Pedestrian* ped, Point& direction);//@todo hides non-virtual function
......@@ -56,7 +58,9 @@ class SubLocalFloorfieldViaFM : public FloorfieldViaFM{
public:
SubLocalFloorfieldViaFM();
SubLocalFloorfieldViaFM(const SubRoom* const subroom, const Building* buildingArg,
const double hxArg, const double hyArg, const double wallAvoidDistance, const bool useDistancefield);
const double hxArg, const double hyArg,
const double wallAvoidDistance,
const bool useDistancefield);
void parseRoom(const SubRoom* const subroomArg, const double hxArg, const double hyArg);
void getDirectionToDestination (Pedestrian* ped, Point& direction);
......
......@@ -52,18 +52,22 @@
//#include "../pedestrian/Pedestrian.h"
//#include "../IO/OutputHandler.h"
int FFRouter::cnt = 0;
FFRouter::FFRouter()
{
}
FFRouter::FFRouter(int id, RoutingStrategy s, bool hasSpecificGoals):Router(id,s) {
FFRouter::FFRouter(int id, RoutingStrategy s, bool hasSpecificGoals, Configuration* config):Router(id,s) {
_config = config;
_building = nullptr;
_hasSpecificGoals = hasSpecificGoals;
_globalFF = nullptr;
_targetWithinSubroom = true; //depending on exit_strat 8 => false, depending on exit_strat 9 => true;
if (s == ROUTING_FF_QUICKEST) {
_mode = quickest;
_recalc_interval = _config->get_recalc_interval();
} else if (s == ROUTING_FF_LOCAL_SHORTEST) {
_mode = local_shortest;
_localShortestSafedPeds.clear();
......@@ -93,14 +97,18 @@ bool FFRouter::Init(Building* building)
{
_building = building;
if (_hasSpecificGoals) {
std::vector<int> goalIDs;
goalIDs.clear();
//get global field to manage goals (which are not in a subroom)
_globalFF = new FloorfieldViaFM(building, 0.25, 0.25, 0.0, false, true);
for (auto &itrGoal : building->GetAllGoals()) {
_globalFF->createMapEntryInLineToGoalID(itrGoal.first);
goalIDs.emplace_back(itrGoal.first);
}
goalToLineUIDmap = _globalFF->getGoalToLineUIDmap();
goalToLineUIDmap2 = _globalFF->getGoalToLineUIDmap2();
goalToLineUIDmap3 = _globalFF->getGoalToLineUIDmap3();
//_globalFF->writeGoalFF("goal.vtk", goalIDs);
}
//get all door UIDs
_allDoorUIDs.clear();
......@@ -281,6 +289,7 @@ bool FFRouter::Init(Building* building)
double tempDistance = ptrToNew->getCostToDestination(rctIt->second,
_CroTrByUID.at(outerPtr.second)->GetCentre());
//Log->Write("#######room %d: calculating distance from door %d to door %d", rctIt->first, rctIt->second, outerPtr.second);
// Point endA = _CroTrByUID.at(*innerPtr)->GetCentre() * .9 +
// _CroTrByUID.at(*innerPtr)->GetPoint1() * .1;
// Point endB = _CroTrByUID.at(*innerPtr)->GetCentre() * .9 +
......@@ -430,6 +439,9 @@ bool FFRouter::ReInit()
#pragma omp critical
_locffviafm.insert(std::make_pair((*pairRoomIt).first, ptrToNew));
if (_mode == quickest) {
ptrToNew->setSpeedThruPeds(_building->GetAllPedestrians().data(), _building->GetAllPedestrians().size(), _mode, 0.5);
}
//SetDistances
vector<int> doorUIDs;
doorUIDs.clear();
......@@ -498,11 +510,11 @@ bool FFRouter::ReInit()
// are not adjacent.
std::pair<int, int> key_ij = std::make_pair(*outerPtr, *innerPtr);
std::pair<int, int> key_ji = std::make_pair(*innerPtr, *outerPtr);
if (tmpdistMatrix.count(key_ij) > 0) {
if ((!(_mode == quickest)) && (tmpdistMatrix.count(key_ij) > 0)) { //only take old value if ffields do not change during sim (quickest do change)
tempDistance = tmpdistMatrix.at(key_ij);
} else {
tempDistance = ptrToNew->getCostToDestination(*outerPtr,
_CroTrByUID.at(*innerPtr)->GetCentre());
_CroTrByUID.at(*innerPtr)->GetCentre(), _mode);
}
// Point endA = _CroTrByUID.at(*innerPtr)->GetCentre() * .9 +
// _CroTrByUID.at(*innerPtr)->GetPoint1() * .1;
......@@ -538,7 +550,8 @@ bool FFRouter::ReInit()
FloydWarshall();
//debug output in file
// _locffviafm[4]->writeFF("ffTreppe.vtk", _allDoorUIDs);
std::string ffname = "MasterFF" + std::to_string(++cnt) + ".vtk";
//_locffviafm[0]->writeFF(ffname, _allDoorUIDs);
//int roomTest = (*(_locffviafm.begin())).first;
//int transTest = (building->GetRoom(roomTest)->GetAllTransitionsIDs())[0];
......@@ -632,12 +645,14 @@ int FFRouter::FindExit(Pedestrian* p)
// }
// }
if (_mode == quickest) {
p->_ticksInThisRoom += 1;
if (p->GetReroutingTime() > 0.) {
p->UpdateReroutingTime();
return p->GetExitIndex();
} else {
p->RerouteIn(5.);
//new version: recalc densityspeed every x seconds
if (p->GetGlobalTime() > timeToRecalc) {
timeToRecalc += _recalc_interval; //@todo: ar.graf: change "5" to value of config file/classmember
// for (auto localfield : _locffviafm) { //@todo: ar.graf: create a list of local ped-ptr instead of giving all peds-ptr
// localfield.second->setSpeedThruPeds(_building->GetAllPedestrians().data(), _building->GetAllPedestrians().size(), _mode, 1.);
// localfield.second->deleteAllFFs();
// }
ReInit();
}
}
double minDist = DBL_MAX;
......@@ -703,7 +718,7 @@ int FFRouter::FindExit(Pedestrian* p)
for(int finalDoor : validFinalDoor) {
//with UIDs, we can ask for shortest path
for (int doorUID : DoorUIDsOfRoom) {
double locDistToDoor = _locffviafm[p->GetRoomID()]->getCostToDestination(doorUID, p->GetPos());
double locDistToDoor = _locffviafm[p->GetRoomID()]->getCostToDestination(doorUID, p->GetPos(), _mode);
if (locDistToDoor < -J_EPS) { //this can happen, if the point is not reachable and therefore has init val -7
continue;
}
......@@ -714,15 +729,16 @@ int FFRouter::FindExit(Pedestrian* p)
(finalDoor != doorUID)){
continue;
}
if (_mode == quickest) {
int locDistToDoorAdd = (_CroTrByUID[doorUID]->_lastTickTime2 > _CroTrByUID[doorUID]->_lastTickTime1)?_CroTrByUID[doorUID]->_lastTickTime2:_CroTrByUID[doorUID]->_lastTickTime1;
locDistToDoor = (locDistToDoor + locDistToDoorAdd * p->Getdt() * p->GetEllipse().GetV0())/2;
}
// if (_mode == quickest) {
// int locDistToDoorAdd = (_CroTrByUID[doorUID]->_lastTickTime2 > _CroTrByUID[doorUID]->_lastTickTime1)?_CroTrByUID[doorUID]->_lastTickTime2:_CroTrByUID[doorUID]->_lastTickTime1;
// locDistToDoor = (locDistToDoor + locDistToDoorAdd * p->Getdt() * p->GetEllipse().GetV0())/2;
// }
if ((_distMatrix.count(key)!=0) && (_distMatrix.at(key) != DBL_MAX)) {
if ( (_mode == local_shortest) &&
(std::find(_localShortestSafedPeds.begin(), _localShortestSafedPeds.end(), p->GetID()) == _localShortestSafedPeds.end()) ) {
locDistToDoor -= _distMatrix.at(key); // -x +x == +0, therefore only locDist is considered
}
//local_shortest comment: this version does not work. it is NOT checked, if a route exists without entering the smoked room again
// if ( (_mode == local_shortest) &&
// (std::find(_localShortestSafedPeds.begin(), _localShortestSafedPeds.end(), p->GetID()) == _localShortestSafedPeds.end()) ) {
// locDistToDoor -= _distMatrix.at(key); // -x +x == +0, therefore only locDist is considered
// }
if ((_distMatrix.at(key) + locDistToDoor) < minDist) {
minDist = _distMatrix.at(key) + locDistToDoor;
bestDoor = key.first; //doorUID
......
......@@ -111,7 +111,7 @@ public:
*
*/
FFRouter();
FFRouter(int id, RoutingStrategy s, bool hasSpecificGoals);
FFRouter(int id, RoutingStrategy s, bool hasSpecificGoals, Configuration* config);
//FFRouter(const Building* const);
/**
......@@ -202,6 +202,7 @@ public:
private:
protected:
Configuration* _config;
std::map< std::pair<int, int> , double > _distMatrix;
std::map< std::pair<int, int> , int > _pathsMatrix;
std::vector<int> _allDoorUIDs;
......@@ -219,8 +220,12 @@ protected:
std::map<int, int> _finalDoors; // _finalDoors[i] the UID of the last door the pedestrian with ID i wants to walk through
int _mode;
double timeToRecalc = 0.;
double _recalc_interval;
bool _hasSpecificGoals;
bool _targetWithinSubroom;
//output filename counter: cnt
static int cnt;
};
#endif /* FFROUTER_H_ */
\ No newline at end of file
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