change tool/map_lanetoxodr. add auto raod contact function.

src/common/common/xodr/OpenDrive/Lane.cpp

@@ -420,6 +420,44 @@ bool LaneSection::FillLaneSectionSample(double s_check, LaneSectionSample& laneS
 	return true;
 }
 
+//Examp i= 1 return lane id = 1 lane
+Lane * LaneSection::GetLeftLaneAt(unsigned int index)
+{
+    int nlaneid = index;
+    Lane * pRtn = NULL;
+
+    unsigned int i;
+    for(i=0;i<mLaneVector.size();i++)
+    {
+        if(mLaneVector.at(i).GetId() == nlaneid)
+        {
+            pRtn = &mLaneVector.at(i);
+            break;
+        }
+    }
+    return pRtn;
+
+}
+
+//Examp i= 1 return lane id = -1 lane
+Lane * LaneSection::GetRightLaneAt(unsigned int index)
+{
+    int nlaneid = index;
+    nlaneid = nlaneid *(-1);
+    Lane * pRtn = NULL;
+
+    unsigned int i;
+    for(i=0;i<mLaneVector.size();i++)
+    {
+        if(mLaneVector.at(i).GetId() == nlaneid)
+        {
+            pRtn = &mLaneVector.at(i);
+            break;
+        }
+    }
+    return pRtn;
+}
+
 /**
 * Destructor. Delete all the members of the vectors: mLeft, mCenter, mRight
 */

src/common/common/xodr/OpenDrive/Lane.h

@@ -182,6 +182,9 @@ public:
 
     void SetSingleSide(string singleSide);
     string GetSingleSide();
+
+    Lane * GetLeftLaneAt(unsigned int index);
+    Lane * GetRightLaneAt(unsigned int index);
 };
 
 

src/common/common/xodr/OpenDrive/RoadGeometry.cpp

@@ -718,7 +718,7 @@ void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double
 {
     if(mbHaveSample &&(mvectorgeosample.size() > 1))
     {
-        double fpos = s_check/0.1;
+        double fpos = (s_check - mS)/0.1;
         unsigned int ipos = fpos;
         double temX,temY,temHDG;
         if(ipos<=0)
@@ -845,13 +845,13 @@ void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, d
     }
     if(mbNormal)
     {
-        pRange = s_check/mLength;
+        pRange = (s_check - mS)/mLength;
         if(pRange<0)pRange = 0.0;
         if(pRange>1.0)pRange = 1.0;
     }
     else
     {
-        pRange = s_check;
+        pRange = (s_check -mS);
     }
     double xtem,ytem;
     xtem = muA + muB * pRange +  muC * pRange*pRange +  muD * pRange*pRange*pRange;
@@ -866,13 +866,13 @@ void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, d
     {
         if(mbNormal)
         {
-            pRange = (s_check-0.1)/mLength;
+            pRange = (s_check -mS-0.1)/mLength;
             if(pRange<0)pRange = 0.0;
             if(pRange>1.0)pRange = 1.0;
         }
         else
         {
-            pRange = s_check - 0.1;
+            pRange = s_check-mS - 0.1;
         }
         xtem = muA + muB * pRange + muC * pRange*pRange + muD * pRange*pRange*pRange;
         ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;

src/tool/map_lanetoxodr/autoroadcontact.cpp

@@ -1,14 +1,17 @@
 #include "autoroadcontact.h"
 
 #include <math.h>
+#include <iostream>
 
 AutoRoadContact::AutoRoadContact()
 {
 
 }
 
-int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,int & turnstraight)
+int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,int & turnstraight,
+                                 std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane)
 {
+
     double road1_start_x,road1_start_y,road1_end_x,road1_end_y,road1_start_hdg,road1_end_hdg;
     double road2_start_x,road2_start_y,road2_end_x,road2_end_y,road2_start_hdg,road2_end_hdg;
     pRoad1->GetGeometryCoords(0,road1_start_x,road1_start_y,road1_start_hdg);
@@ -32,7 +35,7 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
 
     double fdismin = 100000;
     int index = -1;
-    int i;
+    unsigned int i;
     for(i=0;i<4;i++)
     {
         if(fdismin>fdis[i])
@@ -49,6 +52,8 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         return -1;
     }
 
+    LaneSection * pLS1 = 0;
+    LaneSection * pLS2 = 0;
     double from_x,from_y,from_hdg,to_x,to_y,to_hdg;
     switch (contacttype) {
     case 0:
@@ -58,6 +63,8 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         to_y = road2_start_y;
         from_hdg = road1_start_hdg + M_PI;
         to_hdg = road2_start_hdg;
+        pLS1 = pRoad1->GetLaneSection(0);
+        pLS2 = pRoad2->GetLaneSection(0);
         break;
     case 1:
         from_x = road1_start_x;
@@ -66,6 +73,8 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         to_y = road2_end_y;
         from_hdg = road1_start_hdg + M_PI;
         to_hdg = road2_end_hdg+ M_PI;
+        pLS1 = pRoad1->GetLaneSection(0);
+        pLS2 = pRoad2->GetLastLaneSection();
         break;
     case 2:
         from_x = road1_end_x;
@@ -74,6 +83,8 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         to_y = road2_start_y;
         from_hdg = road1_end_hdg ;
         to_hdg = road2_start_hdg;
+        pLS1 = pRoad1->GetLastLaneSection();
+        pLS2 = pRoad1->GetLaneSection(0);
         break;
     case 3:
         from_x = road1_end_x;
@@ -82,6 +93,8 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         to_y = road2_end_y;
         from_hdg = road1_end_hdg ;
         to_hdg = road2_end_hdg + M_PI;
+        pLS1 = pRoad1->GetLastLaneSection();
+        pLS2 = pRoad1->GetLastLaneSection();
         break;
     default:
         break;
@@ -93,7 +106,8 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
     double hdgdiff = to_hdg - from_hdg;
     while(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
     while(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
-    if((hdgdiff<1.0)||(hdgdiff>(2.0*M_PI-1.0)))
+    bool bTurnRight = true;
+    if((hdgdiff<0.5)||(hdgdiff>(2.0*M_PI-0.5)))
     {
         turnstraight = 1;
     }
@@ -105,8 +119,241 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
         }
         else
         {
+            if(hdgdiff>M_PI)
+            {
+                std::cout<<"turn right"<<std::endl;
+                bTurnRight = true;
+            }
+            else
+            {
+                std::cout<<"turn left"<<std::endl;
+                bTurnRight = false;
+            }
             turnstraight = 0;
         }
     }
 
+
+    vector<Lane *> xroad1lane1,xroad1lane2;
+    vector<Lane *> xroad2lane1,xroad2lane2;
+    for(i=0;i<pLS1->GetLeftLaneCount();i++)
+    {
+        Lane * pLane = pLS1->GetLeftLaneAt(i+1);
+        if(pLane == NULL)
+        {
+            std::cout<<"GetLeftLaneError i:"<<i<<std::endl;
+            break;
+        }
+        xroad1lane1.push_back(pLane);
+    }
+    for(i=0;i<pLS1->GetRightLaneCount();i++)
+    {
+        Lane * pLane = pLS1->GetRightLaneAt(i+1);
+        if(pLane == NULL)
+        {
+            std::cout<<"GetRightLaneAt i:"<<i<<std::endl;
+            break;
+        }
+        xroad1lane2.push_back(pLane);
+    }
+    for(i=0;i<pLS2->GetLeftLaneCount();i++)
+    {
+        Lane * pLane = pLS2->GetLeftLaneAt(i+1);
+        if(pLane == NULL)
+        {
+            std::cout<<"GetLeftLaneError i:"<<i<<std::endl;
+            break;
+        }
+        xroad2lane1.push_back(pLane);
+    }
+    for(i=0;i<pLS2->GetRightLaneCount();i++)
+    {
+        Lane * pLane = pLS2->GetRightLaneAt(i+1);
+        if(pLane == NULL)
+        {
+            std::cout<<"GetRightLaneAt i:"<<i<<std::endl;
+            break;
+        }
+        xroad2lane2.push_back(pLane);
+    }
+
+    vector<Lane *> xroad1leftlane,xroad1rightlane;
+    vector<Lane *> xroad2leftlane,xroad2rightlane;
+
+    if((contacttype == 0)||(contacttype == 1))
+    {
+        xroad1leftlane = xroad1lane2;
+        xroad1rightlane = xroad1lane1;
+    }
+    else
+    {
+        xroad1leftlane = xroad1lane1;
+        xroad1rightlane = xroad1lane2;
+    }
+
+    if((contacttype == 1)||(contacttype == 3))
+    {
+        xroad2leftlane = xroad2lane2;
+        xroad2rightlane = xroad2lane1;
+    }
+    else
+    {
+        xroad2leftlane = xroad2lane1;
+        xroad2rightlane = xroad2lane2;
+    }
+
+    if(turnstraight == 1)  //
+    {
+        for(i=0;i<xroad1rightlane.size();i++)
+        {
+            Lane * pLane1 = xroad1rightlane.at(i);
+            Lane * pLane2 = xroad2rightlane.at(i);
+            if((pLane1 == NULL)||(pLane2 == NULL))
+            {
+                break;
+            }
+            if(pLane1->GetType() != pLane2->GetType())
+            {
+                break;
+            }
+            iv::ARC xARC;
+            xARC.from = pLane1->GetId();
+            xARC.to = pLane2->GetId();
+            xARCLane.push_back(xARC);
+        }
+        for(i=0;i<xroad1leftlane.size();i++)
+        {
+            Lane * pLane1 = xroad1leftlane.at(i);
+            Lane * pLane2 = xroad2leftlane.at(i);
+            if((pLane1 == NULL)||(pLane2 == NULL))
+            {
+                break;
+            }
+            if(pLane1->GetType() != pLane2->GetType())
+            {
+                break;
+            }
+            iv::ARC xARC;
+            xARC.from = pLane1->GetId();
+            xARC.to = pLane2->GetId();
+            xARCOpLane.push_back(xARC);
+        }
+    }
+    else
+    {
+        if(turnstraight == 0)
+        {
+            int nindex1 = -1;
+            int nindex2 = -1;
+            for(i=0;i<xroad1rightlane.size();i++)
+            {
+                if(xroad1rightlane.at(i)->GetType() == "driving")
+                {
+                    nindex1 = i;
+                }
+            }
+            for(i=0;i<xroad2rightlane.size();i++)
+            {
+                if(xroad2rightlane.at(i)->GetType() == "driving")
+                {
+                    nindex2 = i;
+                }
+            }
+            if((nindex1<0)||(nindex2<0))
+            {
+                std::cout<<"can't find driving lane"<<std::endl;
+            }
+            if(bTurnRight)
+            {
+                for(i=nindex1;i<xroad1rightlane.size();i++)
+                {
+                    Lane * pLane1 = xroad1rightlane.at(i);
+                    Lane * pLane2 = xroad2rightlane.at(nindex2);nindex2++;
+                    if((pLane1 == NULL)||(pLane2 == NULL))
+                    {
+                        break;
+                    }
+                    if(pLane1->GetType() != pLane2->GetType())
+                    {
+                        break;
+                    }
+                    iv::ARC xARC;
+                    xARC.from = pLane1->GetId();
+                    xARC.to = pLane2->GetId();
+                    xARCLane.push_back(xARC);
+                }
+
+            }
+            else
+            {
+                Lane * pLane1 = xroad1rightlane.at(nindex1);
+                Lane * pLane2 = xroad2rightlane.at(nindex2);
+                if((pLane1 == NULL)||(pLane2 == NULL))
+                {
+                    std::cout<<"Lane NULL"<<std::endl;
+                }
+                if(pLane1->GetType() != pLane2->GetType())
+                {
+                    std::cout<<"Type Not Rigth"<<std::endl;
+                }
+                iv::ARC xARC;
+                xARC.from = pLane1->GetId();
+                xARC.to = pLane2->GetId();
+                xARCLane.push_back(xARC);
+            }
+        }
+        else
+        {
+            int nindex1 = -1;
+            int nindex2 = -1;
+            for(i=0;i<xroad1rightlane.size();i++)
+            {
+                if(xroad1rightlane.at(i)->GetType() == "driving")
+                {
+                    nindex1 = i;
+                    break;
+                }
+            }
+            for(i=0;i<xroad2rightlane.size();i++)
+            {
+                if(xroad2rightlane.at(i)->GetType() == "driving")
+                {
+                    nindex2 = i;
+                    break;
+                }
+            }
+            if((nindex1<0)||(nindex2<0))
+            {
+                std::cout<<"can't find driving lane"<<std::endl;
+            }
+            Lane * pLane1 = xroad1rightlane.at(nindex1);
+            Lane * pLane2 = xroad2rightlane.at(nindex2);
+            if((pLane1 == NULL)||(pLane2 == NULL))
+            {
+                std::cout<<"Lane NULL"<<std::endl;
+            }
+            if(pLane1->GetType() != pLane2->GetType())
+            {
+                std::cout<<"Type Not Rigth"<<std::endl;
+            }
+            iv::ARC xARC;
+            xARC.from = pLane1->GetId();
+            xARC.to = pLane2->GetId();
+            xARCLane.push_back(xARC);
+        }
+    }
+
+    std::cout<<"Lane Contact"<<std::endl;
+    for(i=0;i<xARCLane.size();i++)
+    {
+        std::cout<<" From "<<xARCLane.at(i).from<<" To"<<xARCLane.at(i).to<<std::endl;
+    }
+    std::cout<<"OpLane Contact"<<std::endl;
+    for(i=0;i<xARCOpLane.size();i++)
+    {
+        std::cout<<" From "<<xARCOpLane.at(i).from<<" To"<<xARCOpLane.at(i).to<<std::endl;
+    }
+
+    return 0;
+
 }

src/tool/map_lanetoxodr/autoroadcontact.h

@@ -3,12 +3,22 @@
 
 #include "OpenDrive/OpenDrive.h"
 
+
+namespace  iv {
+struct ARC
+{
+    int from;
+    int to;
+};
+}
+
 class AutoRoadContact
 {
 public:
     AutoRoadContact();
 
-    static int CalcContact(Road * pRoad1,Road * pRoad2,int & contacttype,int & turnstraight);
+    static int CalcContact(Road * pRoad1,Road * pRoad2,int & contacttype,int & turnstraight,
+                           std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane);
 };
 
 #endif // AUTOROADCONTACT_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -1075,6 +1075,7 @@ void MainWindow::CreateTab1View(QTabWidget * p)
     pPB->setText("Clear");
     pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
     nXPos = nXPos + nSpace;
+    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickClearLaneContact()));
 
     pPB = new QPushButton(pGroup);
     pPB->setText("Create Road");
@@ -3027,7 +3028,8 @@ void MainWindow::onClickAutoRoadContact()
 
     int contactype;
     int turnstraight;
-    AutoRoadContact::CalcContact(p1,p2,contactype,turnstraight);
+    std::vector<iv::ARC> xARCLane,xARCOpLane;
+    AutoRoadContact::CalcContact(p1,p2,contactype,turnstraight,xARCLane,xARCOpLane);
 
     switch (contactype) {
     case 0:
@@ -3111,6 +3113,32 @@ void MainWindow::onClickAutoRoadContact()
 
     mpCBRoadType->setCurrentIndex(turnstraight);
 
+    unsigned int i;
+    for(i=0;i<xARCLane.size();i++)
+    {
+        iv::lanecontact xlc;
+        xlc.ml1 = xARCLane.at(i).from;
+        xlc.ml2 = xARCLane.at(i).to;
+        if(mvectorrc.size()  < 1)break;
+        mvectorrc[0].mvectorlc.push_back(xlc);
+
+        char strname[256];
+        snprintf(strname,255,"%dto%d",xlc.ml1,xlc.ml2);
+        mpCBLane1Lane2->addItem(strname);
+    }
+    for(i=0;i<xARCOpLane.size();i++)
+    {
+        iv::lanecontact xlc;
+        xlc.ml1 = xARCOpLane.at(i).from;
+        xlc.ml2 = xARCOpLane.at(i).to;
+        if(mvectorrc.size()  < 1)break;
+        mvectorrc[0].mvectorlcop.push_back(xlc);
+
+        char strname[256];
+        snprintf(strname,255,"%dto%d",xlc.ml1,xlc.ml2);
+        mpCBLane1Lane2op->addItem(strname);
+    }
+
 }
 
 void MainWindow::onClickRoadContact()
@@ -3225,6 +3253,16 @@ void MainWindow::onClickOpLaneContact()
     mpCBLane1Lane2op->addItem(strname);
 }
 
+void MainWindow::onClickClearLaneContact()
+{
+    mpCBLane1Lane2op->clear();
+    mpCBLane1Lane2->clear();
+    if(mvectorrc.size()<1)return;
+    mvectorrc[0].mvectorlc.clear();
+    mvectorrc[0].mvectorlcop.clear();
+
+}
+
 void MainWindow::onClickCreateRoad()
 {
     if(mvectorrc.size()<1)return;

src/tool/map_lanetoxodr/mainwindow.h

@@ -145,6 +145,7 @@ private slots:
     void onClickSave();
     void onClickLaneContact();
     void onClickOpLaneContact();
+    void onClickClearLaneContact();
     void onClickCreateRoad();
     void onChangeRoadType(int index);