GradientModel.cpp 23.9 KB
Newer Older
Arne Graf's avatar
Arne Graf committed
1 2
/**
 * \file        GradientModel.cpp
3 4
 * \date        Apr 15, 2014
 * \version     v0.7
5
 * \copyright   <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
Arne Graf's avatar
Arne Graf committed
6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
 *
 * \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
24 25 26
 * Implementation of classes for force-based models.
 * Actually we've got two different models:
 * 2. Gompertz Model
Arne Graf's avatar
Arne Graf committed
27 28 29
 *
 *
 **/
30

31
#include <math.h>
Arne Graf's avatar
Arne Graf committed
32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48
#include "../pedestrian/Pedestrian.h"
#include "../mpi/LCGrid.h"
#include "../geometry/Wall.h"
#include "../geometry/SubRoom.h"

#include "GradientModel.h"

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

using std::vector;
using std::string;

49
GradientModel::GradientModel(std::shared_ptr<DirectionStrategy> dir, double nuped, double aped, double bped, double cped,
Arne Graf's avatar
Arne Graf committed
50
                             double nuwall, double awall, double bwall, double cwall,
Arne Graf's avatar
Arne Graf committed
51 52
                             double deltaH, double wallAvoidDistance, bool useWallAvoidance,
                             double slowDownDistance)
Arne Graf's avatar
Arne Graf committed
53 54 55 56 57 58 59 60 61 62 63 64
{
     _direction = dir;
     // Force_rep_PED Parameter
     _nuPed = nuped;
     _aPed = aped;
     _bPed = bped;
     _cPed = cped;
     // Force_rep_WALL Parameter
     _nuWall = nuwall;
     _aWall = awall;
     _bWall = bwall;
     _cWall = cwall;
Arne Graf's avatar
Arne Graf committed
65 66 67 68
     // floorfield Parameter
     _deltaH = deltaH;
     _wallAvoidDistance = wallAvoidDistance;
     _useWallAvoidance = useWallAvoidance;
Arne Graf's avatar
Arne Graf committed
69 70
     // anti clipping
     _slowDownDistance = slowDownDistance;
Arne Graf's avatar
Arne Graf committed
71 72 73 74 75 76 77 78 79 80 81 82

     over = new long int;
     under = new long int;
     redircnt = new long int;
     slowcnt = new long int;
     overlapcnt = new long int;

     *over = 0;   //analyze code only - can be removed
     *under = 0;  //analyze code only - can be removed
     *redircnt = 0;  //analyze code only - can be removed
     *slowcnt = 0;  //analyze code only - can be removed
     *overlapcnt = 0;
Arne Graf's avatar
Arne Graf committed
83 84 85
}


86 87
GradientModel::~GradientModel()
{
88 89 90 91 92
    delete over;
    delete under;
    delete redircnt;
    delete slowcnt;
    delete overlapcnt;
93 94
}

95
bool GradientModel::Init (Building* building)
96
{
97

f.mack's avatar
f.mack committed
98
    if(auto dirff = dynamic_cast<DirectionFloorfield*>(_direction.get())){
99
         Log->Write("INFO:\t Init DirectionFloorfield starting ...");
f.mack's avatar
f.mack committed
100
         dirff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
101
         Log->Write("INFO:\t Init DirectionFloorfield done");
102 103
    }

f.mack's avatar
f.mack committed
104
     if(auto dirlocff = dynamic_cast<DirectionLocalFloorfield*>(_direction.get())){
105
          Log->Write("INFO:\t Init Direction LOCAL Floorfield starting ...");
f.mack's avatar
f.mack committed
106
          dirlocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
107 108 109
          Log->Write("INFO:\t Init Direction LOCAL Floorfield done");
     }

f.mack's avatar
f.mack committed
110
     if(auto dirsublocff = dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get())){
Arne Graf's avatar
Arne Graf committed
111
          Log->Write("INFO:\t Init Direction SubLOCAL Floorfield starting ...");
f.mack's avatar
f.mack committed
112
          dirsublocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Arne Graf's avatar
Arne Graf committed
113 114 115 116 117
          Log->Write("INFO:\t Init Direction SubLOCAL Floorfield done");
     }


     const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
118

119 120 121 122
     std::vector<Pedestrian*> pedsToRemove;
     pedsToRemove.clear();
     bool error_occurred = false;
#pragma omp parallel for
123
    for(signed int p=0;p<allPeds.size();p++) {
124 125 126 127 128
         Pedestrian* ped = allPeds[p];
         double cosPhi, sinPhi;
         //a destination could not be found for that pedestrian
         if (ped->FindRoute() == -1) {
              Log->Write(
129 130 131 132
                   "ERROR:\tGradientModel::Init() cannot initialise route. ped %d is scheduled for deletion.\n",ped->GetID());
             //building->DeletePedestrian(ped);
#pragma omp critical(GradientModel_Init_pedsToRemove)
              pedsToRemove.emplace_back(ped);
133 134 135 136 137 138 139 140 141 142
              continue;
         }

         Line* e = ped->GetExitLine();
         const Point& e1 = e->GetPoint1();
         const Point& e2 = e->GetPoint2();
         Point target = (e1 + e2) * 0.5;
         Point d = target - ped->GetPos();
         double dist = d.Norm();
         if (dist != 0.0) {
Oliver Schmidts's avatar
Oliver Schmidts committed
143 144
              cosPhi = d._x / dist;
              sinPhi = d._y / dist;
145 146 147 148
         } else {
              Log->Write(
                   "ERROR: \allPeds::Init() cannot initialise phi! "
                   "dist to target is 0\n");
f.mack's avatar
f.mack committed
149
#pragma omp critical(GradientModel_Init_error_occurred)
150 151
              error_occurred = true;
              //return false;
152
         }
153 154
         // This skips the rest of the initialization if any pedestrian could not be initialized
         if (error_occurred) continue;
155 156 157 158 159 160 161 162

         ped->InitV0(target);

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

164 165 166
     for (auto ped : pedsToRemove) {
          building->DeletePedestrian(ped);
     }
167
     return !error_occurred;
168
}
Arne Graf's avatar
Arne Graf committed
169

170
void GradientModel::ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic)
171 172 173 174 175 176 177
{
     double delta = 0.5;
      // collect all pedestrians in the simulation.
      const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();

     unsigned long nSize;
     nSize = allPeds.size();
Arne Graf's avatar
Arne Graf committed
178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194
     Pedestrian* minAddress = nullptr; //non-elegant fix for faulty neighbours[...] ptr avoidance
     Pedestrian* maxAddress = nullptr;

     if (nSize >= 1) {
        minAddress = allPeds[0];
        maxAddress = allPeds[0];
        for (auto& pedptr:allPeds) {
            if (&(*pedptr) < minAddress) {
                minAddress = &(*pedptr);
            }
            if (pedptr > maxAddress) {
                maxAddress = &(*pedptr);
            }
        }
     }

     int nThreads = omp_get_max_threads();
195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210

     int partSize;
     partSize = (int) (nSize / nThreads);

      #pragma omp parallel  default(shared) num_threads(nThreads)
      {
           vector< Point > result_acc = vector<Point > ();
           result_acc.reserve(nSize);

           const int threadID = omp_get_thread_num();

           int start = threadID*partSize;
           int end;
           end = (threadID + 1) * partSize - 1;
           if ((threadID == nThreads - 1)) end = (int) (nSize - 1);

Arne Graf's avatar
Arne Graf committed
211
//DEBUG start
Arne Graf's avatar
Arne Graf committed
212 213
//           start = 0;
//           end = nSize-1; // loop till p<= end !!!
Arne Graf's avatar
Arne Graf committed
214
//DEBUG end
215 216 217 218 219 220 221 222
           for (int p = start; p <= end; ++p) {

                Pedestrian* ped = allPeds[p];
                Room* room = building->GetRoom(ped->GetRoomID());
                SubRoom* subroom = room->GetSubRoom(ped->GetSubRoomID());

                double normVi = ped->GetV().ScalarProduct(ped->GetV()); //squared
                double HighVel = (ped->GetV0Norm() + delta) * (ped->GetV0Norm() + delta); //(v0+delta)^2
Arne Graf's avatar
Arne Graf committed
223
                if (normVi > HighVel && true) {
224
                     fprintf(stderr, "GradientModel::calculateForce_LC() WARNING: actual velocity (%f) of iped %d "
225 226
                             "is bigger than desired velocity (%f) at time: %fs (periodic=%d)\n",
                             sqrt(normVi), ped->GetID(), ped->GetV0Norm(), current, periodic);
227 228 229 230

                     // remove the pedestrian and abort
                     Log->Write("\tERROR: ped [%d] was removed due to high velocity",ped->GetID());
                     building->DeletePedestrian(ped);
231
                     Log->incrementDeletedAgents();
232 233 234 235 236 237
                     //continue;  //FIXME tolerate first
                     exit(EXIT_FAILURE);
                }

                Point repPed = Point(0,0);
                vector<Pedestrian*> neighbours;
238 239
                building->GetGrid()->GetNeighbourhood(ped,neighbours);
                int size = (int) neighbours.size();
240 241
                for (int i = 0; i < size; i++) {
                     Pedestrian* ped1 = neighbours[i];
Arne Graf's avatar
Arne Graf committed
242 243
                     if ((minAddress > neighbours[i]) || (maxAddress < neighbours[i])) {
                        std::cerr << "## Skiped " << i << " of " << size << " #### " << ped1 << " " << minAddress << " " << maxAddress << std::endl;
244
                        continue;
Arne Graf's avatar
Arne Graf committed
245
                     }
246 247 248 249 250 251 252 253 254 255
                     //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);
Arne Graf's avatar
Arne Graf committed
256
                     if (!isVisible) {
257
                          continue;
Arne Graf's avatar
Arne Graf committed
258
                     }
259 260 261 262 263 264 265 266 267 268
                     if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
                          repPed = repPed + ForceRepPed(ped, ped1);
                     } else {
                          // or in neighbour subrooms
                          SubRoom* sb2=building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
                          if(subroom->IsDirectlyConnectedWith(sb2)) {
                               repPed = repPed + ForceRepPed(ped, ped1);
                          }
                     }
                }
269
                result_acc.push_back(repPed); //only orientation is of interest
270 271 272
           }

           // update
Arne Graf's avatar
Arne Graf committed
273

274 275
           for (int p = start; p <= end; ++p) {
                Pedestrian* ped = allPeds[p];
Arne Graf's avatar
Arne Graf committed
276 277 278 279 280
                if (result_acc[p-start].Norm() > 1) {
                    ++(*over);
                } else {
                    ++(*under);
                }
281
                Point movDirection = (result_acc[p-start].Norm() > 1) ? result_acc[p - start].Normalized() : result_acc[p-start];
282 283
                Point toTarget = (_direction->GetTarget(building->GetRoom(ped->GetRoomID()), ped));
                toTarget = toTarget - ped->GetPos();
284
                if (toTarget.NormSquare() == 0.) {                // this if overcomes shortcomming of floorfield (neggrad[targetpoints] == Point(0., 0.))
285 286 287
                    toTarget += ped->GetV().Normalized();
                }

Arne Graf's avatar
Arne Graf committed
288
                movDirection = (movDirection + toTarget);
289 290
                movDirection = (movDirection.Norm() > 1.) ? movDirection.Normalized() : movDirection;

Arne Graf's avatar
Arne Graf committed
291 292 293
                double desired_speed = ped->GetV0Norm();
                Point oldMov = Point(0., 0.);
                if (desired_speed > 0.) {
294 295
                    oldMov = ped->GetV() / desired_speed; // (GetV() returns a vector: distance travelled in one time-unit (s)
                }                                         // so oldMov is now: vector distance travelled in one time-unit with unit speed = sth around unit-vector or smaller
Arne Graf's avatar
Arne Graf committed
296

297
                //anti jitter               //_V0 = _V0 + (new_v0 - _V0)*( 1 - exp(-t/_tau) );
Arne Graf's avatar
Arne Graf committed
298 299 300
                oldMov = (oldMov.Norm() > 1.)? oldMov.Normalized() : oldMov; //on the safe side ... should not be necessary as it must be [0, 1]
                Point diff = ( oldMov - movDirection) * (.2); // .2 also 80% alte Richtung, 20% neue Richtung
                movDirection = oldMov - diff;
Arne Graf's avatar
Arne Graf committed
301
                movDirection = (movDirection.Norm() > 1.) ? movDirection.Normalized() : movDirection;
302

Arne Graf's avatar
Arne Graf committed
303
                //redirect near wall mechanics:
304 305
                Point dir2Wall = Point{0., 0.};
                double distance2Wall = -1.;
f.mack's avatar
f.mack committed
306 307 308 309 310 311 312 313 314
                if (auto dirff = dynamic_cast<DirectionFloorfield*>(_direction.get())) {
                     dir2Wall = dirff->GetDir2Wall(ped);
                     distance2Wall = dirff->GetDistance2Wall(ped);
                } else if (auto dirlocff = dynamic_cast<DirectionLocalFloorfield*>(_direction.get())) {
                     dir2Wall = dirlocff->GetDir2Wall(ped);
                     distance2Wall = dirlocff->GetDistance2Wall(ped);
                } else if (auto dirsublocff = dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get())) {
                     dir2Wall = dirsublocff->GetDir2Wall(ped);
                     distance2Wall = dirsublocff->GetDistance2Wall(ped);
315 316 317
                } else {
                     Log->Write("ERROR: \t GradNav Model (4) requires any floor field (exit-strat {6,8,9}). None found!");
                }
Arne Graf's avatar
Arne Graf committed
318
                double dotProduct = 0;
Arne Graf's avatar
Arne Graf committed
319
                double antiClippingFactor = 1;
Arne Graf's avatar
Arne Graf committed
320
                if (distance2Wall < _slowDownDistance) {
Arne Graf's avatar
Arne Graf committed
321 322 323
                    dotProduct = movDirection.ScalarProduct(dir2Wall);
                    if ((dotProduct > 0) && (distance2Wall < .5 * _slowDownDistance)) { //acute angle && really close to wall
                        movDirection = movDirection - (dir2Wall*dotProduct); //remove walldirection from movDirection
Arne Graf's avatar
Arne Graf committed
324
                        ++(*redircnt);
Arne Graf's avatar
Arne Graf committed
325
                    }
Arne Graf's avatar
Arne Graf committed
326
                    antiClippingFactor = ( 1 - .5*(dotProduct + fabs(dotProduct)) );
Arne Graf's avatar
Arne Graf committed
327
                    ++(*slowcnt);
328
                }
329

330
                movDirection = movDirection * (antiClippingFactor * ped->GetV0Norm() * deltaT);
331

332
                Point pos_neu = ped->GetPos() + movDirection;
Arne Graf's avatar
Arne Graf committed
333

334
                ped->SetPos(pos_neu);
335
                ped->SetV(movDirection/deltaT);
336 337
                ped->SetPhiPed();
           }
Arne Graf's avatar
Arne Graf committed
338

339
      }//end parallel
Arne Graf's avatar
Arne Graf committed
340
      //std::cerr << "Over : Under  " << *over << " : " << *under << "    (" << *redircnt << ")" << "    (" << *slowcnt << ")" << "    (" << *overlapcnt << ")" << std::endl;
341
}
Arne Graf's avatar
Arne Graf committed
342 343 344

Point GradientModel::ForceDriv(Pedestrian* ped, Room* room) const
{
345
#define DEBUG 0
346

347
     //printf("GradientModel::ForceDriv\n");
Arne Graf's avatar
Arne Graf committed
348 349
     const Point& target = _direction->GetTarget(room, ped);
     Point F_driv;
350 351 352
     //Point e0;
     //const Point& pos = ped->GetPos();
     //double dist = ped->GetExitLine()->DistTo(pos);
Arne Graf's avatar
Arne Graf committed
353 354

     // check if the molified version works
355 356 357 358 359 360 361
     //if (dist > J_EPS_GOAL) {
     //     e0 = ped->GetV0(target);
     //} else {
     //     ped->SetSmoothTurning();
     //     e0 = ped->GetV0();
     //}
     F_driv = ((target.Normalized() * ped->GetV0Norm() - ped->GetV()) * ped->GetMass()) / ped->GetTau();
362

Oliver Schmidts's avatar
Oliver Schmidts committed
363 364
      //double v =  sqrt(ped->GetV()._x*ped->GetV()._x +ped->GetV()._y*ped->GetV()._y);
      //double e0norm = sqrt(e0._x*e0._x +e0._y*e0._y);
Arne Graf's avatar
Arne Graf committed
365

366
#if DEBUG
Oliver Schmidts's avatar
Oliver Schmidts committed
367
      printf( "pos %f %f target %f %f\n", pos._x, pos._y, target._x, target._y);
368
      printf("mass=%f, v0norm=%f, v=%f, e0Norm=%f, tau=%f\n", ped->GetMass(), ped->GetV0Norm(), v , e0norm,ped->GetTau());
Oliver Schmidts's avatar
Oliver Schmidts committed
369 370
      printf("Fdriv=  [%f, %f]\n", F_driv._x, F_driv._y);
      fprintf(stderr, "%d   %f    %f    %f    %f    %f    %f\n", ped->GetID(), ped->GetPos()._x, ped->GetPos()._y, ped->GetV()._x, ped->GetV()._y, target._x, target._y);
371 372
      getc(stdin);
#endif
373

374 375
     //return F_driv;
     return target;
376 377
}

Arne Graf's avatar
Arne Graf committed
378 379
Point GradientModel::ForceRepPed(Pedestrian* ped1, Pedestrian* ped2) const
{
380
     Point F_rep(0.0, 0.0);
Arne Graf's avatar
Arne Graf committed
381 382
     // x- and y-coordinate of the distance between p1 and p2
     Point distp12 = ped2->GetPos() - ped1->GetPos();
383 384
     //const Point& vp1 = ped1->GetV(); // v Ped1
     //const Point& vp2 = ped2->GetV(); // v Ped2
Arne Graf's avatar
Arne Graf committed
385 386
     Point ep12; // x- and y-coordinate of the normalized vector between p1 and p2
     //double K_ij;
387
     double B_ij, f; // tmp2;
Arne Graf's avatar
Arne Graf committed
388 389 390 391 392
     const JEllipse& E1 = ped1->GetEllipse();
     const JEllipse& E2 = ped2->GetEllipse();
     Point AP1inE1 = E1.GetCenter();
     Point AP2inE2 = E2.GetCenter();
     // ActionPoint von E1 in Koordinaten von E2 (transformieren)
393
     Point AP1inE2 = AP1inE1.TransformToEllipseCoordinates(E2.GetCenter(), E2.GetCosPhi(), E2.GetSinPhi());
Arne Graf's avatar
Arne Graf committed
394
     // ActionPoint von E2 in Koordinaten von E1 (transformieren)
395
     Point AP2inE1 = AP2inE2.TransformToEllipseCoordinates(E1.GetCenter(), E1.GetCosPhi(), E1.GetSinPhi());
Arne Graf's avatar
Arne Graf committed
396 397 398 399 400 401 402
     double r1 = (AP1inE1 - E1.PointOnEllipse(AP2inE1)).Norm();
     double r2 = (AP2inE2 - E2.PointOnEllipse(AP1inE2)).Norm();
     //fprintf(stderr, "%f %f %f %f\n",  E1.GetEA(), E1.GetEB(), E2.GetEA(), E2.GetEB());
     //fprintf(stderr, "%f %f\n",  r1, r2);
     const double EPS = 0.001;
     double Distance = distp12.Norm() + EPS; // Molified See Koester2013

Arne Graf's avatar
Arne Graf committed
403

Arne Graf's avatar
Arne Graf committed
404 405 406 407 408 409 410
     // if(ped1->GetID() ==logped)
     // {
     //     printf("ped1=%d ped2=%d  Distance=%f\n",ped1->GetID(), ped2->GetID(), Distance);
     // }
     if (Distance >= J_EPS) {
          ep12 = distp12.Normalized();
     } else {
Arne Graf's avatar
Arne Graf committed
411 412
          //printf("ERROR: \tin GradientModel::forcePedPed() ep12 can not be calculated!!!\n");
          Log->Write("WARNING: \tin GradientModel::forcePedPed() ep12 can not be calculated!!!\n");
Arne Graf's avatar
Arne Graf committed
413 414 415 416 417 418
          Log->Write("\t\t Pedestrians are too near to each other.");
          Log->Write("\t\t Get your model right. Going to exit.");
          return F_rep; // should never happen
          exit(EXIT_FAILURE);
     }
//------------------------- check if others are behind using v0 instead of v
419
     double tmpv = ped1->GetV().ScalarProduct(ep12); // < v^0_i , e_ij >
Arne Graf's avatar
Arne Graf committed
420 421
     double ped2IsBehindv = exp(-exp(-5*tmpv)); //step function: continuous version
     if (ped2IsBehindv < J_EPS) {
Arne Graf's avatar
Arne Graf committed
422 423 424 425 426
     //if (tmpv < 0) {
          return F_rep; // ignore ped2
     }

//--------------------------check if overlapping
Arne Graf's avatar
Arne Graf committed
427 428 429 430 431 432 433 434
     //double* unused = new double;
     //double tmpover = E1.EffectiveDistanceToEllipse(E2, unused);
//     if (tmpover < 0) {
//         std::cerr << "Fehler aaaaa " << std::endl;
//         getc(stdin);
//         ++(*overlapcnt);
//         return ep12 * (-5);
//     }
Arne Graf's avatar
Arne Graf committed
435
//--------------------------check speed diff
436 437
//     tmp2 = (vp1 - vp2).ScalarProduct(ep12); // < v_ij , e_ij >
//     if (tmp2 < 0) {
Arne Graf's avatar
Arne Graf committed
438 439
//          return F_rep; // ignore ped2
//     }
Arne Graf's avatar
Arne Graf committed
440 441 442 443 444 445 446 447 448

     // calculate B_ij
     B_ij = 1.0 - Distance/(r1+r2); //TODO: Simplification to avoid accelerating predecessors
     //Gompertz-function parameter.
     //TODO: Check later if other values are more appropriate
     double b = _bPed, c = _cPed; //repped
     B_ij = exp(-b*exp(-c*B_ij));
     //TODO: check if we need K_ij in the  f
     //f = -ped1->GetMass() * _nuPed * ped1->GetV0Norm() * K_ij * B_ij;
449

Arne Graf's avatar
Arne Graf committed
450 451 452
     f = -ped1->GetMass() * _nuPed * ped1->GetV0Norm() * B_ij;

     F_rep = ep12 * f;
453
     //check isNan
Oliver Schmidts's avatar
Oliver Schmidts committed
454
     if (F_rep._x != F_rep._x || F_rep._y != F_rep._y) {
Arne Graf's avatar
Arne Graf committed
455 456
          char tmp[CLENGTH];
          sprintf(tmp, "\nNAN return ----> p1=%d p2=%d Frepx=%f, Frepy=%f\n", ped1->GetID(),
Oliver Schmidts's avatar
Oliver Schmidts committed
457
                  ped2->GetID(), F_rep._x, F_rep._y);
Arne Graf's avatar
Arne Graf committed
458 459 460 461 462 463
          Log->Write(tmp);
          Log->Write("ERROR:\t fix this as soon as possible");
          return Point(0,0); // FIXME: should never happen
          exit(EXIT_FAILURE);
     }
     return F_rep;
464
}//END Gompertz:ForceRepPed()
Arne Graf's avatar
Arne Graf committed
465 466 467

Point GradientModel::ForceRepRoom(Pedestrian* ped, SubRoom* subroom) const
{
468
     Point f(0., 0.);
Arne Graf's avatar
Arne Graf committed
469
     //first the walls
470 471 472
     for(const auto & wall: subroom->GetAllWalls())
     {
          f += ForceRepWall(ped, wall);
Arne Graf's avatar
Arne Graf committed
473 474
     }
     //then the obstacles
475 476 477 478 479 480

     for(const auto & obst: subroom->GetAllObstacles())
     {
          for(const auto & wall: obst->GetAllWalls())
          {
               f += ForceRepWall(ped, wall);
Arne Graf's avatar
Arne Graf committed
481 482
          }
     }
483

Arne Graf's avatar
Arne Graf committed
484
     // and finally the closed doors
485 486 487 488
     for(auto & goal: subroom->GetAllTransitions())
     {
          if(! goal->IsOpen()) {
               f +=  ForceRepWall(ped,*(static_cast<Line*>(goal)));
Arne Graf's avatar
Arne Graf committed
489 490 491 492 493
          }
     }

     return f;
}
494 495

Point GradientModel::ForceRepWall(Pedestrian* ped, const Line& w) const
Arne Graf's avatar
Arne Graf committed
496
{
497
#define DEBUG 0
Arne Graf's avatar
Arne Graf committed
498
     Point F_wrep = Point(0.0, 0.0);
499 500 501
#if DEBUG
     printf("in GompertzWall\n");
#endif
Arne Graf's avatar
Arne Graf committed
502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523
     // getc(stdin);
     // if direction of pedestrians does not intersect walls --> ignore

     Point pt = w.ShortestPoint(ped->GetPos());
     double wlen = w.LengthSquare();
     if (wlen <= 0.03) { // ignore walls smaller than 0.15m  (15cm)
          return F_wrep;
     }
     Point dist = pt - ped->GetPos(); // x- and y-coordinate of the distance between ped and p
     const double EPS = 0.001; // 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;
     double Radius, B_iw;
     double f;
     Point r;
     Point pinE; // vorher x1, y1
     const JEllipse& E = ped->GetEllipse();
     const Point& v = ped->GetV();

     if (Distance < J_EPS) {
524
          Log->Write("WARNING:\t Gompertz: forceRepWall() ped %d is too near to the wall. Return default values",ped->GetID());
Arne Graf's avatar
Arne Graf committed
525 526 527 528
          return Point(0, 0); //quick and dirty. Should react to the warning and fix the model
     }
     e_iw = dist / Distance;
//------------------------- check if others are behind using v0 instead of v
529 530
     // tmp = ped->GetV0().ScalarProduct(e_iw); // < v^0_i , e_iw >
     double tmpv = v.ScalarProduct(e_iw);
Arne Graf's avatar
Arne Graf committed
531
     //double wallIsBehind = exp(-exp(-5*tmp)); //step function: continuous version
532 533 534 535 536
     // double wallIsBehindv = exp(-exp(-5*tmpv)); //step function: continuous version
     double wallIsBehindv = (tmpv<=0)?0:1;
#if DEBUG

     printf("Distance = %f tmpv=%f\n",Distance, tmpv);
Oliver Schmidts's avatar
Oliver Schmidts committed
537 538 539 540
     printf("v = %f, %f \n", v._x, v._y);
     printf("pos = %f, %f \n", ped->GetPos()._x, ped->GetPos()._y);
     printf("pt = %f, %f \n", pt._x, pt._y);
     printf("e_iw = %f, %f\n",e_iw._x, e_iw._y);
541 542
     printf("WallIsBehind = %f (%f)\n",wallIsBehindv,J_EPS);
#endif
Arne Graf's avatar
Arne Graf committed
543 544 545 546 547 548

     if (wallIsBehindv < J_EPS) { // Wall is behind the direction of motion
          return F_wrep;
     }
//------------------------------------------------------------------------
     // pt in coordinate system of Ellipse
549
     pinE = pt.TransformToEllipseCoordinates(E.GetCenter(), E.GetCosPhi(), E.GetSinPhi());
Arne Graf's avatar
Arne Graf committed
550 551 552 553 554 555 556 557 558 559
     // Punkt auf der Ellipse
     r = E.PointOnEllipse(pinE);
     Radius  = (r - E.GetCenter()).Norm();
     //-------------------------

     const Point& pos = ped->GetPos();
     double distGoal = ped->GetExitLine()->DistToSquare(pos);
     if(distGoal < J_EPS_GOAL*J_EPS_GOAL)
          return F_wrep;

560 561 562 563
     // Line  direction = Line(ped->GetPos(), ped->GetPos() + v*100);
     // if(Distance>Radius && !direction.IntersectionWith(w)) {
     //      return F_wrep;
     // }
Arne Graf's avatar
Arne Graf committed
564 565 566 567 568 569 570 571

//-------------------------

     //TODO: Check later if other values are more appropriate
     //double b = 0.7, c = 3.0;
     double b = _bWall, c = _cWall;
     B_iw = 1.0 - Distance/(Radius);
     B_iw = exp(-b*exp(-c*B_iw));
572 573 574
#if DEBUG
     printf("b=%f, c=%f, a=%f, m=%f\n",b,c,_nuWall, ped->GetMass());
     printf("Distance=%f, Radius=%f, B_iw=%f, G(B_iw)=%f\n",Distance, Radius, 1.0 - Distance/(Radius), B_iw);
Oliver Schmidts's avatar
Oliver Schmidts committed
575
     printf("f= %f, e_iw= %f, %f\n",f, e_iw._x, e_iw._y );
576 577
     getc(stdin);
#endif
Arne Graf's avatar
Arne Graf committed
578 579 580 581 582 583 584
     //f = -ped->GetMass() * _nuWall * ped->GetV0Norm() * K_iw * B_iw;
     f = -ped->GetMass() * _nuWall * B_iw  * ped->GetV0Norm();

     F_wrep = e_iw * f;
     return F_wrep;
}

585
std::string GradientModel::GetDescription()
Arne Graf's avatar
Arne Graf committed
586 587 588 589 590 591 592 593 594 595 596 597 598
{
     string rueck;
     char tmp[CLENGTH];
     sprintf(tmp, "\t\tNu: \t\tPed: %f \tWall: %f\n", _nuPed, _nuWall);
     rueck.append(tmp);
     sprintf(tmp, "\t\ta: \t\tPed: %f \tWall: %f\n", _aPed, _aWall);
     rueck.append(tmp);
     sprintf(tmp, "\t\tb: \t\tPed: %f \tWall: %f\n", _bPed, _bWall);
     rueck.append(tmp);
     sprintf(tmp, "\t\tc: \t\tPed: %f \tWall: %f\n", _cPed, _cWall);
     rueck.append(tmp);
     return rueck;
}
599

600
std::shared_ptr<DirectionStrategy> GradientModel::GetDirection() const
Arne Graf's avatar
Arne Graf committed
601 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 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644
{
     return _direction;
}

double GradientModel::GetNuPed() const
{
     return _nuPed;
}

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

double GradientModel::GetbPed() const
{
     return _bPed;
}

double GradientModel::GetcPed() const
{
     return _cPed;
}

double GradientModel::GetNuWall() const
{
     return _nuWall;
}


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

double GradientModel::GetbWall() const
{
     return _bWall;
}

double GradientModel::GetcWall() const
{
     return _cWall;
}