Example #1
0
// return spacing and id of the nearest pedestrian
my_pair VelocityModel::GetSpacing(Pedestrian* ped1, Pedestrian* ped2, Point ei, int periodic) const
{
      Point distp12 = ped2->GetPos() - ped1->GetPos(); // inversed sign 
      if(periodic){
            double x = ped1->GetPos()._x;
            double x_j = ped2->GetPos()._x;
            
            if((xRight-x) + (x_j-xLeft) <= cutoff){
                 distp12._x = distp12._x + xRight - xLeft;
            }
      }
      double Distance = distp12.Norm();
      double l = 2*ped1->GetEllipse().GetBmax();
      Point ep12;
      if (Distance >= J_EPS) {
            ep12 = distp12.Normalized();
      } else {
            //printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
            Log->Write("WARNING: \tin VelocityModel::GetSPacing() ep12 can not be calculated!!!\n");
            Log->Write("\t\t Pedestrians are too near to each other (%f).", Distance);
            exit(EXIT_FAILURE);
     }

      double condition1 = ei.ScalarProduct(ep12); // < e_i , e_ij > should be positive
      double condition2 = ei.Rotate(0, 1).ScalarProduct(ep12); // theta = pi/2. condition2 should <= than l/Distance
      condition2 = (condition2>0)?condition2:-condition2; // abs

      if((condition1 >=0 ) && (condition2 <= l/Distance))
            // return a pair <dist, condition1>. Then take the smallest dist. In case of equality the biggest condition1
            return  my_pair(distp12.Norm(), ped2->GetID());
      else
            return  my_pair(FLT_MAX, ped2->GetID());
}      
Example #2
0
Point Line::LotPoint(const Point &p) const {
    const Point &r = GetPoint1();
    const Point &s = GetPoint2();
    const Point &t = r - s;
    Point tmp;
    double lambda;

    tmp = p - s;
    lambda = tmp.ScalarProduct(t) / t.ScalarProduct(t);
    Point f = s + t * lambda;
    return f;
}
Example #3
0
/* Punkt auf der Linie mit kürzestem Abstand zu p
 * In der Regel Lotfußpunkt, Ist der Lotfußpunkt nicht im Segment
 * wird der entsprechende Eckpunkt der Line genommen
 * */
Point Line::ShortestPoint(const Point &p) const {

    const Point &t = _point1 - _point2;
    if (_point1 == _point2)
        return _point1;
    Point tmp = p - _point2;
    double lambda = tmp.ScalarProduct(t) / t.ScalarProduct(t);
    Point f = _point2 + t * lambda;

    /* Prüfen ob Punkt in der Linie,sonst entsprechenden Eckpunkt zurückgeben */
    if (lambda < 0)
        f = _point2;
    if (lambda > 1)
        f = _point1;

    return f;
}
Example #4
0
Point VelocityModel::ForceRepPed(Pedestrian* ped1, Pedestrian* ped2, int periodic) const
{      
     Point F_rep(0.0, 0.0);
     // x- and y-coordinate of the distance between p1 and p2
     Point distp12 = ped2->GetPos() - ped1->GetPos();
     
     if(periodic){
            double x = ped1->GetPos()._x;
            double x_j = ped2->GetPos()._x;
            if((xRight-x) + (x_j-xLeft) <= cutoff){
                 distp12._x = distp12._x + xRight - xLeft;
            }
      }
     
     double Distance = distp12.Norm();
     Point ep12; // x- and y-coordinate of the normalized vector between p1 and p2
     double R_ij;
     double l = 2*ped1->GetEllipse().GetBmax();
     
     if (Distance >= J_EPS) {
          ep12 = distp12.Normalized();
     } else {
          //printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
          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");
          printf("ped1 %d  ped2 %d\n", ped1->GetID(), ped2->GetID());
          printf("ped1 at (%f, %f), ped2 at (%f, %f)\n", ped1->GetPos()._x, ped1->GetPos()._y, ped2->GetPos()._x, ped2->GetPos()._y);
          exit(EXIT_FAILURE);
     }
      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

      R_ij = - _aPed * exp((l-Distance)/_DPed);
      F_rep = ep12 * R_ij;

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