LCGrid.cpp 12.3 KB
Newer Older
Ulrich Kemloh's avatar
Ulrich Kemloh committed
1
/**
Weichen's avatar
Weichen committed
2 3
 * \file        LCGrid.cpp
 * \date        Nov 16, 2010
4 5
 * \version     v0.7
 * \copyright   <2009-2015> Forschungszentrum J?lich GmbH. All rights reserved.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
6
 *
Weichen's avatar
Weichen committed
7
 * \section License
8
 * This file is part of JuPedSim.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
9
 *
10
 * JuPedSim is free software: you can redistribute it and/or modify
Weichen's avatar
Weichen committed
11
 * it under the terms of the GNU Lesser General Public License as published by
Ulrich Kemloh's avatar
Ulrich Kemloh committed
12 13 14
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
15
 * JuPedSim is distributed in the hope that it will be useful,
Ulrich Kemloh's avatar
Ulrich Kemloh committed
16 17 18 19
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
Weichen's avatar
Weichen committed
20
 * You should have received a copy of the GNU Lesser General Public License
21 22
 * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
 *
Weichen's avatar
Weichen committed
23 24 25 26 27 28
 * \section Description
 * This class implements the Linked-Cells algorithm
 * \ref{cacs.usc.edu/education/cs596/01-1LinkedListCell.pdf}
 * A grid is laid on the complete geometry and the pedestrians are assigned the cells
 * at each simulation step. Only pedestrians in the neighbouring cells are involved
 * in the force computations.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
29
 *
Weichen's avatar
Weichen committed
30 31
 * The class is static as only one instance is needed per simulation round.
 * This solution is fine for parallelisation as well, at least for OpenMP.
Ulrich Kemloh's avatar
Ulrich Kemloh committed
32 33
 *
 *
Weichen's avatar
Weichen committed
34
 **/
35

Ulrich Kemloh's avatar
Ulrich Kemloh committed
36 37

#include"LCGrid.h"
38
#include "../pedestrian/Pedestrian.h"
tobias schroedter's avatar
tobias schroedter committed
39 40
#include <mutex>

41 42

using namespace std;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
43

tobias schroedter's avatar
tobias schroedter committed
44 45
std::mutex grid_mutex;

Ulrich Kemloh's avatar
Ulrich Kemloh committed
46
//FIXME:
47
#define MAX_AGENT_COUNT  10000 // 1000000
Ulrich Kemloh's avatar
Ulrich Kemloh committed
48

49 50
LCGrid::LCGrid(double boundaries[4], double cellsize, int nPeds)
{
51 52 53 54 55
     _gridXmin=boundaries[0];
     _gridXmax=boundaries[1];
     _gridYmin=boundaries[2];
     _gridYmax=boundaries[3];
     _cellSize=cellsize;
56
     _nPeds = nPeds + MAX_AGENT_COUNT;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
57

58
     // add 1 to ensure that the whole area is covered by cells if not divisible without remainder
59 60
     _gridSizeX = (int) ((_gridXmax - _gridXmin) / _cellSize) + 1 + 2; // 1 dummy cell on each side
     _gridSizeY = (int) ((_gridYmax - _gridYmin) / _cellSize) + 1 + 2; // 1 dummy cell on each side
Ulrich Kemloh's avatar
Ulrich Kemloh committed
61

62
     // allocate memory for cells (2D-array) and initialize
63
     _cellHead= new int *[_gridSizeY];
Ulrich Kemloh's avatar
Ulrich Kemloh committed
64

65 66
     for (int i = 0; i < _gridSizeY; ++i) {
          _cellHead[i]  = new int[_gridSizeX]; // nx columns
Ulrich Kemloh's avatar
Ulrich Kemloh committed
67

68 69
          for (int j = 0; j < _gridSizeX; ++j) {
               _cellHead[i][j] = LIST_EMPTY;
70 71
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
72

73
     // creating and resetting the pedestrians list
74 75
     _list = new int[_nPeds];
     for(int i=0; i<_nPeds; i++) _list[i]=0;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
76

77
     //allocating the place for the peds copy
78 79
     _localPedsCopy=new Pedestrian*[_nPeds];
     for(int i=0; i<_nPeds; i++) _localPedsCopy[i]=nullptr;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
80 81 82

}

83 84
LCGrid::~LCGrid()
{
85 86 87 88
//     for(int i=0; i<_nPeds; i++) {
//          if(_localPedsCopy[i]!=nullptr)
//               delete _localPedsCopy[i];
//     }
89 90 91 92
     delete [] _list;
     delete [] _localPedsCopy;
     for (int i = 0; i < _gridSizeY; ++i)  delete[] _cellHead[i];
     delete[] _cellHead;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
93 94
}

95 96
void LCGrid::ShallowCopy(const vector<Pedestrian*>& peds)
{
97 98
     for(unsigned int p=0; p<peds.size(); p++)
     {
99
          int id= peds[p]->GetID()-1;
100
          _localPedsCopy[id]=peds[p];
101
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
102 103
}

104 105
void LCGrid::Update(const vector<Pedestrian*>& peds)
{
tobias schroedter's avatar
tobias schroedter committed
106
     grid_mutex.lock();
107 108
     ClearGrid();

109 110
     for (auto& ped: peds)
     {
tobias schroedter's avatar
tobias schroedter committed
111
               //Pedestrian* ped = peds[p];
112 113
          int id=ped->GetID()-1;
          // determine the cell coordinates of pedestrian i
Oliver Schmidts's avatar
Oliver Schmidts committed
114 115 116
          int ix = (int) ((ped->GetPos()._x - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
          int iy = (int) ((ped->GetPos()._y - _gridYmin) / _cellSize) + 1;
          //printf("[%f, %f]  ",ped->GetPos()._x, ped->GetPos()._y);
117 118
          // create lists
          //printf("[%d=%d] ",ped->GetPedIndex(),id);
119 120
          _list[id] = _cellHead[iy][ix];
          _cellHead[iy][ix] = id;
121

122
          _localPedsCopy[id]=ped;
tobias schroedter's avatar
tobias schroedter committed
123 124 125 126 127

//          if (ped->GetID() == 71) {
//               std::cout << "pos: " << ped->GetPos().toString() << " ix:"  << ix << " iy:" << iy << std::endl;
//          }

128
     }
tobias schroedter's avatar
tobias schroedter committed
129 130
     grid_mutex.unlock();

Ulrich Kemloh's avatar
Ulrich Kemloh committed
131 132 133
}

// I hope you had called Clear() first
Ulrich Kemloh's avatar
Ulrich Kemloh committed
134
// todo: can be used to solve the issue with MAX_AGENT_COUNT
135 136
void LCGrid::Update(Pedestrian* ped)
{
tobias schroedter's avatar
tobias schroedter committed
137 138
     grid_mutex.lock();

139 140
     int id=ped->GetID()-1;
     // determine the cell coordinates of pedestrian i
Oliver Schmidts's avatar
Oliver Schmidts committed
141 142
     int ix = (int) ((ped->GetPos()._x - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
     int iy = (int) ((ped->GetPos()._y - _gridYmin) / _cellSize) + 1;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
143

144
     // update the list previously created
145 146
     _list[id] = _cellHead[iy][ix];
     _cellHead[iy][ix] = id;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
147

148
     // this is probably a pedestrian coming from the mpi routine, so made a copy
149
     _localPedsCopy[id]=ped;
tobias schroedter's avatar
tobias schroedter committed
150 151
     grid_mutex.unlock();

Ulrich Kemloh's avatar
Ulrich Kemloh committed
152 153
}

154 155 156
void LCGrid::ClearGrid()
{
     // start by resetting the current list
157 158 159
     for (int i = 0; i < _gridSizeY; ++i) {
          for (int j = 0; j < _gridSizeX; ++j) {
               _cellHead[i][j] = LIST_EMPTY;
160 161
          }
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
162

163 164
     for(int i=0; i<_nPeds; i++) {
          _list[i]=LIST_EMPTY;
165
          _localPedsCopy[i]=nullptr;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
166
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
167 168
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
169
void LCGrid::HighlightNeighborhood(int pedID, Building* building)
170
{
171
     // force spotlight activation
Ulrich Kemloh's avatar
Ulrich Kemloh committed
172
     Pedestrian::SetColorMode(BY_SPOTLIGHT);
173
     //darken all
174 175 176
     for(auto&& ped: building->GetAllPedestrians())
     {
          ped->SetSpotlight(false);
177
     }
178

Ulrich Kemloh's avatar
Ulrich Kemloh committed
179
     Pedestrian* ped=building->GetPedestrian(pedID);
180
     //get and highlight the neighborhood
Ulrich Kemloh's avatar
Ulrich Kemloh committed
181 182 183
     if(ped){
          vector<Pedestrian*> neighbours;
          GetNeighbourhood(ped,neighbours);
184 185 186

          for(auto&& p: neighbours)
               p->SetSpotlight(true);
187
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
188
}
189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209
/*
void LCGrid::HighlightNeighborhood(int pedID, Building* building)
{
     // force spotlight activation
     Pedestrian::SetColorMode(BY_SPOTLIGHT);
     //darken all
     //const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
     //for(unsigned int p=0;p<allPeds.size();p++){
     //     allPeds[p]->SetSpotlight(false);
     //}

     for(auto&& ped: building->GetAllPedestrians())
     {
          ped->SetSpotlight(false);
     }

     Pedestrian* ped=building->GetPedestrian(pedID);
     //get and highlight the neighborhood
     if(ped){
          vector<Pedestrian*> neighbours;
          GetNeighbourhood(ped,neighbours);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
210

211 212 213 214 215 216 217 218
          for(auto&& p: neighbours)
               p->SetSpotlight(true);
          //for(unsigned int p=0;p<neighbours.size();p++){
          //     neighbours[p]->SetSpotlight(true);
          //}
     }
}
*/
219 220
void LCGrid::GetNeighbourhood(const Pedestrian* ped, vector<Pedestrian*>& neighbourhood)
{
tobias schroedter's avatar
tobias schroedter committed
221
     grid_mutex.lock();
222

Oliver Schmidts's avatar
Oliver Schmidts committed
223 224
     double xPed=ped->GetPos()._x;
     double yPed=ped->GetPos()._y;
225

226 227
     int l = (int) ((xPed - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
     int k = (int) ((yPed - _gridYmin) / _cellSize) + 1;
228 229 230 231 232 233 234 235

     //-1 to get  correct mapping in the array local
     int myID=ped->GetID()-1;

     // all neighbor cells
     for (int i = l - 1; i <= l + 1; ++i) {
          for (int j = k - 1; j <= k + 1; ++j) {
               //printf(" i=%d j=%d k=%d l=%d\n",i,j,nx,ny);
236
               int p = _cellHead[j][i];
237 238
               // all peds in one cell
               while (p != LIST_EMPTY) {
Oliver Schmidts's avatar
Oliver Schmidts committed
239 240
                    // double x=pLocalPedsCopy[p]->GetPos()._x;
                    // double y=pLocalPedsCopy[p]->GetPos()._y;
241 242 243
                    // double dist=((x-xPed)*(x-xPed) + (y-yPed)*(y-yPed));
                    if(p!=myID) {
                         // if((dist<pCellSize*pCellSize) && (p!=myID)) {
244
                         neighbourhood.push_back(_localPedsCopy[p]);
245 246
                    }
                    // next ped
247 248 249 250
                    p = _list[p];
               }
          }
     }
tobias schroedter's avatar
tobias schroedter committed
251 252 253 254

     if ((myID == 70) && (fmod(Pedestrian::GetGlobalTime() , 45.) == 0) ){
          std::cout << Pedestrian::GetGlobalTime() << ":\t\tNeighborhood of 71 " << neighbourhood.size() << std::endl;

Ozaq's avatar
Ozaq committed
255 256
          for (auto& neighbour : neighbourhood){
               std::cout << "Neighbor added: " << neighbour->GetID() << " at " << neighbour->GetPos().toString() << std::endl;
tobias schroedter's avatar
tobias schroedter committed
257 258 259 260 261 262 263 264
          }
          std::cout << "---------------------------" << std::endl;


     }

     grid_mutex.unlock();

265 266 267 268
}

void LCGrid::GetNeighbourhood(const Point& pos, std::vector<Pedestrian*>& neighbourhood)
{
tobias schroedter's avatar
tobias schroedter committed
269
     grid_mutex.lock();
270

Oliver Schmidts's avatar
Oliver Schmidts committed
271 272
     double xPed=pos._x;
     double yPed=pos._y;
273 274 275 276 277 278 279 280 281 282 283 284 285 286

     int l = (int) ((xPed - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
     int k = (int) ((yPed - _gridYmin) / _cellSize) + 1;

     // all neighbor cells
     for (int i = l - 1; i <= l + 1; ++i) {
          for (int j = k - 1; j <= k + 1; ++j) {
               //printf(" i=%d j=%d k=%d l=%d\n",i,j,nx,ny);
               int p = _cellHead[j][i];
               // all peds in one cell
               while (p != LIST_EMPTY)
               {
                    neighbourhood.push_back(_localPedsCopy[p]);
                    p = _list[p];  // next ped
287 288 289
               }
          }
     }
tobias schroedter's avatar
tobias schroedter committed
290 291
     grid_mutex.unlock();

Ulrich Kemloh's avatar
Ulrich Kemloh committed
292
}
293

294

295 296
double LCGrid::GetCellSize()
{
297
    return _cellSize;
298
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
299 300


301 302
void LCGrid::Dump()
{
303 304
     for(int l =1; l<_gridSizeY-1; l++) {
          for(int k=1; k<_gridSizeX-1; k++) {
305

306
               int     ped = _cellHead[l][k];
307 308 309 310 311 312 313 314

               if(ped==LIST_EMPTY) continue;

               printf("Cell[%d][%d] = { ",l,k);
               // all neighbor cells
               for (int i = l - 1; i <= l + 1; ++i) {
                    for (int j = k - 1; j <= k + 1; ++j) {
                         // dummy cells will be empty
315
                         int p =  _cellHead[i][j];
316 317 318 319
                         // all peds in one cell
                         while (p != LIST_EMPTY) {
                              printf("%d, ",p+1);
                              // next ped
320
                              p = _list[p];
321 322 323 324 325 326
                         }
                    }
               }
               printf("}\n");
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
327 328
}

329 330
void LCGrid::dumpCellsOnly()
{
331 332
     for(int l =1; l<_gridSizeY-1; l++) {
          for(int k=1; k<_gridSizeX-1; k++) {
333

334
               int     ped = _cellHead[l][k];
335 336 337 338 339 340 341

               if(ped==LIST_EMPTY) continue;

               printf("Cell[%d][%d] = { ",l,k);

               // all neighbor cells
               // dummy cells will be empty
342
               int p =  _cellHead[l][k];
343 344 345 346
               // all peds in one cell
               while (p != LIST_EMPTY) {
                    printf("%d, ",p+1);
                    // next ped
347
                    p = _list[p];
348 349 350 351
               }
               printf("}\n");
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
352
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
353 354 355 356

std::string LCGrid::ToXML()
{
     string grid;
357
     for (double x=_gridXmin;x<=_gridXmax;x+=_cellSize)
Ulrich Kemloh's avatar
Ulrich Kemloh committed
358 359 360
     {
          char wall[500] = "";
          grid.append("\t\t<wall>\n");
361
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",x*FAKTOR, _gridYmin * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
362
          grid.append(wall);
363
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",x*FAKTOR, _gridYmax * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
364 365 366
          grid.append(wall);
          grid.append("\t\t</wall>\n");
     }
367
     for (double y=_gridYmin;y<=_gridYmax;y+=_cellSize)
Ulrich Kemloh's avatar
Ulrich Kemloh committed
368 369 370
     {
          char wall[500] = "";
          grid.append("\t\t<wall>\n");
371
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",_gridXmin*FAKTOR, y * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
372
          grid.append(wall);
373
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",_gridXmax*FAKTOR, y * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403
          grid.append(wall);
          grid.append("\t\t</wall>\n");
     }


     //     for (int a=0;a<pGridSizeX;a++)
     //     {
     //          double x= pGrid_xmin +a*pCellSize;
     //          char wall[500] = "";
     //          grid.append("\t\t<wall>\n");
     //          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",x*FAKTOR, pGrid_ymin * FAKTOR);
     //          grid.append(wall);
     //          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",x*FAKTOR, pGrid_ymax * FAKTOR);
     //          grid.append(wall);
     //          grid.append("\t\t</wall>\n");
     //     }
     //
     //     for (int a=0;a<pGridSizeY;a++)
     //     {
     //          double y=pGrid_ymin +a*pCellSize;
     //          char wall[500] = "";
     //          grid.append("\t\t<wall>\n");
     //          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",pGrid_xmin*FAKTOR, y * FAKTOR);
     //          grid.append(wall);
     //          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",pGrid_xmax*FAKTOR, y * FAKTOR);
     //          grid.append(wall);
     //          grid.append("\t\t</wall>\n");
     //     }
     return grid;
}