LCGrid.cpp 11.4 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
 * \version     v0.6
5
 * \copyright   <2009-2014> 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"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
38
#include "../pedestrian/Pedestrian.h"
39
#include "../geometry/Building.h"
Ulrich Kemloh's avatar
Ulrich Kemloh committed
40 41

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


44 45
LCGrid::LCGrid(double boundaries[4], double cellsize, int nPeds)
{
46 47 48 49 50 51 52
     _gridXmin=boundaries[0];
     _gridXmax=boundaries[1];
     _gridYmin=boundaries[2];
     _gridYmax=boundaries[3];
     _cellSize=cellsize;
     _nPeds=nPeds;

Ulrich Kemloh's avatar
Ulrich Kemloh committed
53

54
     // add 1 to ensure that the whole area is covered by cells if not divisible without remainder
55 56
     _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
57

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

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

64 65
          for (int j = 0; j < _gridSizeX; ++j) {
               _cellHead[i][j] = LIST_EMPTY;
66 67
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
68

69
     // creating and resetting the pedestrians list
70 71
     _list = new int[nPeds];
     for(int i=0; i<nPeds; i++) _list[i]=0;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
72

73
     //allocating the place for the peds copy
74 75
     _localPedsCopy=new Pedestrian*[nPeds];
     for(int i=0; i<nPeds; i++) _localPedsCopy[i]=NULL;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
76 77 78

}

79 80
LCGrid::~LCGrid()
{
81 82 83
     for(int i=0; i<_nPeds; i++) {
          if(!_localPedsCopy[i])
               delete _localPedsCopy[i];
84
     }
85 86 87 88
     delete [] _list;
     delete [] _localPedsCopy;
     for (int i = 0; i < _gridSizeY; ++i)  delete[] _cellHead[i];
     delete[] _cellHead;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
89 90
}

91 92
void LCGrid::ShallowCopy(const vector<Pedestrian*>& peds)
{
Ulrich Kemloh's avatar
Ulrich Kemloh committed
93

94 95
     for(unsigned int p=0; p<peds.size(); p++) {
          int id= peds[p]->GetID()-1;
96
          _localPedsCopy[id]=peds[p];
97
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
98 99
}

100 101 102 103 104 105 106 107 108 109
void LCGrid::Update(const vector<Pedestrian*>& peds)
{
     int nSize=peds.size();

     ClearGrid();

     for (int p = 0; p < nSize; p++) {
          Pedestrian* ped = peds[p];
          int id=ped->GetID()-1;
          // determine the cell coordinates of pedestrian i
110 111
          int ix = (int) ((ped->GetPos().GetX() - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
          int iy = (int) ((ped->GetPos().GetY() - _gridYmin) / _cellSize) + 1;
112 113 114
          //printf("[%f, %f]  ",ped->GetPos().GetX(), ped->GetPos().GetY());
          // create lists
          //printf("[%d=%d] ",ped->GetPedIndex(),id);
115 116
          _list[id] = _cellHead[iy][ix];
          _cellHead[iy][ix] = id;
117

118
          _localPedsCopy[id]=ped;
119
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
120 121 122
}

// I hope you had called Clear() first
123 124 125 126
void LCGrid::Update(Pedestrian* ped)
{
     int id=ped->GetID()-1;
     // determine the cell coordinates of pedestrian i
127 128
     int ix = (int) ((ped->GetPos().GetX() - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
     int iy = (int) ((ped->GetPos().GetY() - _gridYmin) / _cellSize) + 1;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
129

130
     // update the list previously created
131 132
     _list[id] = _cellHead[iy][ix];
     _cellHead[iy][ix] = id;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
133

134
     // this is probably a pedestrian coming from the mpi routine, so made a copy
135
     _localPedsCopy[id]=ped;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
136 137
}

138 139 140
void LCGrid::ClearGrid()
{
     // start by resetting the current list
141 142 143
     for (int i = 0; i < _gridSizeY; ++i) {
          for (int j = 0; j < _gridSizeX; ++j) {
               _cellHead[i][j] = LIST_EMPTY;
144 145
          }
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
146

147 148 149
     for(int i=0; i<_nPeds; i++) {
          _list[i]=LIST_EMPTY;
          _localPedsCopy[i]=NULL;
Ulrich Kemloh's avatar
Ulrich Kemloh committed
150
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
151 152
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
153
void LCGrid::HighlightNeighborhood(int pedID, Building* building)
154
{
155
     // force spotlight activation
Ulrich Kemloh's avatar
Ulrich Kemloh committed
156
     Pedestrian::SetColorMode(BY_SPOTLIGHT);
157
     //darken all
158 159 160
     for(auto&& ped: building->GetAllPedestrians())
     {
          ped->SetSpotlight(false);
161
     }
162

Ulrich Kemloh's avatar
Ulrich Kemloh committed
163
     Pedestrian* ped=building->GetPedestrian(pedID);
164
     //get and highlight the neighborhood
Ulrich Kemloh's avatar
Ulrich Kemloh committed
165 166 167
     if(ped){
          vector<Pedestrian*> neighbours;
          GetNeighbourhood(ped,neighbours);
168 169 170

          for(auto&& p: neighbours)
               p->SetSpotlight(true);
171
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
172
}
173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
/*
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
194

195 196 197 198 199 200 201 202
          for(auto&& p: neighbours)
               p->SetSpotlight(true);
          //for(unsigned int p=0;p<neighbours.size();p++){
          //     neighbours[p]->SetSpotlight(true);
          //}
     }
}
*/
203 204 205 206 207 208
void LCGrid::GetNeighbourhood(const Pedestrian* ped, vector<Pedestrian*>& neighbourhood)
{

     double xPed=ped->GetPos().GetX();
     double yPed=ped->GetPos().GetY();

209 210
     int l = (int) ((xPed - _gridXmin) / _cellSize) + 1; // +1 because of dummy cells
     int k = (int) ((yPed - _gridYmin) / _cellSize) + 1;
211 212 213 214 215 216 217 218

     //-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);
219
               int p = _cellHead[j][i];
220 221
               // all peds in one cell
               while (p != LIST_EMPTY) {
222 223 224 225 226
                    // double x=pLocalPedsCopy[p]->GetPos().GetX();
                    // double y=pLocalPedsCopy[p]->GetPos().GetY();
                    // double dist=((x-xPed)*(x-xPed) + (y-yPed)*(y-yPed));
                    if(p!=myID) {
                         // if((dist<pCellSize*pCellSize) && (p!=myID)) {
227
                         neighbourhood.push_back(_localPedsCopy[p]);
228 229
                    }
                    // next ped
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
                    p = _list[p];
               }
          }
     }
}

void LCGrid::GetNeighbourhood(const Point& pos, std::vector<Pedestrian*>& neighbourhood)
{

     double xPed=pos.GetX();
     double yPed=pos.GetY();

     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
255 256 257
               }
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
258
}
259

260

261 262
double LCGrid::GetCellSize()
{
263
    return _cellSize;
264
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
265 266


267 268
void LCGrid::Dump()
{
269 270
     for(int l =1; l<_gridSizeY-1; l++) {
          for(int k=1; k<_gridSizeX-1; k++) {
271

272
               int     ped = _cellHead[l][k];
273 274 275 276 277 278 279 280

               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
281
                         int p =  _cellHead[i][j];
282 283 284 285
                         // all peds in one cell
                         while (p != LIST_EMPTY) {
                              printf("%d, ",p+1);
                              // next ped
286
                              p = _list[p];
287 288 289 290 291 292
                         }
                    }
               }
               printf("}\n");
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
293 294
}

295 296
void LCGrid::dumpCellsOnly()
{
297 298
     for(int l =1; l<_gridSizeY-1; l++) {
          for(int k=1; k<_gridSizeX-1; k++) {
299

300
               int     ped = _cellHead[l][k];
301 302 303 304 305 306 307

               if(ped==LIST_EMPTY) continue;

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

               // all neighbor cells
               // dummy cells will be empty
308
               int p =  _cellHead[l][k];
309 310 311 312
               // all peds in one cell
               while (p != LIST_EMPTY) {
                    printf("%d, ",p+1);
                    // next ped
313
                    p = _list[p];
314 315 316 317
               }
               printf("}\n");
          }
     }
Ulrich Kemloh's avatar
Ulrich Kemloh committed
318
}
Ulrich Kemloh's avatar
Ulrich Kemloh committed
319 320 321 322

std::string LCGrid::ToXML()
{
     string grid;
323
     for (double x=_gridXmin;x<=_gridXmax;x+=_cellSize)
Ulrich Kemloh's avatar
Ulrich Kemloh committed
324 325 326
     {
          char wall[500] = "";
          grid.append("\t\t<wall>\n");
327
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",x*FAKTOR, _gridYmin * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
328
          grid.append(wall);
329
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",x*FAKTOR, _gridYmax * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
330 331 332
          grid.append(wall);
          grid.append("\t\t</wall>\n");
     }
333
     for (double y=_gridYmin;y<=_gridYmax;y+=_cellSize)
Ulrich Kemloh's avatar
Ulrich Kemloh committed
334 335 336
     {
          char wall[500] = "";
          grid.append("\t\t<wall>\n");
337
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",_gridXmin*FAKTOR, y * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
338
          grid.append(wall);
339
          sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",_gridXmax*FAKTOR, y * FAKTOR);
Ulrich Kemloh's avatar
Ulrich Kemloh committed
340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369
          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;
}