Fix problem of intersection of doors

Sometimes train's doors are exactly on the edges of walls.
This commit fixes this problem.

Simulation.cpp

@@ -256,7 +256,10 @@ bool Simulation::InitArgs()
           Log->Write("INFO\tMax          : %d",TT.second->nmax);
           Log->Write("INFO\tnumber doors : %d\n",TT.second->doors.size());
     }
-    Log->Write("Got %d Train Time Tables",_building->GetTrainTimeTables().size());
+    if(_building->GetTrainTimeTables().size())
+         Log->Write("INFO:\tGot %d Train Time Tables",_building->GetTrainTimeTables().size());
+    else
+         Log->Write("WARNING:\tGot %d Train Time Tables",_building->GetTrainTimeTables().size());
     for(auto&& TT: _building->GetTrainTimeTables())
     {
           Log->Write("INFO\tid           : %d",TT.second->id);
@@ -597,6 +600,7 @@ double Simulation::RunBody(double maxSimTime)
                 }
             else{ // quickest needs update even if NeedsUpdate() is false
                  FFRouter* ffrouter = dynamic_cast<FFRouter*>(_routingEngine.get()->GetRouter(ROUTING_FF_QUICKEST));
+                 if(ffrouter != nullptr)
                       if (ffrouter->MustReInit()) {
                            ffrouter->ReInit();
                            ffrouter->SetRecalc(t);
@@ -846,15 +850,29 @@ bool Simulation::correctGeometry(std::shared_ptr<Building> building, std::shared
                   NewWall1.SetType(w1.GetType());
 
                   // add new lines to be controled against overlap with exits
+                  if(NewWall.GetLength() > J_EPS_DIST)
+                  {
                        building->TempAddedWalls[trainId].push_back(NewWall);
+                       subroom->AddWall(NewWall);
+                  }
+
+                  else
+                       std::cout << KRED << ">> WALL did not add: " << NewWall.toString() << "\n" << RESET ;
+
+                  if(NewWall1.GetLength() > J_EPS_DIST)
+                  {
                        building->TempAddedWalls[trainId].push_back(NewWall1);
+                       subroom->AddWall(NewWall1);
+                  }
+                  else
+                       std::cout << KRED << ">> WALL did not add: " << NewWall1.toString() << "\n" << RESET ;
+
                   building->TempAddedDoors[trainId].push_back(*e);
                   building->TempRemovedWalls[trainId].push_back(w1);
-                  subroom->AddWall(NewWall);
-                  subroom->AddWall(NewWall1);
+
                   subroom->RemoveWall(w1);
 
-                  /* std::cout << KRED << "WALL added " << NewWall.toString() << "\n" << RESET ; */
+
                   /* std::cout << KRED << "WALL added " << NewWall1.toString() << "\n" << RESET ; */
                   /* std::cout << KRED << "WALL removed " << w1.toString() << "\n" << RESET ; */
                   /* getc(stdin); */

geometry/Building.cpp

@@ -461,6 +461,8 @@ const std::vector<std::pair<PointWall, PointWall > > Building::GetIntersectionPo
       // collect pairs of pairs
       for(auto door: doors)
       {
+           // std::cout << "================================\n";
+           // std::cout << "door: " << door.toString() <<  "\n";
             PointWall pw1, pw2;
             int nintersections = 0;
             auto n = door.NormalVec();
@@ -470,33 +472,86 @@ const std::vector<std::pair<PointWall, PointWall > > Building::GetIntersectionPo
             auto p22 = door.GetPoint2() - n*scaleFactor;
             auto normalWall1 = Wall(p11, p12);
             auto normalWall2 = Wall(p21, p22);
-            /* std::cout << "normal wall 1: " << normalWall1.toString() << "\n"; */
-            /* std::cout << "normal wall 2: " << normalWall2.toString() << "\n"; */
+            // std::cout << "normal wall 1: " << normalWall1.toString() << "\n";
+            // std::cout << "normal wall 2: " << normalWall2.toString() << "\n";
             for(auto twall: mytrack)
             {
-                  /* std::cout << "  twall " << twall.toString() << "\n"; */
+
                  Point interPoint1, interPoint2;
                  auto res = normalWall1.IntersectionWith(twall, interPoint1);
                  auto res2 = normalWall2.IntersectionWith(twall, interPoint2);
+                 // std::cout << " res " << res << "  res2 " << res2 << "\n";
                  if(res == 1)
                  {
-                        /* std::cout << "intersection at :" << interPoint1.toString() << "\n"; */
+                      if(!twall.NearlyHasEndPoint(interPoint1))
+                      {
+                           // std::cout << "res  twall " << twall.toString() << "\n";
                            pw1 = std::make_pair(interPoint1, twall);
                            nintersections++;
+                           // std::cout << "intersection at :" << interPoint1.toString() << "\n";
+                      }
+                      else // end point
+                      {
+                           if(res2 == 0)
+                           {
+
+                                // std::cout << "twall " << twall.toString() << "\n";
+                                // std::cout << "YY intersection 1 at :" << interPoint1.toString() << "\n";
+                                // std::cout << "YY intersection 2 at :" << interPoint2.toString() << "\n";
+
+                           }
+                           else{
 
+                                // std::cout << "res  twall " << twall.toString() << "\n";
+                                pw1 = std::make_pair(interPoint1, twall);
+                                nintersections++;
+                                // std::cout << "BB: intersection at :" << interPoint1.toString() << "\n";
+                           }
+                       }
                   }
                   if(res2 == 1)
                   {
+                       if(!twall.NearlyHasEndPoint(interPoint2))
+                       {
+                            // std::cout << "res2  twall " << twall.toString() << "\n";
+                            pw2 = std::make_pair(interPoint2, twall);
+                            nintersections++;
+                            // std::cout << "intersection at :" << interPoint2.toString() << "\n";
+                       }
+                       else
+                       {
+                            if(res == 0)
+                            {
+                                 // std::cout << "twall " << twall.toString() << "\n";
+                                 // std::cout << "XX intersection 1 at :" << interPoint1.toString() << "\n";
+                                 // std::cout << "XX intersection 2 at :" << interPoint2.toString() << "\n";
+                            }
+                            else{
+                                 // std::cout << "res2  twall " << twall.toString() << "\n";
                                  pw2 = std::make_pair(interPoint2, twall);
                                  nintersections++;
-                        /* std::cout << "intersection at :" << interPoint2.toString() << "\n"; */
+                                 // std::cout << "CC intersection at :" << interPoint2.toString() << "\n";
                             }
+                       }
+                  }
+                  // std::cout << "door: " << door.toString() << ", intersections: " <<  nintersections << "\n";
+
+
             } // tracks
             // std::cout << "door: " << door.toString() << ", intersections: " <<  nintersections << "\n";
+            // std::cout << "================================\n";
+
             if(nintersections == 2)
                   pws.push_back(std::make_pair(pw1, pw2));
 
+            else
+            {
+                 std::cout << KRED << "Error in GetIntersection. Should be 2 but got  " << nintersections << "\n";
+                                                                                                             exit(-1);
+            }
       }// doors
+      // getc(stdin);
+
       return pws;
 }
 // reset changes made by trainTimeTable[id]

geometry/Line.cpp

@@ -366,7 +366,8 @@ int Line::IntersectionWith(const Point& p1, const Point& p2, Point& p3) const
      double t = (_point1-p1).CrossProduct(s)/(r.CrossProduct(s));
      double u = (_point1-p1).CrossProduct(r)/(r.CrossProduct(s));
 
-     if (0>t || t>1) {
+
+     if (-0.05>t || t>1) {
           return LineIntersectType::NO_INTERSECTION;
      }
 
@@ -464,6 +465,7 @@ int Line::WichSide(const Point& pt)
 
 bool Line::ShareCommonPointWith(const Line& line, Point& P) const
 {
+
      if (line.GetPoint1()==_point1 || line.GetPoint2()==_point1) {
           P = _point1;
           return true;
@@ -488,6 +490,23 @@ bool Line::HasEndPoint(const Point& point) const
      return _point2==point;
 }
 
+bool Line::NearlyHasEndPoint(const Point& point) const
+{
+     // std::cout << _point1.toString() << "\n";
+     // std::cout << _point2.toString() << "\n";
+     // std::cout << point.toString() << "\n";
+
+
+     // std::cout << "--> " << (_point1-point).Norm() << "\n";
+     // std::cout << "--> " << (_point2-point).Norm() << "\n";
+     // std::cout << "<-- " << J_EPS_DIST << "\n";
+
+
+     if ((_point1-point).Norm() <= J_EPS_DIST) return true;
+     return ((_point2-point).Norm() <= J_EPS_DIST);
+}
+
+
 bool Line::IntersectionWithCircle(const Point& centre, double radius /*cm for pedestrians*/)
 {
 

geometry/Line.h

@@ -134,6 +134,7 @@ public:
      bool IsInLineSegment(const Point& p) const;
 
      bool NearlyInLineSegment(const Point& p) const;
+     bool NearlyHasEndPoint(const Point& point) const;
 
      /**
       * @return the distance from the line to the point p

math/VelocityModel.cpp

@@ -293,7 +293,7 @@ void VelocityModel::ComputeNextTimeStep(double current, double deltaT, Building*
                 spacings.clear(); //clear for ped p
 
                 // stuck peds get removed. Warning is thrown. low speed due to jam is omitted.
-                if(ped->GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 30 + ped->GetPremovementTime() &&
+                if(ped->GetTimeInJam() > ped->GetPatienceTime() && ped->GetGlobalTime() > 10000 + ped->GetPremovementTime() &&
                           std::max(ped->GetMeanVelOverRecTime(), ped->GetV().Norm()) < 0.01 &&
                           size == 0 ) // size length of peds neighbour vector
                 {