Commit 2dafeacb authored by Arne Graf's avatar Arne Graf

bug: VoronoiPositioning needs navlines, peds do not have em yet

parent f96dc07b
......@@ -241,16 +241,17 @@ bool Simulation::InitArgs(const ArgumentParser& args)
//perform customs initialisation, like computing the phi for the gcfm
//this should be called after the routing engine has been initialised
// because a direction is needed for this initialisation.
Log->Write("INFO:\t Init Operational Model starting ...");
if(_operationalModel->Init(_building.get())==false)
return false;
Log->Write("INFO:\t Init Operational Model done");
//other initializations
for (auto&& ped: _building->GetAllPedestrians()) {
ped->Setdt(_deltaT);
}
_nPeds = _building->GetAllPedestrians().size();
//_building->WriteToErrorLog();
Log->Write("INFO:\t nPeds recieved");
//get the seed
_seed = args.GetSeed();
......@@ -497,7 +498,7 @@ int Simulation::RunBody(double maxSimTime)
//to break the main simulation loop
ProcessAgentsQueue();
_nPeds = _building->GetAllPedestrians().size();
int initialnPeds = _nPeds;
int initialnPeds = _nPeds;
// main program loop
while ( (_nPeds || !_agentSrcManager.IsCompleted() ) && t < maxSimTime)
{
......
......@@ -352,21 +352,21 @@ bool Building::InitGeometry()
{
minElevation = itr_subroom.second->GetElevation(P1);
}
if(maxElevation < itr_subroom.second->GetElevation(P1))
{
maxElevation = itr_subroom.second->GetElevation(P1);
}
if(minElevation > itr_subroom.second->GetElevation(P2))
{
minElevation = itr_subroom.second->GetElevation(P2);
}
if(maxElevation < itr_subroom.second->GetElevation(P2))
{
maxElevation = itr_subroom.second->GetElevation(P2);
}
}
}
itr_subroom.second->SetMaxElevation(maxElevation);
itr_subroom.second->SetMinElevation(minElevation);
......@@ -839,10 +839,10 @@ Transition* Building::GetTransition(string caption) const
exit(EXIT_FAILURE);
}
Transition* Building::GetTransition(int ID)
Transition* Building::GetTransition(int ID) const //ar.graf: added const 2015-12-10
{
if (_transitions.count(ID) == 1) {
return _transitions[ID];
return _transitions.at(ID);
} else {
if (ID == -1)
return NULL;
......
......@@ -24,7 +24,7 @@
*
*
**/
#ifndef _BUILDING_H
#define _BUILDING_H
......@@ -98,7 +98,7 @@ public:
Room* GetRoom(std::string caption)const;
Transition* GetTransition(std::string caption) const;
Transition* GetTransition(int id) ;
Transition* GetTransition(int id) const;
/**
* Not implemented
......
......@@ -95,11 +95,12 @@ GradientModel::~GradientModel()
bool GradientModel::Init (Building* building)
{
Log->Write("INFO:\t Init DirectionFloorfield starting ...");
if(dynamic_cast<DirectionFloorfield*>(_direction)){
dynamic_cast<DirectionFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
//_floorfield = dynamic_cast<DirectionFloorfield*>(_direction);
}
Log->Write("INFO:\t Init DirectionFloorfield done");
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
for(unsigned int p=0;p<allPeds.size();p++)
......
......@@ -106,6 +106,7 @@ bool AgentsSourcesManager::ProcessAllSources() const
vector<Pedestrian*> peds;
src->RemoveAgentsFromPool(peds,src->GetFrequency());
Log->Write("INFO:\tSource %d generating %d agents (%d remaining)",src->GetId(),peds.size(),src->GetPoolSize());
Log->Write(" " + std::to_string(peds[0]->GetExitIndex()));
//ComputeBestPositionRandom(src.get(), peds);
//todo: compute the optimal position for insertion using voronoi
......
......@@ -283,7 +283,7 @@ Point DirectionFloorfield::GetTarget(Room* room, Pedestrian* ped) const
#endif // DEBUG
Point p;
ffviafm->getDirectionToDestination(ped, p);
ffviafm->getDirectionToFinalDestination(ped, p);
p = p.Normalized(); // @todo: argraf : scale with costvalue: " * ffviafm->getCostToTransition(ped->GetTransitionID(), ped->GetPos()) "
return (p + ped->GetPos());
......@@ -311,6 +311,7 @@ double DirectionFloorfield::GetDistance2Wall(Pedestrian* ped) const
void DirectionFloorfield::Init(Building* building, double stepsize, double threshold, bool useDistancMap) {
//implement mechanic, that can read-in an existing floorfield (from a previous run)
string s = building->GetGeometryFilename();
Log->Write("INFO: \tGeometryFilename <" + s + ">");
s.erase(s.find_last_of(".", string::npos)); // delete ending
if (s.find_last_of("/") != string::npos) {
s.erase(0, s.find_last_of("/")+1); // delete directories before filename (espacially "..")
......@@ -323,6 +324,7 @@ void DirectionFloorfield::Init(Building* building, double stepsize, double thres
} else {
std::chrono::time_point<std::chrono::system_clock> start, end;
start = std::chrono::system_clock::now();
Log->Write("INFO: \tCalling Construtor of FloorfieldViaFM");
ffviafm = new FloorfieldViaFM(building, stepsize, stepsize, threshold, useDistancMap, FF_filename);
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end-start;
......@@ -332,6 +334,7 @@ void DirectionFloorfield::Init(Building* building, double stepsize, double thres
}
DirectionFloorfield::DirectionFloorfield() {
ffviafm = nullptr;
initDone = false;
};
......
......@@ -76,28 +76,46 @@ FloorfieldViaFM::FloorfieldViaFM(const Building* const buildingArg, const double
//ctor
threshold = -1; //negative value means: ignore threshold
threshold = wallAvoidDistance;
building = buildingArg;
if (hxArg != hyArg) std::cerr << "ERROR: hx != hy <=========";
//parse building and create list of walls/obstacles (find xmin xmax, ymin, ymax, and add border?)
Log->Write("INFO: \tStart Parsing: Building");
parseBuilding(buildingArg, hxArg, hyArg);
Log->Write("INFO: \tFinished Parsing: Building");
//testoutput("AALineScan.vtk", "AALineScan.txt", dist2Wall);
prepareForDistanceFieldCalculation(wall);
Log->Write("INFO: \tGrid initialized: Walls");
calculateDistanceField(-1.); //negative threshold is ignored, so all distances get calculated. this is important since distances is used for slowdown/redirect
Log->Write("INFO: \tGrid initialized: Walldistances");
//testoutput("AADistanceField.vtk","AADistanceField.txt", dist2Wall);
//std::cout<< "Test (50/101): " << grid->getKeyAtXY(50., 101.) << " " << grid->get_x_fromKey(grid->getKeyAtXY(50., 101.)) << " " << grid->get_y_fromKey(grid->getKeyAtXY(50., 101.)) << std::endl;
setSpeed(useDistancefield); //use distance2Wall
Log->Write("INFO: \tGrid initialized: Speed");
calculateFloorfield(cost, neggrad);
Point dummy;
int i;
for (auto& element : neggradmap) { // this loop is only to calc all floorfields before going parallel
i = element.first;
getDirectionToDestination(i, grid->getPointFromKey(0), dummy);
}
// Point dummy = Point(0., 0.);
// int i;
// Pedestrian* dummyped = new Pedestrian();
// //NavLine* dummynavline;
// dummyped->SetPos(grid->getPointFromKey(0), true);
// for (auto& element : neggradmap) { // this loop is only to calc all floorfields before going parallel
// i = element.first;
// if (i > 0) {
// dummyped->SetExitIndex(i);
// //dummynavline = new NavLine( (Line) *(buildingArg->GetTransition(i)) );
// dummyped->SetExitLine( buildingArg->GetTransition(i) );
// //delete dummynavline;
// getDirectionToDestination(dummyped, dummy);
// }
// }
//// for (auto& element : goalneggradmap) { // this loop is only to calc all goalfields beforehand
//// i = element.first;
//// dummyped->SetFinalDestination(i);
//// getDirectionToFinalDestination(dummyped, dummy);
//// }
// delete dummyped;
//testoutput("AAFloorfield.vtk","AAFloorfield.txt", cost);
//writeFF(filename); //writing FF-file disabled until extending is complete ( @todo: argraf )
}
......@@ -230,7 +248,7 @@ void FloorfieldViaFM::getDirectionAt(const Point& position, Point& direction){
}
// getDirectionToDestination(int, Point&, Point&) <- obsolete (plz do not use this one); delete soon
void FloorfieldViaFM::getDirectionToDestination(const int destID, const Point& position, Point& direction){
//void FloorfieldViaFM::getDirectionToDestination(const int destID, const Point& position, Point& direction){
// long int key = grid->getKeyAtPoint(position);
// Point* localneggradptr = neggradmap.at(destID);
// double* localcostptr = costmap.at(destID);
......@@ -258,7 +276,7 @@ void FloorfieldViaFM::getDirectionToDestination(const int destID, const Point& p
// }
// direction.SetX(localneggradptr[key].GetX());
// direction.SetY(localneggradptr[key].GetY());
}
//}
void FloorfieldViaFM::getDirectionToDestination(Pedestrian* ped, Point& direction){
const Point& position = ped->GetPos();
......@@ -294,10 +312,11 @@ void FloorfieldViaFM::getDirectionToDestination(Pedestrian* ped, Point& directio
trialfield[i].child = nullptr;
}
clearAndPrepareForFloorfieldReCalc(localcostptr);
std::vector<Line> localline = {(Line) *(ped->GetExitLine())};
std::vector<Line> localline = {Line((Line) *(ped->GetExitLine()))};
setNewGoalAfterTheClear(localcostptr, localline);
calculateFloorfield(localcostptr, localneggradptr);
std::cerr << "new Floorfield " << destID << " : " << localline[0].GetPoint1().GetX() << " " << localline[0].GetPoint1().GetY() << " " << localline[0].GetPoint2().GetX() << " " << localline[0].GetPoint2().GetY() << std::endl;
//std::cerr << "new Floorfield " << destID << " : " << localline[0].GetPoint1().GetX() << " " << localline[0].GetPoint1().GetY() << " " << localline[0].GetPoint2().GetX() << " " << localline[0].GetPoint2().GetY() << std::endl;
Log->Write("new Floorfield " + std::to_string(destID) + " : " + std::to_string(localline[0].GetPoint1().GetX()));
}
}
direction.SetX(localneggradptr[key].GetX());
......@@ -348,7 +367,8 @@ void FloorfieldViaFM::getDirectionToFinalDestination(Pedestrian* ped, Point& dir
setNewGoalAfterTheClear(localcostptr, localline);
calculateFloorfield(localcostptr, localneggradptr);
std::cerr << "new GOALfield " << goalID << " : " << localline[0].GetPoint1().GetX() << " " << localline[0].GetPoint1().GetY() << " " << localline[0].GetPoint2().GetX() << " " << localline[0].GetPoint2().GetY() << std::endl;
//std::cerr << "new GOALfield " << goalID << " : " << localline[0].GetPoint1().GetX() << " " << localline[0].GetPoint1().GetY() << " " << localline[0].GetPoint2().GetX() << " " << localline[0].GetPoint2().GetY() << std::endl;
Log->Write("new GOALfield " + std::to_string(goalID) + " : " + std::to_string(localline[0].GetPoint1().GetX()));
//find closest door and add to cheatmap "goalToLineUID" map
const std::map<int, Transition*>& transitions = building->GetAllTransitions();
......@@ -370,10 +390,11 @@ void FloorfieldViaFM::getDirectionToFinalDestination(Pedestrian* ped, Point& dir
direction.SetY(localneggradptr[key].GetY());
}
double FloorfieldViaFM::getCostToDestination(const int destID, const Point& position) {
double FloorfieldViaFM::getCostToDestination(const int destID, const Point& position) { //not implemented: trigger calc of new ff not working yet
if (costmap.at(destID) == nullptr) {
Point dummy;
getDirectionToDestination(destID, position, dummy); //this call induces the floorfieldcalculation
return -1.0;
//getDirectionToDestination(destID, position, dummy); //this call induces the floorfieldcalculation
}
return costmap.at(destID)[grid->getKeyAtPoint(position)];
}
......@@ -485,6 +506,8 @@ void FloorfieldViaFM::parseBuilding(const Building* const buildingArg, const dou
if (eachwall.GetPoint1().GetY() > yMax) yMax = eachwall.GetPoint1().GetY();
if (eachwall.GetPoint2().GetY() > yMax) yMax = eachwall.GetPoint2().GetY();
}
goalcostmap.emplace(eachgoal.second->GetId(), nullptr);
goalneggradmap.emplace(eachgoal.second->GetId(), nullptr);
}
//create Rect Grid
......@@ -846,8 +869,15 @@ void FloorfieldViaFM::calculateDistanceField(const double thresholdArg) { //if
checkNeighborsAndAddToNarrowband(smallest, biggest, i, [&] (const long int key) { this->checkNeighborsAndCalcDist2Wall(key);} );
}
}
//Log->Write(std::to_string(grid->GetxMax()));
//Log->Write(std::to_string(grid->GetyMax()));
//Log->Write(std::to_string(grid->GetxMin()));
//Log->Write(std::to_string(grid->GetyMin()));
//Log->Write(std::to_string(grid->GetiMax()));
//Log->Write(std::to_string(grid->GetjMax()));
//Log->Write("INFO: \t" + std::to_string(grid->GetnPoints()));
//inital narrowband done, now loop (while not empty: remove smallest, add neighbors of removed)
//long int debugcounter = 0;
while (smallest != nullptr) {
long int keyOfSmallest = smallest->key;
flag[keyOfSmallest] = 3;
......@@ -875,6 +905,7 @@ void FloorfieldViaFM::calculateDistanceField(const double thresholdArg) { //if
smallest = nullptr;
} else {
trialfield[keyOfSmallest].removecurr(smallest, biggest, trialfield+keyOfSmallest);
//Log->Write(std::to_string(debugcounter++) + " " + std::to_string(grid->GetnPoints()));
checkNeighborsAndAddToNarrowband(smallest, biggest, keyOfSmallest, [&] (const long int key) { this->checkNeighborsAndCalcDist2Wall(key);} );
}
}
......
......@@ -59,7 +59,7 @@ class FloorfieldViaFM
//FloorfieldViaFM& operator=(const FloorfieldViaFM& other);
void getDirectionAt(const Point& position, Point& direction); //obsolete
void getDirectionToDestination (const int destID, const Point& position, Point& direction); //obsolete
//void getDirectionToDestination (const int destID, const Point& position, Point& direction); //obsolete
void getDirectionToDestination (Pedestrian* ped, Point& direction);
void getDirectionToFinalDestination(Pedestrian* ped, Point& direction);
double getCostToDestination(const int destID, const Point& position);
......@@ -116,7 +116,7 @@ class FloorfieldViaFM
Point* dirToWall;
Trial* trialfield;
std::map<int, double*> goalcostmap;
std::map<int, int> goalToLineUIDmap;
std::map<int, int> goalToLineUIDmap; // not used yet; not calculated (check all helplines for each and save id of minvalue)
std::map<int, Point*> goalneggradmap;
std::map<int, double*> costmap;
std::map<int, Point*> neggradmap;
......
......@@ -134,7 +134,12 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
if( IsEnoughInSubroom(subroom, new_pos, radius ) )
{
ped->SetPos(center_pos + random_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
Point v;
if (ped->GetExitLine()) {
v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
} else {
v = Point(0., 0.);
}
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
......@@ -142,7 +147,12 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
else
{
ped->SetPos(center_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
Point v;
if (ped->GetExitLine()) {
v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
} else {
v = Point(0., 0.);
}
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
......@@ -151,7 +161,12 @@ bool ComputeBestPositionVoronoiBoost(AgentsSource* src, std::vector<Pedestrian*>
else
{
ped->SetPos(center_pos, true);
Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
Point v;
if (ped->GetExitLine()) {
v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
} else {
v = Point(0., 0.);
}
double speed=ped->GetV0Norm();
v=v*speed;
ped->SetV(v);
......
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