Merge branch '300-tracks-for-trains' into 'develop'

Interface of trains

Closes #300

See merge request !49

IO/GeoFileParser.cpp

@@ -50,6 +50,11 @@ void GeoFileParser::LoadBuilding(Building* building)
           Log->Write("ERROR:\t could not load extra traffic information!");
           exit(EXIT_FAILURE);
      }
+     if(!LoadTrainInfo(building))
+     {
+          Log->Write("ERROR:\t could not load train information!");
+          exit(EXIT_FAILURE);
+     }
 }
 
 bool GeoFileParser::LoadGeometry(Building* building)
@@ -199,6 +204,7 @@ bool GeoFileParser::LoadGeometry(Building* building)
                for (TiXmlElement* xPolyVertices = xSubRoom->FirstChildElement("polygon"); xPolyVertices;
                     xPolyVertices = xPolyVertices->NextSiblingElement("polygon")) {
 
+                    std::string wall_type = xmltoa(xPolyVertices->Attribute("type"), "wall");
                     for (TiXmlElement* xVertex = xPolyVertices->FirstChildElement(
                               "vertex");
                          xVertex && xVertex!=xPolyVertices->LastChild("vertex");
@@ -208,7 +214,7 @@ bool GeoFileParser::LoadGeometry(Building* building)
                          double y1 = xmltof(xVertex->Attribute("py"));
                          double x2 = xmltof(xVertex->NextSiblingElement("vertex")->Attribute("px"));
                          double y2 = xmltof(xVertex->NextSiblingElement("vertex")->Attribute("py"));
-                         subroom->AddWall(Wall(Point(x1, y1), Point(x2, y2)));
+                         subroom->AddWall(Wall(Point(x1, y1), Point(x2, y2), wall_type));
                          //printf("%0.2f %0.2f %0.2f %0.2f\n",x1,y1,x2,y2);
                     }
 
@@ -327,7 +333,7 @@ bool GeoFileParser::LoadGeometry(Building* building)
                }
           }
           else{
-               Log->Write("INFO:\tNot parsing transition from file %s");
+               Log->Write("INFO:\tNot parsing transition from file");
           }
           Log->Write("INFO:\tGot %d transitions", building-> GetAllTransitions().size());
 
@@ -464,7 +470,7 @@ bool GeoFileParser::parseDoorNode(TiXmlElement * xDoor, int id, Building* buildi
           building->GetTransition(id)->TempClose();
           break;
      case DoorState::Error:
-          Log->Write("WARNING:\t Unknown door state: <%s>. open or close. Default: open",
+          Log->Write("WARNING:\t Unknown door state: <%s>. open, close or temp_close. Default: open",
                     stateStr.c_str());
           building->GetTransition(id)->Open();
           break;
@@ -791,6 +797,220 @@ Goal* GeoFileParser::parseWaitingAreaNode(TiXmlElement * e)
      return wa;
 }
 
+bool GeoFileParser::LoadTrainInfo(Building* building)
+{
+     Log->Write("--------\nINFO:\tLoading the train info");
+
+     TiXmlDocument doc(_configuration->GetProjectFile());
+     if (!doc.LoadFile()) {
+          Log->Write("ERROR: \t%s", doc.ErrorDesc());
+          Log->Write("ERROR: \t could not parse the project file");
+          return false;
+     }
+     TiXmlElement* xRootNode = doc.RootElement();
+     if (!xRootNode) {
+          Log->Write("ERROR:\tRoot element does not exist");
+          return false;
+     }
+     if (!xRootNode->FirstChild("train_constraints")) {
+          Log->Write("WARNING:\tNo train constraints were found. Continue.");
+     }
+     bool resTTT = true;
+     bool resType = true;
+     if(xRootNode->FirstChild("train_constraints"))
+     {
+          resTTT = LoadTrainTimetable(building, xRootNode);
+          resType = LoadTrainType(building, xRootNode);
+     }
+
+     return (resTTT && resType);
+}
+bool GeoFileParser::LoadTrainTimetable(Building* building, TiXmlElement * xRootNode)
+{
+     TiXmlNode* xTTTNode = xRootNode->FirstChild("train_constraints")->FirstChild("train_time_table");
+     std::string TTTFilename;
+     if(xTTTNode)
+     {
+          fs::path p(_configuration->GetProjectRootDir());
+          TTTFilename = xTTTNode->FirstChild()->ValueStr();
+          p /= TTTFilename;
+          TTTFilename = p.string();
+          Log->Write("INFO:\tTrain Timetable file <%s> will be parsed", TTTFilename.c_str());
+     }
+     else return true;
+
+
+     TiXmlDocument docTTT(TTTFilename);
+     if (!docTTT.LoadFile()) {
+          Log->Write("ERROR: \t%s", docTTT.ErrorDesc());
+          Log->Write("ERROR: \t could not parse the train timetable file.");
+          return false;
+     }
+     TiXmlElement* xTTT = docTTT.RootElement();
+     if (!xTTT) {
+          Log->Write("ERROR:\tRoot element does not exist in TTT file.");
+          return false;
+     }
+     if (xTTT->ValueStr() != "train_time_table") {
+          Log->Write("ERROR:\tParsing train timetable file. Root element value is not 'train_time_table'.");
+          return false;
+     }
+     for (TiXmlElement* e = xTTT->FirstChildElement("train"); e;
+                    e = e->NextSiblingElement("train")) {
+          std::shared_ptr<TrainTimeTable> TTT = parseTrainTimeTableNode(e);
+
+          if (TTT) {
+               building->AddTrainTimeTable(TTT);
+          }
+     }
+     return true;
+}
+bool GeoFileParser::LoadTrainType(Building* building, TiXmlElement * xRootNode)
+{
+     TiXmlNode* xTTNode = xRootNode->FirstChild("train_constraints")->FirstChild("train_types");
+     std::string TTFilename;
+     if(xTTNode)
+     {
+          fs::path p(_configuration->GetProjectRootDir());
+          TTFilename = xTTNode->FirstChild()->ValueStr();
+          p /= TTFilename;
+          TTFilename = p.string();
+          Log->Write("INFO:\tTrain Type file <%s> will be parsed", TTFilename.c_str());
+     }
+     else return true;
+
+
+     TiXmlDocument docTT(TTFilename);
+     if (!docTT.LoadFile()) {
+          Log->Write("ERROR: \t%s", docTT.ErrorDesc());
+          Log->Write("ERROR: \t could not parse the train type file.");
+          return false;
+     }
+     TiXmlElement* xTT = docTT.RootElement();
+     if (!xTT) {
+          Log->Write("ERROR:\tRoot element does not exist in TT file.");
+          return false;
+     }
+     if (xTT->ValueStr() != "train_type") {
+          Log->Write("ERROR:\tParsing train type file. Root element value is not 'train_type'.");
+          return false;
+     }
+     for (TiXmlElement* e = xTT->FirstChildElement("train"); e;
+                    e = e->NextSiblingElement("train")) {
+          std::shared_ptr<TrainType> TT = parseTrainTypeNode(e);
+          if (TT) {
+               building->AddTrainType(TT);
+          }
+     }
+     return true;
+
+}
+
+std::shared_ptr<TrainTimeTable> GeoFileParser::parseTrainTimeTableNode(TiXmlElement * e)
+{
+     Log->Write("INFO:\tLoading train time table NODE");
+     std::string caption = xmltoa(e->Attribute("caption"), "-1");
+     int id = xmltoi(e->Attribute("id"), -1);
+     std::string type = xmltoa(e->Attribute("type"), "-1");
+     int room_id = xmltoi(e->Attribute("room_id"), -1);
+     int subroom_id = xmltoi(e->Attribute("subroom_id"), -1);
+     int platform_id = xmltoi(e->Attribute("platform_id"), -1);
+     float track_start_x = xmltof(e->Attribute("track_start_x"), -1);
+     float track_start_y = xmltof(e->Attribute("track_start_y"), -1);
+     float track_end_x = xmltof(e->Attribute("track_end_x"), -1);
+     float track_end_y = xmltof(e->Attribute("track_end_y"), -1);
+
+     float train_start_x = xmltof(e->Attribute("train_start_x"),-1);
+     float train_start_y = xmltof(e->Attribute("train_start_y"), -1);
+     float train_end_x = xmltof(e->Attribute("train_end_x"), -1);
+     float train_end_y = xmltof(e->Attribute("train_end_y"), -1);
+
+     float arrival_time = xmltof(e->Attribute("arrival_time"), -1);
+     float departure_time = xmltof(e->Attribute("departure_time"), -1);
+     // @todo: check these values for correctness e.g. arrival < departure
+     Log->Write("INFO:\tTrain time table:");
+     Log->Write("INFO:\t   id: %d", id);
+     Log->Write("INFO:\t   type: %s", type.c_str());
+     Log->Write("INFO:\t   room_id: %d", room_id);
+     Log->Write("INFO:\t   subroom_id: %d", subroom_id);
+     Log->Write("INFO:\t   platform_id: %d", platform_id);
+     Log->Write("INFO:\t   track_start: [%.2f, %.2f]", track_start_x, track_start_y);
+     Log->Write("INFO:\t   track_end: [%.2f, %.2f]", track_end_x, track_end_y);
+     Log->Write("INFO:\t   arrival_time: %.2f", arrival_time);
+     Log->Write("INFO:\t   departure_time: %.2f", departure_time);
+     Point track_start(track_start_x, track_start_y);
+     Point track_end(track_end_x, track_end_y);
+     Point train_start(train_start_x, train_start_y);
+     Point train_end(train_end_x, train_end_y);
+     std::shared_ptr<TrainTimeTable> trainTimeTab = std::make_shared<TrainTimeTable>(
+          TrainTimeTable{
+                    id,
+                    type,
+                    room_id,
+                    subroom_id,
+                    arrival_time,
+                    departure_time,
+                    track_start,
+                    track_end,
+                    train_start,
+                    train_end,
+                    platform_id,
+                    false,
+                    false,
+                    });
+
+     return trainTimeTab;
+}
+
+std::shared_ptr<TrainType> GeoFileParser::parseTrainTypeNode(TiXmlElement * e)
+{
+     Log->Write("INFO:\tLoading train type");
+     // int T_id = xmltoi(e->Attribute("id"), -1);
+     std::string type = xmltoa(e->Attribute("type"), "-1");
+     int agents_max = xmltoi(e->Attribute("agents_max"), -1);
+     float length = xmltof(e->Attribute("length"), -1);
+     // std::shared_ptr<Transition> t = new Transition();
+     // std::shared_ptr<Transition> doors;
+     Transition t;
+     std::vector<Transition> doors;
+
+     for (TiXmlElement* xDoor = e->FirstChildElement("door"); xDoor;
+          xDoor = xDoor->NextSiblingElement("door")) {
+          int D_id = xmltoi(xDoor->Attribute("id"), -1);
+          float x1 = xmltof(xDoor->FirstChildElement("vertex")->Attribute("px"), -1);
+          float y1 = xmltof(xDoor->FirstChildElement("vertex")->Attribute("py"), -1);
+          float x2 = xmltof(xDoor->LastChild("vertex")->ToElement()->Attribute("px"), -1);
+          float y2 = xmltof(xDoor->LastChild("vertex")->ToElement()->Attribute("py"), -1);
+          Point start(x1, y1);
+          Point end(x2, y2);
+          float outflow = xmltof(xDoor->Attribute("outflow"), -1);
+          float dn = xmltoi(xDoor->Attribute("dn"), -1);
+          t.SetID(D_id);
+          t.SetCaption(type + std::to_string(D_id));
+          t.SetPoint1(start);
+          t.SetPoint2(end);
+          t.SetOutflowRate(outflow);
+          t.SetDN(dn);
+          doors.push_back(t);
+     }
+     Log->Write("INFO:\t   type: %s", type.c_str());
+     Log->Write("INFO:\t   capacity: %d", agents_max);
+     Log->Write("INFO:\t   number of doors: %d", doors.size());
+     for(auto d: doors)
+     {
+          Log->Write("INFO\t      door (%d): %s | %s", d.GetID(), d.GetPoint1().toString().c_str(), d.GetPoint2().toString().c_str());
+     }
+
+     std::shared_ptr<TrainType> Type = std::make_shared<TrainType>(
+          TrainType{
+                    type,
+                    agents_max,
+                    length,
+                    doors,
+                    });
+   return Type;
+
+}
 
 GeoFileParser::~GeoFileParser()
 {

IO/GeoFileParser.h

@@ -44,6 +44,11 @@ public:
      Goal* parseGoalNode(TiXmlElement * e);
      Transition* parseTransitionNode(TiXmlElement * xTrans, Building * building);
      Goal* parseWaitingAreaNode(TiXmlElement * e);
+     bool LoadTrainInfo(Building* building);
+     bool LoadTrainTimetable(Building* building, TiXmlElement * xRootNode);
+     bool LoadTrainType(Building* building, TiXmlElement * xRootNode);
+     std::shared_ptr<TrainType> parseTrainTypeNode(TiXmlElement * e);
+     std::shared_ptr<TrainTimeTable> parseTrainTimeTableNode(TiXmlElement * e);
 
 private:
      Configuration* _configuration;

IO/IODispatcher.cpp

@@ -370,16 +370,69 @@ std::string getEventFileName(const std::string & GetProjectFile)
      TiXmlNode* xMainNode = doc.RootElement();
      string eventfile = "";
      if (xMainNode->FirstChild("events_file")) {
-          ret = xMainNode->FirstChild("events_file")->FirstChild()->Value();
-          Log->Write("INFO: \tevents <" + eventfile + ">");
+          ret = xMainNode->FirstChild("events_file")->FirstChild()->ValueStr();
+          Log->Write("INFO: \tevents <" + ret + ">");
      } else {
           Log->Write("INFO: \tNo events found");
           return ret;
      }
      return ret;
 }
+ // <train_constraints>
+ //   <train_time_table>ttt.xml</train_time_table>
+ //   <train_types>train_types.xml</train_types>
+ // </train_constraints>
 
 
+std::string getTrainTimeTableFileName(const std::string & GetProjectFile)
+{
+     std::string ret="";
+
+     TiXmlDocument doc(GetProjectFile);
+     if (!doc.LoadFile()) {
+          Log->Write("ERROR: \t%s", doc.ErrorDesc());
+          Log->Write("ERROR: \tGetTrainTimeTable could not parse the project file");
+          return ret;
+     }
+     TiXmlNode* xMainNode = doc.RootElement();
+     string tttfile = "";
+     if (xMainNode->FirstChild("train_constraints")) {
+          TiXmlNode * xFileNode = xMainNode->FirstChild("train_constraints")->FirstChild("train_time_table");
+
+          if(xFileNode)
+               ret = xFileNode->FirstChild()->ValueStr();
+          Log->Write("INFO: \ttrain_time_table <" + ret + ">");
+     } else {
+          Log->Write("INFO: \tNo events no ttt file found");
+          return ret;
+     }
+     return ret;
+}
+
+
+std::string getTrainTypeFileName(const std::string & GetProjectFile)
+{
+     std::string ret="";
+
+     TiXmlDocument doc(GetProjectFile);
+     if (!doc.LoadFile()) {
+          Log->Write("ERROR: \t%s", doc.ErrorDesc());
+          Log->Write("ERROR: \tGetTrainType could not parse the project file");
+          return ret;
+     }
+     TiXmlNode* xMainNode = doc.RootElement();
+     string tttfile = "";
+     if (xMainNode->FirstChild("train_constraints")) {
+          auto xFileNode = xMainNode->FirstChild("train_constraints")->FirstChild("train_types");
+          if(xFileNode)
+               ret = xFileNode->FirstChild()->ValueStr();
+          Log->Write("INFO: \ttrain_types <" + ret + ">");
+     } else {
+          Log->Write("INFO: \tNo events no train types file found");
+          return ret;
+     }
+     return ret;
+}
 std::string getGoalFileName(const std::string & GetProjectFile)
 {
      std::string ret="";
@@ -411,6 +464,8 @@ void TrajectoriesFLAT::WriteHeader(long nPeds, double fps, Building* building, i
      std::string sourceFileName = getSourceFileName(building->GetProjectFilename());
      std::string goalFileName = getGoalFileName(building->GetProjectFilename());
      std::string eventFileName = getEventFileName(building->GetProjectFilename());
+     std::string trainTimeTableFileName = getTrainTimeTableFileName(building->GetProjectFilename());
+     std::string trainTypeFileName = getTrainTypeFileName(building->GetProjectFilename());
      (void) seed; (void) nPeds;
      char tmp[100] = "";
      sprintf(tmp, "#description: jpscore (%s)", JPSCORE_VERSION);
@@ -436,6 +491,16 @@ void TrajectoriesFLAT::WriteHeader(long nPeds, double fps, Building* building, i
           sprintf(tmp,"#events: %s", eventFileName.c_str());
           Write(tmp);
      }
+     if( trainTimeTableFileName  != "")
+     {
+          sprintf(tmp,"#trainTimeTable: %s", trainTimeTableFileName.c_str());
+          Write(tmp);
+     }
+     if( trainTypeFileName  != "")
+     {
+          sprintf(tmp,"#trainType: %s", trainTypeFileName.c_str());
+          Write(tmp);
+     }
      Write("#ID: the agent ID");
      Write("#FR: the current frame");
      Write("#X,Y,Z: the agents coordinates (in metres)");

IO/IniFileParser.cpp

@@ -1635,6 +1635,9 @@ bool IniFileParser::ParseStrategyNodeToObject(const TiXmlNode& strategyNode)
                     };
                     _config->set_dirStrategy(dynamic_cast<DirectionSubLocalFloorfieldTripsVoronoi*>(_exit_strategy.get()));
                     break;
+               case 12:
+                    _exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionTrain());
+                    break;
 
                default:
                     _exit_strategy = std::shared_ptr<DirectionStrategy>(new DirectionMinSeperationShorterLine());

Simulation.h

@@ -49,6 +49,9 @@
 #include "events/EventManager.h"
 #include "pedestrian/AgentsSourcesManager.h"
 #include "general/Configuration.h"
+#include <filesystem>
+
+
 
 //Forward declarations
 //class AgentsSourcesManager;
@@ -88,6 +91,8 @@ private:
      int _maxSimTime;
 
     bool _gotSources; // is true if we got some sources. Otherwise, false.
+     bool _trainConstraints; // true if inifile has some train constraints
+
     // bool _printPB; // print progressbar
 public:
     /**
@@ -185,6 +190,11 @@ public:
      */
      void UpdateDoorticks() const;
      int GetMaxSimTime() const;
+     void  incrementCountTraj();
+
+     bool correctGeometry(std::shared_ptr<Building> building,  std::shared_ptr<TrainTimeTable>);
+     bool WriteTrajectories(std::string trajName);
+     bool TrainTraffic();
 
      int _countTraj=0; // count number of TXT trajectories to produce
      double _maxFileSize; // in MB

geometry/Building.h

@@ -45,6 +45,43 @@
 #include "Goal.h"
 #include "../general/Configuration.h"
 
+typedef std::pair<Point, Wall> PointWall;
+
+// train schedules: Trains get deleted and added.
+
+struct Platform
+{
+     int id;
+     int rid;
+     int sid;
+     std::map<int, std::vector<Wall> > tracks;
+};
+
+struct TrainTimeTable
+{
+     int id;
+     std::string type;
+     int rid; // room id
+     int sid; // subroom id
+     double tin; // arrival time
+     double tout; //leaving time
+     Point pstart; // track start
+     Point pend; // track end
+     Point tstart; // train start
+     Point tend; // train end
+     int pid; // Platform id
+     bool arrival;
+     bool departure;
+};
+struct TrainType
+{
+     std::string type;
+     int nmax; // agents_max
+     float len; //length
+     std::vector<Transition> doors;
+};
+
+
 class RoutingEngine;
 
 class Pedestrian;
@@ -74,8 +111,9 @@ private:
      std::map<int, Transition*> _transitions;
      std::map<int, Hline*> _hLines;
      std::map<int, Goal*> _goals;
-     std::map<int, std::vector<WaitingArea*>> _sr2wa;
-
+     std::map<std::string, std::shared_ptr<TrainType> > _trainTypes;
+     std::map<int, std::shared_ptr<TrainTimeTable> > _trainTimeTables;
+     std::map<int, std::shared_ptr<Platform> > _platforms;
      /// pedestrians pathway
      bool _savePathway;
      std::ofstream _pathWayStream;
@@ -83,10 +121,13 @@ private:
 public:
      /// constructor
      Building();
+     std::map<int, std::vector<Wall> > TempAddedWalls; // map to trainTimeTable
+     std::map<int, std::vector<Wall> > TempRemovedWalls;
+     std::map<int, std::vector<Transition> > TempAddedDoors;
 
 //    Building(const std::string &, const std::string &, RoutingEngine &, PedDistributor &, double);
      Building(Configuration* config, PedDistributor& pedDistributor);
-
+     bool resetGeometry(std::shared_ptr<TrainTimeTable> tab);
      /// destructor
      virtual ~Building();
 
@@ -208,15 +249,33 @@ public:
      const std::map<int, Hline*>& GetAllHlines() const;
 
      const std::map<int, Goal*>& GetAllGoals() const;
+     // --------------- Trains interface
+     const std::map<std::string, std::shared_ptr<TrainType> >& GetTrainTypes() const;
+
+     const std::map<int, std::shared_ptr<TrainTimeTable> >& GetTrainTimeTables() const;
+
+     const std::map<int, std::shared_ptr<Platform> >& GetPlatforms() const;
 
+     const std::vector<Wall> GetTrackWalls(Point TrackStart, Point TrackEnd, int & room_id, int & subroom_id) const;
+     const std::vector<std::pair<PointWall, PointWall > > GetIntersectionPoints(const std::vector<Transition> doors, const std::vector<Wall>) const;
+
+     // ------------------------------------
      bool AddCrossing(Crossing* line);
 
+     bool RemoveTransition(Transition * line);
+
      bool AddTransition(Transition* line);
 
      bool AddHline(Hline* line);
 
      bool AddGoal(Goal* goal);
 
+     bool AddTrainType(std::shared_ptr<TrainType> TT);
+
+     bool AddTrainTimeTable(std::shared_ptr<TrainTimeTable> TTT);
+
+     bool AddPlatform(std::shared_ptr<Platform> P);
+
      const std::string& GetProjectRootDir() const;
 
      const std::string& GetProjectFilename() const;
@@ -259,7 +318,7 @@ public:
 private:
 
      bool InitInsideGoals();
-
+     bool InitPlatforms();
      void StringExplode(std::string str, std::string separator, std::vector<std::string>* results);
      /** @defgroup auto-correct-geometry
       * functions used to auto-correct the geometry.
@@ -349,7 +408,7 @@ private:
       * @param subroom
       * @return bool
       */
-bool RemoveOverlappingDoors(
+     bool RemoveOverlappingDoors(
           const std::shared_ptr<SubRoom>& subroom) const;
      /** @} */ // end of group
 

geometry/Crossing.cpp

@@ -334,9 +334,9 @@ DoorState Crossing::GetState() const
      return _state;
 }
 
-void Crossing::SetState(DoorState _state)
+void Crossing::SetState(DoorState state)
 {
-     Crossing::_state = _state;
+     Crossing::_state = state;
 }
 
 std::string Crossing::toString() const
@@ -355,6 +355,9 @@ std::string Crossing::toString() const
      case DoorState::TEMP_CLOSE:
           tmp << " temp_close";
           break;
+     case DoorState::Error:
+          tmp << " Error";
+          break;
 
      }
      return tmp.str();

geometry/Line.cpp

@@ -201,6 +201,16 @@ Point Line::LotPoint(const Point& p) const
      return f;
 }
 
+// return true if point the orthogonal projection of p on Line segment is on the
+// line segment.
+bool Line::isBetween(const Point& p) const
+{
+     const Point& t = _point1-_point2;
+     double lambda = (p-_point2).ScalarProduct(t)/t.ScalarProduct(t);
+     return  (lambda>0) && (lambda <1);
+}
+
+
 /* Punkt auf der Linie mit kürzestem Abstand zu p
  * In der Regel Lotfußpunkt, Ist der Lotfußpunkt nicht im Segment
  * wird der entsprechende Eckpunkt der Line genommen
@@ -273,6 +283,12 @@ bool Line::operator!=(const Line& l) const
      return (!(*this==l));
 
 }
+// this function is necessary to use std::set and is basically the same as !=
+bool Line::operator<(const Line& l) const
+{
+     return (!(*this==l));
+}
+
 
 double Line::GetLength() const
 {
@@ -350,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;
      }
 
@@ -448,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;
@@ -472,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";
+
+