Commit 839124ff authored by Mohcine Chraibi's avatar Mohcine Chraibi

Fix issue #117

- todo: Pedestrians gain their v0 on the plane directly when they leave
  the stairs (escalators).
- The smoothing happens only when entering the stair (escalator).
---> See Fig. in issue #117
parent 8be9fe25
......@@ -332,9 +332,38 @@ bool Building::InitGeometry()
if(!obst->ConvertLineToPoly())
return false;
}
double minElevation = 1000;
double maxElevation = -1000;
for(auto && wall:itr_subroom.second->GetAllWalls())
{
Point P1 = wall.GetPoint1();
Point P2 = wall.GetPoint2();
if(minElevation > itr_subroom.second->GetElevation(P1))
{
minElevation = itr_subroom.second->GetElevation(P1);
}
if(maxElevation < itr_subroom.second->GetElevation(P1))
{
maxElevation = itr_subroom.second->GetElevation(P1);
}
if(minElevation > itr_subroom.second->GetElevation(P2))
{
minElevation = itr_subroom.second->GetElevation(P2);
}
if(maxElevation < itr_subroom.second->GetElevation(P2))
{
maxElevation = itr_subroom.second->GetElevation(P2);
}
}
itr_subroom.second->SetMaxElevation(maxElevation);
itr_subroom.second->SetMinElevation(minElevation);
}
}
// look and save the neighbor subroom for improving the runtime
// that information is already present in the crossing/transitions
......
......@@ -65,7 +65,9 @@ SubRoom::SubRoom()
_planeEquation[1]=0.0;
_planeEquation[2]=0.0;
_cosAngleWithHorizontalPlane=0;
_minElevation=0;
_maxElevation=0;
_goalIDs = vector<int> ();
_area = 0.0;
_uid = _static_uid++;
......@@ -454,6 +456,27 @@ double SubRoom::GetCosAngleWithHorizontal() const
}
double SubRoom::GetMinElevation() const
{
return _minElevation;
}
double SubRoom::GetMaxElevation() const
{
return _maxElevation;
}
void SubRoom::SetMinElevation(double minElevation)
{
_minElevation = minElevation;
}
void SubRoom::SetMaxElevation(double maxElevation)
{
_maxElevation = maxElevation;
}
bool SubRoom::CheckObstacles()
{
for(const auto& wall: _walls)
......
......@@ -66,8 +66,9 @@ private:
double _planeEquation[3];
double _cosAngleWithHorizontalPlane;
std::string _type;
double _minElevation;
double _maxElevation;
//different types of navigation lines
std::vector<Crossing*> _crossings;
std::vector<Transition*> _transitions;
......@@ -198,6 +199,22 @@ public:
*/
double GetElevation(const Point & p1) const;
/**
* @return the smallest elevation in subroom
* @see GetPlanEquation
*/
double GetMinElevation() const;
/**
* @return the largest elevation in subroom
* @see GetPlanEquation
*/
double GetMaxElevation() const;
void SetMinElevation(double m);
void SetMaxElevation(double M);
/**
* compute the cosine of the dihedral angle with the Horizontal plane Z=h
* @return the cosine of the angle
......
......@@ -387,11 +387,15 @@ double Pedestrian::GetV0Norm() const
const Point& target = _navLine->GetCentre();
double nav_elevation = sub->GetElevation(target);
double delta = nav_elevation - ped_elevation;
//---------------------------------------------------
//-----------------------------------------
// const Point& pos = GetPos();
// double distanceToTarget = (target-pos).Norm();
// double iniDistanceToTarget = (target-_lastPositions.front()).Norm();
// fprintf(stderr, "%f %f %f %f\n", pos.GetX(), pos.GetY(), sub->GetElevation(_ellipse.GetCenter()), 2.0/(1+exp(-9.0*ped_elevation*ped_elevation)));
//printf("delta = %f, nav_elev = %f, ped_elev= %f\n", delta, nav_elevation, ped_elevation);
// printf("delta = %f, nav_elev = %f, ped_elev= %f, ped=[%f %f] targe=[%f, %f]\n", delta, nav_elevation, ped_elevation, pos.GetX(), pos.GetY(), target.GetX(), target.GetY());
// fprintf(stderr, "%f %f front [%f, %f] nav [%f, %f] dist=%f, iniDist=%f\n", delta, ped_elevation, _lastPositions.front()._x, _lastPositions.front()._y, _navLine->GetCentre()._x, _navLine->GetCentre()._y, distanceToTarget, iniDistanceToTarget);
......@@ -399,37 +403,44 @@ double Pedestrian::GetV0Norm() const
// we are walking on an even plane
//TODO: move _ellipse.GetV0() to _V0Plane
if(fabs(delta)<J_EPS){
// fprintf(stderr, "%f %f %f %f\n", pos.GetX(), pos.GetY(), ped_elevation, _ellipse.GetV0());
return _ellipse.GetV0();
}
// we are walking downstairs
else{
double c = 9.0; // should be chosen so that the func grows fast (but smooth) from 0 to 1
double f = 2.0/(1+exp(-c*ped_elevation*ped_elevation)) - 1; // f in [0, 1]
double c = 9.0; // should be chosen so that the func grows fast (but smooth) from 0 to 1
double f; // f in [0, 1]
if(delta<0)
{
// printf("z=%f, f=%f, v0=%f, v0d=%f, ret=%f\n", ped_elevation, f, _ellipse.GetV0(), _V0DownStairs, (1-f)*_V0DownStairs + f*_ellipse.GetV0());
// getc(stdin);
double speed_down = _V0DownStairs;
if(sub->GetType() == "escalator"){
speed_down = _EscalatorDownStairs;
}
else if(sub->GetType() == "idle_escalator"){
speed_down = _V0IdleEscalatorDownStairs;
}
return (1-f)*speed_down + f*_ellipse.GetV0();
double maxSubElevation = sub->GetMaxElevation();
f = 2.0/(1+exp(-c*(maxSubElevation - ped_elevation)*(maxSubElevation - ped_elevation))) - 1;
double speed_down = _V0DownStairs;
if(sub->GetType() == "escalator"){
speed_down = _EscalatorDownStairs;
}
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);
// 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;
}
//we are walking upstairs
else
{
double speed_up = _V0UpStairs;
if(sub->GetType() == "escalator"){
speed_up = _EscalatorUpStairs;
}
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);
// getc(stdin);
double minSubElevation = sub->GetMinElevation();
f = 2.0/(1+exp(-c*(minSubElevation - ped_elevation)*(minSubElevation - ped_elevation))) - 1;
double speed_up = _V0UpStairs;
if(sub->GetType() == "escalator"){
speed_up = _EscalatorUpStairs;
}
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);
// 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;
}
}
......
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