Skip to content

GitLab

JPScore
JPScore
Commit 9da57d12 authored by Mohcine Chraibi's avatar Mohcine Chraibi
Browse files
Options

Direction of rep force depending on centroid

parent 6b6c2a76
Hide whitespace changes
Inline Side-by-side
Showing with 16 additions and 12 deletions
math/GompertzModel.cpp
View file @ 9da57d12
......@@ -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;
}
......
pedestrian/Pedestrian.cpp
View file @ 9da57d12
......@@ -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