AgentsSourcesManager.cpp 12.4 KB
Newer Older
1 2 3 4 5 6 7 8 9 10
/*
 * AgentsSourcesManager.cpp
 *
 *  Created on: 14.04.2015
 *      Author: piccolo
 */

#include "AgentsSourcesManager.h"
#include "Pedestrian.h"
#include "AgentsQueue.h"
11 12
#include "StartDistribution.h"
#include "PedDistributor.h"
13
#include "AgentsSource.h"
14 15 16
#include "../voronoi/VoronoiDiagramGenerator.h"
#include "../geometry/Building.h"

17
#include "../mpi/LCGrid.h"
18 19 20 21 22 23
#include <iostream>
#include <thread>
#include <chrono>

using namespace std;

24 25
bool AgentsSourcesManager::_isCompleted=false;

26 27 28 29 30 31 32 33
AgentsSourcesManager::AgentsSourcesManager()
{
}

AgentsSourcesManager::~AgentsSourcesManager()
{
}

34
void AgentsSourcesManager::operator()()
35
{
36
     Log->Write("INFO:\tStarting agent manager thread");
37 38 39 40 41 42 43 44 45 46 47 48 49

     //Generate all agents required for the complete simulation
     //It might be more efficient to generate at each frequency step
     for (const auto& src : _sources)
     {
          src->GenerateAgentsAndAddToPool(src->GetMaxAgents(), _building);
          cout<<"generation: "<<src->GetPoolSize()<<endl;
     }

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

     //the loop is updated each x second.
50
     //it might be better to use a timer
51
     _isCompleted = false;
52 53
     bool finished = false;
     long updateFrequency = 5;     // 1 second
54 55
     do
     {
56 57
          int current_time = Pedestrian::GetGlobalTime();

58 59 60 61 62 63
          //first step
          //if(current_time==0){
               //finished=ProcessAllSources();
          //     ProcessAllSources();
          //     //cout<<"here:"<<endl; exit(0);
          //}
64 65
          if ((current_time != _lastUpdateTime)
                    && ((current_time % updateFrequency) == 0))
66
          {
67
               //cout<<"TIME:"<<current_time<<endl;
68
               finished=ProcessAllSources();
69
               _lastUpdateTime = current_time;
70
               cout << "source size: " << _sources.size() << endl;
71
          }
72
          //wait some time
73 74
          //cout<<"sleepinp..."<<endl;
          //std::this_thread::sleep_for(std::chrono::milliseconds(1));
75
     } while (!finished);
76
     Log->Write("INFO:\tTerminating agent manager thread");
77
     _isCompleted = true;exit(0);
78
}
79

80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
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
     for (const auto& src : _sources)
     {
          src->GenerateAgentsAndAddToPool(src->GetMaxAgents(), _building);
          cout<<"generation: "<<src->GetPoolSize()<<endl;
     }

     //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;
     long updateFrequency = 5;     // 1 second
     do
     {
          int current_time = Pedestrian::GetGlobalTime();

          //first step
          //if(current_time==0){
               //finished=ProcessAllSources();
          //     ProcessAllSources();
          //     //cout<<"here:"<<endl; exit(0);
          //}
          if ((current_time != _lastUpdateTime)
                    && ((current_time % updateFrequency) == 0))
          {
               //cout<<"TIME:"<<current_time<<endl;
               finished=ProcessAllSources();
               _lastUpdateTime = current_time;
               cout << "source size: " << _sources.size() << endl;
          }
          //wait some time
          //cout<<"sleepinp..."<<endl;
          //std::this_thread::sleep_for(std::chrono::milliseconds(1));
     } while (!finished);
     Log->Write("INFO:\tTerminating agent manager thread");
Ulrich Kemloh's avatar
Ulrich Kemloh committed
124
     _isCompleted = true;//exit(0);
125 126 127
}

bool AgentsSourcesManager::ProcessAllSources() const
128 129
{
     bool empty=true;
130
     //cout<<"src size: "<<_sources.size()<<endl;
131 132
     for (const auto& src : _sources)
     {
133
          //cout<<"size: "<<src->GetPoolSize()<<endl;//exit(0);
134 135 136
          if (src->GetPoolSize())
          {
               vector<Pedestrian*> peds;
137
               src->RemoveAgentsFromPool(peds,src->GetFrequency());
138
               //cout<<"removing: "<<peds.size()<<" agents from the sources"<<endl;//getc(stdin);exit(0);
139 140 141 142 143 144 145 146 147 148
               ComputeBestPositionRandom(src.get(), peds);
               // compute the optimal position for insertion
               for (auto&& ped : peds)
               {
                    //ComputeBestPositionVoronoi(src.get(), ped);
                    //ped->SetPos(Point(15,15),true);
                    //ped->Dump(ped->GetID());
               }
               AgentsQueue::Add(peds);
               empty = false;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
149
               //cout << "Agents generated: " << peds.size() << endl;
150 151 152 153
          }
          //src->Dump();//exit(0);
     }
     return empty;
154 155
}

156 157
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
          Pedestrian* agent)
158
{
159 160 161 162 163
     auto dist = src->GetStartDistribution();
     double bounds[4];
     dist->Getbounds(bounds);
     int roomID = dist->GetRoomId();
     int subroomID = dist->GetSubroomID();
164 165 166

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

169 170 171 172 173 174 175 176 177 178
     //filter the points that are not in 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..." << endl;
               exit(0);
179
          } else
180 181 182 183 184 185 186 187
          {
               ++iter;
          }
     }

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

     }
192
     // compute the cells and cut with the bounds
193
     const int count = peds.size();
194 195
     float* xValues = new float[count];
     float* yValues = new float[count];
196 197
     //float xValues[count];
     //float yValues[count];
198

199
     for (int i = 0; i < count; i++)
200
     {
201 202
          xValues[i] = peds[i]->GetPos()._x;
          yValues[i] = peds[i]->GetPos()._y;
203 204 205
     }

     VoronoiDiagramGenerator vdg;
206 207
     vdg.generateVoronoi(xValues, yValues, count, bounds[0], bounds[1],
               bounds[2], bounds[3], 3);
208 209 210 211
     vdg.resetIterator();
     vdg.resetVerticesIterator();

     printf("\n------vertices---------\n");
212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240
     //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)
241
     {
242 243
          cout << "dist: " << mp.first << " pos: " << mp.second.toString()
                    << endl;
244
          //agent->SetPos(mp.second, true);
245 246
     }

247 248 249 250 251 252 253
     //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);
254

255
     } else
256 257 258
     {
          cout << "position not set:" << endl;
          cout << "size: " << map_dist_to_position.size() << endl;
259
          cout << " for " << peds.size() << " pedestrians" << endl;
260 261
          exit(0);
     }
262 263 264 265 266 267 268 269
     //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
270
     //exit(0);
271 272
}

273
void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
274
          std::vector<Pedestrian*>& peds) const
275 276 277
{

     //generate the agents with default positions
278 279 280 281 282
     auto dist = src->GetStartDistribution();
     auto subroom = _building->GetRoom(dist->GetRoomId())->GetSubRoom(
               dist->GetSubroomID());
     vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
     double bounds[4] = { 0, 0, 0, 0 };
283 284 285 286 287
     dist->Getbounds(bounds);
     //int roomID = dist->GetRoomId();
     //int subroomID = dist->GetSubroomID();
     // first default Position

288
     for (auto& ped : peds)
289
     {
290
          //ped->Dump(ped->GetID()); continue;
291 292
          int index = -1;

293 294
          AdjustVelocityUsingWeidmann(ped);

295
          //in the case a range was specified
296
          for (unsigned int a = 0; a < positions.size(); a++)
297
          {
298 299 300
               Point pos = positions[a];
               if ((bounds[0] <= pos._x) && (pos._x <= bounds[1])
                         && (bounds[2] <= pos._y) && (pos._y < bounds[3]))
301
               {
302
                    index = a;
303 304 305
                    break;
               }
          }
306
          if (index == -1)
307
          {
308
               if (positions.size())
309
               {
310 311 312
                    Log->Write(
                              "ERROR:\t Cannot distribute pedestrians in the mentioned area [%0.2f,%0.2f,%0.2f,%0.2f]",
                              bounds[0], bounds[1], bounds[2], bounds[3]);
313 314
                    Log->Write("ERROR:\t Specifying a subroom_id might help");
               }
315
          } else
316 317
          {
               const Point& pos = positions[index];
318
               ped->SetPos(pos, true); //true for the initial position
319 320
               positions.erase(positions.begin() + index);

321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338
               //               const Point& start_pos =  Point(_startX, _startY);
               //               if ((std::isnan(start_pos._x) == 0) && (std::isnan(start_pos._y) == 0))
               //               {
               //                    if (_building->GetRoom(ped->GetRoomID())->GetSubRoom(ped->GetSubRoomID())->IsInSubRoom(
               //                              start_pos) == false)
               //                    {
               //                         Log->Write(
               //                                   "ERROR: \t cannot distribute pedestrian %d in Room %d at fixed position %s",
               //                                   *pid, GetRoomId(), start_pos.toString().c_str());
               //                         Log->Write(
               //                                   "ERROR: \t Make sure that the position is inside the geometry and belongs to the specified room / subroom");
               //                         exit(EXIT_FAILURE);
               //                    }
               //
               //                    ped->SetPos(start_pos, true); //true for the initial position
               //                    Log->Write("INFO: \t fixed position for ped %d in Room %d %s", *pid, GetRoomId(),
               //                              start_pos.toString().c_str());
               //               }
339 340 341 342
          }
     }

}
343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368
void AgentsSourcesManager::AdjustVelocityUsingWeidmann(Pedestrian* ped) const
{
     //get the density
     vector<Pedestrian*> neighbours;
     _building->GetGrid()->GetNeighbourhood(ped,neighbours);

     //pers per m2
     double density = 1.0;
     //radius corresponding to a surface of 1m2
     double radius_square=0.564*0.564;
     for(const auto& p: neighbours)
     {
          if( (ped->GetPos()-p->GetPos()).NormSquare()<radius_square)
               density+=1.0;
     }

     //get the velocity
     double density_max=5.4;
     double speed=1.34*(1 - exp(-1.913*(1.0/density-1.0/density_max)));

     //set the velocity vector
     if(ped->FindRoute()!=-1)
          {

          }
}
369 370 371 372 373 374 375 376 377 378 379


void AgentsSourcesManager::GenerateAgents()
{

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

380 381 382 383 384 385 386 387 388
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
     _sources.push_back(src);
}

const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
{
     return _sources;
}
389 390 391

void AgentsSourcesManager::SetBuilding(Building* building)
{
392
     _building = building;
393
}
394 395 396

bool AgentsSourcesManager::IsCompleted() const
{
397
     return _isCompleted;
398
}
399 400 401 402 403 404


Building* AgentsSourcesManager::GetBuilding() const
{
     return _building;
}