AgentsSourcesManager.cpp 8.98 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 23 24 25 26 27 28 29 30 31
#include <iostream>
#include <thread>
#include <chrono>

using namespace std;

AgentsSourcesManager::AgentsSourcesManager()
{

}

AgentsSourcesManager::~AgentsSourcesManager()
{
}

32
void AgentsSourcesManager::operator()()
33
{
34
     Log->Write("INFO:\tStarting agent manager thread");
35 36
     //the loop is updated each second.
     //it might be better to use a timer
37
     _isCompleted = false;
38 39
     bool finished = false;
     long updateFrequency = 5;     // 1 second
40 41
     do
     {
42 43
          int current_time = Pedestrian::GetGlobalTime();

44 45
          if ((current_time != _lastUpdateTime)
                    && ((current_time % updateFrequency) == 0))
46
          {
47
               //cout<<"TIME:"<<current_time<<endl;
48
               finished=ProcessAllSources();
49
               _lastUpdateTime = current_time;
50
               cout << "source size: " << _sources.size() << endl;
51
          }
52
          //wait some time
53 54
          //cout<<"sleepinp..."<<endl;
          //std::this_thread::sleep_for(std::chrono::milliseconds(1));
55
     } while (!finished);
56 57 58
     Log->Write("INFO:\tTerminating agent manager thread");
     _isCompleted = true;
}
59

60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
bool AgentsSourcesManager::ProcessAllSources()
{
     bool empty=true;
     for (const auto& src : _sources)
     {
          if (src->GetPoolSize())
          {
               vector<Pedestrian*> peds;
               src->GenerateByFrequency(peds);
               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;
               cout << "Agents generated: " << peds.size() << endl;
          }
          //src->Dump();//exit(0);
     }
     return empty;
84 85
}

86 87
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
          Pedestrian* agent)
88
{
89 90 91 92 93
     auto dist = src->GetStartDistribution();
     double bounds[4];
     dist->Getbounds(bounds);
     int roomID = dist->GetRoomId();
     int subroomID = dist->GetSubroomID();
94 95 96

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

99 100 101 102 103 104 105 106 107 108
     //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);
109
          } else
110 111 112 113 114 115 116 117
          {
               ++iter;
          }
     }

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

     }
122
     // compute the cells and cut with the bounds
123
     const int count = peds.size();
124 125
     float* xValues = new float[count];
     float* yValues = new float[count];
126 127
     //float xValues[count];
     //float yValues[count];
128

129
     for (int i = 0; i < count; i++)
130
     {
131 132
          xValues[i] = peds[i]->GetPos()._x;
          yValues[i] = peds[i]->GetPos()._y;
133 134 135
     }

     VoronoiDiagramGenerator vdg;
136 137
     vdg.generateVoronoi(xValues, yValues, count, bounds[0], bounds[1],
               bounds[2], bounds[3], 3);
138 139 140 141
     vdg.resetIterator();
     vdg.resetVerticesIterator();

     printf("\n------vertices---------\n");
142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170
     //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)
171
     {
172 173
          cout << "dist: " << mp.first << " pos: " << mp.second.toString()
                    << endl;
174
          //agent->SetPos(mp.second, true);
175 176
     }

177 178 179 180 181 182 183
     //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);
184

185
     } else
186 187 188
     {
          cout << "position not set:" << endl;
          cout << "size: " << map_dist_to_position.size() << endl;
189
          cout << " for " << peds.size() << " pedestrians" << endl;
190 191
          exit(0);
     }
192 193 194 195 196 197 198 199
     //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
200
     //exit(0);
201 202
}

203 204
void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
          std::vector<Pedestrian*>& peds)
205 206 207
{

     //generate the agents with default positions
208 209 210 211 212
     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 };
213 214 215 216 217
     dist->Getbounds(bounds);
     //int roomID = dist->GetRoomId();
     //int subroomID = dist->GetSubroomID();
     // first default Position

218
     for (const auto& ped : peds)
219
     {
220
          //ped->Dump(ped->GetID()); continue;
221 222 223
          int index = -1;

          //in the case a range was specified
224
          for (unsigned int a = 0; a < positions.size(); a++)
225
          {
226 227 228
               Point pos = positions[a];
               if ((bounds[0] <= pos._x) && (pos._x <= bounds[1])
                         && (bounds[2] <= pos._y) && (pos._y < bounds[3]))
229
               {
230
                    index = a;
231 232 233
                    break;
               }
          }
234
          if (index == -1)
235
          {
236
               if (positions.size())
237
               {
238 239 240
                    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]);
241 242
                    Log->Write("ERROR:\t Specifying a subroom_id might help");
               }
243
          } else
244 245
          {
               const Point& pos = positions[index];
246
               ped->SetPos(pos, true); //true for the initial position
247 248
               positions.erase(positions.begin() + index);

249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266
               //               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());
               //               }
267 268 269 270
          }
     }

}
271 272 273 274 275 276 277 278 279
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
     _sources.push_back(src);
}

const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
{
     return _sources;
}
280 281 282

void AgentsSourcesManager::SetBuilding(Building* building)
{
283
     _building = building;
284
}
285 286 287

bool AgentsSourcesManager::IsCompleted() const
{
288
     return _isCompleted;
289
}