change tool/map_lanetoxodr. add merge road. but not complete.

src/tool/map_lanetoxodr/dialogroadmerge.cpp

@@ -0,0 +1,108 @@
+#include "dialogroadmerge.h"
+#include "ui_dialogroadmerge.h"
+
+#include "xodrfunc.h"
+
+#include <math.h>
+
+DialogRoadMerge::DialogRoadMerge(OpenDrive * pxodr,QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::DialogRoadMerge)
+{
+    ui->setupUi(this);
+
+    mpxodr = pxodr;
+}
+
+DialogRoadMerge::~DialogRoadMerge()
+{
+    delete ui;
+}
+
+void DialogRoadMerge::on_pushButton_Merge_clicked()
+{
+    std::string strroad1 = ui->lineEdit_Road1->text().toStdString();
+    std::string strroad2 = ui->lineEdit_Road2->text().toStdString();
+    Road * pRoad1 = xodrfunc::GetRoadByID(mpxodr,strroad1);
+    if(pRoad1 == 0)
+    {
+        QMessageBox::warning(this,"Warning","Not found road1",QMessageBox::YesAll);
+        return;
+    }
+    Road * pRoad2 = xodrfunc::GetRoadByID(mpxodr,strroad2);
+    if(pRoad2 == 0)
+    {
+        QMessageBox::warning(this,"Warning","Not found road2",QMessageBox::YesAll);
+        return;
+    }
+
+    double x1,y1,hdg1;
+    double x2,y2,hdg2;
+    xodrfunc::GetRoadXYByS(pRoad1,pRoad1->GetRoadLength(),x1,y1,hdg1);
+    xodrfunc::GetRoadXYByS(pRoad2,0,x2,y2,hdg2);
+
+    double fdis = sqrt(pow(x1-x2,2)+pow(y1-y2,2));
+
+    if(fdis > 1.0)
+    {
+        char strout[256];
+        snprintf(strout,256,"Road dis very fast. Dis is %6.3f",fdis);
+        QMessageBox::warning(this,"Warning",QString(strout),QMessageBox::YesAll);
+        return;
+    }
+
+    Road xroad;
+    xroad = *pRoad1;
+    double froad1len = pRoad1->GetRoadLength();
+    int nroad2geocount = pRoad2->GetGeometryBlockCount();
+    int i;
+    GeometryBlock * pnewgb;
+    for(i=0;i<nroad2geocount;i++)
+    {
+
+
+        GeometryBlock * pgb = pRoad2->GetGeometryBlock(i);
+
+        RoadGeometry * pgeb = pgb->GetGeometryAt(0);
+        if(pgeb == 0)
+        {
+            continue;
+        }
+        pRoad1->AddGeometryBlock();
+        pnewgb = pRoad1->GetLastAddedGeometryBlock();
+        switch (pgeb->GetGeomType()) {
+        case 0:
+            pnewgb->AddGeometryLine(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),pgeb->GetHdg(),pgeb->GetLength());
+            break;
+        case 1:
+        {
+            GeometrySpiral * pspiral = (GeometrySpiral *)pgeb;
+            pnewgb->AddGeometrySpiral(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),pgeb->GetHdg(),pgeb->GetLength(),
+                                      pspiral->GetCurvatureStart(),pspiral->GetCurvatureEnd());
+        }
+            break;
+        case 2:
+        {
+            GeometryArc * parc = (GeometryArc *)pgeb;
+            pnewgb->AddGeometryArc(pgeb->GetS() + froad1len,parc->GetX(),parc->GetY(),
+                                   parc->GetHdg(),parc->GetLength(),parc->GetCurvature());
+        }
+            break;
+        case 4:
+        {
+            GeometryParamPoly3 * pparmp3d = (GeometryParamPoly3 *)pgeb;
+            pnewgb->AddGeometryParamPoly3(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),
+                                          pgeb->GetHdg(),pgeb->GetLength(),pparmp3d->GetuA(),pparmp3d->GetuB(),
+                                          pparmp3d->GetuC(),pparmp3d->GetuD(),pparmp3d->GetvA(),
+                                          pparmp3d->GetvB(),pparmp3d->GetvC(),pparmp3d->GetvD());
+        }
+        default:
+            break;
+        }
+
+
+    }
+
+    pRoad1->SetRoadLength(pRoad1->GetRoadLength() + pRoad2->GetRoadLength());
+     mpxodr->DeleteRoad(xodrfunc::GetRoadIndex(mpxodr,pRoad2));
+}

src/tool/map_lanetoxodr/dialogroadmerge.h

@@ -0,0 +1,29 @@
+#ifndef DIALOGROADMERGE_H
+#define DIALOGROADMERGE_H
+
+#include <QDialog>
+
+#include "OpenDrive/OpenDrive.h"
+#include <QMessageBox>
+
+namespace Ui {
+class DialogRoadMerge;
+}
+
+class DialogRoadMerge : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogRoadMerge(OpenDrive * pxodr,QWidget *parent = nullptr);
+    ~DialogRoadMerge();
+
+private slots:
+    void on_pushButton_Merge_clicked();
+
+private:
+    Ui::DialogRoadMerge *ui;
+    OpenDrive * mpxodr;
+};
+
+#endif // DIALOGROADMERGE_H

src/tool/map_lanetoxodr/dialogroadmerge.ui

@@ -0,0 +1,78 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogRoadMerge</class>
+ <widget class="QDialog" name="DialogRoadMerge">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>432</width>
+    <height>262</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QPushButton" name="pushButton_Merge">
+   <property name="geometry">
+    <rect>
+     <x>160</x>
+     <y>150</y>
+     <width>89</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Merge</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_Road1">
+   <property name="geometry">
+    <rect>
+     <x>200</x>
+     <y>30</y>
+     <width>131</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_Road2">
+   <property name="geometry">
+    <rect>
+     <x>200</x>
+     <y>90</y>
+     <width>131</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>90</x>
+     <y>40</y>
+     <width>67</width>
+     <height>17</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road1</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_2">
+   <property name="geometry">
+    <rect>
+     <x>90</x>
+     <y>100</y>
+     <width>67</width>
+     <height>17</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road2</string>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -33,6 +33,7 @@ SOURCES += \
     dialogeditlane.cpp \
     dialogeditroadmark.cpp \
     dialoglanefromrtk.cpp \
+    dialogroadmerge.cpp \
     dialogroadsplit.cpp \
     ivxodrtool.cpp \
         main.cpp \
@@ -77,6 +78,7 @@ HEADERS += \
     dialogeditlane.h \
     dialogeditroadmark.h \
     dialoglanefromrtk.h \
+    dialogroadmerge.h \
     dialogroadsplit.h \
     ivxodrtool.h \
         mainwindow.h \
@@ -117,6 +119,7 @@ FORMS += \
         dialogeditlane.ui \
         dialogeditroadmark.ui \
         dialoglanefromrtk.ui \
+        dialogroadmerge.ui \
         dialogroadsplit.ui \
         mainwindow.ui \
         roadeditdialog.ui \

src/tool/map_lanetoxodr/roadeditdialog.cpp

@@ -462,3 +462,21 @@ void RoadEditDialog::on_pushButton_RoadSplit_clicked()
     }
     on_comboBox_Road_currentIndexChanged(ncurindex);
 }
+
+void RoadEditDialog::on_pushButton_RoadMerge_clicked()
+{
+    DialogRoadMerge roadmerge(mpxodr,this);
+    roadmerge.exec();
+
+    unsigned int ncurindex = ui->comboBox_Road->currentIndex();
+    ui->comboBox_Road->clear();
+    int i;
+    int nroadcount = mpxodr->GetRoadCount();
+    for(i=0;i<nroadcount;i++)
+    {
+        const char * strname = mpxodr->GetRoad(i)->GetRoadId().data();
+        ui->comboBox_Road->addItem(strname);
+
+    }
+    on_comboBox_Road_currentIndexChanged(ncurindex);
+}

src/tool/map_lanetoxodr/roadeditdialog.h

@@ -15,6 +15,7 @@
 #include "dialogeditroadmark.h"
 #include "dialoglanefromrtk.h"
 #include "dialogroadsplit.h"
+#include "dialogroadmerge.h"
 
 namespace Ui {
 class RoadEditDialog;
@@ -48,6 +49,8 @@ private slots:
 
     void on_pushButton_RoadSplit_clicked();
 
+    void on_pushButton_RoadMerge_clicked();
+
 private:
     Ui::RoadEditDialog *ui;
     OpenDrive * mpxodr;

src/tool/map_lanetoxodr/roadeditdialog.ui

@@ -262,7 +262,7 @@
   <widget class="QPushButton" name="pushButton_RoadSplit">
    <property name="geometry">
     <rect>
-     <x>878</x>
+     <x>1013</x>
      <y>210</y>
      <width>181</width>
      <height>31</height>
@@ -272,6 +272,58 @@
     <string>Road Split</string>
    </property>
   </widget>
+  <widget class="QPushButton" name="pushButton_RoadMerge">
+   <property name="geometry">
+    <rect>
+     <x>1010</x>
+     <y>283</y>
+     <width>191</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Road Merge</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_MoveRoad">
+   <property name="geometry">
+    <rect>
+     <x>1010</x>
+     <y>356</y>
+     <width>191</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Move Road</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_RotateRoad">
+   <property name="geometry">
+    <rect>
+     <x>1010</x>
+     <y>431</y>
+     <width>191</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Rotate Road</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_MirrorRoad">
+   <property name="geometry">
+    <rect>
+     <x>1010</x>
+     <y>506</y>
+     <width>191</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Mirror Road</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -870,3 +870,148 @@ double xodrfunc::GetAcurateDis(const double x, const double y, Road *pRoad,  con
 
 
 }
+
+
+
+int xodrfunc::GetLineXY(GeometryLine *pline, double soff, double &x, double &y, double &hdg)
+{
+    if(soff<0)return -1;
+
+    hdg = pline->GetHdg();
+    x = pline->GetX() + soff*cos(hdg);
+    y = pline->GetY() + soff*sin(hdg);
+    return 0;
+}
+
+Road * xodrfunc::GetRoadByID(OpenDrive * pxodr,std::string strroadid)
+{
+    Road * pRoad = 0;
+    int nroadcount = pxodr->GetRoadCount();
+    int i;
+    for(i=0;i<nroadcount;i++)
+    {
+        if(pxodr->GetRoad(i)->GetRoadId() == strroadid)
+        {
+            pRoad = pxodr->GetRoad(i);
+            break;
+        }
+    }
+    return pRoad;
+}
+
+int xodrfunc::GetSpiralXY(GeometrySpiral *pspira, double soff, double &x, double &y, double &hdg)
+{
+    pspira->GetCoords(pspira->GetS() + soff,x,y,hdg);
+    return 0;
+}
+
+int xodrfunc::GetArcXY(GeometryArc *parc, double soff, double &x, double &y, double &hdg)
+{
+    if(parc->GetCurvature() == 0)return -1;
+    double R = fabs(1.0/parc->GetCurvature());
+
+    //calculate arc center
+    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
+    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
+
+    double arcdiff = soff/R;
+
+    if(parc->GetCurvature() > 0)
+    {
+        x = x_center + R * cos(parc->GetHdg() + arcdiff - M_PI/2.0);
+        y = y_center + R * sin(parc->GetHdg() + arcdiff - M_PI/2.0);
+        hdg = parc->GetHdg() + arcdiff;
+    }
+    else
+    {
+        x = x_center + R * cos(parc->GetHdg() -arcdiff + M_PI/2.0);
+        y = y_center + R * sin(parc->GetHdg() -arcdiff + M_PI/2.0);
+        hdg = parc->GetHdg() - arcdiff;
+    }
+
+
+
+    return 0;
+}
+
+int xodrfunc::GetParamPoly3XY(GeometryParamPoly3 *pparam3d, double soff, double &x, double &y, double &hdg)
+{
+    double xtem,ytem;
+    double ua,ub,uc,ud,va,vb,vc,vd;
+    ua = pparam3d->GetuA();ub= pparam3d->GetuB();uc= pparam3d->GetuC();ud = pparam3d->GetuD();
+    va = pparam3d->GetvA();vb= pparam3d->GetvB();vc= pparam3d->GetvC();vd = pparam3d->GetvD();
+//        xtem = parc->GetuA() + parc->GetuB() * s * len + parc->GetuC() * s*s *pow(len,2) + parc->GetuD() * s*s*s *pow(len,3);
+//        ytem = parc->GetvA() + parc->GetvB() * s* len + parc->GetvC() * s*s *pow(len,2) + parc->GetvD() * s*s*s *pow(len,3);
+    xtem = ua + ub * soff  + uc * soff * soff  + ud * soff *soff*soff ;
+    ytem = va + vb * soff + vc * soff*soff  + vd * soff*soff*soff ;
+    x = xtem*cos(pparam3d->GetHdg()) - ytem * sin(pparam3d->GetHdg()) + pparam3d->GetX();
+    y = xtem*sin(pparam3d->GetHdg()) + ytem * cos(pparam3d->GetHdg()) + pparam3d->GetY();
+    if(soff<0.3)hdg = pparam3d->GetHdg();
+    else
+    {
+        double soff1 = soff - 0.1;
+        double x1,y1;
+        xtem = ua + ub * soff1  + uc * soff1 * soff1  + ud * soff1 *soff1*soff1 ;
+        ytem = va + vb * soff1 + vc * soff1*soff1  + vd * soff1*soff1*soff1 ;
+        x1 = xtem*cos(pparam3d->GetHdg()) - ytem * sin(pparam3d->GetHdg()) + pparam3d->GetX();
+        y1 = xtem*sin(pparam3d->GetHdg()) + ytem * cos(pparam3d->GetHdg()) + pparam3d->GetY();
+        hdg = CalcHdg(QPointF(x1,y1),QPointF(x,y));
+    }
+    return 0;
+}
+
+int xodrfunc::GetRoadXYByS(Road *pRoad, const double s, double &x, double &y, double &hdg)
+{
+    if(s<0)return -1;
+    if(s>(pRoad->GetRoadLength()+0.1))return -2;
+    if(pRoad == 0)return -3;
+    if(pRoad->GetGeometryBlockCount()<1)return -4;
+    int i;
+    int nroadgeosize = pRoad->GetGeometryBlockCount();
+    RoadGeometry * pgeosel = pRoad->GetGeometryBlock(nroadgeosize -1)->GetGeometryAt(0);
+    for(i=0;i<(nroadgeosize-1);i++)
+    {
+        if(s<pRoad->GetGeometryBlock(i+1)->GetGeometryAt(0)->GetS())
+        {
+
+            pgeosel = pRoad->GetGeometryBlock(0)->GetGeometryAt(0);
+            break;
+        }
+    }
+
+
+    switch (pgeosel->GetGeomType()) {
+    case 0:
+        return GetLineXY((GeometryLine *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
+        break;
+    case 1:
+        return GetSpiralXY((GeometrySpiral *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
+        break;
+    case 2:
+        return GetArcXY((GeometryArc *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
+
+        break;
+    case 3:
+        break;
+    case 4:
+        return GetParamPoly3XY((GeometryParamPoly3 *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
+        break;
+    default:
+        break;
+    }
+    return -5;
+}
+
+int xodrfunc::GetRoadIndex(OpenDrive * pxodr, Road *pRoad)
+{
+    int nroadcount = pxodr->GetRoadCount();
+    int i;
+    for(i=0;i<nroadcount;i++)
+    {
+        if(pxodr->GetRoad(i) == pRoad)
+        {
+            return i;
+        }
+    }
+    return -1;
+}

src/tool/map_lanetoxodr/xodrfunc.h

@@ -49,6 +49,17 @@ public:
                                 const double neary,const double nearhead,int * pnlane= 0);
 
     static std::vector<iv::LanePoint> GetAllLanePoint(Road * pRoad,const double s,const double x, const double y,const double fhdg);
+
+public:
+    static Road * GetRoadByID(OpenDrive * pxodr,std::string strroadid);
+    static int GetRoadXYByS(Road * pRoad,const double s,double &x, double & y, double & hdg);
+
+    static int GetLineXY(GeometryLine * pline,double soff,double &x, double & y, double & hdg);
+    static int GetSpiralXY(GeometrySpiral * pspira,double soff,double &x, double & y, double & hdg);
+    static int GetArcXY(GeometryArc * parc,double soff,double &x, double & y, double & hdg);
+    static int GetParamPoly3XY(GeometryParamPoly3 * pparam3d,double soff,double &x, double & y, double & hdg);
+
+    static int GetRoadIndex(OpenDrive * pxodr, Road * pRoad);
 };
 
 #endif // XODRFUNC_H