VelocityModel.cpp 24.6 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
/**
 * \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
 *
 *
 **/

Qiancheng Xu's avatar
Qiancheng Xu committed
30
# define NOMINMAX
Mohcine Chraibi's avatar
Mohcine Chraibi committed
31
#include "../pedestrian/Pedestrian.h"
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
#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
46 47 48 49
double xRight = 26.0;
double xLeft = 0.0;
double cutoff = 2.0;

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

53
VelocityModel::VelocityModel(std::shared_ptr<DirectionStrategy> dir, double aped, double Dped,
Mohcine Chraibi's avatar
Mohcine Chraibi committed
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70
                             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
71
bool VelocityModel::Init (Building* building)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
72
{
73 74 75
     double _deltaH = building->GetConfig()->get_deltaH();
     double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
     bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
76

77
     if(auto dirff = dynamic_cast<DirectionFloorfield*>(_direction.get())){
f.mack's avatar
f.mack committed
78
        dirff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
79 80 81
        Log->Write("INFO:\t Init DirectionFloorfield done");
    }

f.mack's avatar
f.mack committed
82
     if(auto dirlocff = dynamic_cast<DirectionLocalFloorfield*>(_direction.get())){
83
          Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ...");
f.mack's avatar
f.mack committed
84
          dirlocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
85 86 87
          Log->Write("INFO:\t Init DirectionLOCALFloorfield done");
     }

f.mack's avatar
f.mack committed
88
     if(auto dirsublocff = dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get())){
Arne Graf's avatar
Arne Graf committed
89
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfield starting ...");
f.mack's avatar
f.mack committed
90
          dirsublocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Arne Graf's avatar
Arne Graf committed
91 92 93
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfield done");
     }

94 95 96 97 98 99
     if(auto dirsublocffTrips = dynamic_cast<DirectionSubLocalFloorfieldTrips*>(_direction.get())){
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfieldTrips starting ...");
          dirsublocffTrips->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfieldTrips done");
     }

100 101 102 103 104 105 106 107
     if(auto dirsublocffTripsVoronoi = dynamic_cast<DirectionSubLocalFloorfieldTripsVoronoi*>(_direction.get())){
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfieldTripsVoronoi starting ...");
          dirsublocffTripsVoronoi->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
          Log->Write("INFO:\t Init DirectionSubLOCALFloorfieldTripsVoronoi done");
     }


     const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
Oliver Schmidts's avatar
Oliver Schmidts committed
108
     size_t peds_size = allPeds.size();
109
     std::cout << "Building has " << peds_size << " peds\n";
Oliver Schmidts's avatar
Oliver Schmidts committed
110
    for(unsigned int p=0;p < peds_size;p++)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
111 112 113 114
    {
         Pedestrian* ped = allPeds[p];
         double cosPhi, sinPhi;
         //a destination could not be found for that pedestrian
115 116 117 118
         int ped_is_waiting = 1;// quick and dirty fix
         // we should maybe differentiate between pedestrians who did not find
         // routs because of a bug in the router and these who simplyt just want
         // to wait in waiting areas
119 120 121
         int res = ped->FindRoute();
         if (!ped_is_waiting && res == -1) {
              std::cout << ped->GetID() << " has no route\n";
Mohcine Chraibi's avatar
Mohcine Chraibi committed
122
              Log->Write(
123
                   "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
124
              building->DeletePedestrian(ped);
125
              Log->incrementDeletedAgents();
Oliver Schmidts's avatar
Oliver Schmidts committed
126 127
              p--;
              peds_size--;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
128 129
              continue;
         }
b.schroeder's avatar
b.schroeder committed
130

131 132 133
         // TODO
         // HERE every ped should have a navline already
         //
Mohcine Chraibi's avatar
Mohcine Chraibi committed
134 135


136 137 138
        Point target = Point(0,0);
        if(ped->GetExitLine())
            target = ped->GetExitLine()->ShortestPoint(ped->GetPos());
139 140 141 142
        else{
             std::cout << "Ped " << ped->GetID() << " has no exit line in INIT\n";
             //exit(EXIT_FAILURE);
        }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
143 144 145
         Point d = target - ped->GetPos();
         double dist = d.Norm();
         if (dist != 0.0) {
Oliver Schmidts's avatar
Oliver Schmidts committed
146 147
              cosPhi = d._x / dist;
              sinPhi = d._y / dist;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
148 149
         } else {
              Log->Write(
Ulrich Kemloh's avatar
Ulrich Kemloh committed
150
                   "ERROR: \tallPeds::Init() cannot initialise phi! "
Mohcine Chraibi's avatar
Mohcine Chraibi committed
151 152 153 154
                   "dist to target is 0\n");
              return false;
         }

155
         ped->InitV0(target);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
156 157 158 159 160 161 162 163 164

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

Ulrich Kemloh's avatar
Ulrich Kemloh committed
165
void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
166 167 168
{
      // collect all pedestrians in the simulation.
      const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
169 170
      vector<Pedestrian*> pedsToRemove;
      pedsToRemove.reserve(500);
171 172
      unsigned long nSize;
      nSize = allPeds.size();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
173 174

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

Arne Graf's avatar
Arne Graf committed
176
      //nThreads = 1; //debug only
177
      int partSize;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
178 179
      partSize = ((int)nSize > nThreads)? (int) (nSize / nThreads):(int)nSize;
      if(partSize == (int)nSize)
180
            nThreads = 1; // not worthy to parallelize
tobias schroedter's avatar
tobias schroedter committed
181 182 183


     //TODO richtig parallelisieren!
Mohcine Chraibi's avatar
Mohcine Chraibi committed
184
      #pragma omp parallel default(shared) num_threads(nThreads)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
185 186 187
      {
           vector< Point > result_acc = vector<Point > ();
           result_acc.reserve(nSize);
188
           vector< my_pair > spacings = vector<my_pair > ();
189 190
           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
191 192 193 194
           const int threadID = omp_get_thread_num();

           int start = threadID*partSize;
           int end;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
195 196
           end = (threadID < nThreads - 1) ? (threadID + 1) * partSize - 1: (int) (nSize - 1);
           for (int p = start; p <= end; ++p) {
197
                // printf("\n------------------\nid=%d (%d)\t p=%d\n", threadID, nThreads, p);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
198 199 200 201 202 203 204 205
                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();
tobias schroedter's avatar
tobias schroedter committed
206 207 208 209 210 211 212
////                if (ped->GetID() == 71) {
////                     std::cout << "------------------------------------" << std::endl;
//                     std::cout << Pedestrian::GetGlobalTime() << ":\t\tVelocity Model debug ped: " << ped->GetID()
//                               << "\t" << ped->GetPos().toString() << "\t" << size<< std::endl;
////                }


Mohcine Chraibi's avatar
Mohcine Chraibi committed
213
                for (int i = 0; i < size; i++) {
214

Mohcine Chraibi's avatar
Mohcine Chraibi committed
215
                     Pedestrian* ped1 = neighbours[i];
tobias schroedter's avatar
tobias schroedter committed
216 217 218 219 220 221 222 223 224 225
//                     if (ped->GetID() == 71) {
//                         std::cout << "Velocity Model debug ped1: " << ped1->GetID() << "\t" <<  ped1->GetPos().toString() <<std::endl;
//                     }

//                     std::cout << "Velocity Model debug ped1: " << ped1 << std::endl;
//                     std::cout << ped1->GetID() << std::endl;

                     if (ped1 == nullptr){
                          std::cout << "Velocity Model debug: " << size << std::endl;
                     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
226 227
                     //if they are in the same subroom
                     Point p1 = ped->GetPos();
tobias schroedter's avatar
tobias schroedter committed
228

Mohcine Chraibi's avatar
Mohcine Chraibi committed
229 230 231 232 233 234 235 236 237
                     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
238
                           repPed += ForceRepPed(ped, ped1, periodic);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
239 240 241 242
                     } 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
243
                                repPed += ForceRepPed(ped, ped1, periodic);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
244 245
                          }
                     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
246 247 248 249 250 251 252 253
                } // 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
254
                     // calculate spacing
Mohcine Chraibi's avatar
Mohcine Chraibi committed
255
                     // my_pair spacing_winkel = GetSpacing(ped, ped1);
256 257 258 259 260 261 262 263 264
                      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
265
                }
266
                // @todo: get spacing to walls
267
                // @todo: update direction every DT?
268

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

Mohcine Chraibi's avatar
Mohcine Chraibi committed
272
                // calculate min spacing
273
                std::sort(spacings.begin(), spacings.end(), sort_pred());
274
                double spacing = spacings[0].first;
275 276 277 278 279 280 281 282 283 284
                //============================================================
                // 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);
                }
285
                //============================================================
Mohcine Chraibi's avatar
Mohcine Chraibi committed
286
                //double winkel = spacings[0].second;
287
                //Point tmp;
288
                Point speed = direction.Normalized() *OptimalSpeed(ped, spacing);
289
                result_acc.push_back(speed);
b.schroeder's avatar
b.schroeder committed
290

291

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

Mohcine Chraibi's avatar
Mohcine Chraibi committed
294
                // stuck peds get removed. Warning is thrown. low speed due to jam is omitted.
295
                if(ped->GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 10000 + ped->GetPremovementTime() &&
296 297
                          std::max(ped->GetMeanVelOverRecTime(), ped->GetV().Norm()) < 0.01 &&
                          size == 0 ) // size length of peds neighbour vector
b.schroeder's avatar
b.schroeder committed
298
                {
299
                     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);
300
                      Log->incrementDeletedAgents();
f.mack's avatar
f.mack committed
301
                      #pragma omp critical(VelocityModel_ComputeNextTimeStep_pedsToRemove)
302
                      pedsToRemove.push_back(ped);
b.schroeder's avatar
b.schroeder committed
303 304
                }

Mohcine Chraibi's avatar
Mohcine Chraibi committed
305 306
           } // for p

Mohcine Chraibi's avatar
Mohcine Chraibi committed
307
           #pragma omp barrier
Mohcine Chraibi's avatar
Mohcine Chraibi committed
308 309 310 311 312 313 314 315 316 317 318 319 320 321
           // 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
322
                if (v_neu.Norm() >= J_EPS_V)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
323 324 325
                {
                     ped->SetPhiPed();
                }
326
                ped->SetPos(pos_neu);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
327
                if(periodic){
Oliver Schmidts's avatar
Oliver Schmidts committed
328 329
                      if(ped->GetPos()._x >= xRight){
                            ped->SetPos(Point(ped->GetPos()._x - (xRight - xLeft), ped->GetPos()._y));
330
                            //ped->SetID( ped->GetID() + 1);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
331 332
                      }
                }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
333 334
                ped->SetV(v_neu);
           }
335 336 337 338 339
           // if(threadID == -1 )
           //      std::cout << " result_acc size " << result_acc.size() << "\n";
           //getc(stdin);


Mohcine Chraibi's avatar
Mohcine Chraibi committed
340
      }//end parallel
Mohcine Chraibi's avatar
Mohcine Chraibi committed
341

342 343 344 345 346
      // 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
347 348 349 350
}

Point VelocityModel::e0(Pedestrian* ped, Room* room) const
{
351 352 353
      Point target;
      if(_direction && ped->GetExitLine())
           target = _direction->GetTarget(room, ped); // target is where the ped wants to be after the next timestep
354
      else { //@todo: we need a model for waiting pedestrians
355
           std::cout << ped->GetID() << " VelocityModel::e0 Ped has no navline.\n";
356 357 358 359 360 361 362 363 364 365 366
           //exit(EXIT_FAILURE);
           // set random destination
           std::mt19937 mt(ped->GetBuilding()->GetConfig()->GetSeed());
           std::uniform_real_distribution<double> dist(0, 1.0);
           double random_x = dist(mt);
           double random_y = dist(mt);
           Point P1 = Point(ped->GetPos()._x - random_x, ped->GetPos()._y - random_y);
           Point P2 = Point(ped->GetPos()._x + random_x, ped->GetPos()._y + random_y);
           const NavLine  L = Line(P1, P2);
           ped->SetExitLine((const NavLine *)&L);
           target = P1;
367
      }
368
      Point desired_direction;
Arne Graf's avatar
Arne Graf committed
369
      const Point pos = ped->GetPos();
370 371 372
      double dist = 0.0;
      if(ped->GetExitLine())
           dist = ped->GetExitLine()->DistTo(pos);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
373
      // check if the molified version works
374 375 376
      Point lastE0 = ped->GetLastE0();
      ped->SetLastE0(target-pos);

377 378 379
      if ( (dynamic_cast<DirectionFloorfield*>(_direction.get())) ||
           (dynamic_cast<DirectionLocalFloorfield*>(_direction.get())) ||
           (dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get()))  ) {
380 381 382 383
          desired_direction = target-pos;
          if (desired_direction.NormSquare() < 0.25) {
              desired_direction = lastE0;
              ped->SetLastE0(lastE0);
tobias schroedter's avatar
tobias schroedter committed
384
//              Log->Write("desired_direction: %f    %f", desired_direction._x, desired_direction._y);
385
              //_direction->GetTarget(room, ped);
386
          }
387 388 389 390 391 392
//          if (dist > 1*J_EPS_GOAL) {
//               desired_direction = target - pos; //ped->GetV0(target);
//          } else {
//               desired_direction = lastE0;
//               ped->SetLastE0(lastE0); //keep old vector (revert set operation done 9 lines above)
//          }
393
      } else if (dist > J_EPS_GOAL) {
Arne Graf's avatar
Arne Graf committed
394
          desired_direction = ped->GetV0(target);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
395 396
      } else {
          ped->SetSmoothTurning();
397
          desired_direction = ped->GetV0();
398
      }
399
      //Log->Write("%f    %f", desired_direction._x, desired_direction._y);
400 401 402 403
//      if (desired_direction.NormSquare() < 0.1) {
//           Log->Write("ERROR:\t desired_direction in VelocityModel::e0 is too small (%f, %f)", desired_direction._x, desired_direction._y);
//
//      }
Arne Graf's avatar
Arne Graf committed
404
      return desired_direction;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
405 406 407
}


408
double VelocityModel::OptimalSpeed(Pedestrian* ped, double spacing) const
Mohcine Chraibi's avatar
Mohcine Chraibi committed
409 410 411
{
      double v0 = ped->GetV0Norm();
      double T = ped->GetT();
412
      double l = 2*ped->GetEllipse().GetBmax(); //assume peds are circles with const radius
Mohcine Chraibi's avatar
Mohcine Chraibi committed
413 414 415
      double speed = (spacing-l)/T;
      speed = (speed>0)?speed:0;
      speed = (speed<v0)?speed:v0;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
416
//      (1-winkel)*speed;
417
     //todo use winkel
Mohcine Chraibi's avatar
Mohcine Chraibi committed
418 419 420
      return speed;
}

421
// return spacing and id of the nearest pedestrian
Mohcine Chraibi's avatar
Mohcine Chraibi committed
422
my_pair VelocityModel::GetSpacing(Pedestrian* ped1, Pedestrian* ped2, Point ei, int periodic) const
Mohcine Chraibi's avatar
Mohcine Chraibi committed
423
{
424
      Point distp12 = ped2->GetPos() - ped1->GetPos(); // inversed sign
Mohcine Chraibi's avatar
Mohcine Chraibi committed
425
      if(periodic){
Oliver Schmidts's avatar
Oliver Schmidts committed
426 427
            double x = ped1->GetPos()._x;
            double x_j = ped2->GetPos()._x;
428

Mohcine Chraibi's avatar
Mohcine Chraibi committed
429
            if((xRight-x) + (x_j-xLeft) <= cutoff){
Oliver Schmidts's avatar
Oliver Schmidts committed
430
                 distp12._x = distp12._x + xRight - xLeft;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
431 432
            }
      }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
433
      double Distance = distp12.Norm();
434
      double l = 2*ped1->GetEllipse().GetBmax();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
435 436 437 438 439
      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
440
            Log->Write("WARNING: \tin VelocityModel::GetSPacing() ep12 can not be calculated!!!\n");
441
            Log->Write("\t\t Pedestrians are too near to each other (%f).", Distance);
442
            my_pair(FLT_MAX, ped2->GetID());
443
            exit(EXIT_FAILURE); //TODO
Mohcine Chraibi's avatar
Mohcine Chraibi committed
444
     }
445

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

Mohcine Chraibi's avatar
Mohcine Chraibi committed
450
      if((condition1 >=0 ) && (condition2 <= l/Distance))
451
            // return a pair <dist, condition1>. Then take the smallest dist. In case of equality the biggest condition1
452
            return  my_pair(distp12.Norm(), ped2->GetID());
Mohcine Chraibi's avatar
Mohcine Chraibi committed
453
      else
454
            return  my_pair(FLT_MAX, ped2->GetID());
455
}
Mohcine Chraibi's avatar
Mohcine Chraibi committed
456
Point VelocityModel::ForceRepPed(Pedestrian* ped1, Pedestrian* ped2, int periodic) const
457
{
Mohcine Chraibi's avatar
Mohcine Chraibi committed
458 459 460
     Point F_rep(0.0, 0.0);
     // x- and y-coordinate of the distance between p1 and p2
     Point distp12 = ped2->GetPos() - ped1->GetPos();
461

Mohcine Chraibi's avatar
Mohcine Chraibi committed
462
     if(periodic){
Oliver Schmidts's avatar
Oliver Schmidts committed
463 464
            double x = ped1->GetPos()._x;
            double x_j = ped2->GetPos()._x;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
465
            if((xRight-x) + (x_j-xLeft) <= cutoff){
Oliver Schmidts's avatar
Oliver Schmidts committed
466
                 distp12._x = distp12._x + xRight - xLeft;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
467 468
            }
      }
469

Mohcine Chraibi's avatar
Mohcine Chraibi committed
470 471 472
     double Distance = distp12.Norm();
     Point ep12; // x- and y-coordinate of the normalized vector between p1 and p2
     double R_ij;
473
     double l = 2*ped1->GetEllipse().GetBmax();
474

Mohcine Chraibi's avatar
Mohcine Chraibi committed
475 476 477 478
     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
479 480 481
          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");
482
          printf("ped1 %d  ped2 %d\n", ped1->GetID(), ped2->GetID());
Oliver Schmidts's avatar
Oliver Schmidts committed
483
          printf("ped1 at (%f, %f), ped2 at (%f, %f)\n", ped1->GetPos()._x, ped1->GetPos()._y, ped2->GetPos()._x, ped2->GetPos()._y);
484
          exit(EXIT_FAILURE); //TODO: quick and dirty fix for issue #158
485
          // (sometimes sources create peds on the same location)
486

Mohcine Chraibi's avatar
Mohcine Chraibi committed
487
     }
Mohcine Chraibi's avatar
Mohcine Chraibi committed
488 489 490 491 492 493
      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
494 495 496

      R_ij = - _aPed * exp((l-Distance)/_DPed);
      F_rep = ep12 * R_ij;
Mohcine Chraibi's avatar
Mohcine Chraibi committed
497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527

     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
528
     for(const auto & trans: subroom->GetAllTransitions())
Mohcine Chraibi's avatar
Mohcine Chraibi committed
529
     {
530
          if(!trans->IsOpen())
Mohcine Chraibi's avatar
Mohcine Chraibi committed
531
          {
532
                f +=  ForceRepWall(ped,*(static_cast<Line*>(trans)), centroid, inside);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
533
          }
Mohcine Chraibi's avatar
s  
Mohcine Chraibi committed
534 535 536 537 538 539 540 541 542
          // 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
543 544 545 546 547 548 549
     }

     return f;
}

Point VelocityModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& centroid, bool inside) const
{
550
     Point F_wrep = Point(0.0, 0.0);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
551 552 553 554 555 556 557 558
     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;
559
     double l = ped->GetEllipse().GetBmax();
Mohcine Chraibi's avatar
Mohcine Chraibi committed
560
     double R_iw;
561
     double min_distance_to_wall = 0.001; // 10 cm
562

Mohcine Chraibi's avatar
Mohcine Chraibi committed
563 564 565 566
     if (Distance > min_distance_to_wall) {
           e_iw = dist / Distance;
     }
     else {
Mohcine Chraibi's avatar
s  
Mohcine Chraibi committed
567
          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
568 569
          Point new_dist = centroid - ped->GetPos();
          new_dist = new_dist/new_dist.Norm();
570
          //printf("new distance = (%f, %f) inside=%d\n", new_dist._x, new_dist._y, inside);
Mohcine Chraibi's avatar
Mohcine Chraibi committed
571 572 573 574 575
          e_iw = (inside ? new_dist:new_dist*-1);
     }
     //-------------------------

     const Point& pos = ped->GetPos();
576 577 578 579
     double distGoal = 0.0;
     if(ped->GetExitLine())
          distGoal = ped->GetExitLine()->DistToSquare(pos);

Mohcine Chraibi's avatar
Mohcine Chraibi committed
580 581 582 583 584 585 586 587 588
     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
589
string VelocityModel::GetDescription()
Mohcine Chraibi's avatar
Mohcine Chraibi committed
590 591 592
{
     string rueck;
     char tmp[CLENGTH];
593

Mohcine Chraibi's avatar
Mohcine Chraibi committed
594 595 596 597 598 599 600
     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;
}

601
std::shared_ptr<DirectionStrategy> VelocityModel::GetDirection() const
Mohcine Chraibi's avatar
Mohcine Chraibi committed
602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626
{
     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;
}