From 13509c03a5b078e79f8460f0eb8ff4ab2e5e2c2f Mon Sep 17 00:00:00 2001
From: Arne Graf <ar.graf.cst@gmail.com>
Date: Thu, 3 May 2018 18:18:20 +0200
Subject: [PATCH] directional Escalators implemented; stopping on line bug: WIP
---
IO/GeoFileParser.cpp | 41 ++++++++++++++---
IO/GeoFileParser.h | 2 +-
IO/IniFileParser.cpp | 6 +++
general/Configuration.h | 17 +++++++
geometry/SubRoom.cpp | 63 ++++++++++++++++++++++++++
geometry/SubRoom.h | 24 ++++++++++
math/VelocityModel.cpp | 5 +++
pedestrian/Pedestrian.cpp | 2 +-
routing/DirectionStrategy.cpp | 10 +++++
routing/DirectionStrategy.h | 3 ++
routing/ff_router/ffRouter.cpp | 82 +++++++++++++++++++++++++++++++++-
routing/ff_router/ffRouter.h | 2 +
12 files changed, 248 insertions(+), 9 deletions(-)
diff --git a/IO/GeoFileParser.cpp b/IO/GeoFileParser.cpp
index 6d7350a5..6bf315cd 100644
--- a/IO/GeoFileParser.cpp
+++ b/IO/GeoFileParser.cpp
@@ -132,8 +132,8 @@ bool GeoFileParser::LoadGeometry(Building* building)
SubRoom* subroom = nullptr;
if (type=="stair" || type=="escalator" || type=="idle_escalator") {
- if (xSubRoom->FirstChildElement("up")==NULL) {
- Log->Write("ERROR:\t the attribute <up> and <down> are missing for the "+type);
+ if (xSubRoom->FirstChildElement("up") == NULL) {
+ Log->Write("ERROR:\t the attribute <up> and <down> are missing for the " + type);
Log->Write("ERROR:\t check your geometry file");
return false;
}
@@ -142,10 +142,39 @@ bool GeoFileParser::LoadGeometry(Building* building)
double down_x = xmltof(xSubRoom->FirstChildElement("down")->Attribute("py"), 0.0);
double down_y = xmltof(xSubRoom->FirstChildElement("down")->Attribute("py"), 0.0);
subroom = new Stair();
- ((Stair*) subroom)->SetUp(Point(up_x, up_y));
- ((Stair*) subroom)->SetDown(Point(down_x, down_y));
- }
- else {
+ ((Stair *) subroom)->SetUp(Point(up_x, up_y));
+ ((Stair *) subroom)->SetDown(Point(down_x, down_y));
+ } else if (type =="escalator_up") {
+ if (xSubRoom->FirstChildElement("up") == NULL) {
+ Log->Write("ERROR:\t the attribute <up> and <down> are missing for the " + type);
+ Log->Write("ERROR:\t check your geometry file");
+ return false;
+ }
+ double up_x = xmltof(xSubRoom->FirstChildElement("up")->Attribute("px"), 0.0);
+ double up_y = xmltof(xSubRoom->FirstChildElement("up")->Attribute("py"), 0.0);
+ double down_x = xmltof(xSubRoom->FirstChildElement("down")->Attribute("py"), 0.0);
+ double down_y = xmltof(xSubRoom->FirstChildElement("down")->Attribute("py"), 0.0);
+ subroom = new Escalator();
+ ((Escalator *) subroom)->SetUp(Point(up_x, up_y));
+ ((Escalator *) subroom)->SetDown(Point(down_x, down_y));
+ ((Escalator*) subroom)->SetEscalatorUp();
+ _configuration->set_has_directional_escalators(true);
+ } else if (type == "escalator_down") {
+ if (xSubRoom->FirstChildElement("up") == NULL) {
+ Log->Write("ERROR:\t the attribute <up> and <down> are missing for the " + type);
+ Log->Write("ERROR:\t check your geometry file");
+ return false;
+ }
+ double up_x = xmltof(xSubRoom->FirstChildElement("up")->Attribute("px"), 0.0);
+ double up_y = xmltof(xSubRoom->FirstChildElement("up")->Attribute("py"), 0.0);
+ double down_x = xmltof(xSubRoom->FirstChildElement("down")->Attribute("py"), 0.0);
+ double down_y = xmltof(xSubRoom->FirstChildElement("down")->Attribute("py"), 0.0);
+ subroom = new Escalator();
+ ((Escalator *) subroom)->SetUp(Point(up_x, up_y));
+ ((Escalator *) subroom)->SetDown(Point(down_x, down_y));
+ ((Escalator*) subroom)->SetEscalatorDown();
+ _configuration->set_has_directional_escalators(true);
+ } else {
//normal subroom or corridor
subroom = new NormalSubRoom();
}
diff --git a/IO/GeoFileParser.h b/IO/GeoFileParser.h
index 83029e78..88329a54 100644
--- a/IO/GeoFileParser.h
+++ b/IO/GeoFileParser.h
@@ -40,7 +40,7 @@ public:
virtual bool LoadTrafficInfo(Building* building) override;
private:
- const Configuration* _configuration;
+ Configuration* _configuration;
bool LoadGeometry(Building* building);
diff --git a/IO/IniFileParser.cpp b/IO/IniFileParser.cpp
index 6fe93041..a611f8f3 100644
--- a/IO/IniFileParser.cpp
+++ b/IO/IniFileParser.cpp
@@ -1500,18 +1500,24 @@ bool IniFileParser::ParseStrategyNodeToObject(const TiXmlNode& strategyNode)
pExitStrategy = 9;
_exit_strat_number = 9;
_exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionSubLocalFloorfield());
+ if(!ParseFfOpts(strategyNode)) {
+ return false;
+ };
+ _config->set_dirSubLocal(dynamic_cast<DirectionSubLocalFloorfield*>(_exit_strategy.get()));
break;
case 8:
_exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionLocalFloorfield());
if(!ParseFfOpts(strategyNode)) {
return false;
};
+ _config->set_dirLocal(dynamic_cast<DirectionLocalFloorfield*>(_exit_strategy.get()));
break;
case 9:
_exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionSubLocalFloorfield());
if(!ParseFfOpts(strategyNode)) {
return false;
};
+ _config->set_dirSubLocal(dynamic_cast<DirectionSubLocalFloorfield*>(_exit_strategy.get()));
break;
default:
_exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionMinSeperationShorterLine());
diff --git a/general/Configuration.h b/general/Configuration.h
index 88185ae3..476f86f7 100644
--- a/general/Configuration.h
+++ b/general/Configuration.h
@@ -44,6 +44,8 @@
//This class provides a data container for all configuration parameters.
class AgentsParameters;
+class DirectionSubLocalFloorfield;
+class DirectionLocalFloorfield;
#ifdef _JPS_AS_A_SERVICE
@@ -117,9 +119,12 @@ public:
//ff router
_has_specific_goals = false;
+ _has_directional_escalators = false;
_write_VTK_files = false;
_exit_strat = 9;
_write_VTK_files_direction = false;
+ _dirSubLocal = nullptr;
+ _dirLocal = nullptr;
//for random numbers
_rdGenerator=RandomNumberGenerator();
@@ -270,6 +275,9 @@ public:
void set_has_specific_goals(bool has_specific_goals) { _has_specific_goals = has_specific_goals;}
+ bool get_has_directional_escalators() const { return _has_directional_escalators;}
+ void set_has_directional_escalators(bool has_directional_esc) {_has_directional_escalators = has_directional_esc;}
+
void set_write_VTK_files(bool write_VTK_files) {_write_VTK_files = write_VTK_files;}
bool get_write_VTK_files() const {return _write_VTK_files;}
@@ -278,6 +286,12 @@ public:
int get_exit_strat() const {return _exit_strat;}
+ void set_dirSubLocal(DirectionSubLocalFloorfield* dir) {_dirSubLocal = dir;}
+ void set_dirLocal(DirectionLocalFloorfield* dir) {_dirLocal = dir;}
+
+ DirectionSubLocalFloorfield* get_dirSubLocal() const {return _dirSubLocal;}
+ DirectionLocalFloorfield* get_dirLocal() const {return _dirLocal;}
+
const std::string& GetHostname() const { return _hostname; };
void set_write_VTK_files_direction(bool write_VTK_files_direction) {_write_VTK_files_direction = write_VTK_files_direction;}
@@ -391,10 +405,13 @@ private:
//ff router
bool _has_specific_goals;
+ bool _has_directional_escalators;
bool _write_VTK_files;
bool _write_VTK_files_direction;
int _exit_strat;
+ DirectionSubLocalFloorfield* _dirSubLocal;
+ DirectionLocalFloorfield* _dirLocal;
std::string _hostname;
std::string _trajectoriesFile;
diff --git a/geometry/SubRoom.cpp b/geometry/SubRoom.cpp
index ede9a6c8..f47d08cb 100644
--- a/geometry/SubRoom.cpp
+++ b/geometry/SubRoom.cpp
@@ -79,6 +79,33 @@ SubRoom::SubRoom()
_boostPoly = polygon_type();
}
+SubRoom::SubRoom(const SubRoom& orig) {
+ _id = orig._id;
+ _roomID=orig._roomID;
+ _walls = orig._walls;
+ _poly = orig._poly;
+ _poly_help_constatnt = orig._poly_help_constatnt;
+ _poly_help_multiple = orig._poly_help_multiple;
+ _obstacles=orig._obstacles;
+
+ _crossings = orig._crossings;
+ _transitions = orig._transitions;
+ _hlines = orig._hlines;
+
+ _planeEquation[0]=orig._planeEquation[0];
+ _planeEquation[1]=orig._planeEquation[1];
+ _planeEquation[2]=orig._planeEquation[2];
+ _cosAngleWithHorizontalPlane=orig._cosAngleWithHorizontalPlane;
+ _tanAngleWithHorizontalPlane=orig._tanAngleWithHorizontalPlane;
+ _minElevation=orig._minElevation;
+ _maxElevation=orig._maxElevation;
+
+ _goalIDs = orig._goalIDs;
+ _area = orig._area;
+ _uid = orig._uid;
+ _boostPoly = orig._boostPoly;
+}
+
SubRoom::~SubRoom()
{
for (unsigned int i = 0; i < _obstacles.size(); i++)
@@ -200,6 +227,7 @@ void SubRoom::AddObstacle(Obstacle* obs)
void SubRoom::AddGoalID(int ID)
{
+ Log->Write("ERROR:\t Adding a Goal directly to a subroom is not allowed. Plz add Crossing/Transition instead!");
if (std::find(_goalIDs.begin(), _goalIDs.end(), ID) != _goalIDs.end()) {
Log->Write("WARNING: \tAdded existing GoalID to Subroom %d", this->GetSubRoomID());
//if occurs, plz assert, that ID is a UID of any line of the goal and not a number given by the user (ar.graf)
@@ -833,6 +861,7 @@ NormalSubRoom::NormalSubRoom() : SubRoom()
{
}
+NormalSubRoom::NormalSubRoom(const NormalSubRoom& orig) : SubRoom(orig) {}
NormalSubRoom::~NormalSubRoom()
{
@@ -1138,6 +1167,12 @@ Stair::Stair() : NormalSubRoom()
pDown = Point();
}
+Stair::Stair(const Stair &orig) : NormalSubRoom(orig),
+pUp (orig.pUp),
+pDown (orig.pDown)
+{
+}
+
Stair::~Stair()
{
@@ -1443,5 +1478,33 @@ std::vector<Point> SubRoom::StartLLCorner(const std::vector<Point> &polygon)
}
+/// Escalator
+
+Escalator::Escalator(): Stair() {
+
+}
+
+Escalator::Escalator(const Escalator& orig) : Stair(orig), isEscalator_Up(orig.isEscalator_Up)
+{}
+
+Escalator::~Escalator() {}
+
+// Setter-Funktionen
+void Escalator::SetEscalatorUp() {
+ isEscalator_Up = true;
+}
+void Escalator::SetEscalatorDown(){
+ isEscalator_Up = false;
+}
+
+// Getter-Funktionen
+bool Escalator::IsEscalatorUp() const {
+ return isEscalator_Up;
+}
+bool Escalator::IsEscalatorDown() const{
+ return !isEscalator_Up;
+}
+
+
#endif // _SIMULATOR
diff --git a/geometry/SubRoom.h b/geometry/SubRoom.h
index f5e65907..0e2bd0a3 100644
--- a/geometry/SubRoom.h
+++ b/geometry/SubRoom.h
@@ -111,6 +111,7 @@ public:
* Constructor
*/
SubRoom();
+ SubRoom(const SubRoom& orig);
/**
* Destructor
@@ -451,5 +452,28 @@ public:
virtual bool ConvertLineToPoly(const std::vector<Line*>& goals);
};
+/************************************************************
+ Escalator
+************************************************************/
+
+class Escalator : public Stair {
+private:
+ bool isEscalator_Up;
+
+public:
+ Escalator();
+ Escalator(const Escalator& orig);
+ virtual ~Escalator();
+
+ // Setter-Funktionen
+ void SetEscalatorUp();
+ void SetEscalatorDown();
+
+ // Getter-Funktionen
+ bool IsEscalatorUp() const;
+ bool IsEscalatorDown() const;
+
+};
+
#endif /* _SUBROOM_H */
diff --git a/math/VelocityModel.cpp b/math/VelocityModel.cpp
index eccca319..a96cc841 100644
--- a/math/VelocityModel.cpp
+++ b/math/VelocityModel.cpp
@@ -312,6 +312,7 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const
if (desired_direction.NormSquare() < 0.25) {
desired_direction = lastE0;
ped->SetLastE0(lastE0);
+ Log->Write("%f %f", desired_direction._x, desired_direction._y);
}
// if (dist > 1*J_EPS_GOAL) {
// desired_direction = target - pos; //ped->GetV0(target);
@@ -325,6 +326,10 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const
ped->SetSmoothTurning();
desired_direction = ped->GetV0();
}
+ //Log->Write("%f %f", desired_direction._x, desired_direction._y);
+ if (desired_direction.NormSquare() < 0.1) {
+ Log->Write("ERROR:\t desired_direction in VelocityModel::e0 is too small.");
+ }
return desired_direction;
}
diff --git a/pedestrian/Pedestrian.cpp b/pedestrian/Pedestrian.cpp
index 0fbcf1e3..1af2a5fb 100644
--- a/pedestrian/Pedestrian.cpp
+++ b/pedestrian/Pedestrian.cpp
@@ -230,7 +230,7 @@ void Pedestrian::SetTau(double tau)
void Pedestrian::SetT(double T)
{
- _tau = T;
+ _T = T;
}
diff --git a/routing/DirectionStrategy.cpp b/routing/DirectionStrategy.cpp
index 41650103..011a5a8c 100644
--- a/routing/DirectionStrategy.cpp
+++ b/routing/DirectionStrategy.cpp
@@ -381,6 +381,11 @@ double DirectionLocalFloorfield::GetDistance2Wall(Pedestrian* ped) const
return _locffviafm.at(ped->GetRoomID())->getDistance2WallAt(ped->GetPos());
}
+double DirectionLocalFloorfield::GetDistance2Target(Pedestrian* ped, int UID) {
+ int roomID = ped->GetRoomID();
+ return _locffviafm.at(roomID)->getCostToDestination(UID, ped->GetPos());
+}
+
void DirectionLocalFloorfield::Init(Building* buildingArg, double stepsize,
double threshold, bool useDistanceMap) {
_stepsize = stepsize;
@@ -477,6 +482,11 @@ double DirectionSubLocalFloorfield::GetDistance2Wall(Pedestrian* ped) const
return _locffviafm.at(ped->GetSubRoomUID())->getDistance2WallAt(ped->GetPos());
}
+double DirectionSubLocalFloorfield::GetDistance2Target(Pedestrian* ped, int UID) {
+ int subroomUID = ped->GetSubRoomUID();
+ return _locffviafm.at(subroomUID)->getCostToDestination(UID, ped->GetPos());
+}
+
void DirectionSubLocalFloorfield::Init(Building* buildingArg, double stepsize,
double threshold, bool useDistanceMap) {
_stepsize = stepsize;
diff --git a/routing/DirectionStrategy.h b/routing/DirectionStrategy.h
index 8f20cd07..219b7157 100644
--- a/routing/DirectionStrategy.h
+++ b/routing/DirectionStrategy.h
@@ -87,6 +87,7 @@ public:
virtual Point GetDir2Wall(Pedestrian* ped) const;
virtual double GetDistance2Wall(Pedestrian* ped) const;
+
private:
FloorfieldViaFM* _ffviafm;
bool _initDone;
@@ -101,6 +102,7 @@ public:
virtual Point GetTarget(Room* room, Pedestrian* ped) const;
virtual Point GetDir2Wall(Pedestrian* ped) const;
virtual double GetDistance2Wall(Pedestrian* ped) const;
+ virtual double GetDistance2Target(Pedestrian* ped, int UID);
protected:
std::map<int, UnivFFviaFM*> _locffviafm;
@@ -121,6 +123,7 @@ public:
virtual Point GetTarget(Room* room, Pedestrian* ped) const;
virtual Point GetDir2Wall(Pedestrian* ped) const;
virtual double GetDistance2Wall(Pedestrian* ped) const;
+ virtual double GetDistance2Target(Pedestrian* ped, int UID);
protected:
std::map<int, UnivFFviaFM*> _locffviafm;
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
index c85253cf..9190bd1e 100644
--- a/routing/ff_router/ffRouter.cpp
+++ b/routing/ff_router/ffRouter.cpp
@@ -141,6 +141,7 @@ bool FFRouter::Init(Building* building)
}
}
//make unique
+ std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end());
_allDoorUIDs.erase( std::unique(_allDoorUIDs.begin(),_allDoorUIDs.end()), _allDoorUIDs.end());
//cleanse maps
@@ -248,6 +249,40 @@ bool FFRouter::Init(Building* building)
} // otherDoor
} // roomAndCroTrVector
+ if (_config->get_has_directional_escalators()) {
+ _directionalEscalatorsUID.clear();
+ _penaltyList.clear();
+ for (auto room : building->GetAllRooms()) {
+ for (auto subroom : room.second->GetAllSubRooms()) {
+ if ((subroom.second->GetType() == "escalator_up") || (subroom.second->GetType() == "escalator_down")) {
+ _directionalEscalatorsUID.emplace_back(subroom.second->GetUID());
+ }
+ }
+ }
+ for (int subUID : _directionalEscalatorsUID) {
+ Escalator* escalator = (Escalator*) building->GetSubRoomByUID(subUID);
+ std::vector<int> lineUIDs = escalator->GetAllGoalIDs();
+ assert(lineUIDs.size() == 2);
+ if (escalator->IsEscalatorUp()) {
+ if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
+ } else {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
+ }
+ } else { //IsEscalatorDown
+ if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
+ } else {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
+ }
+ }
+ }
+ for (auto key : _penaltyList) {
+ _distMatrix.erase(key);
+ _distMatrix.insert(std::make_pair(key, DBL_MAX));
+ }
+ }
+
FloydWarshall();
//debug output in file
@@ -340,6 +375,41 @@ bool FFRouter::ReInit()
} //secondDoor(s)
} //firstDoor(s)
} //allRooms
+
+ if (_config->get_has_directional_escalators()) {
+ _directionalEscalatorsUID.clear();
+ _penaltyList.clear();
+ for (auto room : _building->GetAllRooms()) {
+ for (auto subroom : room.second->GetAllSubRooms()) {
+ if ((subroom.second->GetType() == "escalator_up") || (subroom.second->GetType() == "escalator_down")) {
+ _directionalEscalatorsUID.emplace_back(subroom.second->GetUID());
+ }
+ }
+ }
+ for (int subUID : _directionalEscalatorsUID) {
+ Escalator* escalator = (Escalator*) _building->GetSubRoomByUID(subUID);
+ std::vector<int> lineUIDs = escalator->GetAllGoalIDs();
+ assert(lineUIDs.size() == 2);
+ if (escalator->IsEscalatorUp()) {
+ if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
+ } else {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
+ }
+ } else { //IsEscalatorDown
+ if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
+ } else {
+ _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
+ }
+ }
+ }
+ for (auto key : _penaltyList) {
+ _distMatrix.erase(key);
+ _distMatrix.insert(std::make_pair(key, DBL_MAX));
+ }
+ }
+
FloydWarshall();
_plzReInit = false;
return true;
@@ -456,7 +526,14 @@ 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(), _mode);
+ //double locDistToDoor = _locffviafm[p->GetRoomID()]->getCostToDestination(doorUID, p->GetPos(), _mode);
+ double locDistToDoor = 0.;
+ if (_targetWithinSubroom) {
+ locDistToDoor = _config->get_dirSubLocal()->GetDistance2Target(p, doorUID);
+ } else {
+ locDistToDoor = _config->get_dirLocal()->GetDistance2Target(p, doorUID);
+ }
+
if (locDistToDoor < -J_EPS) { //for old ff: //this can happen, if the point is not reachable and therefore has init val -7
continue;
}
@@ -472,6 +549,9 @@ int FFRouter::FindExit(Pedestrian* p)
if ((_distMatrix.at(key) + locDistToDoor) < minDist) {
minDist = _distMatrix.at(key) + locDistToDoor;
bestDoor = key.first; //doorUID
+ if (locDistToDoor == 0.) {
+ bestDoor = _pathsMatrix[key]; //@todo: @ar.graf: check this hack
+ }
bestFinalDoor = key.second;
}
}
diff --git a/routing/ff_router/ffRouter.h b/routing/ff_router/ffRouter.h
index e2ac697f..4da43c2f 100644
--- a/routing/ff_router/ffRouter.h
+++ b/routing/ff_router/ffRouter.h
@@ -192,6 +192,8 @@ protected:
//std::map< std::pair<int, int> , SubRoom* > _subroomMatrix;
std::vector<int> _allDoorUIDs;
std::vector<int> _localShortestSafedPeds;
+ std::vector<int> _directionalEscalatorsUID;
+ std::vector<std::pair<int, int>> _penaltyList;
const Building* _building;
std::map<int, UnivFFviaFM*> _locffviafm; // the actual type might be CentrePointLocalFFViaFM
FloorfieldViaFM* _globalFF;
--
GitLab