VelocityModel.cpp 20.8 KB
Newer Older
Mohcine Chraibi's avatar
Mohcine Chraibi committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
/**
 * \file        VelocityModel.cpp
 * \date        Aug. 07, 2015
 * \version     v0.7
 * \copyright   <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
 *
 * \section License
 * This file is part of JuPedSim.
 *
 * JuPedSim is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * JuPedSim is distributed in the hope that it will be useful,
 * 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.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
 *
 * \section Description
 * Implementation of first-order model
 * 3. Velocity Model: Tordeux2015
 *
 *
 **/


#include "../pedestrian/Pedestrian.h"
Mohcine Chraibi's avatar
Mohcine Chraibi committed
32
//#include "../routing/DirectionStrategy.h"
Mohcine Chraibi's avatar
Mohcine Chraibi committed
33 34 35 36
#include "../mpi/LCGrid.h"
#include "../geometry/Wall.h"
#include "../geometry/SubRoom.h"

37

Mohcine Chraibi's avatar
Mohcine Chraibi committed
38 39 40 41 42 43 44 45 46
#include "VelocityModel.h"

#ifdef _OPENMP
#include <omp.h>
#else
#define omp_get_thread_num() 0
#define omp_get_max_threads()  1
#endif

Mohcine Chraibi's avatar
Mohcine Chraibi committed
47 48 49 50
double xRight = 26.0;
double xLeft = 0.0;
double cutoff = 2.0;

Mohcine Chraibi's avatar
Mohcine Chraibi committed
51 52 53
using std::vector;
using std::string;

54
VelocityModel::VelocityModel(std::shared_ptr<DirectionStrategy> dir, double aped, double Dped,
Mohcine Chraibi's avatar
Mohcine Chraibi committed
55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71
                             double awall, double Dwall)
{
     _direction = dir;
     // Force_rep_PED Parameter
     _aPed = aped;
     _DPed = Dped;
     // Force_rep_WALL Parameter
     _aWall = awall;
     _DWall = Dwall;
}


VelocityModel::~VelocityModel()
{

}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
72
bool VelocityModel::Init (Building* building)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
73
{
Arne Graf's avatar
Arne Graf committed
74

f.mack's avatar
f.mack committed
75
    if(auto dirff = dynamic_cast<DirectionFloorfield*>(_direction.get())){
Arne Graf's avatar
Arne Graf committed
76
        Log->Write("INFO:\t Init DirectionFloorfield starting ...");
Arne Graf's avatar
Arne Graf committed
77 78 79
        double _deltaH = building->GetConfig()->get_deltaH();
        double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
        bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
f.mack's avatar
f.mack committed
80
        dirff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Arne Graf's avatar
Arne Graf committed
81 82 83
        Log->Write("INFO:\t Init DirectionFloorfield done");
    }

f.mack's avatar
f.mack committed
84
     if(auto dirlocff = dynamic_cast<DirectionLocalFloorfield*>(_direction.get())){
85
          Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ...");
Larissa Ellerbrock's avatar
Larissa Ellerbrock committed
86 87 88
          double _deltaH = building->GetConfig()->get_deltaH();
          double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
          bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
f.mack's avatar
f.mack committed
89
          dirlocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
90 91 92
          Log->Write("INFO:\t Init DirectionLOCALFloorfield done");
     }

f.mack's avatar
f.mack committed
93
     if(auto dirsublocff = dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get())){
Arne Graf's avatar
Arne Graf committed
94
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfield starting ...");
Larissa Ellerbrock's avatar
Larissa Ellerbrock committed
95 96 97
          double _deltaH = building->GetConfig()->get_deltaH();
          double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
          bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
f.mack's avatar
f.mack committed
98
          dirsublocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Arne Graf's avatar
Arne Graf committed
99 100 101
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfield done");
     }

Mohcine Chraibi's avatar
Mohcine Chraibi committed
102
    const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
Oliver Schmidts's avatar
Oliver Schmidts committed
103 104
     size_t peds_size = allPeds.size();
    for(unsigned int p=0;p < peds_size;p++)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
105 106 107 108 109 110
    {
         Pedestrian* ped = allPeds[p];
         double cosPhi, sinPhi;
         //a destination could not be found for that pedestrian
         if (ped->FindRoute() == -1) {
              Log->Write(
111
                   "ERROR:\tVelocityModel::Init() cannot initialise route. ped %d is deleted in Room %d %d.\n",ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID());
Oliver Schmidts's avatar
Oliver Schmidts committed
112 113 114
              building->DeletePedestrian(ped);
              p--;
              peds_size--;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
115 116
              continue;
         }
b.schroeder's avatar
b.schroeder committed
117

Mohcine Chraibi's avatar
Mohcine Chraibi committed
118

Mohcine Chraibi's avatar
Mohcine Chraibi committed
119 120


Arne Graf's avatar
Arne Graf committed
121
         Point target = ped->GetExitLine()->ShortestPoint(ped->GetPos());
Mohcine Chraibi's avatar
Mohcine Chraibi committed
122 123 124
         Point d = target - ped->GetPos();
         double dist = d.Norm();
         if (dist != 0.0) {
Oliver Schmidts's avatar
Oliver Schmidts committed
125 126
              cosPhi = d._x / dist;
              sinPhi = d._y / dist;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
127 128
         } else {
              Log->Write(
Ulrich Kemloh's avatar
Ulrich Kemloh committed
129
                   "ERROR: \tallPeds::Init() cannot initialise phi! "
Mohcine Chraibi's avatar
Mohcine Chraibi committed
130 131 132 133
                   "dist to target is 0\n");
              return false;
         }

134
         ped->InitV0(target);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
135 136 137 138 139 140 141 142 143

         JEllipse E = ped->GetEllipse();
         E.SetCosPhi(cosPhi);
         E.SetSinPhi(sinPhi);
         ped->SetEllipse(E);
    }
    return true;
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
144
void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
145 146 147
{
      // collect all pedestrians in the simulation.
      const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
148 149
      vector<Pedestrian*> pedsToRemove;
      pedsToRemove.reserve(500);
150 151
      unsigned long nSize;
      nSize = allPeds.size();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
152 153

      int nThreads = omp_get_max_threads();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
154

Arne Graf's avatar
Arne Graf committed
155
      //nThreads = 1; //debug only
156
      int partSize;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
157 158
      partSize = ((int)nSize > nThreads)? (int) (nSize / nThreads):(int)nSize;
      if(partSize == (int)nSize)
159
            nThreads = 1; // not worthy to parallelize
Mohcine Chraibi's avatar
Mohcine Chraibi committed
160
      #pragma omp parallel default(shared) num_threads(nThreads)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
161 162 163
      {
           vector< Point > result_acc = vector<Point > ();
           result_acc.reserve(nSize);
164
           vector< my_pair > spacings = vector<my_pair > ();
165 166
           spacings.reserve(nSize); // larger than needed
           spacings.push_back(my_pair(100, 1)); // in case there are no neighbors
Mohcine Chraibi's avatar
Mohcine Chraibi committed
167 168 169 170
           const int threadID = omp_get_thread_num();

           int start = threadID*partSize;
           int end;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
171 172 173
           end = (threadID < nThreads - 1) ? (threadID + 1) * partSize - 1: (int) (nSize - 1);
           for (int p = start; p <= end; ++p) {
                 // printf("\n------------------\nid=%d\t p=%d\n", threadID, p);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194
                Pedestrian* ped = allPeds[p];
                Room* room = building->GetRoom(ped->GetRoomID());
                SubRoom* subroom = room->GetSubRoom(ped->GetSubRoomID());
                Point repPed = Point(0,0);
                vector<Pedestrian*> neighbours;
                building->GetGrid()->GetNeighbourhood(ped,neighbours);

                int size = (int) neighbours.size();
                for (int i = 0; i < size; i++) {
                     Pedestrian* ped1 = neighbours[i];
                     //if they are in the same subroom
                     Point p1 = ped->GetPos();
                     Point p2 = ped1->GetPos();
                     //subrooms to consider when looking for neighbour for the 3d visibility
                     vector<SubRoom*> emptyVector;
                     emptyVector.push_back(subroom);
                     emptyVector.push_back(building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID()));
                     bool isVisible = building->IsVisible(p1, p2, emptyVector, false);
                     if (!isVisible)
                          continue;
                     if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
195
                           repPed += ForceRepPed(ped, ped1, periodic);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
196 197 198 199
                     } else {
                          // or in neighbour subrooms
                          SubRoom* sb2=building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
                          if(subroom->IsDirectlyConnectedWith(sb2)) {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
200
                                repPed += ForceRepPed(ped, ped1, periodic);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
201 202
                          }
                     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
203 204 205 206 207 208 209 210
                } // for i
                //repulsive forces to walls and closed transitions that are not my target
                Point repWall = ForceRepRoom(allPeds[p], subroom);

                // calculate new direction ei according to (6)
                Point direction = e0(ped, room) + repPed + repWall;
                for (int i = 0; i < size; i++) {
                      Pedestrian* ped1 = neighbours[i];
Mohcine Chraibi's avatar
Mohcine Chraibi committed
211
                     // calculate spacing
Mohcine Chraibi's avatar
Mohcine Chraibi committed
212
                     // my_pair spacing_winkel = GetSpacing(ped, ped1);
213 214 215 216 217 218 219 220 221
                      if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
                            spacings.push_back(GetSpacing(ped, ped1, direction, periodic));
                      } else {
                            // or in neighbour subrooms
                            SubRoom* sb2=building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
                            if(subroom->IsDirectlyConnectedWith(sb2)) {
                                  spacings.push_back(GetSpacing(ped, ped1, direction, periodic));
                            }
                      }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
222
                }
223
                // @todo: get spacing to walls
224
                // @todo: update direction every DT?
225

Mohcine Chraibi's avatar
Mohcine Chraibi committed
226
                // if(ped->GetID()==-10)
227
                //       std::cout << "time: " << ped->GetGlobalTime() << "  |  updateRate  " <<ped->GetUpdateRate() << "   modulo " <<fmod(ped->GetGlobalTime(), ped->GetUpdateRate())<<std::endl;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
228

Mohcine Chraibi's avatar
Mohcine Chraibi committed
229
                // calculate min spacing
230
                std::sort(spacings.begin(), spacings.end(), sort_pred());
231
                double spacing = spacings[0].first;
232 233 234 235 236 237 238 239 240 241
                //============================================================
                // TODO: Hack for Head on situations: ped1 x ------> | <------- x ped2
                if(0 && direction.NormSquare() < 0.5)
                {
                      double pi_half = 1.57079663;
                      double alpha =  pi_half*exp(-spacing);
                      direction = e0(ped, room).Rotate(cos(alpha), sin(alpha));
                      printf("\nRotate %f, %f, norm = %f alpha = %f, spacing = %f\n", direction._x, direction._y, direction.NormSquare(), alpha, spacing);
                      getc(stdin);
                }
242
                //============================================================
Mohcine Chraibi's avatar
Mohcine Chraibi committed
243
                //double winkel = spacings[0].second;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
244
                //Point tmp;
245
                Point speed = direction.Normalized() *OptimalSpeed(ped, spacing);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
246
                result_acc.push_back(speed);                
b.schroeder's avatar
b.schroeder committed
247

Mohcine Chraibi's avatar
Mohcine Chraibi committed
248
                spacings.clear(); //clear for ped p
249

Mohcine Chraibi's avatar
Mohcine Chraibi committed
250
                // stuck peds get removed. Warning is thrown. low speed due to jam is omitted.
251 252 253
                if(ped->GetGlobalTime() > 30 + ped->GetPremovementTime() &&
                          std::max(ped->GetMeanVelOverRecTime(), ped->GetV().Norm()) < 0.01 &&
                          size == 0 ) // size length of peds neighbour vector
b.schroeder's avatar
b.schroeder committed
254
                {
255
                      Log->Write("WARNING:\tped %d with vmean  %f has been deleted in room [%i]/[%i] after time %f s (current=%f\n", ped->GetID(), ped->GetMeanVelOverRecTime(), ped->GetRoomID(), ped->GetSubRoomID(), ped->GetGlobalTime(), current);
f.mack's avatar
f.mack committed
256
                      #pragma omp critical(VelocityModel_ComputeNextTimeStep_pedsToRemove)
257
                      pedsToRemove.push_back(ped);
b.schroeder's avatar
b.schroeder committed
258 259
                }

Mohcine Chraibi's avatar
Mohcine Chraibi committed
260 261
           } // for p

Mohcine Chraibi's avatar
Mohcine Chraibi committed
262
           #pragma omp barrier
Mohcine Chraibi's avatar
Mohcine Chraibi committed
263 264 265 266 267 268 269 270 271 272 273 274 275 276
           // update
           for (int p = start; p <= end; ++p) {
                Pedestrian* ped = allPeds[p];

                Point v_neu = result_acc[p - start];
                Point pos_neu = ped->GetPos() + v_neu * deltaT;

               //Jam is based on the current velocity
                if ( v_neu.Norm() >= ped->GetV0Norm()*0.5) {
                     ped->ResetTimeInJam();
                } else {
                     ped->UpdateTimeInJam();
                }
                //only update the position if the velocity is above a threshold
277
                if (v_neu.Norm() >= J_EPS_V)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
278 279 280
                {
                     ped->SetPhiPed();
                }
281
                ped->SetPos(pos_neu);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
282
                if(periodic){
Oliver Schmidts's avatar
Oliver Schmidts committed
283 284
                      if(ped->GetPos()._x >= xRight){
                            ped->SetPos(Point(ped->GetPos()._x - (xRight - xLeft), ped->GetPos()._y));
285
                            //ped->SetID( ped->GetID() + 1);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
286 287
                      }
                }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
288 289 290
                ped->SetV(v_neu);
           }
      }//end parallel
Mohcine Chraibi's avatar
Mohcine Chraibi committed
291

292 293 294 295 296
      // remove the pedestrians that have left the building
      for (unsigned int p = 0; p<pedsToRemove.size(); p++) {
            building->DeletePedestrian(pedsToRemove[p]);
      }
      pedsToRemove.clear();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
297 298 299 300
}

Point VelocityModel::e0(Pedestrian* ped, Room* room) const
{
f.mack's avatar
f.mack committed
301
      const Point target = _direction->GetTarget(room, ped); // target is where the ped wants to be after the next timestep
Mohcine Chraibi's avatar
Mohcine Chraibi committed
302
      Point desired_direction;
Arne Graf's avatar
Arne Graf committed
303
      const Point pos = ped->GetPos();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
304 305
      double dist = ped->GetExitLine()->DistTo(pos);
      // check if the molified version works
306 307 308
      Point lastE0 = ped->GetLastE0();
      ped->SetLastE0(target-pos);

309 310 311
      if ( (dynamic_cast<DirectionFloorfield*>(_direction.get())) ||
           (dynamic_cast<DirectionLocalFloorfield*>(_direction.get())) ||
           (dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get()))  ) {
312
          if (dist > 50*J_EPS_GOAL) {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
313
               desired_direction = target - pos; //ped->GetV0(target);
314
          } else {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
315
               desired_direction = lastE0;
316 317 318 319
               ped->SetLastE0(lastE0); //keep old vector (revert set operation done 9 lines above)
          }
      }
      else if (dist > J_EPS_GOAL) {
Mohcine Chraibi's avatar
Mohcine Chraibi committed
320
            desired_direction = ped->GetV0(target);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
321 322
      } else {
          ped->SetSmoothTurning();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
323
          desired_direction = ped->GetV0();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
324
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
325
     return desired_direction;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
326 327 328
}


329
double VelocityModel::OptimalSpeed(Pedestrian* ped, double spacing) const
Mohcine Chraibi's avatar
Mohcine Chraibi committed
330 331 332
{
      double v0 = ped->GetV0Norm();
      double T = ped->GetT();
333
      double l = 2*ped->GetEllipse().GetBmax(); //assume peds are circles with const radius
Mohcine Chraibi's avatar
Mohcine Chraibi committed
334 335 336
      double speed = (spacing-l)/T;
      speed = (speed>0)?speed:0;
      speed = (speed<v0)?speed:v0;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
337
//      (1-winkel)*speed;
338
     //todo use winkel
Mohcine Chraibi's avatar
Mohcine Chraibi committed
339 340 341
      return speed;
}

342
// return spacing and id of the nearest pedestrian
Mohcine Chraibi's avatar
Mohcine Chraibi committed
343
my_pair VelocityModel::GetSpacing(Pedestrian* ped1, Pedestrian* ped2, Point ei, int periodic) const
Mohcine Chraibi's avatar
Mohcine Chraibi committed
344
{
345
      Point distp12 = ped2->GetPos() - ped1->GetPos(); // inversed sign
Mohcine Chraibi's avatar
Mohcine Chraibi committed
346
      if(periodic){
Oliver Schmidts's avatar
Oliver Schmidts committed
347 348
            double x = ped1->GetPos()._x;
            double x_j = ped2->GetPos()._x;
349

Mohcine Chraibi's avatar
Mohcine Chraibi committed
350
            if((xRight-x) + (x_j-xLeft) <= cutoff){
Oliver Schmidts's avatar
Oliver Schmidts committed
351
                 distp12._x = distp12._x + xRight - xLeft;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
352 353
            }
      }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
354
      double Distance = distp12.Norm();
355
      double l = 2*ped1->GetEllipse().GetBmax();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
356 357 358 359 360
      Point ep12;
      if (Distance >= J_EPS) {
            ep12 = distp12.Normalized();
      } else {
            //printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
Mohcine Chraibi's avatar
Mohcine Chraibi committed
361
            Log->Write("WARNING: \tin VelocityModel::GetSPacing() ep12 can not be calculated!!!\n");
362
            Log->Write("\t\t Pedestrians are too near to each other (%f).", Distance);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
363 364
            exit(EXIT_FAILURE);
     }
365

366
      double condition1 = ei.ScalarProduct(ep12); // < e_i , e_ij > should be positive
Mohcine Chraibi's avatar
Mohcine Chraibi committed
367 368
      double condition2 = ei.Rotate(0, 1).ScalarProduct(ep12); // theta = pi/2. condition2 should <= than l/Distance
      condition2 = (condition2>0)?condition2:-condition2; // abs
369

Mohcine Chraibi's avatar
Mohcine Chraibi committed
370
      if((condition1 >=0 ) && (condition2 <= l/Distance))
371
            // return a pair <dist, condition1>. Then take the smallest dist. In case of equality the biggest condition1
372
            return  my_pair(distp12.Norm(), ped2->GetID());
Mohcine Chraibi's avatar
Mohcine Chraibi committed
373
      else
374
            return  my_pair(FLT_MAX, ped2->GetID());
375
}
Mohcine Chraibi's avatar
Mohcine Chraibi committed
376
Point VelocityModel::ForceRepPed(Pedestrian* ped1, Pedestrian* ped2, int periodic) const
377
{
Mohcine Chraibi's avatar
Mohcine Chraibi committed
378 379 380
     Point F_rep(0.0, 0.0);
     // x- and y-coordinate of the distance between p1 and p2
     Point distp12 = ped2->GetPos() - ped1->GetPos();
381

Mohcine Chraibi's avatar
Mohcine Chraibi committed
382
     if(periodic){
Oliver Schmidts's avatar
Oliver Schmidts committed
383 384
            double x = ped1->GetPos()._x;
            double x_j = ped2->GetPos()._x;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
385
            if((xRight-x) + (x_j-xLeft) <= cutoff){
Oliver Schmidts's avatar
Oliver Schmidts committed
386
                 distp12._x = distp12._x + xRight - xLeft;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
387 388
            }
      }
389

Mohcine Chraibi's avatar
Mohcine Chraibi committed
390 391 392
     double Distance = distp12.Norm();
     Point ep12; // x- and y-coordinate of the normalized vector between p1 and p2
     double R_ij;
393
     double l = 2*ped1->GetEllipse().GetBmax();
394

Mohcine Chraibi's avatar
Mohcine Chraibi committed
395 396 397 398
     if (Distance >= J_EPS) {
          ep12 = distp12.Normalized();
     } else {
          //printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
Mohcine Chraibi's avatar
Mohcine Chraibi committed
399 400 401
          Log->Write(KRED "\nWARNING: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!" RESET);
          Log->Write("\t\t Pedestrians are too near to each other (dist=%f).", Distance);
          Log->Write("\t\t Maybe the value of <a> in force_ped should be increased. Going to exit.\n");
402
          printf("ped1 %d  ped2 %d\n", ped1->GetID(), ped2->GetID());
Oliver Schmidts's avatar
Oliver Schmidts committed
403
          printf("ped1 at (%f, %f), ped2 at (%f, %f)\n", ped1->GetPos()._x, ped1->GetPos()._y, ped2->GetPos()._x, ped2->GetPos()._y);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
404 405
          exit(EXIT_FAILURE);
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
406 407 408 409 410 411
      Point ei = ped1->GetV().Normalized();
      if(ped1->GetV().NormSquare()<0.01){
            ei = ped1->GetV0().Normalized();
      }
      double condition1 = ei.ScalarProduct(ep12); // < e_i , e_ij > should be positive
      condition1 = (condition1>0)?condition1:0; // abs
412 413 414

      R_ij = - _aPed * exp((l-Distance)/_DPed);
      F_rep = ep12 * R_ij;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451

     return F_rep;
}//END Velocity:ForceRepPed()

Point VelocityModel::ForceRepRoom(Pedestrian* ped, SubRoom* subroom) const
{
     Point f(0., 0.);
     const Point& centroid = subroom->GetCentroid();
     bool inside = subroom->IsInSubRoom(centroid);
     //first the walls
     for(const auto & wall: subroom->GetAllWalls())
     {
           f += ForceRepWall(ped, wall, centroid, inside);
     }

     //then the obstacles
     for(const auto & obst: subroom->GetAllObstacles())
     {
          if(obst->Contains(ped->GetPos()))
          {
               Log->Write("ERROR:\t Agent [%d] is trapped in obstacle in room/subroom [%d/%d]",ped->GetID(),subroom->GetRoomID(), subroom->GetSubRoomID());
               exit(EXIT_FAILURE);
          }
          else
          for(const auto & wall: obst->GetAllWalls())
          {
                f += ForceRepWall(ped, wall, centroid, inside);
          }
     }

     // and finally the closed doors
     for(const auto & goal: subroom->GetAllTransitions())
     {
          if(! goal->IsOpen())
          {
                f +=  ForceRepWall(ped,*(static_cast<Line*>(goal)), centroid, inside);
          }
Mohcine Chraibi's avatar
s  
Mohcine Chraibi committed
452 453 454 455 456 457 458 459 460
          // int uid1= goal->GetUniqueID();
          // int uid2=ped->GetExitIndex();
          // // ignore my transition consider closed doors
          // //closed doors are considered as wall

          // if((uid1 != uid2) || (goal->IsOpen()==false ))
          // {
          //      f +=  ForceRepWall(ped,*(static_cast<Line*>(goal)), centroid, inside);
          // }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
461 462 463 464 465 466 467
     }

     return f;
}

Point VelocityModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& centroid, bool inside) const
{
468
     Point F_wrep = Point(0.0, 0.0);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
469 470 471 472 473 474 475 476
     Point pt = w.ShortestPoint(ped->GetPos());

     Point dist = pt - ped->GetPos(); // x- and y-coordinate of the distance between ped and p
     const double EPS = 0.000; // molified see Koester2013
     double Distance = dist.Norm() + EPS; // distance between the centre of ped and point p
     //double vn = w.NormalComp(ped->GetV()); //normal component of the velocity on the wall
     Point e_iw; // x- and y-coordinate of the normalized vector between ped and pt
     //double K_iw;
477
     double l = ped->GetEllipse().GetBmax();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
478
     double R_iw;
479
     double min_distance_to_wall = 0.001; // 10 cm
480

Mohcine Chraibi's avatar
Mohcine Chraibi committed
481 482 483 484
     if (Distance > min_distance_to_wall) {
           e_iw = dist / Distance;
     }
     else {
Mohcine Chraibi's avatar
s  
Mohcine Chraibi committed
485
          Log->Write("WARNING:\t Velocity: forceRepWall() ped %d [%f, %f] is too near to the wall [%f, %f]-[%f, %f] (dist=%f)", ped->GetID(), ped->GetPos()._y, ped->GetPos()._y, w.GetPoint1()._x, w.GetPoint1()._y,  w.GetPoint2()._x, w.GetPoint2()._y,Distance);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
486 487
          Point new_dist = centroid - ped->GetPos();
          new_dist = new_dist/new_dist.Norm();
488
          //printf("new distance = (%f, %f) inside=%d\n", new_dist._x, new_dist._y, inside);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
489 490 491 492 493 494 495 496 497 498 499 500 501 502 503
          e_iw = (inside ? new_dist:new_dist*-1);
     }
     //-------------------------

     const Point& pos = ped->GetPos();
     double distGoal = ped->GetExitLine()->DistToSquare(pos);
     if(distGoal < J_EPS_GOAL*J_EPS_GOAL)
          return F_wrep;
//-------------------------
     R_iw = - _aWall * exp((l-Distance)/_DWall);
     F_wrep = e_iw * R_iw;

     return F_wrep;
}

Ulrich Kemloh's avatar
Ulrich Kemloh committed
504
string VelocityModel::GetDescription()
Mohcine Chraibi's avatar
Mohcine Chraibi committed
505 506 507
{
     string rueck;
     char tmp[CLENGTH];
508

Mohcine Chraibi's avatar
Mohcine Chraibi committed
509 510 511 512 513 514 515
     sprintf(tmp, "\t\ta: \t\tPed: %f \tWall: %f\n", _aPed, _aWall);
     rueck.append(tmp);
     sprintf(tmp, "\t\tD: \t\tPed: %f \tWall: %f\n", _DPed, _DWall);
     rueck.append(tmp);
     return rueck;
}

516
std::shared_ptr<DirectionStrategy> VelocityModel::GetDirection() const
Mohcine Chraibi's avatar
Mohcine Chraibi committed
517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541
{
     return _direction;
}


double VelocityModel::GetaPed() const
{
     return _aPed;
}

double VelocityModel::GetDPed() const
{
     return _DPed;
}


double VelocityModel::GetaWall() const
{
     return _aWall;
}

double VelocityModel::GetDWall() const
{
     return _DWall;
}