Adjust incoming pedestrian velocity according to the Weidman FD

pedestrian/AgentsSourcesManager.cpp

@@ -166,7 +166,7 @@ void AgentsSourcesManager::ComputeBestPositionVoronoi(AgentsSource* src,
      vector<Pedestrian*> peds;
      _building->GetPedestrians(roomID, subroomID, peds);
 
-     //filter the points that are not in the boundaries
+     //filter the points that are not within the boundaries
      for (auto&& iter = peds.begin(); iter != peds.end();)
      {
           const Point& pos = (*iter)->GetPos();
@@ -281,17 +281,11 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
      vector<Point> positions = PedDistributor::PossiblePositions(*subroom);
      double bounds[4] = { 0, 0, 0, 0 };
      dist->Getbounds(bounds);
-     //int roomID = dist->GetRoomId();
-     //int subroomID = dist->GetSubroomID();
-     // first default Position
 
      for (auto& ped : peds)
      {
-          //ped->Dump(ped->GetID()); continue;
           int index = -1;
 
-          AdjustVelocityUsingWeidmann(ped);
-
           //in the case a range was specified
           for (unsigned int a = 0; a < positions.size(); a++)
           {
@@ -312,59 +306,62 @@ void AgentsSourcesManager::ComputeBestPositionRandom(AgentsSource* src,
                               bounds[0], bounds[1], bounds[2], bounds[3]);
                     Log->Write("ERROR:\t Specifying a subroom_id might help");
                }
-          } else
+          }
+          else
           {
                const Point& pos = positions[index];
                ped->SetPos(pos, true); //true for the initial position
                positions.erase(positions.begin() + index);
 
-               //               const Point& start_pos =  Point(_startX, _startY);
-               //               if ((std::isnan(start_pos._x) == 0) && (std::isnan(start_pos._y) == 0))
-               //               {
-               //                    if (_building->GetRoom(ped->GetRoomID())->GetSubRoom(ped->GetSubRoomID())->IsInSubRoom(
-               //                              start_pos) == false)
-               //                    {
-               //                         Log->Write(
-               //                                   "ERROR: \t cannot distribute pedestrian %d in Room %d at fixed position %s",
-               //                                   *pid, GetRoomId(), start_pos.toString().c_str());
-               //                         Log->Write(
-               //                                   "ERROR: \t Make sure that the position is inside the geometry and belongs to the specified room / subroom");
-               //                         exit(EXIT_FAILURE);
-               //                    }
-               //
-               //                    ped->SetPos(start_pos, true); //true for the initial position
-               //                    Log->Write("INFO: \t fixed position for ped %d in Room %d %s", *pid, GetRoomId(),
-               //                              start_pos.toString().c_str());
-               //               }
+               //at this point we have a position
+               AdjustVelocityUsingWeidmann(ped);
           }
      }
-
 }
+
 void AgentsSourcesManager::AdjustVelocityUsingWeidmann(Pedestrian* ped) const
 {
      //get the density
      vector<Pedestrian*> neighbours;
      _building->GetGrid()->GetNeighbourhood(ped,neighbours);
 
-     //pers per m2
+     //density in pers per m2
      double density = 1.0;
      //radius corresponding to a surface of 1m2
-     double radius_square=0.564*0.564;
+     //double radius_square=0.564*0.564;
+
      for(const auto& p: neighbours)
      {
-          if( (ped->GetPos()-p->GetPos()).NormSquare()<radius_square)
+          if( (ped->GetPos()-p->GetPos()).NormSquare()<=1.0)
                density+=1.0;
      }
-
+     density=density/M_PI;
      //get the velocity
      double density_max=5.4;
-     double speed=1.34*(1 - exp(-1.913*(1.0/density-1.0/density_max)));
-
+     //speed from taken from weidmann FD
+     double speed=1.34*(1-exp(-1.913*(1.0/density-1.0/density_max)));
+     if(speed>=ped->GetV0Norm())
+     {
+          speed=ped->GetV0Norm();
+     }
      //set the velocity vector
      if(ped->FindRoute()!=-1)
-          {
+     {
+          //get the next destination point
+          Point v =(ped->GetExitLine()->ShortestPoint(ped->GetPos())- ped->GetPos()).Normalized();
+          v=v*speed;
+          ped->SetV(v);
+          cout<<"density: "<<density<<endl;
+     }
+     else
+     {
+          Log->Write("ERROR:\t no route could be found for agent [%d] going to [%d]",ped->GetID(),ped->GetFinalDestination());
+         //cout<<"density: "<<density<<endl;
+         //cout<<"find:route"<<ped->FindRoute()<<endl;
+         //that will be most probably be fixed in the next computation step.
+         // so do not abort
+     }
 
-          }
 }