AgentsSourcesManager.cpp 8.13 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 34 35
#include <iostream>
#include <thread>
#include <chrono>

using namespace std;

AgentsSourcesManager::AgentsSourcesManager()
{

}

AgentsSourcesManager::~AgentsSourcesManager()
{
}

void AgentsSourcesManager::operator()(int value)
{
     //the loop is updated each second.
     //it might be better to use a timer
36 37
     bool finished = false;
     long updateFrequency = 5;     // 1 second
38 39
     do
     {
40 41 42

          int current_time = Pedestrian::GetGlobalTime();

43
          if ((current_time != _lastUpdateTime) && ((current_time % updateFrequency) == 0))
44 45
          {

46 47
               finished = true;
               for (const auto& src : _sources)
48
               {
49
                    if (src->GetPoolSize())
50
                    {
51

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

75
void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src, Pedestrian* agent)
76
{
77 78 79 80 81
     auto dist = src->GetStartDistribution();
     double bounds[4];
     dist->Getbounds(bounds);
     int roomID = dist->GetRoomId();
     int subroomID = dist->GetSubroomID();
82 83 84

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

87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
     //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)
     {
          //random position in the area

     }
110
     // compute the cells and cut with the bounds
111
     const int count = peds.size();
112 113 114
     float xValues[count];
     float yValues[count];

115
     for (int i = 0; i < count; i++)
116
     {
117 118
          xValues[i] = peds[i]->GetPos()._x;
          yValues[i] = peds[i]->GetPos()._y;
119 120 121
     }

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

     printf("\n------vertices---------\n");
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 152 153 154 155
     //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)
156
     {
157 158
          cout << "dist: " << mp.first << " pos: " << mp.second.toString() << endl;
          //agent->SetPos(mp.second, true);
159 160
     }

161 162 163 164 165 166 167
     //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);
168

169 170 171 172 173 174 175
     }
     else
     {
          cout << "position not set:" << endl;
          cout << "size: " << map_dist_to_position.size() << endl;
          exit(0);
     }
176 177 178 179 180 181 182 183
     //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
184
     //exit(0);
185 186
}

187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 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 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254
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);
     double bounds[4];
     dist->Getbounds(bounds);
     //int roomID = dist->GetRoomId();
     //int subroomID = dist->GetSubroomID();
     // first default Position

     for(const auto& ped: peds)
     {
ped->Dump(ped->GetID()); continue;
          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);

//               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());
//               }
          }
     }

}
255 256 257 258 259 260 261 262 263
void AgentsSourcesManager::AddSource(std::shared_ptr<AgentsSource> src)
{
     _sources.push_back(src);
}

const std::vector<std::shared_ptr<AgentsSource> >& AgentsSourcesManager::GetSources() const
{
     return _sources;
}
264 265 266

void AgentsSourcesManager::SetBuilding(Building* building)
{
267
     _building = building;
268
}