push

src/tool/map_collectfromveh/collectconvert.cpp

@@ -485,5 +485,303 @@ void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *p
     p1->GetGeometryCoords(p1->GetRoadLength() - 0.001,startx,starty,starthdg);
     p2->GetGeometryCoords(0,endx,endy,endhdg);
 
+    //Create Geo
+    double R = 6.0;
+    std::vector<geobase> xvectorgeo;
+    std::vector<geobase> xvectorgeo1,xvectorgeo2;
+    switch(pconnect->mmode())
+    {
+    case iv::map::roadconnect_ConnectMode_TURN:
+        xvectorgeo = geofit::CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
+        break;
+    case iv::map::roadconnect_ConnectMode_STRAIGHT:
+        {
+            double fdis = sqrt(pow(startx - endx,2) +pow(starty -endy,2));
+            if((fdis<3) || (starthdg == endhdg) )
+            {
+                xvectorgeo = geofit::CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
+            }
+            else
+            {
+                xvectorgeo = geofit::CreateBezierGeo(startx,starty,starthdg,endx,endy,endhdg);
+            }
+//
+        }
+        break;
+    case iv::map::roadconnect_ConnectMode_UTURN:
+        xvectorgeo = geofit::CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
+        break;
+    default:
+        break;
+    }
+
+    if(pconnect->mmode() == iv::map::roadconnect_ConnectMode_UTURN)
+    {
+        for(i=0;i<(int)xvectorgeo.size()/2;i++)
+        {
+            xvectorgeo1.push_back(xvectorgeo.at(i));
+        }
+        for(i=(int)xvectorgeo.size()/2;i<(int)xvectorgeo.size();i++)
+        {
+            xvectorgeo2.push_back(xvectorgeo.at(i));
+        }
+    }
+
+    if(xvectorgeo.size() == 0)
+    {
+        std::cout<<" CollectConvert::convertconnect Create Road Fail."<<std::endl;
+//        QMessageBox::warning(this,"warn","Create Road Fail.");
+        return;
+    }
+
+    double xroadlen = 0;
+
+    if(pconnect->mmode() == iv::map::roadconnect_ConnectMode_UTURN)
+    {
+        for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
+
+        pxodr->AddRoad("",xroadlen, QString::number(groadid).toStdString(),"-1");
+        groadid++;
+        Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
+
+//        p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);
+
+
+        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];
+                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],false);
+                break;
+            }
+            s = s + xvectorgeo[j].mfLen;
+        }
+
+        p->AddLaneSection(0);
+        LaneSection * pLS = p->GetLaneSection(0);
+        pLS->SetS(0);
+        pLS->AddLane(0,0,"none",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(groadid).toStdString(),"-1");groadid++;
+        int index2 = mxodr.AddRoad("",xroadlen2, QString::number(groadid).toStdString(),"-1");groadid++;
+        Road * proad2 = mxodr.GetRoad(index2);
+        Road * proad1 = mxodr.GetRoad(index1);
+
+
+        p1 = mxodr.GetRoad(nroad1index);
+        p2 = mxodr.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],false);
+                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 = mvectorrc[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] =  getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
+            strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
+
+        }
+        for(i=0;i<nlanecount;i++)
+        {
+            pewidth[i] =  getlanewidth(p2,mvectorrc[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 = mvectorrc[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] =  getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
+                strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
+            }
+            for(i=0;i<noplanecount;i++)
+            {
+                pewidth[i] =  getlanewidth(p2,mvectorrc[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_collectfromveh/collectconvert.h

@@ -47,6 +47,8 @@ private:
 
     std::string LaneTypeToStr(iv::map::collectvehroadpoint_LaneType xType);
 
+    void convertconnect(iv::map::roadconnect * pconnect,OpenDrive * pxodr);
+
 
 };