// 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());
}
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;
}
/* 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;
}
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()