Commit 9da57d12 authored by Mohcine Chraibi's avatar Mohcine Chraibi

Direction of rep force depending on centroid

parent 6b6c2a76
......@@ -347,7 +347,7 @@ Point GompertzModel::ForceRepRoom(Pedestrian* ped, SubRoom* subroom) const
{
Point f(0., 0.);
Point centroid = subroom->GetCentroid();
bool inside = subroom->IsConvex(); // would like to call subroom->is_inside(centroide)
bool inside = subroom->IsInSubRoom(centroid);
//first the walls
for(const auto & wall: subroom->GetAllWalls())
{
......@@ -392,7 +392,7 @@ Point GompertzModel::ForceRepRoom(Pedestrian* ped, SubRoom* subroom) const
Point GompertzModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& centroid, bool inside) const
{
#define DEBUG 1
#define DEBUG 0
Point F_wrep = Point(0.0, 0.0);
#if DEBUG
printf("=========\n\tEnter GompertzWall with PED=%d, wall=[%.2f, %.2f]--[%.2f, %.2f]\n", ped->GetID(), w.GetPoint1().GetX(), w.GetPoint1().GetY(), w.GetPoint2().GetX(), w.GetPoint2().GetY());
......@@ -402,9 +402,9 @@ Point GompertzModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& c
Point pt = w.ShortestPoint(ped->GetPos());
double wlen = w.LengthSquare();
if (wlen <= 0.03) { // ignore walls smaller than 0.15m (15cm)
return F_wrep;
}
// 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
......@@ -425,7 +425,9 @@ Point GompertzModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& c
Log->Write("WARNING:\t Gompertz: forceRepWall() ped %d is too near to the wall",ped->GetID());
Log->Write("INFO:\t\t --- take subroom centroid ",ped->GetID());
Point new_dist = centroid - ped->GetPos();
e_iw = new_dist/new_dist.Norm();
new_dist = new_dist/new_dist.Norm();
e_iw = ((inside==true)? new_dist:new_dist*-1);
Distance = EPS;
}
......
......@@ -421,7 +421,8 @@ double Pedestrian::GetV0Norm() const
else if(sub->GetType() == "idle_escalator"){
speed_down = _V0IdleEscalatorDownStairs;
}
// printf("z=%f, f=%f, v0=%f, v0d=%f, ret=%f\n", ped_elevation, f, _ellipse.GetV0(), _V0DownStairs, (1-f)*_ellipse.GetV0() + f*speed_down);
if(_id==-47)
printf("DOWN max_e=%f, z=%f, f=%f, v0=%f, v0d=%f, ret=%f\n", maxSubElevation, ped_elevation, f, _ellipse.GetV0(), _V0DownStairs, (1-f)*_ellipse.GetV0() + f*speed_down);
// fprintf(stderr, "%f %f %f %f\n", pos.GetX(), pos.GetY(), ped_elevation, (1-f)*_ellipse.GetV0() + f*speed_down);
// getc(stdin);
return (1-f)*_ellipse.GetV0() + f*speed_down;
......@@ -438,7 +439,8 @@ double Pedestrian::GetV0Norm() const
else if(sub->GetType() == "idle_escalator"){
speed_up = _V0IdleEscalatorUpStairs;
}
// printf("z=%f, f=%f, v0=%f, speed_up=%f, ret=%f\n", ped_elevation, f, _ellipse.GetV0(), speed_up, (1-f)*_ellipse.GetV0() + f*speed_up);
if(_id==-47)
printf("UP min_e=%f, z=%f, f=%f, v0=%f, speed_up=%f, ret=%f\n", minSubElevation, ped_elevation, f, _ellipse.GetV0(), speed_up, (1-f)*_ellipse.GetV0() + f*speed_up);
// fprintf(stderr, "%f %f %f %f\n", pos.GetX(), pos.GetY(), ped_elevation, (1-f)*_ellipse.GetV0() + f*speed_up);
// getc(stdin);
return (1-f)*_ellipse.GetV0() + f*speed_up;
......@@ -495,7 +497,7 @@ void Pedestrian::InitV0(const Point& target)
const Point& Pedestrian::GetV0(const Point& target)
{
#define DEBUG 0
#define DEBUG 1
const Point& pos = GetPos();
Point delta = target - pos;
Point new_v0;
......@@ -508,12 +510,12 @@ const Point& Pedestrian::GetV0(const Point& target)
_V0 = _V0 + (new_v0 - _V0)*( 1 - exp(-t/_tau) );
#if DEBUG
if(_id==24){
printf("Goal Line=[%f, %f]-[%f, %f]\n", _navLine->GetPoint1().GetX(), _navLine->GetPoint1().GetY(), _navLine->GetPoint2().GetX(), _navLine->GetPoint2().GetY());
if(_id==-159){
printf("=====\nGoal Line=[%f, %f]-[%f, %f]\n", _navLine->GetPoint1().GetX(), _navLine->GetPoint1().GetY(), _navLine->GetPoint2().GetX(), _navLine->GetPoint2().GetY());
printf("Ped=%d, sub=%d, room=%d pos=[%f, %f], target=[%f, %f]\n", _id, _subRoomID, _roomID, pos.GetX(), pos.GetY(), target.GetX(), target.GetY());
printf("Ped=%d : BEFORE new_v0=%f %f norm = %f\n", _id, new_v0.GetX(), new_v0.GetY(), new_v0.Norm());
printf("ped=%d: t=%f, _newOrientationFlag=%d, neworientationDelay=%d, _DistToBlockade=%f\n", _id,t, _newOrientationFlag, _newOrientationDelay, _distToBlockade);
printf("_v0=[%f, %f] norm = %f\n", _V0.GetX(), _V0.GetY(), _V0.Norm());
printf("_v0=[%f, %f] norm = %f\n=====\n", _V0.GetX(), _V0.GetY(), _V0.Norm());
}
// getc(stdin);
#endif
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment