AgentsSourcesManager.cpp 24.2 KB
Newer Older
1 2 3 4
/**
 * \file        AgentsSourcesManager.cpp
 * \date        Apr 14, 2015
 * \version     v0.7
Mohcine Chraibi's avatar
Mohcine Chraibi committed
5
 * \copyright   <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
6
 *
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
 * \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
 * This class is responsible for materialising agent in a given location at a given frequency up to a maximum number.
 * The optimal position where to put the agents is given by various algorithms, for instance
 * the Voronoi algorithm or the Mitchell Best candidate algorithm.
 *
 **/
29 30 31

#include "AgentsSourcesManager.h"
#include "Pedestrian.h"
32

33
#include "../mpi/LCGrid.h"
34
#include <thread>
35
#include "AgentsQueue.h"
36

37
#include "../voronoi-boost/VoronoiPositionGenerator.h"
Mohcine Chraibi's avatar
Mohcine Chraibi committed
38
#define UNUSED(x) [&x]{}()  // c++11 silence warnings
39

40 41
using namespace std;

42
bool AgentsSourcesManager::_isCompleted=true;
43

44 45 46 47 48 49 50 51
AgentsSourcesManager::AgentsSourcesManager()
{
}

AgentsSourcesManager::~AgentsSourcesManager()
{
}

52
void AgentsSourcesManager::operator()()
53
{
54
     Run();
55
}
56

57 58 59 60 61 62
void AgentsSourcesManager::Run()
{
     Log->Write("INFO:\tStarting agent manager thread");

     //Generate all agents required for the complete simulation
     //It might be more efficient to generate at each frequency step
63
     //TODO  this loop is exactly GenerateAgents( --> REFACTOR)
64 65 66 67 68 69 70 71 72 73 74 75
     for (const auto& src : _sources)
     {
          src->GenerateAgentsAndAddToPool(src->GetMaxAgents(), _building);
     }

     //first call ignoring the return value
     ProcessAllSources();

     //the loop is updated each x second.
     //it might be better to use a timer
     _isCompleted = false;
     bool finished = false;
76 77

     SetBuildingUpdated(false);
78
     long updateFrequency = 1; //TODO parse this from inifile
79
     //std::cout << KMAG << "RUN Starting thread manager with _lastUpdateTime " << _lastUpdateTime<< std::endl;
80
     do
Mohcine Chraibi's avatar
Mohcine Chraibi committed
81
     { //@todo: break if max simulation time is reached.
Mohcine Chraibi's avatar
Mohcine Chraibi committed
82
          int current_time = (int)Pedestrian::GetGlobalTime();
83
          if ((current_time != _lastUpdateTime)
84
              && ((current_time % updateFrequency) == 0))
Mohcine Chraibi's avatar
Mohcine Chraibi committed
85
          {
86
               //        std::cout << ">> RUN: current_time " << current_time << " last update  " << _lastUpdateTime << std::endl;
87 88 89
               finished=ProcessAllSources();
               _lastUpdateTime = current_time;
          }
90 91 92 93 94 95 96 97 98 99 100 101
          // wait for main thread to update building

          while(! IsBuildingUpdated())
          {
               std::cout << " sourceManager waiting ... \n";
               std::this_thread::sleep_for(std::chrono::milliseconds(1));
          }
          SetBuildingUpdated(false);

          std::cout << " NOT waiting ... \n";


102
     } while (!finished);
103 104 105
     // std::cout << "Terminating agent manager thread" << RESET << std::endl;
      Log->Write("INFO:\tTerminating agent manager thread");
      _isCompleted = true;
106 107
}

108
     bool AgentsSourcesManager::ProcessAllSources() const
109
     {
110 111 112 113 114 115 116 117 118
          std::cout << "\nSTART   AgentsSourcesManager::ProcessAllSources()\n";

          bool empty=true;
          double current_time = Pedestrian::GetGlobalTime();
          for(auto pp: _building->GetAllPedestrians())
               std::cout<< KMAG << "BUL: agentssourcesManager: " << pp->GetPos()._x << ", " << pp->GetPos()._y << RESET << std::endl;


          for (const auto& src : _sources)
119
          {
120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151

               std::cout << KRED << "\nprocessing src: " <<  src->GetId() << " -- current time: " << current_time << " number of peds in building " << _building->GetAllPedestrians().size() << RESET << std::endl;
// for(auto pp: _building->GetAllPedestrians())                                                                                                                  std::cout<< KYEL << "BUL: agentssourcesManager: " << pp->GetPos()._x << ", " << pp->GetPos()._y << RESET << std::endl;

               if (src->GetPoolSize() && (src->GetPlanTime() <= current_time) )// maybe diff<eps
               {
                    vector<Pedestrian*> peds;
                    src->RemoveAgentsFromPool(peds, src->GetFrequency());
                    Log->Write("> INFO:\tSource %d generating %d agents (%d remaining)\n",src->GetId(),peds.size(),src->GetPoolSize());
                    //ComputeBestPositionRandom(src.get(), peds);
                    //todo: here every pedestrian needs an exitline
                    std::cout << "\tCalling ComputeBestPositionVoronoiBoost\n";

                    if( !ComputeBestPositionVoronoiBoost(src.get(), peds, _building) )
                         Log->Write("WARNING:\tThere was no place for some pedestrians");

                    for (auto p: peds)
                         std::cout<< KGRN << "PED: agentssourcesManager: " << p->GetPos()._x << ", " << p->GetPos()._y << RESET << std::endl;

                                                                                                                                   for(auto pp: _building->GetAllPedestrians())                                                                                                                  std::cout<< KBLU << "In source BUL: agentssourcesManager: " << pp->GetPos()._x << ", " << pp->GetPos()._y << RESET << std::endl;

                                                                                                                                                                                                                                                                                                                                                                                                                       std::cout << "ADD TO QUEUE " << peds.size() << std::endl;


                                                                                                                                                                                                                                                                                                                                                                                                                                                                      AgentsQueueIn::Add(peds);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                      empty = false;
                                                                                                                                                                                                                                                                                                                                                                                                                                                                      src->Dump();
               }
                                                                                                                                                                                                                                                                                                                                                                                                                                                       if (src->GetPlanTime() > current_time) // for the case we still expect
                                                                                                                                                                                                                                                                                                                                                                                                                                                            // agents coming
                                                                                                                                                                                                                                                                                                                                                                                                                                                            empty = false;
                                                                                                                                                                                                                                                                                                                                                                                                                                    //src->Dump();//exit(0);
152
          }
153 154 155 156 157 158 159 160 161 162 163 164
                              std::cout << "LEAVE   AgentsSourcesManager::ProcessAllSources()\n";
                    std::cout << " Source building: "<<  _building << " size "  << _building->GetAllPedestrians().size()<< std::endl;

                                                                                                                           for(auto pp: _building->GetAllPedestrians())
                                                                                                                                std::cout<< KBLU << "BUL: agentssourcesManager: " << pp->GetPos()._x << ", " << pp->GetPos()._y << RESET << std::endl;
                                                                                                                                                                                                                                            std::cout << "========================\n";



                                                                                                                                                                                                                                                         //

                                                                                                                                                                                                                                                         return empty;
165
     }
166

167 168
//4 agents frequency, just for an example
void AgentsSourcesManager::ComputeBestPositionDummy(AgentsSource* src,
169
                                                    vector<Pedestrian*>& peds)const
170
{
Mohcine Chraibi's avatar
Mohcine Chraibi committed
171
     UNUSED(src);
172 173 174 175 176 177
     peds[0]->SetPos( Point(10,5.5) );
     peds[1]->SetPos( Point(10,4.9) );
     peds[2]->SetPos( Point(10,4.3) );
     peds[3]->SetPos( Point(10,3.7) );

     /*peds[0]->SetPos( Point(10,5.4) );
178 179
       peds[1]->SetPos( Point(10,4.6) );
       peds[2]->SetPos( Point(10,3.8) );*/
180 181 182 183 184 185 186 187 188 189 190

     for(auto&& ped : peds)
     {
          Point v = (ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
          double speed=ped->GetV0Norm();
          v=v*speed;
          ped->SetV(v);
     }
}

void AgentsSourcesManager::ComputeBestPositionCompleteRandom(AgentsSource* src,
191
                                                             vector<Pedestrian*>& peds)const
192 193 194 195
{
     auto dist = src->GetStartDistribution();
     auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
     vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
196
     double seed = time(0);
197
     //TODO: get the seed from the simulation
Mohcine Chraibi's avatar
Mohcine Chraibi committed
198 199 200
     std:: cout << "seed: "<< seed << std::endl;

     srand (seed);
201 202 203 204 205 206 207 208 209

     for (auto& ped : peds)
     {
          if( positions.size() )
          {
               int index = rand()%positions.size();
               Point new_pos = positions[index];
               positions.erase(positions.begin() + index);
               ped->SetPos(new_pos, true);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
210 211
               std:: cout << "pos: " << new_pos._x << new_pos._y << std::endl;

212 213 214 215 216 217 218 219 220 221 222 223
               AdjustVelocityByNeighbour(ped);
          }
          else
          {
               Log->Write("\t No place for a pedestrian");
               break;
          }
     }

}

/*
224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340
  void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
  Pedestrian* agent) const
  {
  auto dist = src->GetStartDistribution();
  double bounds[4];
  dist->Getbounds(bounds);
  int roomID = dist->GetRoomId();
  int subroomID = dist->GetSubroomID();

  //Get all pedestrians in that location
  vector<Pedestrian*> peds;
  _building->GetPedestrians(roomID, subroomID, peds);

  //filter the points that are not within the boundaries
  for (auto&& iter = peds.begin(); iter != peds.end();)
  {
  const Point& pos = (*iter)->GetPos();
  if ((bounds[0] <= pos._x && pos._x <= bounds[1])
  && (bounds[1] <= pos._y && pos._y <= bounds[2]))
  {
  iter = peds.erase(iter);
  cout << "removing (testing only)..." << endl;
  exit(0);
  } else
  {
  ++iter;
  }
  }

  //special case with 1, 2 or only three pedestrians in the area
  if (peds.size() < 3)
  {
  //TODO/random position in the area
  return;

  }
  // compute the cells and cut with the bounds
  const int count = peds.size();
  float* xValues = new float[count];
  float* yValues = new float[count];
  //float xValues[count];
  //float yValues[count];

  for (int i = 0; i < count; i++)
  {
  xValues[i] = peds[i]->GetPos()._x;
  yValues[i] = peds[i]->GetPos()._y;
  }

  VoronoiDiagramGenerator vdg;
  vdg.generateVoronoi(xValues, yValues, count, bounds[0], bounds[1],
  bounds[2], bounds[3], 3);
  vdg.resetIterator();
  vdg.resetVerticesIterator();

  printf("\n------vertices---------\n");
  //collect the positions
  vector<Point> positions;
  float x1, y1;
  while (vdg.getNextVertex(x1, y1))
  {
  printf("GOT Point (%f,%f)\n", x1, y1);
  positions.push_back(Point(x1, y1));
  }

  //look for the biggest spot
  map<double, Point> map_dist_to_position;

  for (auto&& pos : positions)
  {
  double min_dist = FLT_MAX;

  for (auto&& ped : peds)
  {
  double dist = (pos - ped->GetPos()).NormSquare();
  if (dist < min_dist)
  {
  min_dist = dist;
  }
  }
  map_dist_to_position[min_dist] = pos;
  }

  //list the result
  for (auto&& mp : map_dist_to_position)
  {
  cout << "dist: " << mp.first << " pos: " << mp.second.toString()
  << endl;
  //agent->SetPos(mp.second, true);
  }

  //the elements are ordered.
  // so the last one has the largest distance
  if (!map_dist_to_position.empty())
  {
  agent->SetPos(map_dist_to_position.rbegin()->second, true);
  cout << "position:" << agent->GetPos().toString() << endl;
  //exit(0);

  } else
  {
  cout << "position not set:" << endl;
  cout << "size: " << map_dist_to_position.size() << endl;
  cout << " for " << peds.size() << " pedestrians" << endl;
  exit(0);
  }
  //exit(0);
  // float x1,y1,x2,y2;
  //while(vdg.getNext(x1,y1,x2,y2))
  //{
  //     printf("GOT Line (%f,%f)->(%f,%f)\n",x1,y1,x2, y2);
  //
  //}
  //compute the best position
  //exit(0);
  }
*/
341 342


343

344
void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
345
                                                     std::vector<Pedestrian*>& peds) const
346 347 348
{

     //generate the agents with default positions
349 350
     auto dist = src->GetStartDistribution();
     auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(
351
          dist->GetSubroomID());
352 353
     vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
     double bounds[4] = { 0, 0, 0, 0 };
354 355
     dist->Getbounds(bounds);

356 357
     std::vector<Pedestrian*> peds_without_place;

358 359
     vector<Point> extra_positions;

360 361
     std::vector<Pedestrian*>::iterator iter_ped;
     for (iter_ped = peds.begin(); iter_ped != peds.end(); )
362
     {
363 364 365
          //need to be called at each iteration
          SortPositionByDensity(positions, extra_positions);

366
          int index = -1;
367
          double radius = ( (*iter_ped)->GetEllipse() ).GetBmax()   ;
368 369

          //in the case a range was specified
370
          //just take the first element
371
          for (unsigned int a = 0; a < positions.size(); a++)
372
          {
373
               Point pos = positions[a];
374 375
               //cout<<"checking: "<<pos.toString()<<endl;
               // for positions inside bounds, check it there is enough space
376
               if ((bounds[0] <= pos._x) && (pos._x <= bounds[1])
377
                   && (bounds[2] <= pos._y) && (pos._y < bounds[3]))
378
               {
379

380
                    bool enough_space = true;
381

382 383 384
                    //checking enough space!!
                    vector<Pedestrian*> neighbours;
                    _building->GetGrid()->GetNeighbourhood(pos,neighbours);
385

386 387 388 389 390 391
                    for (const auto& ngh: neighbours)
                         if(  (ngh->GetPos() - pos).NormSquare() < 4*radius*radius )
                         {
                              enough_space = false;
                              break;
                         }
392 393


394 395 396 397 398
                    if( enough_space )
                    {
                         index = a;
                         break;
                    }
399

400 401
               }
          }
402
          if (index == -1)
403
          {
404
               if (positions.size())
405
               {
406
                    Log->Write(
407 408
                         "ERROR:\t AgentSourceManager Cannot distribute pedestrians in the mentioned area [%0.2f,%0.2f,%0.2f,%0.2f]",
                         bounds[0], bounds[1], bounds[2], bounds[3]);
409 410 411
                    Log->Write("     \t Specifying a subroom_id might help");
                    Log->Write("     \t %d positions were available",positions.size());
                    //exit(EXIT_FAILURE);
412
               }
413 414 415
               //dump the pedestrian, move iterator
               peds_without_place.push_back(*iter_ped);
               iter_ped=peds.erase(iter_ped);
416
          }
417
          else //we found a position with enough space
418 419 420
          {
               const Point& pos = positions[index];

421 422 423
               extra_positions.push_back(pos);
               (*iter_ped)->SetPos(pos, true); //true for the initial position
               positions.erase(positions.begin() + index);
424

425 426 427 428 429 430
               //at this point we have a position
               //so we can adjust the velocity
               //AdjustVelocityUsingWeidmann(ped);
               AdjustVelocityByNeighbour( (*iter_ped) );
               //move iterator
               iter_ped++;
431

432
          }
433 434

          //return the pedestrians without place
435
     }
436
     if(peds_without_place.size()>0)
437
          src->AddAgentsToPool(peds_without_place);
438
}
439

440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472
void AgentsSourcesManager::AdjustVelocityByNeighbour(Pedestrian* ped) const
{
     //get the density
     vector<Pedestrian*> neighbours;
     _building->GetGrid()->GetNeighbourhood(ped,neighbours);

     double speed=0.0;
     double radius_square=0.56*0.56;//corresponding to an area of 1m3
     int count=0;

     for(const auto& p: neighbours)
     {
          //only pedes in a sepcific rance
          if( (ped->GetPos()-p->GetPos()).NormSquare()<=radius_square)
          {
               //only peds with the same destination
               if(ped->GetExitIndex()==p->GetExitIndex())
               {
                    double dist1=ped->GetDistanceToNextTarget();
                    double dist2=p->GetDistanceToNextTarget();
                    //only peds in front of me
                    if(dist2<dist1)
                    {
                         speed+=p->GetV().Norm();
                         count++;
                    }
               }
          }

     }
     //mean speed
     if(count==0)
     {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
473
          speed=ped->GetEllipse().GetV0(); // FIXME:  bad fix for: peds without navline (ar.graf)
474
          //speed=ped->GetV0Norm();
475 476 477 478 479 480 481 482 483 484 485 486 487 488 489
     }
     else
     {
          speed=speed/count;
     }

     if(ped->FindRoute()!=-1)
     {
          //get the next destination point
          Point v =(ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
          v=v*speed;
          ped->SetV(v);
     }
     else
     {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
490
          Log->Write(">> ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination());
491 492 493 494 495 496
          //that will be most probably be fixed in the next computation step.
          // so do not abort
     }

}

497 498 499 500 501 502
void AgentsSourcesManager::AdjustVelocityUsingWeidmann(Pedestrian* ped) const
{
     //get the density
     vector<Pedestrian*> neighbours;
     _building->GetGrid()->GetNeighbourhood(ped,neighbours);

503
     //density in pers per m2
504 505
     double density = 1.0;
     //radius corresponding to a surface of 1m2
506
     //double radius_square=0.564*0.564;
507
     double radius_square=1.0;
508

509 510
     for(const auto& p: neighbours)
     {
511
          if( (ped->GetPos()-p->GetPos()).NormSquare()<=radius_square)
512 513
               density+=1.0;
     }
514 515
     density=density/(radius_square*M_PI);

516 517
     //get the velocity
     double density_max=5.4;
518

519 520 521 522 523 524
     //speed from taken from weidmann FD
     double speed=1.34*(1-exp(-1.913*(1.0/density-1.0/density_max)));
     if(speed>=ped->GetV0Norm())
     {
          speed=ped->GetV0Norm();
     }
525

526 527
     //set the velocity vector
     if(ped->FindRoute()!=-1)
528 529 530 531 532
     {
          //get the next destination point
          Point v =(ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
          v=v*speed;
          ped->SetV(v);
533
          //cout<<"density: "<<density<<endl;
534 535 536
     }
     else
     {
537
          Log->Write(">>> SOURCE ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination());
538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558
          //that will be most probably be fixed in the next computation step.
          // so do not abort
     }

}

void AgentsSourcesManager::SortPositionByDensity(std::vector<Point>& positions, std::vector<Point>& extra_positions) const
{
     std::multimap<double,Point> density2pt;
     //std::map<double,Point> density2pt;

     for(auto&& pt:positions)
     {
          vector<Pedestrian*> neighbours;
          _building->GetGrid()->GetNeighbourhood(pt,neighbours);
          //density in pers per m2
          double density = 0.0;
          double radius_square=0.40*0.40;

          for(const auto& p: neighbours)
          {
559 560
               //FIXME: p  can be null, if deleted in the main simulation thread.
               if( p && (pt-p->GetPos()).NormSquare()<=radius_square)
561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581
                    density+=1.0;
          }

          //consider the extra positions
          for(const auto& ptx: extra_positions)
          {
               if( (ptx-pt).NormSquare()<=radius_square)
                    density+=1.0;
          }
          density=density/(radius_square*M_PI);

          density2pt.insert(std::pair<double,Point>(density,pt));

     }

     //cout<<"------------------"<<positions.size()<<"-------"<<endl;
     positions.clear();
     for(auto&& d: density2pt)
     {
          positions.push_back(d.second);
          //     printf("density [%lf, %s]\n",d.first, d.second.toString().c_str());
582
     }
583 584

}
585 586 587 588 589 590 591 592 593 594 595


void AgentsSourcesManager::GenerateAgents()
{

     for (const auto& src : _sources)
     {
          src->GenerateAgentsAndAddToPool(src->GetMaxAgents(), _building);
     }
}

596 597 598
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
     _sources.push_back(src);
599
     _isCompleted=false;//at least one source was provided
600 601 602 603 604 605
}

const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
{
     return _sources;
}
606 607 608

void AgentsSourcesManager::SetBuilding(Building* building)
{
609
     _building = building;
610
}
611 612 613

bool AgentsSourcesManager::IsCompleted() const
{
614
     return _isCompleted;
615
}
616 617


618 619 620 621 622 623 624 625 626 627
bool AgentsSourcesManager::IsBuildingUpdated() const
{
     return _buildingUpdated;
}

void AgentsSourcesManager::SetBuildingUpdated(bool update)
{
     _buildingUpdated = update;
}

628 629 630 631
Building* AgentsSourcesManager::GetBuilding() const
{
     return _building;
}
632 633 634 635 636 637 638 639 640 641

long AgentsSourcesManager::GetMaxAgentNumber() const
{
     long pop=0;
     for (const auto& src : _sources)
     {
          pop+=src->GetMaxAgents();
     }
     return pop;
}