change tool/map_lanetoxdor. add make all roadcontact. but not complete.

src/tool/map_lanetoxodr/autoroadcontact.cpp

@@ -3,13 +3,26 @@
 #include <math.h>
 #include <iostream>
 
+#include "xodrfunc.h"
+
 AutoRoadContact::AutoRoadContact()
 {
 
 }
 
+/**
+ * @brief AutoRoadContact::CalcContact
+ * @param pRoad1
+ * @param pRoad2
+ * @param contacttype 0:start to start  1:start to end 2:end to start 3:end to end
+ * @param turnstraight  0: turn  1:straight 2:u-turn
+ * @param xARCLane   right lane contact
+ * @param xARCOpLane left lane contact
+ * @param fDisTolerance    distance must below this value
+ * @return
+ */
 int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,int & turnstraight,
-                                 std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane)
+                                 std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane, const double fDisTolerance)
 {
 
     double road1_start_x,road1_start_y,road1_end_x,road1_end_y,road1_start_hdg,road1_end_hdg;
@@ -49,6 +62,18 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         }
     }
 
+    if(fdismin > fDisTolerance)
+    {
+        std::cout<<"dis is grater than Distance Tolerance. Tolerance is "<<fDisTolerance<<std::endl;
+        return -3;
+    }
+
+    if(fdismin < 0.1)
+    {
+        std::cout<<"dis is less than 0.1"<<std::endl;
+        return -4;
+    }
+
     contacttype = index;
 
     if(contacttype == -1)
@@ -381,3 +406,250 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
     return 0;
 
 }
+
+std::vector<Road *> AutoRoadContact::GetRelaRoad(OpenDrive *pxodr, Road *pRoad)
+{
+    std::vector<Road *> xvectorRoadRela;
+    unsigned int i;
+    if(pRoad->GetPredecessor() != 0)
+    {
+        if(pRoad->GetPredecessor()->GetElementType() == "road")
+        {
+            std::string roadid = pRoad->GetPredecessor()->GetElementId();
+            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
+            if(pRoadTem != 0)
+            {
+                xvectorRoadRela.push_back(pRoadTem);
+            }
+        }
+        if(pRoad->GetPredecessor()->GetElementType() == "junction")
+        {
+            std::string junctionid = pRoad->GetPredecessor()->GetElementId();
+            Junction * pJunction = xodrfunc::GetJunctionByID(pxodr,junctionid);
+            if(pJunction != 0)
+            {
+                unsigned int njucntionconcount = pJunction->GetJunctionConnectionCount();
+                for(i=0;i<njucntionconcount;i++)
+                {
+                    JunctionConnection * pJC = pJunction->GetJunctionConnection(i);
+                    if(pJC == NULL)continue;
+                    if(pJC->GetIncomingRoad() == pRoad->GetRoadId())
+                    {
+                        std::string roadid = pJC->GetConnectingRoad();
+                        Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
+                        if(pRoadTem != 0)
+                        {
+                            xvectorRoadRela.push_back(pRoadTem);
+                        }
+                    }
+                    else
+                    {
+                        if(pJC->GetConnectingRoad() == pRoad->GetRoadId())
+                        {
+                            std::string roadid = pJC->GetIncomingRoad();
+                            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
+                            if(pRoadTem != 0)
+                            {
+                                xvectorRoadRela.push_back(pRoadTem);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+    if(pRoad->GetSuccessor() != 0)
+    {
+        if(pRoad->GetSuccessor()->GetElementType() == "road")
+        {
+            std::string roadid = pRoad->GetSuccessor()->GetElementId();
+            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
+            if(pRoadTem != 0)
+            {
+                xvectorRoadRela.push_back(pRoadTem);
+            }
+        }
+        if(pRoad->GetSuccessor()->GetElementType() == "junction")
+        {
+            std::string junctionid = pRoad->GetSuccessor()->GetElementId();
+            Junction * pJunction = xodrfunc::GetJunctionByID(pxodr,junctionid);
+            if(pJunction != 0)
+            {
+                unsigned int njucntionconcount = pJunction->GetJunctionConnectionCount();
+                for(i=0;i<njucntionconcount;i++)
+                {
+                    JunctionConnection * pJC = pJunction->GetJunctionConnection(i);
+                    if(pJC == NULL)continue;
+                    if(pJC->GetIncomingRoad() == pRoad->GetRoadId())
+                    {
+                        std::string roadid = pJC->GetConnectingRoad();
+                        Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
+                        if(pRoadTem != 0)
+                        {
+                            xvectorRoadRela.push_back(pRoadTem);
+                        }
+                    }
+                    else
+                    {
+                        if(pJC->GetConnectingRoad() == pRoad->GetRoadId())
+                        {
+                            std::string roadid = pJC->GetIncomingRoad();
+                            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
+                            if(pRoadTem != 0)
+                            {
+                                xvectorRoadRela.push_back(pRoadTem);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    return xvectorRoadRela;
+
+
+}
+
+bool AutoRoadContact::RoadToRoad(OpenDrive * pxodr,Road * pRoad1,bool bRoad1Start,Road * pRoad2,bool bRoad2Start,std::vector<Road *> & xvectorthroughroad)
+{
+    RoadLink * pLink;
+    if(bRoad1Start == false)
+    {
+        pLink = pRoad1->GetSuccessor();
+    }
+    else
+    {
+        pLink = pRoad1->GetPredecessor();
+    }
+
+    if(pLink->GetElementType() == "road")
+    {
+        std::string strroadid = pLink->GetElementId();
+        Road * pcenterroad = xodrfunc::GetRoadByID(pxodr,strroadid);
+
+    }
+    return false;
+}
+bool AutoRoadContact::IsExist(OpenDrive *pxodr, iv::RoadContactUnit xRCU)
+{
+    Road * pRoad1 = xRCU.mpRoad1;
+    Road * pRoad2 = xRCU.mpRoad2;
+    std::vector<Road *> xvectorRoad1Rela;
+    std::vector<Road *> xvectorRoad2Rela;
+    xvectorRoad1Rela = AutoRoadContact::GetRelaRoad(pxodr,pRoad1);
+    xvectorRoad2Rela = AutoRoadContact::GetRelaRoad(pxodr,pRoad2);
+
+    if((xRCU.mcontactype ==0)||(xRCU.mcontactype == 1))
+    {
+        if(pRoad1->GetPredecessor() == NULL)
+        {
+            return false;
+        }
+        RoadLink * pLink = pRoad1->GetPredecessor();
+        if(pLink->GetElementType() == "road")
+        {
+            Road * pRoadCenter = xodrfunc::GetRoadByID(pxodr,pLink->GetElementId());
+            if(pRoadCenter != 0)
+            {
+                if(pRoadCenter == pRoad2)
+                {
+                    return true;
+                }
+                else
+                {
+                    if(pLink->GetContactPoint() == "end")
+                    {
+                        RoadLink * pLinkCenter = pRoadCenter->GetPredecessor();
+                        if(pLinkCenter == NULL)
+                        {
+                            return false;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    //Not turn
+    if(xRCU.mturnstraight != 0)
+    {
+        unsigned int i;
+        for(i=0;i<xvectorRoad2Rela.size();i++)
+        {
+            if(xvectorRoad2Rela[i] == pRoad1)
+            {
+                return true;
+            }
+        }
+        for(i=0;i<xvectorRoad1Rela.size();i++)
+        {
+            if(xvectorRoad1Rela[i] == pRoad2)
+            {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance)
+{
+    unsigned int nRoadCount = pxodr->GetRoadCount();
+    unsigned i;
+    std::vector<iv::RoadContactUnit> xvectorRCU;
+    for(i=0;i<nRoadCount;i++)
+    {
+        Road * pnowRoad = pxodr->GetRoad(i);
+        unsigned int j;
+        for(j=0;j<nRoadCount;j++)
+        {
+            if(j != i)
+            {
+                Road * potherRoad = pxodr->GetRoad(j);
+                int contactype;
+                int turnstraight;
+                std::vector<iv::ARC> xARCLane,xARCOpLane;
+                int nARCRtn = CalcContact(pnowRoad,potherRoad,contactype,turnstraight,xARCLane,xARCOpLane);
+                if(nARCRtn == 0)
+                {
+                    std::cout<<" Road:"<<pnowRoad->GetRoadId()<<" Road:"<<potherRoad->GetRoadId()
+                            <<" contact type: "<<turnstraight<<std::endl;
+
+                    if(xARCLane.size()>0 || (xARCOpLane.size()>0))
+                    {
+                        iv::RoadContactUnit rcu;
+                        rcu.mARCLane = xARCLane;
+                        rcu.mARCOpLane = xARCOpLane;
+                        rcu.mcontactype = contactype;
+                        rcu.mturnstraight = turnstraight;
+                        rcu.mpRoad1 = pnowRoad;
+                        rcu.mpRoad2 = potherRoad;
+                        xvectorRCU.push_back(rcu);
+                    }
+                }
+            }
+        }
+    }
+
+    //Delete Repeate strait
+    for(i=0;i<xvectorRCU.size();i++)
+    {
+        unsigned int j;
+        for(j=(i+1);j<xvectorRCU.size();j++)
+        {
+            if((xvectorRCU[i].mpRoad1 == xvectorRCU[j].mpRoad2)&&(xvectorRCU[i].mpRoad2 == xvectorRCU[j].mpRoad1))
+            {
+                if((xvectorRCU[i].mturnstraight == 1)&&(xvectorRCU[j].mturnstraight == 1))
+                {
+                    std::cout<<"because repeate straight.erase Road:"<<xvectorRCU[j].mpRoad1->GetRoadId()
+                            <<" Road:"<<xvectorRCU[j].mpRoad2->GetRoadId()
+                            <<" contact type: "<<xvectorRCU[j].mturnstraight<<std::endl;
+                    xvectorRCU.erase(xvectorRCU.begin()+j);
+                }
+            }
+        }
+    }
+
+    return 0;
+}

src/tool/map_lanetoxodr/autoroadcontact.h

@@ -10,6 +10,16 @@ struct ARC
     int from;
     int to;
 };
+
+struct RoadContactUnit
+{
+    Road * mpRoad1;
+    Road * mpRoad2;
+    int mcontactype;
+    int mturnstraight;
+    std::vector<iv::ARC> mARCLane;
+    std::vector<iv::ARC> mARCOpLane;
+};
 }
 
 class AutoRoadContact
@@ -18,7 +28,17 @@ public:
     AutoRoadContact();
 
     static int CalcContact(Road * pRoad1,Road * pRoad2,int & contacttype,int & turnstraight,
-                           std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane);
+                           std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane,const double fDisTolerance = 300.0);
+
+    static int MakeAllContact(OpenDrive * pxodr,const double fDisTolerance = 30.0);
+
+    static bool IsExist(OpenDrive * pxodr,iv::RoadContactUnit xRCU);
+
+
+private:
+    static std::vector<Road * > GetRelaRoad(OpenDrive * pxodr,Road * pRoad);
+
+    static bool RoadToRoad(OpenDrive * pxodr,Road * pRoad1,bool bRoad1Start,Road * pRoad2,bool bRoad2Start,std::vector<Road *> & xvectorthroughroad);
 };
 
 #endif // AUTOROADCONTACT_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -5643,3 +5643,27 @@ void MainWindow::on_actionEdit_Road_Borrow_triggered()
 
     mpfb->SetOpenDrive(mxodr);
 }
+
+void MainWindow::on_actionMake_All_Road_Contact_triggered()
+{
+    QMessageBox::StandardButton button;
+    button=QMessageBox::question(this,tr("路口道路创建"),QString(tr("是否让软件自动创建所有路口道路？")),QMessageBox::Yes|QMessageBox::No);
+    if(button==QMessageBox::No)
+    {
+        return;
+    }
+    else if(button==QMessageBox::Yes)
+    {
+
+    }
+
+#ifdef OPENDRIVE_EDITONLY
+    QMessageBox::warning(this,"Warnig","This Function is not comlete.Please Wait.",QMessageBox::YesAll);
+    return;
+#endif
+
+
+    on_actionAutoConnect_triggered();
+
+    AutoRoadContact::MakeAllContact(&mxodr);
+}

src/tool/map_lanetoxodr/mainwindow.h

@@ -207,6 +207,8 @@ private slots:
 
     void on_actionEdit_Road_Borrow_triggered();
 
+    void on_actionMake_All_Road_Contact_triggered();
+
 private:
 
 

src/tool/map_lanetoxodr/mainwindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>400</width>
-    <height>300</height>
+    <width>670</width>
+    <height>443</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -23,7 +23,7 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>400</width>
+     <width>670</width>
      <height>28</height>
     </rect>
    </property>
@@ -46,6 +46,7 @@
     <addaction name="separator"/>
     <addaction name="actionBack"/>
     <addaction name="separator"/>
+    <addaction name="actionMake_All_Road_Contact"/>
    </widget>
    <widget class="QMenu" name="menuTool">
     <property name="title">
@@ -135,6 +136,11 @@
     <string>Edit Road Borrow</string>
    </property>
   </action>
+  <action name="actionMake_All_Road_Contact">
+   <property name="text">
+    <string>Make All Road Contact</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -1119,6 +1119,22 @@ Road * xodrfunc::GetRoadByID(OpenDrive * pxodr,std::string strroadid)
     return pRoad;
 }
 
+Junction * xodrfunc::GetJunctionByID(OpenDrive * pxodr,std::string strjunctionid)
+{
+    Junction * pJunction = 0;
+    int njunctioncount = pxodr->GetJunctionCount();
+    int i;
+    for(i=0;i<njunctioncount;i++)
+    {
+        if(pxodr->GetJunction(i)->GetId() == strjunctionid)
+        {
+            pJunction = pxodr->GetJunction(i);
+            break;
+        }
+    }
+    return pJunction;
+}
+
 int xodrfunc::GetSpiralXY(GeometrySpiral *pspira, double soff, double &x, double &y, double &hdg)
 {
     pspira->GetCoords(pspira->GetS() + soff,x,y,hdg);

src/tool/map_lanetoxodr/xodrfunc.h

@@ -59,6 +59,7 @@ public:
 
 public:
     static Road * GetRoadByID(OpenDrive * pxodr,std::string strroadid);
+    static Junction * GetJunctionByID(OpenDrive * pxodr,std::string strjunctionid);
     static int GetRoadXYByS(Road * pRoad,const double s,double &x, double & y, double & hdg);
 
     static int GetLineXY(GeometryLine * pline,double soff,double &x, double & y, double & hdg);