AgentsSourcesManager.cpp 8.85 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 32 33
#include <iostream>
#include <thread>
#include <chrono>

using namespace std;

AgentsSourcesManager::AgentsSourcesManager()
{

}

AgentsSourcesManager::~AgentsSourcesManager()
{
}

void AgentsSourcesManager::operator()(int value)
{
34
     //cout<<"starting thread"<<endl;
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
          if ((current_time != _lastUpdateTime) && ((current_time % updateFrequency) == 0))
45 46
          {

47
               //cout<<"TIME:"<<current_time<<endl;
48 49
               finished = true;
               for (const auto& src : _sources)
50
               {
51
                    if (src->GetPoolSize())
52
                    {
53

54 55
                         vector<Pedestrian*> peds;
                         src->GenerateByFrequency(peds);
56
                         ComputeBestPositionRandom(src.get(), peds);
57
                         // compute the optimal position for insertion
58
                         for (auto&& ped : peds)
59
                         {
60 61
                              //ComputeBestPositionVoronoi(src.get(), ped);
                              //ped->SetPos(Point(15,15),true);
62
                              //ped->Dump(ped->GetID());
63
                         }
64
                         AgentsQueue::Add(peds);
65
                         finished = false;
66
                         cout<<"Agents generated: "<<peds.size()<<endl;
67
                    }
68
                    //src->Dump();//exit(0);
69
               }
70
               _lastUpdateTime = current_time;
71
          //cout<<"size: "<<_sources.size()<<endl;
72
          }
73 74
          //wait some time
          std::this_thread::sleep_for(std::chrono::milliseconds(10));
75
     } while (!finished);
76 77

     _isCompleted=true;
78 79
}

80
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src, Pedestrian* agent)
81
{
82 83 84 85 86
     auto dist = src->GetStartDistribution();
     double bounds[4];
     dist->Getbounds(bounds);
     int roomID = dist->GetRoomId();
     int subroomID = dist->GetSubroomID();
87 88 89

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

92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111
     //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);
          }
          else
          {
               ++iter;
          }
     }

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

     }
116
     // compute the cells and cut with the bounds
117
     const int count = peds.size();
118 119 120 121
     float* xValues= new float[count];
     float* yValues= new float[count];
     //float xValues[count];
     //float yValues[count];
122

123
     for (int i = 0; i < count; i++)
124
     {
125 126
          xValues[i] = peds[i]->GetPos()._x;
          yValues[i] = peds[i]->GetPos()._y;
127 128 129
     }

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

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

169 170 171 172 173 174 175
     //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);
176

177 178 179 180 181
     }
     else
     {
          cout << "position not set:" << endl;
          cout << "size: " << map_dist_to_position.size() << endl;
182
          cout <<" for " <<peds.size()<<" pedestrians"<<endl;
183 184
          exit(0);
     }
185 186 187 188 189 190 191 192
     //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
193
     //exit(0);
194 195
}

196 197 198 199 200 201 202
void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src, std::vector<Pedestrian*>& peds)
{

     //generate the agents with default positions
     auto dist=src->GetStartDistribution();
     auto subroom=_building->GetRoom(dist->GetRoomId())->GetSubRoom(dist->GetSubroomID());
     vector<Point> positions=PedDistributor::PossiblePositions(*subroom);
203
     double bounds[4]={0,0,0,0};
204 205 206 207 208 209 210
     dist->Getbounds(bounds);
     //int roomID = dist->GetRoomId();
     //int subroomID = dist->GetSubroomID();
     // first default Position

     for(const auto& ped: peds)
     {
211
          //ped->Dump(ped->GetID()); continue;
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 241
          int index = -1;

          //in the case a range was specified
          for (unsigned int a=0;a<positions.size();a++)
          {
               Point pos=positions[a];
               if((bounds[0]<=pos._x) &&
                         (pos._x <= bounds[1])&&
                         (bounds[2]<=pos._y) &&
                         (pos._y < bounds[3]))
               {
                    index=a;
                    break;
               }
          }
          if(index==-1)
          {
               if(positions.size())
               {
                    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]);
                    Log->Write("ERROR:\t Specifying a subroom_id might help");
               }
          }
          else
          {
               const Point& pos = positions[index];
               ped->SetPos(pos,true); //true for the initial position
               positions.erase(positions.begin() + index);

242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259
               //               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());
               //               }
260 261 262 263
          }
     }

}
264 265 266 267 268 269 270 271 272
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
     _sources.push_back(src);
}

const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
{
     return _sources;
}
273 274 275

void AgentsSourcesManager::SetBuilding(Building* building)
{
276
     _building = building;
277
}
278 279 280 281 282

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