change tool/map_lanetoxodr. complete road optimize function.

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

@@ -867,13 +867,13 @@ void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, d
     {
         if(mbNormal)
         {
-            pRange = (s_check -mS-0.1)/mLength;
+            pRange = (s_check -mS-0.001)/mLength;
             if(pRange<0)pRange = 0.0;
             if(pRange>1.0)pRange = 1.0;
         }
         else
         {
-            pRange = s_check-mS - 0.1;
+            pRange = s_check-mS - 0.001;
         }
         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/dialogroadoptimize.cpp

@@ -0,0 +1,346 @@
+#include "dialogroadoptimize.h"
+#include "ui_dialogroadoptimize.h"
+
+#include <math.h>
+#include <memory>
+
+#include "geofit.h"
+
+DialogRoadOptimize::DialogRoadOptimize(Road * pRoad,OpenDrive * pxodr,QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::DialogRoadOptimize)
+{
+    ui->setupUi(this);
+
+    ui->lineEdit_disthresh->setText("0.1");
+    ui->lineEdit_hdgthresh->setText("0.01");
+    ui->lineEdit_arcbezel->setText("3.0");
+    ui->lineEdit_linebezel->setText("10.0");
+    ui->lineEdit_roadoffthresh->setText("0.5");
+
+    mpRoad = pRoad;
+    mpxodr = pxodr;
+
+    std::vector<double> fvectordis,fvectorhdg;
+    int nrtn = CheckRoadQuality(pRoad,fvectordis,fvectorhdg);
+
+    const int noutbufsize = 100000;
+    std::shared_ptr<char> pstrout_ptr = std::shared_ptr<char>(new char[noutbufsize]);
+    char strline[1000];
+    snprintf(pstrout_ptr.get(),noutbufsize,"Check point:%d\n",nrtn);
+    if(nrtn > 0)
+    {
+        int i;
+        for(i=0;i<nrtn;i++)
+        {
+            snprintf(strline,1000,"%d  distance:%f   hdgdiff:%f\n",i,fvectordis[i],fvectorhdg[i]);
+            strncat(pstrout_ptr.get(),strline,noutbufsize);
+        }
+    }
+
+    ui->plainTextEdit->setPlainText(pstrout_ptr.get());
+
+    setWindowTitle("Road Optimize");
+}
+
+DialogRoadOptimize::~DialogRoadOptimize()
+{
+    delete ui;
+}
+
+int DialogRoadOptimize::CheckRoadQuality(Road *pRoad, std::vector<double> & fvectordis,
+                                         std::vector<double> & fvectorhdg)
+{
+    if(pRoad->GetGeometryBlockCount() <=1)
+    {
+        return 0;
+    }
+
+    int nrtn = 0;
+
+    unsigned int i;
+    for(i=0;i<(pRoad->GetGeometryBlockCount()-1);i++)
+    {
+        GeometryBlock * pgeob1 =  pRoad->GetGeometryBlock(i);
+        GeometryBlock * pgeob2 =  pRoad->GetGeometryBlock(i+1);
+        double x1,y1,hdg1;
+        double x2,y2,hdg2;
+        pgeob1->GetGeometryAt(0)->GetCoords(pgeob1->GetGeometryAt(0)->GetS() + pgeob1->GetGeometryAt(0)->GetLength(),
+                                            x1,y1,hdg1);
+        pgeob2->GetGeometryAt(0)->GetCoords(pgeob2->GetGeometryAt(0)->GetS() ,
+                                            x2,y2,hdg2);
+
+        double fdis = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
+        double fhdgdiff = hdg2 - hdg1;
+        while (fhdgdiff>M_PI)fhdgdiff = fhdgdiff -2.0*M_PI;
+        while(fhdgdiff<=(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI;
+        fvectordis.push_back(fdis);
+        fvectorhdg.push_back(fhdgdiff);
+        nrtn++;
+
+    }
+    return nrtn;
+}
+
+void DialogRoadOptimize::on_pushButton_Optimize_clicked()
+{
+
+    double fdisthresh = ui->lineEdit_disthresh->text().toDouble();
+    double fhdgthresh = ui->lineEdit_hdgthresh->text().toDouble();
+    double flinebezel = ui->lineEdit_linebezel->text().toDouble();
+    double farcbezel = ui->lineEdit_arcbezel->text().toDouble();
+    double froadoffthresh = ui->lineEdit_roadoffthresh->text().toDouble();
+
+    if(fdisthresh<0.00001)
+    {
+        fdisthresh = 0.00001;
+        std::cout<<"fdisthresh change to "<<fdisthresh<<std::endl;
+    }
+    if(fhdgthresh<0.001)
+    {
+        fhdgthresh = 0.001;
+        std::cout<<"fhdgthesh change to "<<fhdgthresh<<std::endl;
+    }
+    if(flinebezel<1.0)
+    {
+        flinebezel = 1.0;
+        std::cout<<"flinebezel change to "<<flinebezel<<std::endl;
+    }
+    if(farcbezel<1.0)
+    {
+        farcbezel = 1.0;
+        std::cout<<"farcbezel change to "<<farcbezel<<std::endl;
+    }
+    if(froadoffthresh<0.05)
+    {
+        froadoffthresh = 0.05;
+        std::cout<<"froadoffthresh change to "<<froadoffthresh<<std::endl;
+    }
+
+    std::vector<double> fvectordis,fvectorhdg;
+    Road * pRoad = mpRoad;
+    int nrtn = CheckRoadQuality(pRoad,fvectordis,fvectorhdg);
+    if(nrtn == 0)
+    {
+        QMessageBox::information(this,"Info","Not Need Optimize",QMessageBox::YesAll);
+        return;
+    }
+
+    int nmsgbufsize = 100000;
+    std::shared_ptr<char> strmsg_ptr = std::shared_ptr<char>(new char[nmsgbufsize]);
+    snprintf(strmsg_ptr.get(),nmsgbufsize,"Change pos: \n");
+
+    int nchange = 0;
+    Road * pnewroad = mpRoad;
+    int i;
+    int nowgeobpos = 0;
+    GeometryBlock * pgeobnow = pnewroad->GetGeometryBlock(0);
+    std::vector<GeometryBlock > * pvectorgeo = pnewroad->GetGeometryBlockVector();
+    for(i=0;i<nrtn;i++)
+    {
+        if((fvectordis[i]>fdisthresh)||(fabs(fvectorhdg[i])>fhdgthresh))
+        {
+            pgeobnow = pnewroad->GetGeometryBlock(nowgeobpos);
+            RoadGeometry * pgeo = pgeobnow->GetGeometryAt(0);
+            int ngeotype = pgeo->GetGeomType();
+            std::cout<<"geo type : "<<ngeotype<<std::endl;
+            if((pgeo->GetGeomType() ==0) ||(pgeo->GetGeomType() == 2))
+            {
+                double fthresh = flinebezel;
+                if(ngeotype == 2)fthresh = farcbezel;
+                if(pgeo->GetLength()>(fthresh+1.0))
+                {
+                    double xkeeplen = pgeo->GetLength() - fthresh;
+                    double x_start,y_start,hdg_start,x_end,y_end,hdg_end;
+                    pgeo->GetCoords(pgeo->GetS()+xkeeplen,x_start,y_start,hdg_start);
+                    pnewroad->GetGeometryBlock(nowgeobpos+1)->GetGeometryAt(0)->GetCoords(pgeo->GetS() + pgeo->GetLength(),
+                                                                                 x_end,y_end,hdg_end);
+                    std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(x_start,y_start,
+                                                                              hdg_start,x_end,y_end,hdg_end);
+
+
+                    GeometryBlock xgb;
+                    qDebug("be len: %f ",xvectorgeo[0].mfLen);
+                    xgb.AddGeometryParamPoly3(pgeo->GetS() + xkeeplen,x_start,y_start,hdg_start,xvectorgeo[0].mfLen,
+                                              xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1],
+                            xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3],
+                            xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1],
+                            xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3],false);
+                    double fdisgeo = GetGeo1toGeo2MaxDis(&xgb,&(pvectorgeo->at(nowgeobpos)));
+
+                    if(fdisgeo<= froadoffthresh)
+                    {
+                        pgeo->SetLength(xkeeplen);
+                        pvectorgeo->insert(pvectorgeo->begin() + nowgeobpos+1,xgb);
+                        nowgeobpos = nowgeobpos+2;
+                        char strout[1000];
+                        snprintf(strout,1000,"geo %d to geo %d. Insert a bezel.geo off:%f.\n",i,i+1,fdisgeo);
+                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
+                        nchange++;
+                    }
+                    else
+                    {
+                        char strout[1000];
+                        snprintf(strout,1000,"geo %d to geo %d. Optimize off %f exceed thresh.\n",i,i+1,fdisgeo);
+                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
+                        nchange++;
+                        nowgeobpos++;
+                    }
+                }
+                else
+                {
+                    double x_start,y_start,hdg_start,x_end,y_end,hdg_end;
+                    pgeo->GetCoords(pgeo->GetS(),x_start,y_start,hdg_start);
+                    pnewroad->GetGeometryBlock(nowgeobpos+1)->GetGeometryAt(0)->GetCoords(pgeo->GetS() + pgeo->GetLength(),
+                                                                                 x_end,y_end,hdg_end);
+                    std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(x_start,y_start,
+                                                                              hdg_start,x_end,y_end,hdg_end);
+
+                    qDebug("be len: %f ",xvectorgeo[0].mfLen);
+                    GeometryBlock xgb;
+                    xgb.AddGeometryParamPoly3(pgeo->GetS(),x_start,y_start,hdg_start,xvectorgeo[0].mfLen,
+                                              xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1],
+                            xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3],
+                            xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1],
+                            xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3],false);
+
+                    double fdisgeo = GetGeo1toGeo2MaxDis(&xgb,&(pvectorgeo->at(nowgeobpos)));
+                    if(fdisgeo <= froadoffthresh)
+                    {
+                        pvectorgeo->erase(pvectorgeo->begin()+nowgeobpos);
+                        pvectorgeo->insert(pvectorgeo->begin()+nowgeobpos,xgb);
+                        nowgeobpos = nowgeobpos+1;
+                        char strout[1000];
+                        snprintf(strout,1000,"geo %d to geo %d. geo %d replace by a bezel. geo off:%f.\n",i,i+1,i,fdisgeo);
+                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
+                        nchange++;
+                    }
+                    else
+                    {
+                        char strout[1000];
+                        snprintf(strout,1000,"geo %d to geo %d. Optimize off %f exceed thresh.\n",i,i+1,fdisgeo);
+                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
+                        nchange++;
+                        nowgeobpos++;
+                    }
+     //               break;
+
+
+
+                }
+
+                unsigned int j;
+                for(j=1;j<pvectorgeo->size();j++)
+                {
+                    pvectorgeo->at(j).GetGeometryAt(0)->SetS(pvectorgeo->at(j-1).GetGeometryAt(0)->GetS()
+                                                             +pvectorgeo->at(j-1).GetGeometryAt(0)->GetLength());
+                }
+
+            }
+            else
+            {
+                nowgeobpos++;
+            }
+
+        }
+        else
+        {
+            char strout[1000];
+            snprintf(strout,1000,"geo %d to geo %d. Not Need Optimize.\n",i,i+1);
+            strncat(strmsg_ptr.get(),strout,nmsgbufsize);
+            nowgeobpos++;
+        }
+    }
+
+    if(nchange > 0)
+    {
+        RoadGeometry * pgeolast = pvectorgeo->at(pvectorgeo->size()-1).GetGeometryAt(0);
+        pnewroad->SetRoadLength(pgeolast->GetS() + pgeolast->GetLength());
+    }
+
+    char strout[1000];
+    snprintf(strout,1000,"Optimize %d position.",nchange);
+    strncat(strmsg_ptr.get(),strout,nmsgbufsize);
+
+    QMessageBox::information(this,"Optimize Result",strmsg_ptr.get(),QMessageBox::YesAll);
+
+
+
+}
+
+double DialogRoadOptimize::GetGeo1toGeo2MaxDis(GeometryBlock *pgeob1, GeometryBlock *pgeob2)
+{
+    RoadGeometry * prg1,*prg2;
+    prg1 = pgeob1->GetGeometryAt(0);
+    prg2 = pgeob2->GetGeometryAt(0);
+    double fstep = 0.1;
+    int nCount = (int)(prg1->GetLength()/fstep);
+    if(nCount == 0)
+    {
+        return sqrt(pow(prg1->GetX() - prg2->GetX(),2)+pow(prg1->GetY() - prg2->GetY(),2));
+    }
+    int i;
+
+    double rg1_s = prg1->GetS();
+    double rg2_s = prg2->GetS();
+//    double rg1_len = prg1->GetLength();
+    double rg2_len = prg2->GetLength();
+    double fdismax = 0;
+    double fsmax = 0;
+    for(i=0;i<nCount;i++)
+    {
+        double s = fstep * i;
+        double x1,y1,hdg1;
+        double fstep2 = 0.5;
+        prg1->GetCoords(rg1_s+s,x1,y1,hdg1);
+        int nCount2 =(int)( rg2_len/fstep2);
+        double snear = 0;
+        double sdis = 1000.0;
+        if(nCount2 == 0)
+        {
+            snear = 0;
+        }
+        else
+        {
+            int j;
+            for(j=0;j<nCount2;j++)
+            {
+                double x2,y2,hdg2;
+                double s2 = j*fstep2;
+                prg2->GetCoords(rg2_s +s2,x2,y2,hdg2);
+                double fdis = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
+                if(fdis <sdis)
+                {
+                    snear = s2;
+                    sdis = fdis;
+                }
+            }
+        }
+        double snear_low = snear - fstep2;
+        double snear_high = snear  + fstep2;
+        if(snear_low<0)snear_low = 0;
+        if(snear_high > rg2_len)snear_high = rg2_len;
+        double s3 = snear_low;
+        while(s3<=snear_high)
+        {
+            double x3,y3,hdg3;
+            prg2->GetCoords(rg2_s+s3,x3,y3,hdg3);
+            double fdis = sqrt(pow(x3-x1,2)+pow(y3-y1,2));
+            if(fdis < sdis)
+            {
+                snear = s3;
+                sdis = fdis;
+            }
+            s3 = s3 +0.01;
+        }
+
+        if(sdis > fdismax)
+        {
+            fdismax = sdis;
+            fsmax = snear;
+        }
+    }
+
+    std::cout<<"dismax : "<<fdismax<<std::endl;
+    return fdismax;
+}

src/tool/map_lanetoxodr/dialogroadoptimize.h

@@ -0,0 +1,40 @@
+#ifndef DIALOGROADOPTIMIZE_H
+#define DIALOGROADOPTIMIZE_H
+
+#include <QDialog>
+
+#include "OpenDrive/OpenDrive.h"
+#include <QMessageBox>
+
+#include <vector>
+
+namespace Ui {
+class DialogRoadOptimize;
+}
+
+class DialogRoadOptimize : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogRoadOptimize(Road * pRoad,OpenDrive * pxodr,QWidget *parent = nullptr);
+    ~DialogRoadOptimize();
+
+
+public:
+    static int CheckRoadQuality(Road * pRoad, std::vector<double> & fvectordis,
+                                std::vector<double> & fvectorhdg);
+
+    static double GetGeo1toGeo2MaxDis(GeometryBlock * pgeob1,GeometryBlock * pgeob2);
+
+private slots:
+    void on_pushButton_Optimize_clicked();
+
+private:
+    Ui::DialogRoadOptimize *ui;
+
+    Road * mpRoad;
+    OpenDrive * mpxodr;
+};
+
+#endif // DIALOGROADOPTIMIZE_H

src/tool/map_lanetoxodr/dialogroadoptimize.ui

@@ -0,0 +1,170 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogRoadOptimize</class>
+ <widget class="QDialog" name="DialogRoadOptimize">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>790</width>
+    <height>567</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>50</x>
+     <y>20</y>
+     <width>171</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road Quality:</string>
+   </property>
+  </widget>
+  <widget class="QPlainTextEdit" name="plainTextEdit">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>70</y>
+     <width>711</width>
+     <height>251</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_Optimize">
+   <property name="geometry">
+    <rect>
+     <x>600</x>
+     <y>478</y>
+     <width>151</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Optimize</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_disthresh">
+   <property name="geometry">
+    <rect>
+     <x>220</x>
+     <y>340</y>
+     <width>141</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_2">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>342</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>距离阈值</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_3">
+   <property name="geometry">
+    <rect>
+     <x>430</x>
+     <y>342</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>航向阈值（弧度)</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_hdgthresh">
+   <property name="geometry">
+    <rect>
+     <x>610</x>
+     <y>340</y>
+     <width>141</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_4">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>412</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>直线贝塞尔最大值</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_linebezel">
+   <property name="geometry">
+    <rect>
+     <x>220</x>
+     <y>410</y>
+     <width>141</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_5">
+   <property name="geometry">
+    <rect>
+     <x>430</x>
+     <y>412</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>圆弧贝塞尔最大值</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_arcbezel">
+   <property name="geometry">
+    <rect>
+     <x>610</x>
+     <y>410</y>
+     <width>141</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_roadoffthresh">
+   <property name="geometry">
+    <rect>
+     <x>220</x>
+     <y>478</y>
+     <width>141</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_6">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>480</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>道路偏离允许值</string>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/map_lanetoxodr/geofit.cpp

@@ -862,16 +862,16 @@ int geofit::CreateBezier(double xstart, double ystart, double hdg_start, double
     if(hdgdiff>M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
     double fdis = sqrt(pow(xend-xstart,2)+pow(yend-ystart,2));
 //    if(fabs(hdgdiff)/fdis > 0.1)
-    if(fdis<3.0)
-    {
-        std::cout<<"bezier is not ok. use line."<<std::endl;
-        ntype = 0;
-        fxy[0] = xstart;
-        fxy[1] = ystart;
-        *fhdg = flinehdg;
-        *flen = fdis;
-        return 0;
-    }
+//    if(fdis<3.0)
+//    {
+//        std::cout<<"bezier is not ok. use line."<<std::endl;
+//        ntype = 0;
+//        fxy[0] = xstart;
+//        fxy[1] = ystart;
+//        *fhdg = flinehdg;
+//        *flen = fdis;
+//        return 0;
+//    }
 
     std::cout<<"bezer is ok. use bezier"<<std::endl;
 
@@ -891,6 +891,15 @@ int geofit::CreateBezier(double xstart, double ystart, double hdg_start, double
     if(vratio > 1)vratio = 1;
     if(vratio < 0)vratio = 0;
 
+    hdgdiff = hdg_end - hdg_start;
+    if(hdgdiff<(M_PI*(-1)))hdgdiff = hdgdiff + 2.0*M_PI;
+    if(hdgdiff>M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
+    double frr = fabs(hdgdiff) * 0.1 +0.1;
+//    if(frr == 0)frr = 0.1;
+
+    uratio = frr;
+    vratio = frr;
+
     double c2base = sqrt(pow(x2,2) + pow(y2,2));
 //    yc1 = 0; xc1 = x2 * uratio;
     yc1 = 0; xc1 = c2base * uratio;

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -3427,7 +3427,7 @@ void MainWindow::onClickCreateRoad()
     case 1:
         {
             double fdis = sqrt(pow(startx - endx,2) +pow(starty -endy,2));
-            if((fdis<10) || (starthdg == endhdg) ||(mpCBStraightType->isChecked()))
+            if((fdis<3) || (starthdg == endhdg) ||(mpCBStraightType->isChecked()))
             {
                 xvectorgeo = CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
             }

src/tool/map_lanetoxodr/mainwindow.ui

@@ -159,6 +159,11 @@
     <string>Hide Road</string>
    </property>
   </action>
+  <action name="actionRoad_Optimize">
+   <property name="text">
+    <string>Road Optimize</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -43,6 +43,7 @@ SOURCES += \
     dialogroadmirror.cpp \
     dialogroadmove.cpp \
     dialogroadobject.cpp \
+    dialogroadoptimize.cpp \
     dialogroadrotate.cpp \
     dialogroadsplit.cpp \
     filebackup.cpp \
@@ -91,6 +92,7 @@ HEADERS += \
     dialogroadmirror.h \
     dialogroadmove.h \
     dialogroadobject.h \
+    dialogroadoptimize.h \
     dialogroadrotate.h \
     dialogroadsplit.h \
     filebackup.h \
@@ -133,6 +135,7 @@ FORMS += \
         dialogroadmirror.ui \
         dialogroadmove.ui \
         dialogroadobject.ui \
+        dialogroadoptimize.ui \
         dialogroadrotate.ui \
         dialogroadsplit.ui \
         mainwindow.ui \

src/tool/map_lanetoxodr/roaddigit.cpp

@@ -331,19 +331,23 @@ void RoadDigit::CalcLine(double fspace)
 
             double flastx = pg->GetX();
             double flasty = pg->GetY();
+            double pp3_s = pg->GetS();
             while(s <= ppp3->GetLength())
             {
-                double pr=s;
-                if((arclength > 0)&&(bNormal))pr = s/arclength;
-                xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
-                ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
-                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
-                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
+//                double pr=s;
+//                if((arclength > 0)&&(bNormal))pr = s/arclength;
+//                xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
+//                ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
+//                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
+//                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
 
                 rdu.mS = pg->GetS() + s;
-                rdu.mX = x;
-                rdu.mY = y;
-                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(flastx,flasty),QPointF(x,y));
+                ppp3->GetCoords(rdu.mS,rdu.mX,rdu.mY,rdu.mfHdg);
+//                rdu.mS = pg->GetS() + s;
+//                rdu.mX = x;
+//                rdu.mY = y;
+//                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(flastx,flasty),QPointF(x,y));
+//                rdu.mS = pp3_s+s;
                 mvectorRDU.push_back(rdu);
 
                 s = s+ sstep;
@@ -352,18 +356,22 @@ void RoadDigit::CalcLine(double fspace)
             }
             if(s != ppp3->GetLength())
             {
-                double pr=s;
-                if((arclength > 0)&&(bNormal))pr = s/arclength;
-                xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
-                ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
-                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
-                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
 
-                rdu.mS = pg->GetS() + s;
-                rdu.mX = x;
-                rdu.mY = y;
-                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(flastx,flasty),QPointF(x,y));
+                rdu.mS = pg->GetS() + ppp3->GetLength();
+                ppp3->GetCoords(rdu.mS,rdu.mX,rdu.mY,rdu.mfHdg);
                 mvectorRDU.push_back(rdu);
+//                double pr=s;
+//                if((arclength > 0)&&(bNormal))pr = s/arclength;
+//                xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
+//                ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
+//                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
+//                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
+
+//                rdu.mS = pg->GetS() + s;
+//                rdu.mX = x;
+//                rdu.mY = y;
+//                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(flastx,flasty),QPointF(x,y));
+//                mvectorRDU.push_back(rdu);
             }
             }
             break;

src/tool/map_lanetoxodr/roadeditdialog.cpp

@@ -651,3 +651,15 @@ void RoadEditDialog::on_pushButton_EditRoadObjects_clicked()
     roadobject.exec();
 
 }
+
+void RoadEditDialog::on_pushButton_RoadOptimize_clicked()
+{
+    if(mpCurRoad == 0)
+    {
+        QMessageBox::warning(this,"Warning","Not Select Road");
+        return;
+    }
+
+    DialogRoadOptimize dlgroadopt(mpCurRoad,mpxodr, this);
+    dlgroadopt.exec();
+}

src/tool/map_lanetoxodr/roadeditdialog.h

@@ -21,6 +21,7 @@
 #include "dialogroadmirror.h"
 #include "dialoglaneoffset.h"
 #include "dialogroadobject.h"
+#include "dialogroadoptimize.h"
 
 #include "roaddigit.h"
 #include "xodrscenfunc.h"
@@ -69,6 +70,8 @@ private slots:
 
     void on_pushButton_EditRoadObjects_clicked();
 
+    void on_pushButton_RoadOptimize_clicked();
+
 private:
     bool IsDrawMark(double s);
 

src/tool/map_lanetoxodr/roadeditdialog.ui

@@ -350,6 +350,19 @@
     <string>Edit Road Objects</string>
    </property>
   </widget>
+  <widget class="QPushButton" name="pushButton_RoadOptimize">
+   <property name="geometry">
+    <rect>
+     <x>1010</x>
+     <y>280</y>
+     <width>181</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road Optimize</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>