Commit 3d913f77 authored by tobias schroedter's avatar tobias schroedter

FF now navigates ped to subroom, which contains a waiting area

parent fbef8318
Pipeline #16429 failed with stages
in 11 seconds
......@@ -363,6 +363,7 @@ bool Building::InitGeometry()
if (s2) s2->AddNeighbor(s1);
}
Log->Write("INFO: \tInit Geometry successful!!!\n");
return true;
......
This diff is collapsed.
/**
* \file FloorfieldViaFMTrips.h
* \date Mar 05, 2015
* \version N/A (v0.6)
* \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
*
* JuPedSim is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* JuPedSim is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
*
* \section Description
* Implementation of classes for ...
*
*
**/
//remark:
//refac the code to use enums instead of integer values where integer values code sth
//was considered, but enum classes do not implicitly cast to int
//rather use makros/masks like in plain C? or just makros (defines)?
//this would make it easier to read
#ifndef FloorfieldViaFMTrips_H
#define FloorfieldViaFMTrips_H
#include <vector>
#include <unordered_set>
#include <cmath>
#include <functional>
#include "mesh/RectGrid.h"
#include "../../geometry/Wall.h"
#include "../../geometry/Point.h"
#include "../../geometry/Building.h"
#include "../../geometry/SubRoom.h" //check: should Room.h include SubRoom.h??
#include "./mesh/Trial.h"
#include "../../pedestrian/Pedestrian.h"
//maybe put following in macros.h
#define LOWSPEED 0.001
class TrialPTrips
{
public:
long int key;
int* flag;
double* cost;
double* speed;
Point* neggrad;
TrialPTrips() {
key = 0;
flag = nullptr;
cost = nullptr;
speed = nullptr;
neggrad = nullptr;
}
TrialPTrips(long int keyArg, double* t, double* f, int* flagArg, Point* neggradArg) {
key = keyArg;
cost = t;
speed = f;
flag = flagArg;
neggrad = neggradArg;
}
~TrialPTrips(){}
bool operator <(const TrialPTrips& rhs) const
{
return this->cost[this->key] < rhs.cost[rhs.key];
}
bool operator >(const TrialPTrips& rhs) const
{
return this->cost[this->key] > rhs.cost[rhs.key];
}
bool operator ==(const TrialPTrips& rhs) const
{
return this->cost[this->key] == rhs.cost[rhs.key];
}
};
class FloorfieldViaFMTrips
{
public:
FloorfieldViaFMTrips();
FloorfieldViaFMTrips(const std::string&);
FloorfieldViaFMTrips(const Building* const buildingArg, const double hxArg, const double hyArg,
const double wallAvoidDistance, const bool useDistancefield, const bool onlyRoomsWithExits);
//FloorfieldViaFMTrips(const FloorfieldViaFMTrips* const refFM);
virtual ~FloorfieldViaFMTrips();
FloorfieldViaFMTrips(const FloorfieldViaFMTrips& other); //will not make a copy; only takes geometry info
//FloorfieldViaFMTrips& operator=(const FloorfieldViaFMTrips& other);
//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); // shall not be used any more, therefore not virtual
virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
//void getDirectionToUIDParallel(int destID, const long int key, Point& direction);
virtual void getDirectionToDestination (Pedestrian* ped, Point& direction);
//void getDirectionToFinalDestination(Pedestrian* ped, Point& direction); //this is router buissness! problem in multi-storage buildings
void createMapEntryInLineToGoalID(const int goalID, bool isInside);
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);
double getDistance2WallAt(const Point& position);
int getSubroomUIDAt(const Point& position);
void parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
void parseBuildingForExits(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
void prepareForDistanceFieldCalculation(const bool withExits);
template <typename T>
void drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value);
void setSpeed(bool useDistance2WallArg);
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
void calculateFloorfield(std::vector<Line>& wallArg, double* costarray, Point* neggradarray, double* speedarray);
void calculateDistanceField(const double thresholdArg); //make private
void checkNeighborsAndAddToNarrowband(std::priority_queue<TrialPTrips, std::vector<TrialPTrips>, std::greater<TrialPTrips> >& trialfield, TrialPTrips key,
std::function<void (TrialPTrips)> calc);
void calcDist2Wall(TrialPTrips);
void calcFloorfield(TrialPTrips);
//void (*checkNeighborsAndCalc)(const long int key);
inline double onesidedCalc(double xy, double hDivF);
inline double twosidedCalc(double x, double y, double hDivF);
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 SubRoom* isInside(const long int key);
SubRoom* GetSubroom(Pedestrian* p);
std::map<int, int> getGoalToLineUIDmap() const
{
return _goalToLineUIDmap;
}
std::map<int, int> getGoalToLineUIDmap2() const
{
return _goalToLineUIDmap2;
}
std::map<int, int> getGoalToLineUIDmap3() const
{
return _goalToLineUIDmap3;
}
RectGrid* getGrid() const
{
return _grid;
}
#ifdef TESTING
void setGrid(RectGrid* gridArg) {_grid = gridArg;}
#endif //TESTING
protected:
RectGrid* _grid = nullptr;
std::vector<Line> _wall;
std::vector<Line> _exitsFromScope;
unsigned int _numOfExits;
const Building* _building;
//GridPoint Data in independant arrays (shared primary key)
// changed to threadsafe creation when needed: int* flag;
int* _gcode = nullptr; //gridcode (see Macros.h)
SubRoom* * _subrooms = nullptr; // this is an array (first asterisk) of pointers (second asterisk)
double* _dist2Wall = nullptr;
double* _speedInitial = nullptr;
double* _modifiedspeed = nullptr;
double* _densityspeed = nullptr;
double* _cost = nullptr;
//long int* secKey; //secondary key to address ... not used yet
Point* _neggrad = nullptr; //gradients
Point* _dirToWall = nullptr;
// changed to threadsafe creation when needed: Trial* trialfield;
std::map<int, double*> _goalcostmap;
std::map<int, int> _goalToLineUIDmap; //key is the goalID and value is the UID of closest transition -> it maps goal to LineUID
std::map<int, int> _goalToLineUIDmap2;
std::map<int, int> _goalToLineUIDmap3;
std::map<int, Point*> _goalneggradmap;
std::map<int, double*> _costmap;
std::map<int, Point*> _neggradmap;
// use an unordered_set for faster access (it is accessed within a critical region)
std::unordered_set<int> _floorfieldsBeingCalculated;
bool maps_deleted = false; // @todo f.mack remove
double _threshold;
bool _useDistanceToWall;
};
// very similar to FloorfieldViaFMTrips, but the calculation of floorfields starts at the center of the door only, not on the whole line
// this happens in order to avoid "door hopping" (for a pedestrian, it is shorter to go to a nearby door and skip half the door width)
class CentrePointFFViaFMTrips : public virtual FloorfieldViaFMTrips {
public:
virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
};
#endif // FloorfieldViaFMTrips_H
This diff is collapsed.
//
// Created by arne on 5/9/17.
//
/**
* \file UnivFFviaFMTrips.h
* \date May 09, 2017
* \version N/A (v0.8.x)
* \copyright <2017-2020> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
*
* JuPedSim is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* JuPedSim is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
*
* \section Description
* Implementation of classes for a reworked floorfield. A floorfield in general
* yields a cost field to a specific goal and a correlated vectorfield for the
* optimal path in terms of the cost value.
*
* Rework focused on a cleaner structure and less inheritance (no diamond) and
* less workarounds.
*
*
**/
#ifndef JPSCORE_UnivFFviaFMTrips_H
#define JPSCORE_UnivFFviaFMTrips_H
#include <string>
#include <vector>
#include <map>
#include <float.h>
#include "../../general/Macros.h"
class Pedestrian;
class Room;
class SubRoom;
class Building;
class Configuration;
class Point;
class RectGrid;
class Line;
class Goal;
class CompareCost { //this class is used in std::priority_queue in UnivFFviaFMTrips::calcFF
public:
CompareCost(double* costarray) : _costarray(costarray) {}
bool operator() (const int a, const int b) const {
return _costarray[a] > _costarray[b];
}
private:
double* _costarray = nullptr;
};
class UnivFFviaFMTrips {
public:
UnivFFviaFMTrips(Room* a, Building* b, double c, double e, bool f);
UnivFFviaFMTrips(SubRoom* a, Building* b, double c, double e, bool f);
UnivFFviaFMTrips(Room* a, Configuration* const b, double hx, double wallAvoid, bool useWallDistances, std::map<int, Goal*> goals);
UnivFFviaFMTrips(Room* a, Configuration* const b, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors, std::map<int, Goal*> goals);
UnivFFviaFMTrips(SubRoom* sr, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances, std::map<int, Goal*> goals);
UnivFFviaFMTrips(SubRoom* subRoomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors, std::map<int, Goal*> goals);
void create(std::vector<Line>& walls, std::map<int, Line>& doors, std::vector<int> targetUIDs, int mode,
double spacing, double wallAvoidDist, bool useWallDistances);
void recreateAllForQuickest();
UnivFFviaFMTrips() {};
UnivFFviaFMTrips(UnivFFviaFMTrips&){};
virtual ~UnivFFviaFMTrips();
void addTarget(const int uid, Line* door, double* costarray = nullptr, Point* gradarray = nullptr);
void addTarget(const int uid, double* costarray = nullptr, Point* gradarray = nullptr);
void addAllTargets();
void addAllTargetsParallel();
void addTargetsParallel(std::vector<int> wantedDoors);
std::vector<int> getKnownDoorUIDs();
void setUser(int userArg);
void setMode(int modeArg);
void setSpeedMode(int speedModeArg);
SubRoom** getSubRoomFF();
SubRoom* getSubRoom(const Point& pos);
double getCostToDestination(const int destID, const Point& position, int mode);
double getCostToDestination(const int destID, const Point& position);
double getDistanceBetweenDoors(const int door1_ID, const int door2_ID);
RectGrid* getGrid();
virtual void getDirectionToUID(int destID, long int key, Point& direction, int mode);
void getDirectionToUID(int destID, long int key, Point& direction);
virtual void getDirectionToUID(int destID, const Point& pos, Point& direction, int mode);
void getDirectionToUID(int destID, const Point& pos, Point& direction);
double getDistance2WallAt(const Point& pos);
void getDir2WallAt(const Point& pos, Point& p);
void writeFF(const std::string&, std::vector<int> targetID);
void createRectGrid(std::vector<Line>& walls, std::map<int, Line>& doors, double spacing);
void processGeometry(std::vector<Line>&walls, std::map<int, Line>& doors);
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>
void drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value);
template <typename T>
void drawLinesOnGrid(Line& line, T* const target, const T value);
template <typename T>
void drawLinesOnWall(std::vector<Line>& wallArg, T* const target, const T value);
template <typename T>
void drawLinesOnWall(Line& line, T* const target, const T value);
void calcFF(double*, Point*, const double* const);
void calcCost(const long int key, double* cost, Point* dir, const double* const speed);
void calcDF(double*, Point*, const double* const);
void calcDist(const long int key, double* cost, Point* dir, const double* const speed);
inline double onesidedCalc(double xy, double hDivF);
inline double twosidedCalc(double x, double y, double hDivF);
private:
Building* _building = nullptr;
Configuration* _configuration = nullptr;
int _room = -1; //not set
int _mode = LINESEGMENT; //default
int _user = DISTANCE_AND_DIRECTIONS_USED; //default
int _speedmode = FF_HOMO_SPEED; //default
int _scope = 0; //not set / unknown
bool _directCalculation = true;
RectGrid* _grid = nullptr;
long int _nPoints = 0;
std::vector<double*> _speedFieldSelector;
int* _gridCode = nullptr;
SubRoom* * _subrooms = nullptr; // this is an array (first asterisk) of pointers (second asterisk)
double _wallAvoidDistance = 0.;
bool _useWallDistances = false; //could be used in DirectionStrategy even if mode _speedmode is FF_HOMO_SPEED
//the following maps are responsible for dealloc the arrays
std::map<int, double*> _costFieldWithKey;
std::map<int, Point*> _directionFieldWithKey;
std::vector<int> _uids;
std::map<int, Line> _doors;
std::vector<int> _toDo;
std::map<int, Point> _subroomUIDtoInsidePoint;
std::map<int, SubRoom*> _subroomUIDtoSubRoomPtr;
std::map<SubRoom*, Point> _subRoomPtrTOinsidePoint;
};
#endif //JPSCORE_UnivFFviaFMTrips_H
......@@ -108,7 +108,11 @@ bool FFRouterTrips::Init(Building* building)
std::cout << std::endl;
for (auto &itrGoal : building->GetAllGoals()) {
std::cout << "Goal ID: " << itrGoal.second->GetId() << std::endl;
_globalFF->createMapEntryInLineToGoalID(itrGoal.first);
if(WaitingArea* wa = dynamic_cast<WaitingArea*>(itrGoal.second)) {
_globalFF->createMapEntryInLineToGoalID(itrGoal.first, true);
}else{
_globalFF->createMapEntryInLineToGoalID(itrGoal.first, false);
}
goalIDs.emplace_back(itrGoal.first);
}
_goalToLineUIDmap = _globalFF->getGoalToLineUIDmap();
......@@ -470,10 +474,27 @@ int FFRouterTrips::FindExit(Pedestrian* ped)
// std::cout << "ExitLine: " << ped->GetExitLine() << std::endl;
std::cout << "ExitIndex: " << ped->GetExitIndex() << std::endl << std::endl;
for (auto& goal : _goalToLineUIDmap){
std::cout << goal.first << " -> " << goal.second << std::endl;
}
SubRoom* subroom = _building->GetSubRoomByUID(ped->GetSubRoomUID());
Goal* goal = _building->GetFinalGoal(ped->GetFinalDestination());
int ret;
// Check if current position is already waiting area
// yes: set next goal and return findExit(p)
int ret = FindExit1(ped);
if (subroom->IsInSubRoom(goal->GetCentroid())){
std::cout << "Ped and Goal in same subroom: " << subroom->IsInSubRoom(goal->GetCentroid()) << std::endl;
int bestDoor = 31;
ped->SetExitIndex(bestDoor);
ped->SetExitLine(_CroTrByUID.at(bestDoor));
}else{
ret = FindExit1(ped);
}
std::cout << "Ped[" << ped->GetID() << "] in (" << ped->GetRoomID() << ", " << ped->GetSubRoomID()
<< "/" << ped->GetSubRoomUID() << "): " << std::endl;
......@@ -527,6 +548,7 @@ int FFRouterTrips::FindExit1(Pedestrian* p)
}
}
std::vector<int> DoorUIDsOfRoom;
DoorUIDsOfRoom.clear();
if (_building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->IsInSubRoom(p->GetPos())) {
......@@ -624,12 +646,12 @@ int FFRouterTrips::FindExit1(Pedestrian* p)
}
//at this point, bestDoor is either a crossing or a transition
if ((!_targetWithinSubroom) && (_CroTrByUID.count(bestDoor) != 0)) {
while (!_CroTrByUID[bestDoor]->IsTransition()) {
std::pair<int, int> key = std::make_pair(bestDoor, bestFinalDoor);
bestDoor = _pathsMatrix[key];
}
}
// if ((!_targetWithinSubroom) && (_CroTrByUID.count(bestDoor) != 0)) {
// while (!_CroTrByUID[bestDoor]->IsTransition()) {
// std::pair<int, int> key = std::make_pair(bestDoor, bestFinalDoor);
// bestDoor = _pathsMatrix[key];
// }
// }
//#pragma omp critical(finalDoors)
// _finalDoors.emplace(std::make_pair(p->GetID(), bestFinalDoor));
......
......@@ -23,6 +23,11 @@ TripsRouter::~TripsRouter()
bool TripsRouter::Init(Building* building)
{
Log->Write("TripsRouter::Init");
// Create Floorfield for each subroom
// If goal inside room, start wave from each edge to the outside
return true;
}
......@@ -36,8 +41,6 @@ int TripsRouter::FindExit(Pedestrian* p)
// Check if current position is already waiting area
// yes: set next goal and return findExit(p)
p->SetExitIndex(17);
p->SetExitLine(0);
return 17;
return 0;
}
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