AgentsSourcesManager.cpp 11.6 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 18 19 20 21 22
#include <iostream>
#include <thread>
#include <chrono>

using namespace std;

23 24
bool AgentsSourcesManager::_isCompleted=false;

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

AgentsSourcesManager::~AgentsSourcesManager()
{
}

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

     //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.
49
     //it might be better to use a timer
50
     _isCompleted = false;
51 52
     bool finished = false;
     long updateFrequency = 5;     // 1 second
53 54
     do
     {
55 56
          int current_time = Pedestrian::GetGlobalTime();

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

bool AgentsSourcesManager::ProcessAllSources() const
127 128
{
     bool empty=true;
129
     //cout<<"src size: "<<_sources.size()<<endl;
130 131
     for (const auto& src : _sources)
     {
132
          //cout<<"size: "<<src->GetPoolSize()<<endl;//exit(0);
133 134 135
          if (src->GetPoolSize())
          {
               vector<Pedestrian*> peds;
136
               src->RemoveAgentsFromPool(peds,src->GetFrequency());
137
               //cout<<"removing: "<<peds.size()<<" agents from the sources"<<endl;//getc(stdin);exit(0);
138 139 140 141 142 143 144 145 146 147
               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
148
               //cout << "Agents generated: " << peds.size() << endl;
149 150 151 152
          }
          //src->Dump();//exit(0);
     }
     return empty;
153 154
}

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

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

168 169 170 171 172 173 174 175 176 177
     //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);
178
          } else
179 180 181 182 183 184 185 186
          {
               ++iter;
          }
     }

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

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

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

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

     printf("\n------vertices---------\n");
211 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
     //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)
240
     {
241 242
          cout << "dist: " << mp.first << " pos: " << mp.second.toString()
                    << endl;
243
          //agent->SetPos(mp.second, true);
244 245
     }

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

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

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

     //generate the agents with default positions
277 278 279 280 281
     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 };
282 283 284 285 286
     dist->Getbounds(bounds);
     //int roomID = dist->GetRoomId();
     //int subroomID = dist->GetSubroomID();
     // first default Position

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

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

318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335
               //               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());
               //               }
336 337 338 339
          }
     }

}
340 341 342 343 344 345 346 347 348 349 350


void AgentsSourcesManager::GenerateAgents()
{

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

351 352 353 354 355 356 357 358 359
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
     _sources.push_back(src);
}

const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
{
     return _sources;
}
360 361 362

void AgentsSourcesManager::SetBuilding(Building* building)
{
363
     _building = building;
364
}
365 366 367

bool AgentsSourcesManager::IsCompleted() const
{
368
     return _isCompleted;
369
}
370 371 372 373 374 375


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