change map_lanetoxodr, prepare optimize geometry, not complete.

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -1470,3 +1470,105 @@ Lane * xodrfunc::GetLaneByID(LaneSection *pLS, int nlane)
     }
     return 0;
 }
+
+void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
+{
+    int ngeobsize = static_cast<int>(pRoad->GetGeometryBlockCount());
+    int i;
+    bool boptimize = false;
+    for(i=1;i<(ngeobsize-1);i++)
+    {
+        RoadGeometry *  pgeo1 =  pRoad->GetGeometryBlock(i)->GetGeometryAt(0);
+        RoadGeometry *  pgeo2 =  pRoad->GetGeometryBlock(i+1)->GetGeometryAt(0);
+        RoadGeometry *  pgeo0 =  pRoad->GetGeometryBlock(i-1)->GetGeometryAt(0);
+
+        double x,y,hdgend;
+        pgeo0->GetCoords(pgeo0->GetS() + pgeo0->GetLength()- 0.001,x,y,hdgend);
+
+        std::cout<<"i: "<<i<<" front end hdg: "<<hdgend<<" now hdg:  "<<pgeo1->GetHdg()<<std::endl;
+
+        if((pgeo1 == NULL) || (pgeo2 == NULL) || (pgeo0 == NULL))
+        {
+            continue;
+        }
+
+        if((pgeo1->GetLength()<5)  && (pgeo1->GetLength() > 0.001) && ((pgeo1->GetGeomType() == 0) || (pgeo1->GetGeomType() == 4)))
+        {
+            double fhdgdiff = pgeo2->GetHdg() - pgeo1->GetHdg();
+            while(fhdgdiff>M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+            while(fhdgdiff<(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI;
+            if(fabs(fhdgdiff)/pgeo1->GetLength() > 0.05)
+            {
+                qDebug("need optimize, index: %d ",i);
+                //Optimize
+
+                if(fhdgdiff > 0)
+                {
+                    if(pgeo0->GetGeomType() == 2)
+                    {
+                        GeometryArc * pArc = (GeometryArc * )pgeo0;
+                        if(pArc->GetCurvature() >0)
+                        {
+                            double fobjx,fobjy;
+                            fobjx = pgeo2->GetX() + pgeo1->GetLength() * cos(pgeo2->GetHdg() + M_PI);
+                            fobjy = pgeo2->GetY() + pgeo1->GetLength() * sin(pgeo2->GetHdg() + M_PI);
+
+                            double fdis = sqrt(pow(pgeo1->GetX() -fobjx,2) + pow(pgeo1->GetY() - fobjy,2));
+                            qDebug("dis, %f",fdis);
+
+                            double fopvalue = fdis;
+                            if(fopvalue > maxopvalue)
+                            {
+                                fopvalue = maxopvalue;
+                                double fhdg = CalcHdg(QPointF(pgeo1->GetX(),pgeo1->GetY()),QPointF(fobjx,fobjy));
+                                fobjx = pgeo1->GetX() + fopvalue * cos(fhdg);
+                                fobjy = pgeo1->GetY() + fopvalue * sin(fhdg);
+                            }
+
+                            double farcdis = sqrt(pow(fobjx - pgeo0->GetX(),2)+ pow(fobjy - pgeo0->GetY(),2));
+                            double fhdgarctoline = CalcHdg(QPointF(pgeo0->GetX(),pgeo0->GetY()),QPointF(fobjx,fobjy));
+                            double fhdgdiff_arcline = fhdgarctoline - pgeo0->GetHdg();
+                            while(fhdgdiff_arcline>M_PI)fhdgdiff_arcline = fhdgdiff_arcline - 2.0*M_PI;
+                            while(fhdgdiff_arcline<(-M_PI))fhdgdiff_arcline = fhdgdiff_arcline + 2.0*M_PI;
+
+                            double R = (farcdis/2.0)/sin(fabs(fhdgdiff_arcline));
+                            double curvature = 1.0/R;
+                            double farclen = R*2.0* fabs(fhdgdiff_arcline);
+                            pgeo0->SetLength(farclen);
+                            pArc->SetCurvature(curvature);
+
+                            double flinehdg = CalcHdg(QPointF(fobjx,fobjy),QPointF(pgeo2->GetX(),pgeo2->GetY()));
+
+                            pgeo1->SetX(fobjx);
+                            pgeo1->SetY(fobjy);
+                            pgeo1->SetHdg(flinehdg);
+                            pgeo1->SetLength(sqrt(pow(pgeo2->GetX() - fobjx,2)+pow(pgeo2->GetY() - fobjy,2)));
+                            boptimize = true;
+
+
+
+                        }
+                    }
+                }
+
+                i = i+2;  //Because Optimize, so move 2 step.
+            }
+        }
+
+    }
+
+
+    if(boptimize)
+    {
+        ngeobsize = static_cast<int>(pRoad->GetGeometryBlockCount());
+        double snow = 0;
+        for(i=0;i<=(ngeobsize-1);i++)
+        {
+            RoadGeometry *  pgeo1 =  pRoad->GetGeometryBlock(i)->GetGeometryAt(0);
+            pgeo1->SetS(snow);
+            snow = snow + pgeo1->GetLength();
+        }
+        pRoad->SetRoadLength(snow);
+    }
+
+}

src/common/common/xodr/xodrfunc/xodrfunc.h

@@ -82,6 +82,8 @@ public:
 
     static int GetDrivingLane(Road * pRoad,const int nLS,const int nsuggestlane);
     static Lane * GetLaneByID(LaneSection * pLS,int nlane);
+
+    static void optimizeroad(Road * pRoad,const double maxopvalue = 0.5);
 };
 
 #endif // XODRFUNC_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -6106,5 +6106,76 @@ void MainWindow::on_actionExport_Current_Road_triggered()
 {
     int index = mpCBRoad->currentIndex();
     qDebug("index: %d",index);
+
+    int nroadsize = static_cast<int>(mxodr.GetRoadCount());
+
+    if((index<0)||(index>=nroadsize))
+    {
+        QMessageBox::warning(this,tr("Warning"),tr("Road Index Error."),QMessageBox::YesAll);
+        return;
+    }
+
+    QString strsavepath = QFileDialog::getSaveFileName(this,tr("Save Select Road"),getenv("HOME"),"*.xodr");
+
+    if(strsavepath.isEmpty())return;
+
+    if(strsavepath.length()>4)
+    {
+        if(strsavepath.right(5) == ".xodr" )
+        {
+
+        }
+        else
+        {
+            strsavepath.append(".xodr");
+        }
+    }
+    else
+    {
+        strsavepath.append(".xodr");
+    }
+
+    OpenDrive * pxodr = new OpenDrive();
+
+    unsigned short int revMajor,revMinor;
+    std::string name,date;
+    float version;
+    double north,south,east,west,lat0,lon0,hdg0;
+    mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+
+    pxodr->SetHeader(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+
+    pxodr->GetRoadVector()->push_back(mxodr.GetRoadVector()->at(index));
+
+    OpenDriveXmlWriter x(pxodr);
+    x.WriteFile(strsavepath.toStdString());
+
+    delete pxodr;
+
+
+
+}
+
+
+void MainWindow::on_actionOptimize_Road_triggered()
+{
+    int index = mpCBRoad->currentIndex();
+    qDebug("index: %d",index);
+
+    int nroadsize = static_cast<int>(mxodr.GetRoadCount());
+
+    if((index<0)||(index>=nroadsize))
+    {
+        QMessageBox::warning(this,tr("Warning"),tr("Road Index Error."),QMessageBox::YesAll);
+        return;
+    }
+
+
+    Road * pRoad = mxodr.GetRoad(index);
+
+    if(pRoad == NULL)return;
+
+    xodrfunc::optimizeroad(pRoad);
+
 }
 

src/tool/map_lanetoxodr/mainwindow.h

@@ -253,6 +253,8 @@ private slots:
 
     void on_actionExport_Current_Road_triggered();
 
+    void on_actionOptimize_Road_triggered();
+
 private:
 
 

src/tool/map_lanetoxodr/mainwindow.ui

@@ -49,6 +49,7 @@
     <addaction name="actionBack"/>
     <addaction name="separator"/>
     <addaction name="actionMake_All_Road_Contact"/>
+    <addaction name="actionOptimize_Road"/>
    </widget>
    <widget class="QMenu" name="menuTool">
     <property name="title">
@@ -236,6 +237,11 @@
     <string>Export Current Road</string>
    </property>
   </action>
+  <action name="actionOptimize_Road">
+   <property name="text">
+    <string>Optimize Road</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>