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

geometry/Building.cpp

@@ -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
 

geometry/SubRoom.cpp

@@ -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)

geometry/SubRoom.h

@@ -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

pedestrian/Pedestrian.cpp

@@ -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;
            }
      }