change tool/map_lanetoxodr. complete auto make all road contact.

src/tool/map_lanetoxodr/autoconnect.cpp

@@ -304,6 +304,8 @@ static std::vector<iv::roadpoint> getstartpoint(Road * p1)
 
 int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
 {
+    proad->GetGeometryCoords(proad->GetRoadLength(),x,y,hdg);
+    return 0;
     GeometryBlock * pblock = proad->GetLastGeometryBlock();
 
     RoadGeometry * pgeo = pblock->GetLastGeometry();
@@ -346,8 +348,18 @@ int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
         double xtem1,ytem1,x1,y1;
         GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
         double s = ppoly3->GetLength();
-        xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
-        ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
+        if(ppoly3->GetNormal())
+        {
+            xtem = ppoly3->GetuA() + ppoly3->GetuB()  + ppoly3->GetuC()   + ppoly3->GetuD()  ;
+            ytem = ppoly3->GetvA() + ppoly3->GetvB() + ppoly3->GetvC()   + ppoly3->GetvD() ;
+        }
+        else
+        {
+            xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
+            ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
+        }
+//        xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
+//        ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
         x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
         y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
         s = ppoly3->GetLength()*0.999;
@@ -355,12 +367,23 @@ int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
         double fr = 1.0;
         if(s>0)
         {
+            double frel = 0.99;
+            if(ppoly3->GetNormal() == false)
+            {
+                xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
+                ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
+            }
+            else
+            {
+                xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * frel + ppoly3->GetuC() *frel*frel   + ppoly3->GetuD()*frel*frel*frel  ;
+                ytem1 = ppoly3->GetvA() + ppoly3->GetvB()*frel + ppoly3->GetvC()*frel*frel   + ppoly3->GetvD()*frel*frel*frel ;
+            }
             fr = 1.0;
-            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * fr  + ppoly3->GetuC() * fr*fr  + ppoly3->GetuD() * fr*fr*fr ;
-            ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * fr + ppoly3->GetvC() * fr*fr  + ppoly3->GetvD() * fr*fr*fr ;
-            x1 = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
-            y1 = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
-            hdg = geofit::CalcHdg(xtem1,ytem1,x1,y1);
+//            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * fr  + ppoly3->GetuC() * fr*fr  + ppoly3->GetuD() * fr*fr*fr ;
+//            ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * fr + ppoly3->GetvC() * fr*fr  + ppoly3->GetvD() * fr*fr*fr ;
+            x1 = xtem1*cos(ppoly3->GetHdg()) - ytem1 * sin(ppoly3->GetHdg()) + ppoly3->GetX();
+            y1 = xtem1*sin(ppoly3->GetHdg()) + ytem1 * cos(ppoly3->GetHdg()) + ppoly3->GetY();
+            hdg = geofit::CalcHdg(x1,y1,x,y);
         }
         else
         {

src/tool/map_lanetoxodr/autoroadcontact.cpp

@@ -1,8 +1,12 @@
 #include "autoroadcontact.h"
 
+#include "mainwindow.h"
+
 #include <math.h>
 #include <iostream>
 
+extern MainWindow * gw;
+
 #include "xodrfunc.h"
 #include "xodrdijkstra.h"
 
@@ -308,6 +312,7 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
             if((nindex1<0)||(nindex2<0))
             {
                 std::cout<<"can't find driving lane"<<std::endl;
+                return -3;
             }
             if(bTurnRight)
             {
@@ -375,6 +380,7 @@ int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,in
             if((nindex1<0)||(nindex2<0))
             {
                 std::cout<<"can't find driving lane"<<std::endl;
+                return -3;
             }
             Lane * pLane1 = xroad1rightlane.at(nindex1);
             Lane * pLane2 = xroad2rightlane.at(nindex2);
@@ -656,7 +662,7 @@ int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance
     }
 
     int RCUSize = xvectorRCU.size();
-    for(i=0;i<RCUSize;i++)
+    for(i=0;i<xvectorRCU.size();i++)
     {
         int nlr1 = 0;
         int nlr2 = 0;
@@ -700,11 +706,635 @@ int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance
                 std::cout<<std::endl;
                 xvectorRCU.erase(xvectorRCU.begin()+i);
                 i = i-1;
+
+                    if(xvp.size() == 3)
+                    {
+                        int nroadid = xvp[1].mroadid;
+                        std::cout<<" found junction road . id: "<<nroadid<<std::endl;
+                        int k = 0;
+                        bool bdel = false;
+                        for(k=0;k<xvectorRCU.size();k++)
+                        {
+                            if((atoi(xvectorRCU[k].mpRoad1->GetRoadId().data()) == nroadid)||(atoi(xvectorRCU[k].mpRoad2->GetRoadId().data()) == nroadid))
+                            {
+                               xvectorRCU.erase(xvectorRCU.begin() + k);
+                               k = k-1;
+                               bdel = true;
+                            }
+                        }
+                        if(bdel)
+                        {
+                            i = -1;
+                        }
+
+                    }
+
             }
         }
 
     }
 
+    std::cout<<" RCU Size is "<<xvectorRCU.size()<<std::endl;
+
+//    for(i=0;i<xvectorRCU.size();i++)
+    std::vector<std::vector<iv::roadcontact>> xvectorrcs;
+    for(i=0;i<xvectorRCU.size();i++)
+    {
+        std::vector<iv::roadcontact> xvectorrc;
+        xvectorrc.clear();
+        iv::roadcontact rc;
+        if((xvectorRCU[i].mcontactype == 0) ||(xvectorRCU[i].mcontactype == 1))
+        {
+            rc.mncon1 = 0;
+        }
+        else
+        {
+            rc.mncon1 = 1;
+        }
+        if((xvectorRCU[i].mcontactype == 0) ||(xvectorRCU[i].mcontactype == 2))
+        {
+            rc.mncon2 = 0;
+        }
+        else
+        {
+            rc.mncon2 = 1;
+        }
+        rc.mnroad1id = atoi(xvectorRCU[i].mpRoad1->GetRoadId().data());
+        rc.mnroad2id = atoi(xvectorRCU[i].mpRoad2->GetRoadId().data());
+        unsigned int j;
+        for(j=0;j<xvectorRCU[i].mARCLane.size();j++)
+        {
+             iv::lanecontact lt;
+             lt.ml1 = xvectorRCU[i].mARCLane[j].from;
+             lt.ml2 = xvectorRCU[i].mARCLane[j].to;
+             rc.mvectorlc.push_back(lt);
+        }
+        for(j=0;j<xvectorRCU[i].mARCOpLane.size();j++)
+        {
+             iv::lanecontact lt;
+             lt.ml1 = xvectorRCU[i].mARCOpLane[j].from;
+             lt.ml2 = xvectorRCU[i].mARCOpLane[j].to;
+             rc.mvectorlcop.push_back(lt);
+        }
+        xvectorrc.push_back(rc);
+        xvectorrcs.push_back(xvectorrc);
+//        CreateContactRoad(xvectorrc,pxodr,xvectorRCU[i].mturnstraight);
+    }
+
+    for(i=0;i<xvectorrcs.size();i++)
+    {
+        CreateContactRoad(xvectorrcs[i],pxodr,xvectorRCU[i].mturnstraight);
+    }
+
     return 0;
 }
 
+void AutoRoadContact::CreateContactRoad(std::vector<iv::roadcontact> & xvectorrc,OpenDrive * pxodr,int nConType)
+{
+    if(xvectorrc.size()<1)return;
+
+    Road * p1, *p2;
+    int nroad1index;
+    int nroad2index;
+
+    int i;
+    bool bhavep1 = false;
+    bool bhavep2 = false;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        if(xvectorrc[0].mnroad1id == atoi(pxodr->GetRoad(i)->GetRoadId().data()))
+        {
+            bhavep1 = true;
+            p1 = pxodr->GetRoad(i);
+            nroad1index = i;
+            break;
+        }
+    }
+
+    if(bhavep1 == false)
+    {
+        std::cout<<"Road not found"<<std::endl;
+        return;
+    }
+
+    double off1,off2;
+
+
+
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        if(xvectorrc[0].mnroad2id == atoi(pxodr->GetRoad(i)->GetRoadId().data()))
+        {
+            bhavep2 = true;
+            p2 = pxodr->GetRoad(i);
+            nroad2index = i;
+            break;
+        }
+    }
+
+    if(bhavep2 == false)
+    {
+        std::cout<<"Road not found"<<std::endl;
+        return;
+    }
+
+    if(xvectorrc[0].mvectorlc.size()<1)
+    {
+        std::cout<<"No Lane Contact."<<std::endl;;
+        return;
+    }
+
+    double startx,starty,starthdg;
+    double endx,endy,endhdg;
+
+    double startheight,endheight;
+
+    bool bFromstart,bTostart;
+    if(xvectorrc[0].mncon1 == 0)
+    {
+        bFromstart = true;
+        starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
+        off1 = MainWindow::getoff(p1,xvectorrc[0].mvectorlc[0].ml1,true);
+        startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
+        starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
+
+        if(p1->GetLaneOffsetCount()>0)
+        {
+            off1 = off1 - p1->GetLaneOffset(0)->Geta();
+        }
+        startx = startx + off1 * cos(starthdg -M_PI/2.0);
+        starty = starty + off1 * sin(starthdg -M_PI/2.0);
+
+        startheight = 0;
+        if(p1->GetElevationCount()>0)
+        {
+            startheight = p1->GetElevation(0)->GetA();
+        }
+
+ //       if(mvectorrc[0].mvectorlc[0].ml1<0)
+            starthdg = starthdg +M_PI;if(starthdg >=2.0*M_PI)starthdg = starthdg -2.0*M_PI;
+    }
+    else
+    {
+        bFromstart = false;
+        if(GetEndPoint(p1,startx,starty,starthdg) != 0)
+        {
+            std::cout<<"Get End Point Error."<<std::endl;
+            return;
+        }
+        off1 = MainWindow::getoff(p1,xvectorrc[0].mvectorlc[0].ml1,false);
+        if(p1->GetLaneOffsetCount()>0)
+        {
+            LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);
+            double froadlen = p1->GetRoadLength();
+            double sdis = froadlen - pLO->GetS();
+            double foffset = pLO->Geta() + pLO->Getb()*(sdis) + pLO->Getc() * sdis * sdis
+                    +pLO->Getd() * sdis * sdis * sdis;
+            off1 = off1 - foffset;
+        }
+        startx = startx + off1 * cos(starthdg -M_PI/2.0);
+        starty = starty + off1 * sin(starthdg -M_PI/2.0);
+
+
+
+        startheight = 0;
+        if(p1->GetElevationCount()>0)
+        {
+            startheight = p1->GetElevation(0)->GetA()
+                    +p1->GetElevation(0)->GetB() * pow(p1->GetRoadLength(),1)
+                    +p1->GetElevation(0)->GetC() * pow(p1->GetRoadLength(),2)
+                    +p1->GetElevation(0)->GetD() * pow(p1->GetRoadLength(),3);
+        }
+    }
+
+    if(xvectorrc[0].mncon2 == 0)
+    {
+        bTostart = true;
+        off2 = MainWindow::getoff(p2,xvectorrc[0].mvectorlc[0].ml2,true);
+        endx = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
+        endy = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
+        endhdg = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
+
+        if(p2->GetLaneOffsetCount()>0)
+        {
+            off2 = off2 - p2->GetLaneOffset(0)->Geta();
+        }
+        endx = endx + off2 * cos(endhdg -M_PI/2.0);
+        endy = endy + off2 * sin(endhdg -M_PI/2.0);
+
+        endheight = 0;
+        if(p2->GetElevationCount()>0)
+        {
+            endheight = p2->GetElevation(0)->GetA();
+        }
+    }
+    else
+    {
+        bTostart = false;
+        off2 = MainWindow::getoff(p2,xvectorrc[0].mvectorlc[0].ml2,false);
+        if(GetEndPoint(p2,endx,endy,endhdg) != 0)
+        {
+            std::cout<<"get end error."<<std::endl;
+            return;
+        }
+        if(p2->GetLaneOffsetCount()>0)
+        {
+            LaneOffset * pLO = p2->GetLaneOffset(p2->GetLaneOffsetCount()-1);
+            double froadlen = p2->GetRoadLength();
+            double sdis = froadlen - pLO->GetS();
+            double foffset = pLO->Geta() + pLO->Getb()*(sdis) + pLO->Getc() * sdis * sdis
+                    +pLO->Getd() * sdis * sdis * sdis;
+            off2 = off2 - foffset;
+        }
+        endx = endx + off2 * cos(endhdg -M_PI/2.0);
+        endy = endy + off2 * sin(endhdg -M_PI/2.0);
+        endhdg = endhdg +M_PI;if(endhdg >=2.0*M_PI)endhdg = endhdg -2.0*M_PI;
+
+        endheight = 0;
+        if(p2->GetElevationCount()>0)
+        {
+            endheight = p2->GetElevation(0)->GetA()
+                    +p2->GetElevation(0)->GetB() * pow(p2->GetRoadLength(),1)
+                    +p2->GetElevation(0)->GetC() * pow(p2->GetRoadLength(),2)
+                    +p2->GetElevation(0)->GetD() * pow(p2->GetRoadLength(),3);
+        }
+    }
+
+
+    //Create Geo
+    double R = 6.0;
+    std::vector<geobase> xvectorgeo;
+    std::vector<geobase> xvectorgeo1,xvectorgeo2;
+    switch(nConType)
+    {
+    case 0:
+        xvectorgeo = geofit::CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
+        break;
+    case 1:
+        xvectorgeo = geofit::CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
+        break;
+    case 2:
+        xvectorgeo = geofit::CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
+        break;
+    default:
+        break;
+    }
+
+    if(nConType == 2)
+    {
+        for(i=0;i<xvectorgeo.size()/2;i++)
+        {
+            xvectorgeo1.push_back(xvectorgeo.at(i));
+        }
+        for(i=xvectorgeo.size()/2;i<xvectorgeo.size();i++)
+        {
+            xvectorgeo2.push_back(xvectorgeo.at(i));
+        }
+    }
+
+    if(xvectorgeo.size() == 0)
+    {
+        std::cout<<"Waring Create Road Fail."<<std::endl;
+        return;
+    }
+
+    double xroadlen = 0;
+
+    if(nConType != 2)
+    {
+        for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
+
+        pxodr->AddRoad("",xroadlen, QString::number(gw->CreateRoadID()).toStdString(),"-1");
+        Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
+
+        p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);
+
+        p1 = pxodr->GetRoad(nroad1index);
+        p2 = pxodr->GetRoad(nroad2index);
+
+        double s = 0;
+        int j;
+        j= 0;
+        for(j=0;j<xvectorgeo.size();j++)
+        {
+            p->AddGeometryBlock();
+            GeometryBlock * pgb = p->GetGeometryBlock(j);
+
+            geobase * pline;
+            geobase * pbez;
+            geobase * parc;
+            switch(xvectorgeo[j].mnType)
+            {
+            case 0:
+                pline = &xvectorgeo[j];
+                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
+                break;
+            case 1:
+                parc = &xvectorgeo[j];
+                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+                break;
+            case 2:
+                pbez = &xvectorgeo[j];
+                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
+                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
+                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
+                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
+                break;
+            }
+            s = s + xvectorgeo[j].mfLen;
+        }
+
+        p->AddLaneSection(0);
+        LaneSection * pLS = p->GetLaneSection(0);
+        pLS->SetS(0);
+        pLS->AddLane(0,0,"none",false);
+
+        double * pswidth,*pewidth;
+        std::vector<std::string> strvectorlanetype;
+        int nlanecount = xvectorrc[0].mvectorlc.size();
+        pswidth = new double[nlanecount];
+        pewidth = new double[nlanecount];
+        std::shared_ptr<double> ppswidth,ppewidth;
+        ppswidth.reset(pswidth);
+        ppewidth.reset(pewidth);
+
+        for(i=0;i<nlanecount;i++)
+        {
+            pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
+            strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
+
+        }
+        for(i=0;i<nlanecount;i++)
+        {
+            pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlc.at(i).ml2,bTostart);
+        }
+
+        double * pa,*pb;
+        pa = new double[nlanecount];
+        pb = new double[nlanecount];
+        std::shared_ptr<double> ppa,ppb;
+        ppa.reset(pa);
+        ppb.reset(pb);
+
+        for(i=0;i<nlanecount;i++)
+        {
+            pa[i] = pswidth[i];
+            pb[i] = (pewidth[i] - pa[i])/xroadlen;
+        }
+
+        for(i=0;i<nlanecount;i++)
+        {
+            pLS->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
+            Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
+
+            pLL->AddWidthRecord(0,pa[i],pb[i],
+                                0,0);
+            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+
+        }
+
+        int noplanecount = xvectorrc[0].mvectorlcop.size();
+        if(noplanecount > 0)
+        {
+            pswidth = new double[noplanecount];
+            pewidth = new double[noplanecount];
+            ppswidth.reset(pswidth);
+            ppewidth.reset(pewidth);
+
+            strvectorlanetype.clear();
+            for(i=0;i<noplanecount;i++)
+            {
+                pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
+                strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
+            }
+            for(i=0;i<noplanecount;i++)
+            {
+                pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
+            }
+
+            pa = new double[noplanecount];
+            pb = new double[noplanecount];
+            ppa.reset(pa);
+            ppb.reset(pb);
+
+            for(i=0;i<noplanecount;i++)
+            {
+                pa[i] = pswidth[i];
+                pb[i] = (pewidth[i] - pa[i])/xroadlen;
+            }
+
+            for(i=0;i<noplanecount;i++)
+            {
+                pLS->AddLane(1,(i+1),strvectorlanetype[i],false,false);
+                Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
+
+                pLL->AddWidthRecord(0,pa[i],pb[i],
+                                    0,0);
+                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+
+            }
+        }
+
+    }
+    else
+    {
+        double xroadlen1 = 0;
+        double xroadlen2 = 0;
+        for(i=0;i<xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
+        for(i=0;i<xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
+
+        int index1 = pxodr->AddRoad("",xroadlen1, QString::number(gw->CreateRoadID()).toStdString(),"-1");
+        int index2 = pxodr->AddRoad("",xroadlen2, QString::number(gw->CreateRoadID()).toStdString(),"-1");
+        Road * proad2 = pxodr->GetRoad(index2);
+        Road * proad1 = pxodr->GetRoad(index1);
+
+        proad1->AddElevation(0,startheight,(endheight-startheight)/(xroadlen1+xroadlen2),0,0);
+        proad2->AddElevation(0,startheight+xroadlen1*(endheight-startheight)/(xroadlen1+xroadlen2),
+                             (endheight-startheight)/(xroadlen1+xroadlen2),
+                             0,0);
+
+        p1 = pxodr->GetRoad(nroad1index);
+        p2 = pxodr->GetRoad(nroad2index);
+
+//        OpenDrive * px = &mxodr;
+        double s = 0;
+        int j;
+        j= 0;
+        for(j=0;j<xvectorgeo1.size();j++)
+        {
+            proad1->AddGeometryBlock();
+            GeometryBlock * pgb = proad1->GetGeometryBlock(j);
+
+            geobase * pline;
+            geobase * pbez;
+            geobase * parc;
+            switch(xvectorgeo1[j].mnType)
+            {
+            case 0:
+                pline = &xvectorgeo1[j];
+                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
+                break;
+            case 1:
+                parc = &xvectorgeo1[j];
+                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+                break;
+            case 2:
+                pbez = &xvectorgeo1[j];
+                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
+                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
+                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
+                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
+                break;
+            }
+            s = s + xvectorgeo1[j].mfLen;
+        }
+
+        s=0.0;
+        for(j=0;j<xvectorgeo2.size();j++)
+        {
+            proad2->AddGeometryBlock();
+            GeometryBlock * pgb = proad2->GetGeometryBlock(j);
+
+            geobase * pline;
+            geobase * pbez;
+            geobase * parc;
+            switch(xvectorgeo2[j].mnType)
+            {
+            case 0:
+                pline = &xvectorgeo2[j];
+                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
+                break;
+            case 1:
+                parc = &xvectorgeo2[j];
+                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+                break;
+            case 2:
+                pbez = &xvectorgeo2[j];
+                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
+                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
+                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
+                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
+                break;
+            }
+            s = s + xvectorgeo2[j].mfLen;
+        }
+
+        proad1->AddLaneSection(0);
+        LaneSection * pLS1 = proad1->GetLaneSection(0);
+        pLS1->SetS(0);
+        pLS1->AddLane(0,0,"none",false);
+
+        proad2->AddLaneSection(0);
+        LaneSection * pLS2 = proad2->GetLaneSection(0);
+        pLS2->SetS(0);
+        pLS2->AddLane(0,0,"none",false);
+
+        double * pswidth,*pewidth;
+        int nlanecount = xvectorrc[0].mvectorlc.size();
+        std::vector<std::string> strvectorlanetype;
+        pswidth = new double[nlanecount];
+        pewidth = new double[nlanecount];
+        std::shared_ptr<double> ppswidth,ppewidth;
+        ppswidth.reset(pswidth);
+        ppewidth.reset(pewidth);
+
+        for(i=0;i<nlanecount;i++)
+        {
+            pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
+            strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
+
+        }
+        for(i=0;i<nlanecount;i++)
+        {
+            pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlc.at(i).ml2,bTostart);
+        }
+
+        double * pa,*pb;
+        pa = new double[nlanecount];
+        pb = new double[nlanecount];
+        std::shared_ptr<double> ppa,ppb;
+        ppa.reset(pa);
+        ppb.reset(pb);
+
+        for(i=0;i<nlanecount;i++)
+        {
+            pa[i] = pswidth[i];
+            pb[i] = (pewidth[i] - pa[i])/(xroadlen1+xroadlen2);
+        }
+
+        for(i=0;i<nlanecount;i++)
+        {
+            pLS1->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
+            Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);
+
+            pLL->AddWidthRecord(0,pa[i],pb[i],
+                                0,0);
+            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+
+            pLS2->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
+            pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);
+
+            pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
+                                0,0);
+            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+
+        }
+        int noplanecount = xvectorrc[0].mvectorlcop.size();
+        if(noplanecount > 0)
+        {
+            pswidth = new double[noplanecount];
+            pewidth = new double[noplanecount];
+            ppswidth.reset(pswidth);
+            ppewidth.reset(pewidth);
+
+            strvectorlanetype.clear();
+            for(i=0;i<noplanecount;i++)
+            {
+                pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
+                strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
+            }
+            for(i=0;i<noplanecount;i++)
+            {
+                pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
+            }
+
+            pa = new double[noplanecount];
+            pb = new double[noplanecount];
+            ppa.reset(pa);
+            ppb.reset(pb);
+
+            for(i=0;i<noplanecount;i++)
+            {
+                pa[i] = pswidth[i];
+                pb[i] = (pewidth[i] - pa[i])/xroadlen;
+            }
+
+            for(i=0;i<noplanecount;i++)
+            {
+                pLS1->AddLane(1,(i+1),strvectorlanetype[i],false,false);
+                Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);
+
+                pLL->AddWidthRecord(0,pa[i],pb[i],
+                                    0,0);
+                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+
+                pLS2->AddLane(1,(i+1),strvectorlanetype[i],false,false);
+                pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);
+
+                pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
+                                    0,0);
+                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
+
+            }
+        }
+
+
+    }
+
+
+}
+
+

src/tool/map_lanetoxodr/autoroadcontact.h

@@ -3,6 +3,9 @@
 
 #include "OpenDrive/OpenDrive.h"
 
+#include "rawtype.h"
+#include "geofit.h"
+
 
 namespace  iv {
 struct ARC
@@ -42,6 +45,9 @@ 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);
+
+    static void CreateContactRoad(std::vector<iv::roadcontact> & xvectorrc,OpenDrive * pxodr,int nConType);
+
 };
 
 #endif // AUTOROADCONTACT_H

src/tool/map_lanetoxodr/geofit.cpp

@@ -947,4 +947,295 @@ geofit & geofit::Inst()
     return x;
 }
 
+std::vector<geobase> geofit::CreateLineGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg)
+{
+
+//    std::vector<geobase> xvectorgeo;
+//    geobase xgeobezier;
+//    int nbtype;
+//    double fabc[3],fxy[2],fblen,fbhdg;
+//    geofit x;
+//    x.CreateBezier(startx,starty,starthdg,
+//                 endx,endy,endhdg,
+//                 0.35,0.35,xgeobezier.mfu,xgeobezier.mfv,xgeobezier.mfLen,
+//                 nbtype,fabc,&fbhdg,fxy,&fblen);
+//    if(nbtype == 2)
+//    {
+//    xgeobezier.mfHdg = starthdg;
+//    xgeobezier.mfX = startx;
+//    xgeobezier.mfY = starty;
+//    xgeobezier.mnType = 2;
+//    }
+//    else
+//    {
+//        xgeobezier.mnType = 0; //Line
+//        xgeobezier.mfHdgStart = fbhdg;
+//        xgeobezier.mfHdg = fbhdg;
+//        xgeobezier.mfX = fxy[0];
+//        xgeobezier.mfY = fxy[1];
+//        xgeobezier.mfLen = fblen;
+//    }
+//    xvectorgeo.push_back(xgeobezier);
+//    return xvectorgeo;
+    geobase linegeo;
+    linegeo.mnType = 0;
+    linegeo.mfX = startx;
+    linegeo.mfY = starty;
+    linegeo.mfHdg = geofit::CalcHdg(startx,starty,endx,endy);
+    linegeo.mfLen = sqrt(pow(endx - startx,2)+pow(endy - starty,2));
+    std::vector<geobase> xvectorgeo;
+    xvectorgeo.push_back(linegeo);
+    return xvectorgeo;
+}
+
+std::vector<geobase> geofit::CreateTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg,double R)
+{
+    std::vector<geobase>  xvectorgeo;
+    xvectorgeo.clear();
+    if(starthdg == endhdg)
+    {
+        std::cout<<"hdg same use line contact"<<std::endl;
+        return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
+    }
+    double  a1,c1,a2,c2;
+    double inter_x,inter_y;
+    if((starthdg == M_PI/2.0)||(starthdg == 3.0*M_PI/2.0))
+    {
+        a2 = tan(endhdg) *(-1);
+        c2 = 0-a2*endx - endy;
+        a1 = 1;
+        c1 = startx *(-1);
+        inter_x = startx;
+        inter_y = 0 - a2*inter_x-c2;
+    }
+    else
+    {
+        if((endhdg == M_PI/2.0)||(endhdg == 3.0*M_PI/2.0))
+        {
+            a1 = tan(starthdg) *(-1);
+            c1 = 0-a1*startx - starty;
+            a2 = 1;
+            c2 = endx *(-1);
+            inter_x = endx;
+            inter_y = 0 - a1*inter_x-c1;
+        }
+        else
+        {
+            a1 = tan(starthdg) *(-1);
+            a2 = tan(endhdg) *(-1);
+            c1 = 0-a1*startx - starty;
+            c2 = 0-a2*endx - endy;
+            inter_x = (c1-c2)/(a2-a1);
+            inter_y = 0 - a1*inter_x - c1;
+        }
+    }
+
+    double dis1,dis2;
+    dis1 =sqrt(pow(inter_x - startx,2)+pow(inter_y - starty,2));
+    dis2 =sqrt(pow(inter_x - endx,2)+pow(inter_y - endy,2));
+
+    if((dis1<1.0)||(dis2<1.0))
+    {
+        std::cout<<"use line connect."<<std::endl;
+        char strinfo[256];
+        snprintf(strinfo,256,"Use Line Connect");
+  //      QMessageBox::information(this,"Info",strinfo,QMessageBox::YesAll);
+        return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
+
+    }
+
+    if((R>=dis1 )||(R>=dis2))
+    {
+        R = dis1 - 0.3;
+        if(R>=dis2)
+        {
+            R = dis2 - 0.3;
+        }
+        char strinfo[256];
+        snprintf(strinfo,256,"Change Radius to %f",R);
+ //       QMessageBox::information(this,"Info",strinfo,QMessageBox::YesAll);
+    }
+
+    double hdgdiff = endhdg - starthdg;
+    if(hdgdiff >= M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
+    if(hdgdiff <= (-M_PI))hdgdiff = hdgdiff + 2.0*M_PI;
+
+
+    double slen = R*tan(fabs(hdgdiff/2.0));
+    if(slen <(R+0.3))
+    {
+        R = slen - 0.3;
+        std::cout<<"Change Radius to "<<R<<std::endl;
+
+        slen = R*tan(fabs(hdgdiff/2.0));
+    }
+
+    if((dis1<slen)||(dis2<slen))
+    {
+        std::cout<<"radius is big. use line."<<std::endl;
+        char strinfo[256];
+        snprintf(strinfo,256,"Use Line Connect");
+ //       QMessageBox::information(this,"Info",strinfo,QMessageBox::YesAll);
+        return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
+    }
+
+    double p1_x,p1_y,p2_x,p2_y;
+    p1_x = inter_x+ slen * cos(starthdg +M_PI);
+    p1_y = inter_y+ slen * sin(starthdg +M_PI);
+    p2_x = inter_x+slen*cos(endhdg);
+    p2_y = inter_y+slen*sin(endhdg);
+
+    geobase linegeo;
+    linegeo.mnType = 0;
+    linegeo.mfX = startx;
+    linegeo.mfY = starty;
+    linegeo.mfHdg = starthdg;
+    linegeo.mfLen = sqrt(pow(p1_x - startx,2)+pow(p1_y - starty,2));
+    xvectorgeo.push_back(linegeo);
+
+    geobase arcgeo;
+    arcgeo.mnType = 1;
+    arcgeo.mfX = p1_x;
+    arcgeo.mfY = p1_y;
+    arcgeo.mfHdg =  starthdg;
+    arcgeo.mfHdgStart = starthdg;
+    arcgeo.mfLen = R*fabs(hdgdiff);
+    arcgeo.mR =  R*(fabs(hdgdiff)/hdgdiff);
+    arcgeo.mfEndX = p2_x;
+    arcgeo.mfEndY = p2_y;
+    xvectorgeo.push_back(arcgeo);
+
+    linegeo.mnType = 0;
+    linegeo.mfX = p2_x;
+    linegeo.mfY = p2_y;
+    linegeo.mfHdg = endhdg;
+    linegeo.mfLen = sqrt(pow(p2_x - endx,2)+pow(p2_y - endy,2));
+    xvectorgeo.push_back(linegeo);
+
+    return xvectorgeo;
+
+
+
+}
+
+std::vector<geobase> geofit::CreateUTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg, double fextend)
+{
+    std::vector<geobase>  xvectorgeo;
+
+
+    double p1_x,p1_y, p2_x,p2_y,p1_hdg,p2_hdg;
+
+    p1_x = fextend*cos(starthdg) + startx;
+    p1_y = fextend*sin(starthdg) + starty;
+
+    p2_x = fextend*cos(endhdg + M_PI) + endx;
+    p2_y = fextend*sin(endhdg + M_PI) + endy;
+
+    p1_hdg = starthdg;
+    p2_hdg = endhdg;
+
+    if(starthdg == endhdg)
+    {
+        std::cout<<" hdg is same, can't create u turn."<<std::endl;
+        return xvectorgeo;
+    }
+
+    double hdgse = geofit::CalcHdg(p1_x,p1_y,p2_x,p2_y);
+    double hdgdiff = hdgse - p1_hdg;
+    if(hdgdiff < 0)hdgdiff = hdgdiff  + 2.0*M_PI;
+
+    bool bPA = true; //Positive
+    if(hdgdiff >=M_PI)bPA = false;
+
+    double xdiff;
+    if(bPA)xdiff = hdgdiff - M_PI/2.0;
+    else xdiff = hdgdiff - 3.0*M_PI/2.0;
+
+    double xdis = sqrt(pow(p1_x-p2_x,2)+pow(p1_y-p2_y,2));
+    double R = xdis/(2.0*cos(xdiff));
+
+    double x_center,y_center;
+
+    double xhdgtocenter;
+
+    if(bPA)
+    {
+        xhdgtocenter = p1_hdg + M_PI/2.0;
+        if(xhdgtocenter  >= 2.0*M_PI)xhdgtocenter = xhdgtocenter - M_PI*2.0;
+    }
+    else
+    {
+        xhdgtocenter = p1_hdg - M_PI/2.0;
+        if(xhdgtocenter < 0)xhdgtocenter = xhdgtocenter + 2.0*M_PI;
+    }
+
+    x_center = p1_x + R*cos(xhdgtocenter);
+    y_center = p1_y + R*sin(xhdgtocenter);
+
+    double xhdgcentertoarc;
+    if(bPA)
+    {
+        xhdgcentertoarc = hdgse - M_PI/2.0;
+        if(xhdgcentertoarc <0)xhdgcentertoarc = xhdgcentertoarc + 2.0*M_PI;
+    }
+    else
+    {
+        xhdgcentertoarc = hdgse + M_PI/2.0;
+        if(xhdgcentertoarc >= M_PI*2.0)xhdgcentertoarc = xhdgcentertoarc - M_PI*2.0;
+    }
+
+    double p3_x,p3_y,p3_hdg;
+    p3_hdg = hdgse;
+    p3_x = x_center + R* cos(xhdgcentertoarc);
+    p3_y = y_center + R* sin(xhdgcentertoarc);
+
+//    pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+    geobase c2,c3;
+    c2.mfX = p1_x;
+    c2.mfY = p1_y;
+    c2.mfHdgStart = p1_hdg;
+    c3.mfX = p3_x;
+    c3.mfY = p3_y;
+    c3.mfHdgStart = hdgse;
+    c2.mnType = 1;
+    c3.mnType = 1;
+    if(bPA)
+    {
+        c2.mfLen = hdgdiff * R;
+        c2.mR = R;
+        c3.mfLen = c2.mfLen;
+        c3.mR = R;
+    }
+    else
+    {
+        c2.mfLen = (2.0*M_PI - hdgdiff) * R;
+
+        c2.mR = R*(-1);
+
+        c3.mfLen = c2.mfLen;
+        c3.mR = R*(-1);
+    }
+    geobase l1,l2;
+    if(fextend != 0)
+    {
+        l1.mfX = startx;
+        l1.mfY = starty;
+        l1.mfHdg = starthdg;
+        l1.mfLen = fextend;
+        l1.mnType = 0;
+        l2.mfX = p2_x;
+        l2.mfY = p2_y;
+        l2.mfHdg = p2_hdg;
+        l2.mfLen = fextend;
+        l2.mnType = 0;
+        xvectorgeo.push_back(l1);
+    }
+    xvectorgeo.push_back(c2);
+    xvectorgeo.push_back(c3);
+    if(fextend != 0)xvectorgeo.push_back(l2);
+
+    return xvectorgeo;
+}
+
+
 

src/tool/map_lanetoxodr/geofit.h

@@ -74,6 +74,11 @@ public:
 
     static geofit & Inst();
 
+    static std::vector<geobase> CreateLineGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg);
+    static std::vector<geobase> CreateTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double R);
+    static std::vector<geobase> CreateUTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double fextend);
+
+
 };
 
 #endif // GEOFIT_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -5496,6 +5496,7 @@ void MainWindow::on_actionSplit_Road_triggered()
 void MainWindow::UpdateScene()
 {
 
+
     int i;
     int nsize = mvectorviewitem.size();
     for(i=0;i<nsize;i++)
@@ -5576,6 +5577,7 @@ void MainWindow::UpdateScene()
         }
     }
     mbRefresh = false;
+//    mpscene->addLine(0,0,1000,100);
 }
 
 
@@ -5666,4 +5668,16 @@ void MainWindow::on_actionMake_All_Road_Contact_triggered()
     on_actionAutoConnect_triggered();
 
     AutoRoadContact::MakeAllContact(&mxodr);
+
+    updateJunction();
+
+    updateCBRoad();
+    mbRefresh = true;
+    update();
+    if(mpCBViewMode->currentIndex() == 1)
+    {
+        UpdateScene();
+    }
+
+    mpfb->SetOpenDrive(mxodr);
 }

src/tool/map_lanetoxodr/mainwindow.h

@@ -364,6 +364,10 @@ private:
 public:
     int CreateRoadID(int ntype = 0); //Create Road ID for new road. default:road  1:not create by lane road.
 
+    static double getoff(Road * p,int nlane,bool bstart = true);
+    double getlanewidth(Road * p,int nlane,bool bstart = true);
+    std::string getlanetype(Road * p,int nlane,bool bstart = true);
+
 private:
     void UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
@@ -396,9 +400,8 @@ private:
     std::vector<geobase> CreateTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double R);
     std::vector<geobase> CreateUTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double fextend);
 
-    double getoff(Road * p,int nlane,bool bstart = true);
-    double getlanewidth(Road * p,int nlane,bool bstart = true);
-    std::string getlanetype(Road * p,int nlane,bool bstart = true);
+
+
 
     void ChangeXODRRoadID(OpenDrive * pxodr,int index,int newid);
     void ChangeXODRJunctionID(OpenDrive * pxodr,int index,int newid);

src/tool/map_lanetoxodr/rawtype.h

@@ -1,6 +1,8 @@
 #ifndef RAWTYPE_H
 #define RAWTYPE_H
 
+#include <vector>
+
 namespace iv {
 struct lanepoint
 {