Commit 13509c03 authored by Arne Graf's avatar Arne Graf

directional Escalators implemented; stopping on line bug: WIP

parent 565a7fb3
Pipeline #9309 failed with stages
in 5 minutes and 16 seconds
......@@ -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();
}
......
......@@ -40,7 +40,7 @@ public:
virtual bool LoadTrafficInfo(Building* building) override;
private:
const Configuration* _configuration;
Configuration* _configuration;
bool LoadGeometry(Building* building);
......
......@@ -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());
......
......@@ -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;
......
......@@ -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
......@@ -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 */
......@@ -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;
}
......
......@@ -230,7 +230,7 @@ void Pedestrian::SetTau(double tau)
void Pedestrian::SetT(double T)
{
_tau = T;
_T = T;
}
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
}
}
......
......@@ -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;
......
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