Merge branch 'master' of http://47.96.250.93:3000/adc_pilot/modularization

src/tool/map_lanetoxodr/dialogeditlane.cpp

@@ -115,30 +115,116 @@ void DialogEditLane::on_comboBox_Width_currentIndexChanged(int index)
 void DialogEditLane::on_pushButton_AddLeftLane_clicked()
 {
     if(mpCurLS == 0)return;
+    std::string strtype = ui->comboBox_LaneType->currentText().toStdString();
+    mpCurLS->AddLane(1,mpCurLS->GetLeftLaneCount()+1,strtype,false);
+    Lane * pLane = mpCurLS->GetLastLeftLane();
+    if(pLane != 0)
+    {
+        double soffoset = ui->lineEdit_sOffset->text().toDouble();
+        double a = ui->lineEdit_a->text().toDouble();
+        double b = ui->lineEdit_b->text().toDouble();
+        double c = ui->lineEdit_c->text().toDouble();
+        double d = ui->lineEdit_d->text().toDouble();
+        pLane->AddWidthRecord(soffoset,a,b,c,d);
+    }
+    on_comboBox_LaneSection_currentIndexChanged(ui->comboBox_LaneSection->currentIndex());
  //   mpCurLS->AddLane()
 }
 
 void DialogEditLane::on_pushButton_AddRIghtLane_clicked()
 {
-
+    if(mpCurLS == 0)return;
+    std::string strtype = ui->comboBox_LaneType->currentText().toStdString();
+    mpCurLS->AddLane(-1,(-1)*(mpCurLS->GetRightLaneCount()+1),strtype,false);
+    Lane * pLane = mpCurLS->GetLastRightLane();
+    if(pLane != 0)
+    {
+        double soffoset = ui->lineEdit_sOffset->text().toDouble();
+        double a = ui->lineEdit_a->text().toDouble();
+        double b = ui->lineEdit_b->text().toDouble();
+        double c = ui->lineEdit_c->text().toDouble();
+        double d = ui->lineEdit_d->text().toDouble();
+        pLane->AddWidthRecord(soffoset,a,b,c,d);
+    }
+    on_comboBox_LaneSection_currentIndexChanged(ui->comboBox_LaneSection->currentIndex());
 }
 
 void DialogEditLane::on_pushButton_DeleteLane_clicked()
 {
+    int index = ui->comboBox_Lane->currentText().toInt();
+    if(index == 0)
+    {
+        QMessageBox::warning(this,"Warning","Can't delete center Lane.",QMessageBox::YesAll);
+        return;
+    }
+
+    if(mpCurLS == 0)return;
+    int nlanecount = mpCurLS->GetLaneCount();
+    int i;
+    bool bFind = false;
+    for(i=0;i<nlanecount;i++)
+    {
+        if(mpCurLS->GetLane(i)->GetId() == index)
+        {
+            bFind = true;
+            break;
+        }
+    }
+    mpCurLS->DeleteLane(i);
 
+    on_comboBox_LaneSection_currentIndexChanged(ui->comboBox_LaneSection->currentIndex());
 }
 
 void DialogEditLane::on_pushButton_AddLaneWidth_clicked()
 {
-
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(ui->comboBox_Lane->currentIndex());
+    if(pLane != 0)
+    {
+        double soffoset = ui->lineEdit_sOffset->text().toDouble();
+        double a = ui->lineEdit_a->text().toDouble();
+        double b = ui->lineEdit_b->text().toDouble();
+        double c = ui->lineEdit_c->text().toDouble();
+        double d = ui->lineEdit_d->text().toDouble();
+        pLane->AddWidthRecord(soffoset,a,b,c,d);
+    }
+   on_comboBox_Lane_currentIndexChanged(ui->comboBox_Lane->currentIndex());
 }
 
 void DialogEditLane::on_pushButton_ChangeLaneWidth_clicked()
-{
+{    
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(ui->comboBox_Lane->currentIndex());
+    if(pLane != 0)
+    {
+        LaneWidth * pLaneWidth = pLane->GetLaneWidth(ui->comboBox_Width->currentIndex());
+        if(pLaneWidth != 0)
+        {
+            pLaneWidth->SetS(ui->lineEdit_sOffset->text().toDouble());
+            pLaneWidth->SetA(ui->lineEdit_a->text().toDouble());
+            pLaneWidth->SetB(ui->lineEdit_b->text().toDouble());
+            pLaneWidth->SetC(ui->lineEdit_c->text().toDouble());
+            pLaneWidth->SetD(ui->lineEdit_d->text().toDouble());
+        }
 
+    }
+    on_comboBox_Width_currentIndexChanged(ui->comboBox_Width->currentIndex());
 }
 
 void DialogEditLane::on_pushButton_DeleteLaneWidth_clicked()
 {
-
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(ui->comboBox_Lane->currentIndex());
+    if(pLane != 0)
+    {
+        if(pLane->GetLaneWidthCount() <= 1)
+        {
+            QMessageBox::warning(this,"Warning","Don't remove all lane width.",QMessageBox::YesAll);
+        }
+        else
+        {
+            pLane->DeleteLaneWidth(ui->comboBox_Width->currentIndex());
+        }
+    }
+    on_comboBox_Lane_currentIndexChanged(ui->comboBox_Lane->currentIndex());
 }

src/tool/map_lanetoxodr/dialogeditlane.h

@@ -5,6 +5,8 @@
 
 #include "OpenDrive/OpenDrive.h"
 
+#include <QMessageBox>
+
 namespace Ui {
 class DialogEditLane;
 }

src/tool/map_lanetoxodr/dialogeditroadmark.cpp

@@ -0,0 +1,187 @@
+#include "dialogeditroadmark.h"
+#include "ui_dialogeditroadmark.h"
+
+
+const std::string roadcolorchoice[] = {"standard","blue","green","red","white","yellow","orange"};
+const std::string lanechangechoice[] = {"both","none","increase","decrease"};
+const std::string marktypechoice[] = {"none","solid","broken","solid solid","solid broken",
+                                     "broken solid","broken broken"};
+
+
+DialogEditRoadMark::DialogEditRoadMark(Road * pRoad,QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::DialogEditRoadMark)
+{
+    mpRoad = pRoad;
+    ui->setupUi(this);
+
+    int i;
+    for(i=0;i<7;i++)ui->comboBox_color->addItem(QString(roadcolorchoice[i].data()));
+    for(i=0;i<4;i++)ui->comboBox_laneChange->addItem(QString(lanechangechoice[i].data()));
+    for(i=0;i<7;i++)ui->comboBox_MarkType->addItem(QString(marktypechoice[i].data()));
+    ui->comboBox_MarkType->setCurrentIndex(1);
+    ui->comboBox_weight->addItem("standard");
+    ui->comboBox_weight->addItem("bold");
+
+    int nsection = pRoad->GetLaneSectionCount();
+    for(i=0;i<nsection;i++)
+    {
+        LaneSection * pLS =  pRoad->GetLaneSection(i);
+        QString stritemname = QString("s=") + QString::number(pLS->GetS());
+        ui->comboBox_LaneSection->addItem(stritemname);
+    }
+    if(nsection > 0)on_comboBox_LaneSection_currentIndexChanged(0);
+
+}
+
+DialogEditRoadMark::~DialogEditRoadMark()
+{
+    delete ui;
+}
+
+void DialogEditRoadMark::on_comboBox_LaneSection_currentIndexChanged(int index)
+{
+    if(mpRoad == 0)return;
+    LaneSection * pLS = mpRoad->GetLaneSection(index);
+    ui->comboBox_Lane->clear();
+    mpCurLS = pLS;
+    if(pLS == 0)return;
+    int nLaneCount = pLS->GetLaneCount();
+
+    int i;
+    for(i=0;i<nLaneCount;i++)
+    {
+        Lane * pLane = pLS->GetLane(i);
+        QString stritemname = QString::number(pLane->GetId());
+        ui->comboBox_Lane->addItem(stritemname);
+    }
+
+    if(nLaneCount > 0)on_comboBox_Lane_currentIndexChanged(0);
+}
+
+void DialogEditRoadMark::on_comboBox_Lane_currentIndexChanged(int index)
+{
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(index);
+    ui->comboBox_Mark->clear();
+    mpCurLane = pLane;
+    if(pLane == 0)return;
+    ui->comboBox_Mark->clear();
+    int nRoadMarkCount = pLane->GetLaneRoadMarkCount();
+    if(nRoadMarkCount == 0)
+    {
+        ui->lineEdit_sOffset->setText("");
+        ui->lineEdit_width->setText("");
+        return;
+    }
+    int i;
+    for(i=0;i<nRoadMarkCount;i++)
+    {
+        ui->comboBox_Mark->addItem(QString("s=")+QString::number(pLane->GetLaneRoadMark(i)->GetS()));
+    }
+    on_comboBox_Mark_currentIndexChanged(0);
+}
+
+void DialogEditRoadMark::on_comboBox_Mark_currentIndexChanged(int index)
+{
+    if(mpCurLane == 0)return;
+    LaneRoadMark * pLaneRoadMark = mpCurLane->GetLaneRoadMark(index);
+    if(pLaneRoadMark == 0)return;
+    ui->comboBox_MarkType->setCurrentIndex(1);
+    std::string strmarktype = pLaneRoadMark->GetType();
+    int i;
+    const int typechoicecount = 7;
+    for(i=0;i<typechoicecount;i++)
+    {
+        if(strmarktype == marktypechoice[i])break;
+    }
+    if(i<typechoicecount)ui->comboBox_MarkType->setCurrentIndex(i);
+    else ui->comboBox_MarkType->setCurrentIndex(1);
+
+    std::string strlanechange = pLaneRoadMark->GetLaneChange();
+    const int lanechagnechoicecount = 4;
+    for(i=0;i<lanechagnechoicecount;i++)
+    {
+        if(strlanechange == lanechangechoice[i])break;
+    }
+    if(i<lanechagnechoicecount)ui->comboBox_laneChange->setCurrentIndex(i);
+    else ui->comboBox_laneChange->setCurrentIndex(0);
+
+    std::string strweight = pLaneRoadMark->GetWeight();
+    if(strweight == "bold")ui->comboBox_weight->setCurrentIndex(1);
+    else ui->comboBox_weight->setCurrentIndex(0);
+
+    std::string strcolor = pLaneRoadMark->GetColor();
+    const int lanemarkcolorcount = 7;
+    for(i=0;i<lanemarkcolorcount;i++)
+    {
+        if(strcolor == roadcolorchoice[i])break;
+    }
+    if(i<lanemarkcolorcount)ui->comboBox_color->setCurrentIndex(i);
+    else ui->comboBox_color->setCurrentIndex(0);
+
+    ui->lineEdit_sOffset->setText(QString::number(pLaneRoadMark->GetS()));
+    ui->lineEdit_width->setText(QString::number(pLaneRoadMark->GetWidth()));
+
+
+
+
+}
+
+void DialogEditRoadMark::on_pushButton_AddLaneRoadMark_clicked()
+{
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(ui->comboBox_Lane->currentIndex());
+    if(pLane != 0)
+    {
+        double soffset = ui->lineEdit_sOffset->text().toDouble();
+        double fwidth = ui->lineEdit_width->text().toDouble();
+        if(fwidth<=0)fwidth = 0.15;
+        std::string strcolor = ui->comboBox_color->currentText().toStdString();
+        std::string strlanechagne = ui->comboBox_laneChange->currentText().toStdString();
+        std::string strmarktype = ui->comboBox_MarkType->currentText().toStdString();
+        std::string strweight = ui->comboBox_weight->currentText().toStdString();
+        pLane->AddRoadMarkRecord(soffset,strmarktype,strweight,strcolor,fwidth,strlanechagne);
+
+    }
+    on_comboBox_Lane_currentIndexChanged(ui->comboBox_Lane->currentIndex());
+}
+
+void DialogEditRoadMark::on_pushButton_ChangeLaneRoadMark_clicked()
+{
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(ui->comboBox_Lane->currentIndex());
+    if(pLane != 0)
+    {
+        LaneRoadMark * pLaneRoadMark = pLane->GetLaneRoadMark(ui->comboBox_Mark->currentIndex());
+        if(pLaneRoadMark != 0)
+        {
+            pLaneRoadMark->SetColor(ui->comboBox_color->currentText().toStdString());
+            pLaneRoadMark->SetLaneChange(ui->comboBox_laneChange->currentText().toStdString());
+            pLaneRoadMark->SetS(ui->lineEdit_sOffset->text().toDouble());
+            double fwidth = ui->lineEdit_width->text().toDouble();
+            if(fwidth<=0)fwidth = 0.15;
+            pLaneRoadMark->SetWidth(fwidth);
+            pLaneRoadMark->SetType(ui->comboBox_MarkType->currentText().toStdString());
+            pLaneRoadMark->SetWeight(ui->comboBox_weight->currentText().toStdString());
+        }
+
+    }
+//    on_comboBox_Width_currentIndexChanged(ui->comboBox_Width->currentIndex());
+}
+
+void DialogEditRoadMark::on_pushButton_DeleteLaneRoadMark_clicked()
+{
+    if(mpCurLS == 0)return;
+    Lane * pLane = mpCurLS->GetLane(ui->comboBox_Lane->currentIndex());
+    if(pLane != 0)
+    {
+        LaneRoadMark * pLaneRoadMark = pLane->GetLaneRoadMark(ui->comboBox_Mark->currentIndex());
+        if(pLaneRoadMark != 0)
+        {
+            pLane->DeleteLaneRoadMark(ui->comboBox_Mark->currentIndex());
+        }
+
+    }
+    on_comboBox_Lane_currentIndexChanged(ui->comboBox_Lane->currentIndex());
+}

src/tool/map_lanetoxodr/dialogeditroadmark.h

@@ -0,0 +1,43 @@
+#ifndef DIALOGEDITROADMARK_H
+#define DIALOGEDITROADMARK_H
+
+#include <QDialog>
+
+#include "OpenDrive/OpenDrive.h"
+
+#include <QMessageBox>
+
+namespace Ui {
+class DialogEditRoadMark;
+}
+
+class DialogEditRoadMark : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogEditRoadMark(Road * pRoad,QWidget *parent = nullptr);
+    ~DialogEditRoadMark();
+
+private slots:
+    void on_comboBox_LaneSection_currentIndexChanged(int index);
+
+    void on_comboBox_Lane_currentIndexChanged(int index);
+
+    void on_comboBox_Mark_currentIndexChanged(int index);
+
+    void on_pushButton_AddLaneRoadMark_clicked();
+
+    void on_pushButton_ChangeLaneRoadMark_clicked();
+
+    void on_pushButton_DeleteLaneRoadMark_clicked();
+
+private:
+    Ui::DialogEditRoadMark *ui;
+    Road * mpRoad;
+    LaneSection * mpCurLS = 0;
+    Lane * mpCurLane = 0;
+    LaneRoadMark * mpCurLaneRoadMark = 0;
+};
+
+#endif // DIALOGEDITROADMARK_H

src/tool/map_lanetoxodr/dialogeditroadmark.ui

@@ -0,0 +1,265 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogEditRoadMark</class>
+ <widget class="QDialog" name="DialogEditRoadMark">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>746</width>
+    <height>423</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QLabel" name="label_4">
+   <property name="geometry">
+    <rect>
+     <x>33</x>
+     <y>213</y>
+     <width>91</width>
+     <height>21</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>sOffset</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_8">
+   <property name="geometry">
+    <rect>
+     <x>249</x>
+     <y>263</y>
+     <width>91</width>
+     <height>20</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>laneChange</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_DeleteLaneRoadMark">
+   <property name="geometry">
+    <rect>
+     <x>513</x>
+     <y>337</y>
+     <width>181</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Delete Lane RoadMark</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_7">
+   <property name="geometry">
+    <rect>
+     <x>30</x>
+     <y>264</y>
+     <width>81</width>
+     <height>20</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>width</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_sOffset">
+   <property name="geometry">
+    <rect>
+     <x>120</x>
+     <y>206</y>
+     <width>111</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_AddLaneRoadMark">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>337</y>
+     <width>181</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Add Lane RoadMark</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_Lane">
+   <property name="geometry">
+    <rect>
+     <x>160</x>
+     <y>86</y>
+     <width>221</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_MarkType">
+   <property name="geometry">
+    <rect>
+     <x>480</x>
+     <y>146</y>
+     <width>221</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_9">
+   <property name="geometry">
+    <rect>
+     <x>420</x>
+     <y>155</y>
+     <width>67</width>
+     <height>17</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Type</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_2">
+   <property name="geometry">
+    <rect>
+     <x>30</x>
+     <y>96</y>
+     <width>67</width>
+     <height>17</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Lane</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_width">
+   <property name="geometry">
+    <rect>
+     <x>120</x>
+     <y>256</y>
+     <width>111</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_3">
+   <property name="geometry">
+    <rect>
+     <x>30</x>
+     <y>156</y>
+     <width>81</width>
+     <height>17</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>roadMark</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>30</x>
+     <y>36</y>
+     <width>151</width>
+     <height>21</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Lane Section</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_ChangeLaneRoadMark">
+   <property name="geometry">
+    <rect>
+     <x>280</x>
+     <y>337</y>
+     <width>171</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Change Lane RoadMark</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_LaneSection">
+   <property name="geometry">
+    <rect>
+     <x>160</x>
+     <y>26</y>
+     <width>221</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_6">
+   <property name="geometry">
+    <rect>
+     <x>499</x>
+     <y>212</y>
+     <width>71</width>
+     <height>21</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>color</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_Mark">
+   <property name="geometry">
+    <rect>
+     <x>160</x>
+     <y>146</y>
+     <width>221</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_5">
+   <property name="geometry">
+    <rect>
+     <x>250</x>
+     <y>210</y>
+     <width>91</width>
+     <height>21</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>weight</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_weight">
+   <property name="geometry">
+    <rect>
+     <x>350</x>
+     <y>210</y>
+     <width>131</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_color">
+   <property name="geometry">
+    <rect>
+     <x>570</x>
+     <y>210</y>
+     <width>141</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_laneChange">
+   <property name="geometry">
+    <rect>
+     <x>350</x>
+     <y>260</y>
+     <width>131</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -30,6 +30,7 @@ QMAKE_LFLAGS += -no-pie
 SOURCES += \
     autoconnect.cpp \
     dialogeditlane.cpp \
+    dialogeditroadmark.cpp \
     ivxodrtool.cpp \
         main.cpp \
         mainwindow.cpp \
@@ -70,6 +71,7 @@ SOURCES += \
 HEADERS += \
     autoconnect.h \
     dialogeditlane.h \
+    dialogeditroadmark.h \
     ivxodrtool.h \
         mainwindow.h \
     rawtype.h \
@@ -106,6 +108,7 @@ HEADERS += \
 
 FORMS += \
         dialogeditlane.ui \
+        dialogeditroadmark.ui \
         mainwindow.ui \
         roadeditdialog.ui \
         speeddialog.ui \

src/tool/map_lanetoxodr/roadeditdialog.cpp

@@ -17,7 +17,7 @@ RoadEditDialog::RoadEditDialog(OpenDrive * pxodr,QWidget *parent) :
     myview->setObjectName(QStringLiteral("graphicsView"));
     myview->setGeometry(QRect(30, 300, 900, 500));
 
-    connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));
+ //   connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));
 
     image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
     myview->setCacheMode(myview->CacheBackground);
@@ -406,4 +406,17 @@ void RoadEditDialog::on_pushButton_EditLane_clicked()
     }
     DialogEditLane laned(mpCurRoad,this);
     int res = laned.exec();
+    on_comboBox_Road_currentIndexChanged(ui->comboBox_Road->currentIndex());
+}
+
+void RoadEditDialog::on_pushButton_EditRoadMark_clicked()
+{
+    if(mpCurRoad == 0)
+    {
+        QMessageBox::warning(this,"Warning","Not Select Road");
+        return;
+    }
+    DialogEditRoadMark roadmarkd(mpCurRoad,this);
+    roadmarkd.exec();
+    on_comboBox_Road_currentIndexChanged(ui->comboBox_Road->currentIndex());
 }

src/tool/map_lanetoxodr/roadeditdialog.h

@@ -12,6 +12,7 @@
 #include "roadviewitem.h"
 
 #include "dialogeditlane.h"
+#include "dialogeditroadmark.h"
 
 namespace Ui {
 class RoadEditDialog;
@@ -39,6 +40,8 @@ private slots:
 
     void on_pushButton_EditLane_clicked();
 
+    void on_pushButton_EditRoadMark_clicked();
+
 private:
     Ui::RoadEditDialog *ui;
     OpenDrive * mpxodr;

src/tool/map_lanetoxodr/roadeditdialog.ui

@@ -233,6 +233,19 @@
     <string>Edit Lane</string>
    </property>
   </widget>
+  <widget class="QPushButton" name="pushButton_EditRoadMark">
+   <property name="geometry">
+    <rect>
+     <x>350</x>
+     <y>210</y>
+     <width>181</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Edit RoadMark</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/roadviewitem.cpp

@@ -0,0 +1,944 @@
+#include "roadviewitem.h"
+
+#include <math.h>
+
+#include "geofit.h"
+
+#include <QtWidgets>
+
+#include <QFont>
+
+roadviewitem::roadviewitem(Road * pRoad)
+{
+    mpRoad = pRoad;
+    mbNeedCalc = true;
+}
+
+QRectF roadviewitem::boundingRect() const
+{
+//    qDebug("ratio is %f",mfratio);
+//    return QRectF((mfxmin - mfwidth - 15)*mfratio,(mfymin-mfwidth-15)*mfratio,(mfxmax - mfxmin + 2.0*mfwidth+30)*mfratio,
+//                  (mfymax - mfymin+2.0*mfwidth+30)*mfratio);
+    return QRectF( - 10000,-10000 ,20000,
+                  20000);
+ //   return QRectF(-10000,-10000,20000,20000);
+//    float fwidth = mfwidth * mfratio;
+//    return QRectF(mfxmin - fwidth - 15,mfymin-fwidth-15,mfxmax - mfxmin + 2.0*fwidth+30,
+//                  mfymax - mfymin+2.0*fwidth+30);
+    return QRectF(mfxmin - 10000,mfymin-10000,mfxmax - mfxmin + 20000,
+                  mfymax - mfymin+20000);
+
+
+ //   return QRectF(mfxmin - fwidth - 15,mfymin-fwidth-15,mfxmax - mfxmin + 2.0*fwidth+30,mfymax - mfymin+2.0*fwidth+30);
+//    return QRectF(-10000,-10000,20000,20000);
+}
+
+void roadviewitem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
+{
+    (void)option;
+
+    int i;
+
+//    if((mfratio/mfoldcalratio>3.0)||(mfratio/mfoldcalratio<0.3))
+//    {
+//        mbNeedCalc = true;
+//    }
+
+    if(mfratio != mfoldcalratio)mbNeedCalc = true;
+//    qDebug("painter");
+    Q_UNUSED(option);
+    Q_UNUSED(widget);
+
+    if(mbNeedCalc)
+    {
+        CalcLine();
+        CalcLane();
+    }
+
+    painter->setPen(Qt::NoPen);
+//    painter->setBrush(Qt::darkGray);
+//    painter->drawRect(0,0,mnlen,mnwidth*mfratio);
+
+    if(mbShowLine)
+    {
+        if(mbMark)painter->setBrush(Qt::red);
+        else
+            painter->setBrush(Qt::white);
+
+
+        for(i=0;i<mvectorline.size();i++)
+        {
+            painter->drawPolygon(mvectorline[i].mPoints,4);
+        }
+    }
+
+    if(mbShowLane)
+    {
+        painter->setBrush(Qt::darkGray);
+        int nsize = mvectorlaneview.size();
+        for(i=0;i<nsize;i++)
+        {
+            switch (mvectorlaneview[i].ntype) {
+            case 2:
+                painter->setBrush(Qt::darkGray);
+                break;
+            case 8:
+                painter->setBrush(Qt::red);
+                break;
+            case 9:
+                painter->setBrush(QColor(0xB2,0xB2,0xD6));
+                break;
+            default:
+                painter->setBrush(Qt::darkGreen);
+                break;
+            }
+            painter->drawPolygon(mvectorlaneview[i].mPoints,4);
+        }
+    }
+
+    painter->setBrush(Qt::white);
+
+    int nsize = mvectorlanemark.size();
+    for(i=0;i<nsize;i++)
+    {
+        switch (mvectorlanemark[i].ncolor) {
+        case 0:
+            painter->setBrush(Qt::white);
+            break;
+        case 1:
+            painter->setBrush(Qt::blue);
+            break;
+        case 2:
+            painter->setBrush(Qt::green);
+            break;
+        case 3:
+            painter->setBrush(Qt::red);
+            break;
+        case 4:
+            painter->setBrush(Qt::white);
+            break;
+        case 5:
+            painter->setBrush(Qt::yellow);
+            break;
+        case 6:
+            painter->setBrush(Qt::yellow); //orange use yellow replace
+            break;
+        default:
+            painter->setBrush(Qt::white);
+            break;
+        }
+        painter->drawPolygon(mvectorlanemark[i].mPoints,4);
+        painter->setBrush(Qt::white);
+    }
+
+
+
+    QPen pen;
+    pen.setWidth(1);
+    pen.setColor(Qt::white);
+    painter->setPen(pen);
+
+    QFont font = painter->font();
+    font.setPixelSize(3);
+    painter->setFont(font);
+
+    if(mbShowRoadID)
+    {
+        painter->drawText(mtextx,mtexty,mpRoad->GetRoadId().data());
+    }
+
+    painter->setBrush(Qt::NoBrush);
+    if(mbShowLine)
+    {
+        if(mbMark)
+        {
+            if(mvectorline.size()>0)
+            {
+                painter->drawEllipse(mvectorline[0].mstartx-1.5,mvectorline[0].mstarty-1.5,3,3);
+            }
+        }
+    }
+
+//        QPen pen;
+//        pen.setWidth(1);
+//        pen.setColor(Qt::white);
+//        painter->setPen(pen);
+//        for(i=0;i<mvectorline.size();i++)
+//        {
+//            painter->drawLine(mvectorline[i].mstartx,mvectorline[i].mstarty,
+//                              mvectorline[i].mendx,mvectorline[i].mendy);
+//        }
+ //       painter->drawLine(1,0,mnlen-1,0);
+
+
+}
+
+
+bool roadviewitem::IsDrawMark(double s)
+{
+    const double dotdis = 10.0;
+    const double dotlen = 5.0;
+
+    double  y = fmod(s,dotdis);
+    if(y>dotlen)return true;
+    else
+    {
+        return false;
+    }
+}
+
+void roadviewitem::CalcLane()
+{
+    if(mbShowLane == false)return;
+    int i,j;
+    int nsize = mvectorline.size();
+    mvectorlaneview.clear();
+    mvectorlanemark.clear();
+
+    double flmw = 0.15;
+
+
+    for(i=0;i<mvectorlaneline.size();i++)
+    {
+        for(j=0;j<(mvectorlaneline[i].mvectorlanelinese.size()-1);j++)
+        {
+            std::vector<iv::LanePoint> xvepre = mvectorlaneline[i].mvectorlanelinese[j].mvectorlanepoint;
+            std::vector<iv::LanePoint> xvenxt = mvectorlaneline[i].mvectorlanelinese[j+1].mvectorlanepoint;
+            if((xvepre.size()<2)||(xvenxt.size()<2)||(xvenxt.size() != xvepre.size()))
+            {
+                continue;
+            }
+            int k;
+            for(k=0;k<(xvepre.size()-1);k++)
+            {
+
+                iv::laneviewdata lv;
+                lv.mPoints[0].setX(xvepre[k].mfX);
+                lv.mPoints[0].setY(xvepre[k].mfY*(-1.0));
+                lv.mPoints[1].setX(xvenxt[k].mfX);
+                lv.mPoints[1].setY(xvenxt[k].mfY*(-1.0));
+                lv.mPoints[2].setX(xvenxt[k+1].mfX);
+                lv.mPoints[2].setY(xvenxt[k+1].mfY*(-1.0));
+                lv.mPoints[3].setX(xvepre[k+1].mfX);
+                lv.mPoints[3].setY(xvepre[k+1].mfY*(-1.0));
+                if(xvepre[k].mnlane>0)lv.ntype = xvepre[k].mnlanetype;
+                else lv.ntype = xvepre[k+1].mnlanetype;
+                mvectorlaneview.push_back(lv);
+            }
+
+
+        }
+    }
+
+
+    for(i=0;i<mvectorlaneline.size();i++)
+    {
+        for(j=0;j<(mvectorlaneline[i].mvectorlanelinese.size()-1);j++)
+        {
+            std::vector<iv::LanePoint> xvepre = mvectorlaneline[i].mvectorlanelinese[j].mvectorlanepoint;
+            std::vector<iv::LanePoint> xvenxt = mvectorlaneline[i].mvectorlanelinese[j+1].mvectorlanepoint;
+            if((xvepre.size()<2)||(xvenxt.size()<2)||(xvenxt.size() != xvepre.size()))
+            {
+                continue;
+            }
+            int k;
+            for(k=0;k<(xvepre.size());k++)
+            {
+                iv::lanemark lv;
+                    int nmarktype = xvepre[k].mnlanemarktype;
+                    if(nmarktype >= 0)
+                    {
+                    if(nmarktype<2)
+                    {
+                        if((nmarktype == 0)||(IsDrawMark(xvepre[k].mS)))
+                        {
+                            lv.mPoints[0].setX(xvepre[k].mfX + 0.5*flmw * cos(xvenxt[k].mfhdg - M_PI/2.0));
+                            lv.mPoints[0].setY((xvepre[k].mfY+0.5*flmw * sin(xvenxt[k].mfhdg - M_PI/2.0))*(-1.0));
+                            lv.mPoints[1].setX(xvenxt[k].mfX+ 0.5*flmw * cos(xvepre[k].mfhdg - M_PI/2.0));
+                            lv.mPoints[1].setY((xvenxt[k].mfY+0.5*flmw * sin(xvepre[k].mfhdg - M_PI/2.0))*(-1.0));
+
+                            lv.mPoints[2].setX(xvenxt[k].mfX + 0.5*flmw * cos(xvenxt[k].mfhdg + M_PI/2.0));
+                            lv.mPoints[2].setY((xvenxt[k].mfY+0.5*flmw * sin(xvenxt[k].mfhdg + M_PI/2.0))*(-1.0));
+                            lv.mPoints[3].setX(xvepre[k].mfX+ 0.5*flmw * cos(xvepre[k].mfhdg + M_PI/2.0));
+                            lv.mPoints[3].setY((xvepre[k].mfY+0.5*flmw * sin(xvepre[k].mfhdg + M_PI/2.0))*(-1.0));
+                            lv.ncolor = xvepre[k].mnlanecolor;
+                            mvectorlanemark.push_back(lv);
+                        }
+                    }
+                    else
+                    {
+                        if((nmarktype == 2)||(nmarktype == 3)||(IsDrawMark(xvepre[k].mS)))
+                        {
+                            lv.mPoints[0].setX(xvepre[k].mfX + flmw * cos(xvenxt[k].mfhdg + M_PI/2.0));
+                            lv.mPoints[0].setY((xvepre[k].mfY+flmw * sin(xvenxt[k].mfhdg + M_PI/2.0))*(-1.0));
+                            lv.mPoints[1].setX(xvenxt[k].mfX+ flmw * cos(xvepre[k].mfhdg + M_PI/2.0));
+                            lv.mPoints[1].setY((xvenxt[k].mfY+flmw * sin(xvepre[k].mfhdg + M_PI/2.0))*(-1.0));
+
+                            lv.mPoints[2].setX(xvenxt[k].mfX + 2*flmw * cos(xvenxt[k].mfhdg + M_PI/2.0));
+                            lv.mPoints[2].setY((xvenxt[k].mfY+2*flmw * sin(xvenxt[k].mfhdg + M_PI/2.0))*(-1.0));
+                            lv.mPoints[3].setX(xvepre[k].mfX+ 2*flmw * cos(xvepre[k].mfhdg + M_PI/2.0));
+                            lv.mPoints[3].setY((xvepre[k].mfY+2*flmw * sin(xvepre[k].mfhdg + M_PI/2.0))*(-1.0));
+                            lv.ncolor = xvepre[k].mnlanecolor;
+                            mvectorlanemark.push_back(lv);
+                        }
+                        if((nmarktype == 2)||(nmarktype == 4)||(IsDrawMark(xvepre[k].mS)))
+                        {
+                            lv.mPoints[0].setX(xvepre[k].mfX + flmw * cos(xvenxt[k].mfhdg - M_PI/2.0));
+                            lv.mPoints[0].setY((xvepre[k].mfY+flmw * sin(xvenxt[k].mfhdg - M_PI/2.0))*(-1.0));
+                            lv.mPoints[1].setX(xvenxt[k].mfX+ flmw * cos(xvepre[k].mfhdg - M_PI/2.0));
+                            lv.mPoints[1].setY((xvenxt[k].mfY+flmw * sin(xvepre[k].mfhdg - M_PI/2.0))*(-1.0));
+
+                            lv.mPoints[2].setX(xvenxt[k].mfX + 2*flmw * cos(xvenxt[k].mfhdg - M_PI/2.0));
+                            lv.mPoints[2].setY((xvenxt[k].mfY+2*flmw * sin(xvenxt[k].mfhdg - M_PI/2.0))*(-1.0));
+                            lv.mPoints[3].setX(xvepre[k].mfX+ 2*flmw * cos(xvepre[k].mfhdg - M_PI/2.0));
+                            lv.mPoints[3].setY((xvepre[k].mfY+2*flmw * sin(xvepre[k].mfhdg - M_PI/2.0))*(-1.0));
+                            lv.ncolor = xvepre[k].mnlanecolor;
+
+                            mvectorlanemark.push_back(lv);
+                        }
+                    }
+                    }
+
+
+
+
+ //               }
+
+            }
+
+
+        }
+    }
+
+
+    return;
+
+//    qDebug("size is %d ",nsize);
+    for(i=0;i<nsize;i++)
+    {
+//        qDebug("i = %d",i);
+        double fdis = sqrt(pow(mvectorline[i].mendx - mvectorline[i].mstartx,2)+pow(mvectorline[i].mendy - mvectorline[i].mstarty,2));
+        if(fdis == 0)continue;
+        std::vector<iv::LanePoint> xvectorlp1 = xodrfunc::GetAllLanePoint(mpRoad,mvectorline[i].mE - fdis,
+                                                                          mvectorline[i].mstartx,mvectorline[i].mstarty *(-1),mvectorline[i].mfHdg);
+        std::vector<iv::LanePoint> xvectorlp2 = xodrfunc::GetAllLanePoint(mpRoad,mvectorline[i].mE,
+                                                                          mvectorline[i].mendx,mvectorline[i].mendy *(-1),mvectorline[i].mfHdg);
+
+
+        if(xvectorlp1.size() == xvectorlp2.size())
+        {
+            for(j=0;j<(xvectorlp1.size());j++)
+            {
+                iv::laneviewdata lv;
+                iv::LanePoint lpstart1,lpstart2,lpend1,lpend2;
+                int nlane1_index_start,nlane2_index_start;
+                int nlane1_index_end,nlane2_index_end;
+                nlane1_index_start = j;
+                lpstart1 = xvectorlp1[nlane1_index_start];
+                if(lpstart1.mnlane == 0)
+                {
+                    continue;
+                }
+                nlane1_index_end = 1000;
+                nlane2_index_end = 1000;
+
+                if(lpstart1.mnlane>0)nlane2_index_start  = j+1;
+                else nlane2_index_start = j-1;
+                if((nlane2_index_start < 0)||(nlane2_index_start>= xvectorlp1.size()))
+                {
+                    std::cout<<"nlane2_index_start error."<<std::endl;
+                    continue;
+                }
+
+                for(int k=0;k<xvectorlp2.size();k++)
+                {
+                    if(xvectorlp2[k].mnlane == xvectorlp1[nlane1_index_start].mnlane)
+                    {
+                        nlane1_index_end = k;
+                    }
+                    if(xvectorlp2[k].mnlane == xvectorlp1[nlane2_index_start].mnlane)
+                    {
+                        nlane2_index_end = k;
+                    }
+                    if((nlane1_index_end != 1000)&&(nlane2_index_end != 1000))
+                    {
+                        break;
+                    }
+                }
+                if((nlane1_index_end == 1000)||(nlane2_index_end == 1000))
+                {
+                    continue;
+                }
+
+                lpstart1 = xvectorlp1[nlane1_index_start];
+                lpstart2 = xvectorlp1[nlane2_index_start];
+                lpend1 = xvectorlp2[nlane1_index_end];
+                lpend2 = xvectorlp2[nlane2_index_end];
+
+
+                lv.mPoints[0].setX(lpstart1.mfX);
+                lv.mPoints[0].setY(lpstart1.mfY*(-1));
+                lv.mPoints[1].setX(lpstart2.mfX);
+                lv.mPoints[1].setY(lpstart2.mfY*(-1));
+                lv.mPoints[2].setX(lpend2.mfX);
+                lv.mPoints[2].setY(lpend2.mfY*(-1));
+                lv.mPoints[3].setX(lpend1.mfX);
+                lv.mPoints[3].setY(lpend1.mfY*(-1));
+                mvectorlaneview.push_back(lv);
+  //              qDebug("push size is %d ",mvectorlaneview.size());
+
+            }
+        }
+    }
+}
+
+
+void roadviewitem::CalcLine()
+{
+
+//    Road * pRoad = mpRoad;
+    mvectorline.clear();
+
+    if(mbShowLane)
+    {
+        mvectorlaneline.clear();
+    }
+    int j;
+    double S = 0;
+    int nlanesec = 0;
+    double lanesection_s = 0;
+    double lane_s = 0;
+    if(mpRoad->GetLaneSectionCount()<1)
+    {
+        qDebug("no lane,return;");
+        return;
+    }
+    LaneSection * pLS = mpRoad->GetLaneSection(0);
+    lanesection_s = pLS->GetS();
+
+    iv::lanesectionlinese xlsl;
+    int nLastSection;
+    if(mbShowLane)
+    {
+        if(mpRoad->GetGeometryBlockCount() > 0)
+        {
+
+            double fx = mpRoad->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
+            double fy = mpRoad->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
+            double fhdg = mpRoad->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
+            std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(mpRoad,0,fx,fy,fhdg);
+            iv::lanesectionpoint xvecsp;
+            xvecsp.mvectorlanepoint = xveclp;
+            xlsl.mnsec = 0;
+            nLastSection = 0;
+            xlsl.mvectorlanelinese.push_back(xvecsp);
+        }
+    }
+
+    for(j=0;j<mpRoad->GetGeometryBlockCount();j++)
+    {
+        if(nlanesec < (mpRoad->GetLaneSectionCount() -1))
+        {
+            if(mpRoad->GetLaneSection(nlanesec+1)->GetS() <= S)
+            {
+                nlanesec++;
+                pLS = mpRoad->GetLaneSection(nlanesec);
+                lanesection_s = pLS->GetS();
+                lane_s = 0;
+            }
+        }
+
+        GeometryBlock * pgeob = mpRoad->GetGeometryBlock(j);
+        double x,y;
+        double x_center,y_center;
+        double R;
+        RoadGeometry * pg;
+        GeometryArc * parc;
+        GeometryParamPoly3 * ppp3;
+        GeometrySpiral *pSpiral;
+        double rel_x,rel_y,rel_hdg;
+        pg = pgeob->GetGeometryAt(0);
+
+        x = pg->GetX();
+        y = pg->GetY();
+        iv::lineviewdata xline;
+
+
+        switch (pg->GetGeomType()) {
+        case 0:
+            {
+            xline.mstartx = x;
+            xline.mstarty = y *(-1.0);
+            int ncount = pg->GetLength() * 10.0/mfratio;
+            double fstep;
+            if(ncount > 0)fstep = pg->GetLength()/ncount;
+            int i;
+            for(i=1;i<ncount;i++)
+            {
+                double xtem,ytem;
+                xtem = x + (i*fstep)*cos(pg->GetHdg());
+                ytem = y + (i*fstep)*sin(pg->GetHdg());
+                xline.mendx = xtem;
+                xline.mendy = ytem*(-1.0);
+                xline.mE = xline.mE + fstep;
+                xline.mfHdg = pg->GetHdg();
+                CalcPoints(&xline);
+                mvectorline.push_back(xline);
+                if(mbShowLane)
+                {
+                    std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(mpRoad,xline.mE,xline.mendx,xline.mendy*(-1.0),xline.mfHdg);
+                    if(xveclp.size() > 0)
+                    {
+                        if(xveclp[0].mnLaneSection != nLastSection)
+                        {
+                            mvectorlaneline.push_back(xlsl);
+                            iv::lanesectionpoint xvecsp;
+                            xvecsp.mvectorlanepoint = xveclp;
+                            xlsl.mnsec = xveclp[0].mnLaneSection;
+                            nLastSection =xlsl.mnsec;
+                            xlsl.mvectorlanelinese.push_back(xvecsp);
+                        }
+                        else
+                        {
+                            if(xlsl.mvectorlanelinese.size() > 0)
+                            {
+                                if((xlsl.mvectorlanelinese[0].mvectorlanepoint.size() > 0)&&(xlsl.mvectorlanelinese[0].mvectorlanepoint.size() == xveclp.size()))
+                                {
+                                    iv::lanesectionpoint xvecsp;
+                                    xvecsp.mvectorlanepoint = xveclp;
+                                    xlsl.mnsec = xveclp[0].mnLaneSection;
+                                    nLastSection =xlsl.mnsec;
+                                    xlsl.mvectorlanelinese.push_back(xvecsp);
+                                }
+                                else
+                                {
+                                    mvectorlaneline.push_back(xlsl);
+                                    iv::lanesectionpoint xvecsp;
+                                    xvecsp.mvectorlanepoint = xveclp;
+                                    xlsl.mnsec = xveclp[0].mnLaneSection;
+                                    nLastSection =xlsl.mnsec;
+                                    xlsl.mvectorlanelinese.push_back(xvecsp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                xline.mstartx = xtem;
+                xline.mstarty = ytem*(-1.0);
+            }
+            if(ncount > 1)xline.mE = xline.mE + (pg->GetLength() -(ncount -1)*fstep);
+            else xline.mE = xline.mE + pg->GetLength();
+            xline.mendx = x + pg->GetLength() * cos(pg->GetHdg());
+            xline.mendy = (y + pg->GetLength() * sin(pg->GetHdg()))*(-1.0);
+
+//            painter->drawLine(QPoint(x*mnfac,y*mnfac*(-1)),
+//                              QPoint((x + pg->GetLength() * cos(pg->GetHdg()))*mnfac,(y + pg->GetLength() * sin(pg->GetHdg()))*mnfac*(-1)));
+            CalcPoints(&xline);
+            mvectorline.push_back(xline);
+            }
+            break;
+
+        case 1:
+            pSpiral = (GeometrySpiral * )pg;
+            {
+               int ncount = pSpiral->GetLength() * mfratio;
+               if(ncount < 2)ncount = 2;
+               double sstep = pSpiral->GetLength()/((double)ncount);
+               int k;
+               double x0,y0,hdg0,s0;
+               x0 = pSpiral->GetX();
+               y0 = pSpiral->GetY();
+               s0 = pSpiral->GetS();
+               hdg0 = pSpiral->GetHdg() ;
+               pSpiral->GetCoords(s0,rel_x,rel_y,rel_hdg);
+
+               x = rel_x;
+               y = rel_y;
+               xline.mstartx = x;
+               xline.mstarty = y *(-1.0);
+               for(k=1;k<=ncount;k++)
+               {
+                   pSpiral->GetCoords(s0+sstep*k,rel_x,rel_y,rel_hdg);
+
+                   x = rel_x;
+                   y = rel_y;
+                   xline.mendx = x;
+                   xline.mendy = y*(-1.0);
+                   xline.mE = xline.mE + sstep *k;
+                   xline.mfHdg = xodrfunc::CalcHdg(QPointF(xline.mstartx,xline.mstarty*(-1)),QPointF(xline.mendx,xline.mendy*(-1)));
+                   CalcPoints(&xline);
+                   mvectorline.push_back(xline);
+                   if(mbShowLane)
+                   {
+                       std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(mpRoad,xline.mE,xline.mendx,xline.mendy*(-1.0),xline.mfHdg);
+                       if(xveclp.size() > 0)
+                       {
+                           if(xveclp[0].mnLaneSection != nLastSection)
+                           {
+                               mvectorlaneline.push_back(xlsl);
+                               iv::lanesectionpoint xvecsp;
+                               xvecsp.mvectorlanepoint = xveclp;
+                               xlsl.mnsec = xveclp[0].mnLaneSection;
+                               nLastSection =xlsl.mnsec;
+                               xlsl.mvectorlanelinese.push_back(xvecsp);
+                           }
+                           else
+                           {
+                               if(xlsl.mvectorlanelinese.size() > 0)
+                               {
+                                   if((xlsl.mvectorlanelinese[0].mvectorlanepoint.size() > 0)&&(xlsl.mvectorlanelinese[0].mvectorlanepoint.size() == xveclp.size()))
+                                   {
+                                       iv::lanesectionpoint xvecsp;
+                                       xvecsp.mvectorlanepoint = xveclp;
+                                       xlsl.mnsec = xveclp[0].mnLaneSection;
+                                       nLastSection =xlsl.mnsec;
+                                       xlsl.mvectorlanelinese.push_back(xvecsp);
+                                   }
+                                   else
+                                   {
+                                       mvectorlaneline.push_back(xlsl);
+                                       iv::lanesectionpoint xvecsp;
+                                       xvecsp.mvectorlanepoint = xveclp;
+                                       xlsl.mnsec = xveclp[0].mnLaneSection;
+                                       nLastSection =xlsl.mnsec;
+                                       xlsl.mvectorlanelinese.push_back(xvecsp);
+                                   }
+                               }
+                           }
+                       }
+                   }
+
+                   xline.mstartx = x;
+                   xline.mstarty = y*(-1.0);
+
+               }
+            }
+
+//                    qDebug("spi");
+            break;
+        case 2:
+            {
+            parc = (GeometryArc *)pg;
+            R = abs(1.0/parc->GetCurvature());
+            if(parc->GetCurvature() > 0)
+            {
+                x_center = pg->GetX() + R *cos(pg->GetHdg() + M_PI/2.0);
+                y_center = pg->GetY() + R * sin(pg->GetHdg() + M_PI/2.0);
+            }
+            else
+            {
+                x_center = pg->GetX() + R *cos(pg->GetHdg() -M_PI/2.0);
+                y_center = pg->GetY() + R * sin(pg->GetHdg() - M_PI/2.0);
+            }
+
+            int k;
+            int ncount = parc->GetLength() * 3.0/mfratio;
+            if(ncount< 2)ncount = 2;
+            double curv = parc->GetCurvature();
+            double hdgstep;
+            double hdg0 = parc->GetHdg();
+            double hdgnow = parc->GetHdg();
+            if(ncount > 0) hdgstep= (parc->GetLength()/R)/ncount;
+
+            double x_draw,y_draw;
+
+            if(curv > 0)
+            {
+                hdgnow =  hdg0 ;
+                x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
+                y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
+            }
+            else
+            {
+                hdgnow = hdg0;
+                x_draw = x_center + R *cos(hdgnow  + M_PI/2.0);
+                y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
+            }
+
+
+            xline.mstartx = x_draw;
+            xline.mstarty = y_draw *(-1.0);
+
+            for(k=1;k<=ncount;k++)
+            {
+
+
+                if(curv > 0)
+                {
+                    hdgnow =  hdg0 + k*hdgstep;
+                    x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
+                    y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
+                }
+                else
+                {
+                    hdgnow = hdg0 - k * hdgstep;
+                    x_draw = x_center + R *cos(hdgnow  + M_PI/2.0);
+                    y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
+                }
+                xline.mendx = x_draw;
+                xline.mendy = y_draw*(-1.0);
+                xline.mE = xline.mE + hdgstep * R;
+                CalcPoints(&xline);
+                xline.mfHdg = xodrfunc::CalcHdg(QPointF(xline.mstartx,xline.mstarty*(-1)),QPointF(xline.mendx,xline.mendy*(-1)));
+                mvectorline.push_back(xline);
+                if(mbShowLane)
+                {
+                    std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(mpRoad,xline.mE,xline.mendx,xline.mendy*(-1.0),xline.mfHdg);
+                    if(xveclp.size() > 0)
+                    {
+                        if(xveclp[0].mnLaneSection != nLastSection)
+                        {
+                            mvectorlaneline.push_back(xlsl);
+                            iv::lanesectionpoint xvecsp;
+                            xvecsp.mvectorlanepoint = xveclp;
+                            xlsl.mnsec = xveclp[0].mnLaneSection;
+                            nLastSection =xlsl.mnsec;
+                            xlsl.mvectorlanelinese.push_back(xvecsp);
+                        }
+                        else
+                        {
+                            if(xlsl.mvectorlanelinese.size() > 0)
+                            {
+                                if((xlsl.mvectorlanelinese[0].mvectorlanepoint.size() > 0)&&(xlsl.mvectorlanelinese[0].mvectorlanepoint.size() == xveclp.size()))
+                                {
+                                    iv::lanesectionpoint xvecsp;
+                                    xvecsp.mvectorlanepoint = xveclp;
+                                    xlsl.mnsec = xveclp[0].mnLaneSection;
+                                    nLastSection =xlsl.mnsec;
+                                    xlsl.mvectorlanelinese.push_back(xvecsp);
+                                }
+                                else
+                                {
+                                    mvectorlaneline.push_back(xlsl);
+                                    iv::lanesectionpoint xvecsp;
+                                    xvecsp.mvectorlanepoint = xveclp;
+                                    xlsl.mnsec = xveclp[0].mnLaneSection;
+                                    nLastSection =xlsl.mnsec;
+                                    xlsl.mvectorlanelinese.push_back(xvecsp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                xline.mstartx = x_draw;
+                xline.mstarty = y_draw*(-1.0);
+            }
+            }
+            break;
+        case 4:
+            {
+            ppp3 = (GeometryParamPoly3 * )pg;
+            int ncount = ppp3->GetLength()* 3.0/ mfratio;
+            if(ncount < 2)ncount = 2;
+            double sstep;
+            if(ncount > 0)sstep = ppp3->GetLength()/ncount;
+            else sstep = 10000.0;
+            double s = 0;
+            double xtem,ytem;
+            xtem = ppp3->GetuA() +  ppp3->GetuB() * s +  ppp3->GetuC() * s*s +  ppp3->GetuD() * s*s*s;
+            ytem = ppp3->GetvA() + ppp3->GetvB() * s + ppp3->GetvC() * s*s + ppp3->GetvD() * s*s*s;
+            x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
+            y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
+            s = s+ sstep;
+            xline.mstartx = x;
+            xline.mstarty = y *(-1.0);
+            while(s <= ppp3->GetLength())
+            {
+
+                xtem = ppp3->GetuA() +  ppp3->GetuB() * s +  ppp3->GetuC() * s*s +  ppp3->GetuD() * s*s*s;
+                ytem = ppp3->GetvA() + ppp3->GetvB() * s + ppp3->GetvC() * s*s + ppp3->GetvD() * s*s*s;
+                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
+                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
+                xline.mendx = x;
+                xline.mendy = y*(-1.0);
+                xline.mE = xline.mE + sstep;
+                CalcPoints(&xline);
+                xline.mfHdg = xodrfunc::CalcHdg(QPointF(xline.mstartx,xline.mstarty*(-1)),QPointF(xline.mendx,xline.mendy*(-1)));
+                mvectorline.push_back(xline);
+                if(mbShowLane)
+                {
+                    std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(mpRoad,xline.mE,xline.mendx,xline.mendy*(-1.0),xline.mfHdg);
+                    if(xveclp.size() > 0)
+                    {
+                        if(xveclp[0].mnLaneSection != nLastSection)
+                        {
+                            mvectorlaneline.push_back(xlsl);
+                            iv::lanesectionpoint xvecsp;
+                            xvecsp.mvectorlanepoint = xveclp;
+                            xlsl.mnsec = xveclp[0].mnLaneSection;
+                            nLastSection =xlsl.mnsec;
+                            xlsl.mvectorlanelinese.push_back(xvecsp);
+                        }
+                        else
+                        {
+                            if(xlsl.mvectorlanelinese.size() > 0)
+                            {
+                                if((xlsl.mvectorlanelinese[0].mvectorlanepoint.size() > 0)&&(xlsl.mvectorlanelinese[0].mvectorlanepoint.size() == xveclp.size()))
+                                {
+                                    iv::lanesectionpoint xvecsp;
+                                    xvecsp.mvectorlanepoint = xveclp;
+                                    xlsl.mnsec = xveclp[0].mnLaneSection;
+                                    nLastSection =xlsl.mnsec;
+                                    xlsl.mvectorlanelinese.push_back(xvecsp);
+                                }
+                                else
+                                {
+                                    mvectorlaneline.push_back(xlsl);
+                                    iv::lanesectionpoint xvecsp;
+                                    xvecsp.mvectorlanepoint = xveclp;
+                                    xlsl.mnsec = xveclp[0].mnLaneSection;
+                                    nLastSection =xlsl.mnsec;
+                                    xlsl.mvectorlanelinese.push_back(xvecsp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                xline.mstartx = x;
+                xline.mstarty = y*(-1.0);
+                s = s+ sstep;
+            }
+            }
+            break;
+        default:
+            break;
+        }
+
+//         painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
+
+    }
+
+    mvectorlaneline.push_back(xlsl);
+
+    CalcMaxMin();
+
+    mbNeedCalc = false;
+    mfoldcalratio = mfratio;
+
+    return;
+
+    if(mvectorline.size()<0)return;
+    if(mvectorline.size() <= 2)
+    {
+        mtextx = (mvectorline[0].mstartx + mvectorline[0].mendx)/2.0;
+        mtexty = (mvectorline[0].mstarty + mvectorline[0].mendy)/2.0;
+    }
+    else
+    {
+        int i;
+        mtextx = mvectorline[0].mendx;
+        mtexty = mvectorline[0].mendy;
+        for(i=0;i<mvectorline.size();i++)
+        {
+            qDebug("run i is %d size is %d ",i,mvectorline.size());
+            if(mvectorline[i].mE >= (mpRoad->GetRoadLength()/2.0))
+            {
+                mtextx = mvectorline[i].mendx;
+                mtexty = mvectorline[i].mendy;
+                break;
+            }
+        }
+    }
+
+
+
+
+
+}
+
+void roadviewitem::setratio(float ratio)
+{
+    mfratio = ratio;
+}
+
+void roadviewitem::CalcPoints(iv::lineviewdata *plinedata)
+{
+    float fwidth = mfwidth * mfratio;
+    double fhdg = geofit::CalcHdg(plinedata->mstartx,plinedata->mstarty,plinedata->mendx,plinedata->mendy);
+    plinedata->mPoints[0].setX(plinedata->mstartx + fwidth * cos(fhdg -M_PI/2.0)/2.0 );
+    plinedata->mPoints[0].setY((plinedata->mstarty + fwidth * sin(fhdg -M_PI/2.0)/2.0 )*(1.0));
+    plinedata->mPoints[1].setX(plinedata->mendx + fwidth * cos(fhdg -M_PI/2.0)/2.0 );
+    plinedata->mPoints[1].setY((plinedata->mendy + fwidth * sin(fhdg -M_PI/2.0)/2.0 )*(1.0));
+    plinedata->mPoints[2].setX(plinedata->mendx + fwidth * cos(fhdg +M_PI/2.0)/2.0 );
+    plinedata->mPoints[2].setY((plinedata->mendy + fwidth * sin(fhdg +M_PI/2.0)/2.0 )*(1.0));
+    plinedata->mPoints[3].setX(plinedata->mstartx + fwidth * cos(fhdg +M_PI/2.0)/2.0 );
+    plinedata->mPoints[3].setY((plinedata->mstarty + fwidth * sin(fhdg +M_PI/2.0)/2.0 )*(1.0));
+
+
+
+
+}
+
+void roadviewitem::CalcMaxMin()
+{
+    if(mvectorline.size()<1)return;
+    mfxmax = mvectorline[0].mstartx;
+    mfxmin = mvectorline[0].mstartx;
+    mfymax = mvectorline[0].mstarty;
+    mfymin = mvectorline[0].mstarty;
+    int i;
+    int nsize = mvectorline.size();
+    if(nsize < 1)return;
+    mtextx = mvectorline[0].mendx;
+    mtexty = mvectorline[0].mendy;
+    bool bFindTextPos = false;
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        x = mvectorline[i].mstartx;
+        y = mvectorline[i].mstarty;
+        if(mfxmin> x)mfxmin = x;
+        if(mfymin > y)mfymin = y;
+        if(mfxmax < x)mfxmax = x;
+        if(mfymax < y)mfymax = y;
+        x = mvectorline[i].mendx;
+        y = mvectorline[i].mendy;
+        if(mfxmin> x)mfxmin = x;
+        if(mfymin > y)mfymin = y;
+        if(mfxmax < x)mfxmax = x;
+        if(mfymax < y)mfymax = y;
+
+        if(bFindTextPos == false)
+        {
+//            double xE = mvectorline[i].mE;
+            if(mvectorline[i].mE >= (mpRoad->GetRoadLength()/2.0))
+            {
+                mtextx = mvectorline[i].mendx;
+                mtexty = mvectorline[i].mendy;
+                bFindTextPos = true;
+            }
+        }
+
+    }
+}
+
+void roadviewitem::setmark(bool bMark)
+{
+    mbMark = bMark;
+}
+
+Road * roadviewitem::GetRoad()
+{
+    return mpRoad;
+}
+
+void roadviewitem::setlaneshow(bool bShow)
+{
+    mbShowLane = bShow;
+    mbNeedCalc = true;
+
+}
+
+void roadviewitem::setroadidshow(bool bShow)
+{
+    mbShowRoadID = bShow;
+//    mbNeedCalc = true;
+}
+
+void roadviewitem::setlineshow(bool bShow)
+{
+    mbShowLine = bShow;
+}

src/tool/map_lanetoxodr/roadviewitem.h

@@ -0,0 +1,108 @@
+#ifndef ROADVIEWITEM_H
+#define ROADVIEWITEM_H
+
+#include <QGraphicsItem>
+
+#include <vector>
+
+#include "OpenDrive/OpenDrive.h"
+
+#include "xodrfunc.h"
+
+
+namespace iv {
+struct lineviewdata
+{
+    double mE;
+    double mstartx;
+    double mstarty;
+    double mendx;
+    double mendy;
+    double mfHdg;
+    QPointF mPoints[4];
+};
+
+}
+namespace iv{
+
+
+
+struct lanesectionpoint
+{
+
+    std::vector<iv::LanePoint> mvectorlanepoint;
+};
+
+struct lanesectionlinese
+{
+    int mnsec = 0;
+    std::vector<iv::lanesectionpoint> mvectorlanelinese;
+};
+
+}
+
+
+namespace iv {
+struct laneviewdata
+{
+    double mS;
+    double mLane;
+    int ntype; //0 driving 1 border 2 none 3 sidewalk
+    QPointF mPoints[4];
+};
+
+}
+
+namespace iv {
+struct lanemark
+{
+    int ncolor = 0;//White. 1 yellow
+    int mnmarktype;
+    QPointF mPoints[4];
+};
+
+}
+
+class roadviewitem : public QGraphicsItem
+{
+public:
+    roadviewitem(Road * pRoad);
+    QRectF boundingRect() const override;
+    void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) override;
+    void setratio(float ratio);
+    void setmark(bool bMark);
+    Road * GetRoad();
+    void setlaneshow(bool bShow);
+    void setlineshow(bool bShow);
+    void setroadidshow(bool bShow);
+
+private:
+    double mfwidth = 1.0;
+    double mfratio = 1.0;
+    Road * mpRoad;
+    std::vector<iv::lineviewdata> mvectorline;
+    std::vector<iv::laneviewdata> mvectorlaneview;
+    std::vector<iv::lanemark> mvectorlanemark;
+    std::vector<iv::lanesectionlinese> mvectorlaneline;
+    void CalcLine();
+    void CalcLane();
+    float mfoldcalratio;
+    bool mbNeedCalc;
+
+    float mfxmin,mfymin,mfxmax,mfymax;
+
+    inline void  CalcPoints(iv::lineviewdata * plinedata);
+    void CalcMaxMin();
+
+    double mtextx;
+    double mtexty;
+    bool mbMark = false;
+
+    bool mbShowLane = true;
+    bool mbShowLine = false;
+    bool mbShowRoadID = true;
+
+    bool IsDrawMark(double s);
+};
+
+#endif // ROADVIEWITEM_H

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -0,0 +1,810 @@
+#include "xodrfunc.h"
+
+#include "limits"
+#include <iostream>
+
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
+#include <Eigen/QR>
+#include <Eigen/SVD>
+
+#include <QDebug>
+#include <QPointF>
+
+xodrfunc::xodrfunc()
+{
+
+}
+
+
+inline double xodrfunc::calcpointdis(QPointF p1,QPointF p2)
+{
+    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
+}
+
+bool xodrfunc::pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
+{
+    double hdg = CalcHdg(poingarc,point1);
+    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
+    else hdg = hdg - M_PI/2.0;
+    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
+    if(hdg < 0)hdg = hdg + 2.0*M_PI;
+
+    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
+
+    double hdgdiff = hdg - parc->GetHdg();
+
+    if(hdgrange >= 0 )
+    {
+        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
+    }
+    else
+    {
+        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
+    }
+
+    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
+    return false;
+
+}
+
+/**
+  * @brief CalcHdg
+  * 计算点0到点1的航向
+  * @param p0        Point 0
+  * @param p1        Point 1
+**/
+double xodrfunc::CalcHdg(QPointF p0, QPointF p1)
+{
+    double x0,y0,x1,y1;
+    x0 = p0.x();
+    y0 = p0.y();
+    x1 = p1.x();
+    y1 = p1.y();
+    if(x0 == x1)
+    {
+        if(y0 < y1)
+        {
+            return M_PI/2.0;
+        }
+        else
+            return M_PI*3.0/2.0;
+    }
+
+    double ratio = (y1-y0)/(x1-x0);
+
+    double hdg = atan(ratio);
+
+    if(ratio > 0)
+    {
+        if(y1 > y0)
+        {
+
+        }
+        else
+        {
+            hdg = hdg + M_PI;
+        }
+    }
+    else
+    {
+        if(y1 > y0)
+        {
+            hdg = hdg + M_PI;
+        }
+        else
+        {
+            hdg = hdg + 2.0*M_PI;
+        }
+    }
+
+    return hdg;
+}
+
+/**
+ * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
+ * @param parc
+ * @param xnow
+ * @param ynow
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+double xodrfunc::GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead,double & frels)
+{
+
+    double s = 0.1;
+    double fdismin = 100000.0;
+    frels = 0;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+
+    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
+    if(fdis<fdismin)
+    {
+        fdismin = fdis;
+        nearhead = parc->GetHdg();
+        nearx = parc->GetX();
+        neary = parc->GetY();
+    }
+
+    while(s < parc->GetLength())
+    {
+
+        double x, y,xtem,ytem;
+        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
+        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
+        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
+        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
+
+        double hdg = CalcHdg(QPointF(xold,yold),QPointF(x,y));
+        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+            frels = s;
+        }
+
+        xold = x;
+        yold = y;
+        s = s+ 0.1;
+    }
+
+    return fdismin;
+
+}
+
+
+/**
+  * @brief GetArcDis
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param parc        pointer to a arc geomery
+  * @param x           current x
+  * @param y           current y
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+**/
+
+double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
+                        double & neary,double & nearhead,double & frels)
+{
+
+    if((parc->GetS()>370)&&(parc->GetS()<370.1))
+    {
+        int a = 1;
+    }
+
+    if(parc->GetCurvature() == 0.0)return 1000.0;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+    frels = 0.0;
+
+    //calculate arc center
+    double x_center,y_center;
+    if(parc->GetCurvature() > 0)
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
+    }
+    else
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
+    }
+
+    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
+
+
+    QPointF arcpoint;
+    arcpoint.setX(x_center);arcpoint.setY(y_center);
+
+    QPointF pointnow;
+    pointnow.setX(x);pointnow.setY(y);
+    QPointF point1,point2;
+    point1.setX(x_center + (R * cos(hdgltoa)));
+    point1.setY(y_center + (R * sin(hdgltoa)));
+    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
+    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
+
+    //calculat dis
+    bool bp1inarc,bp2inarc;
+    bp1inarc =pointinarc(parc,arcpoint,point1);
+    bp2inarc =pointinarc(parc,arcpoint,point2);
+    double fdis[4];
+    fdis[0] = calcpointdis(pointnow,point1);
+    fdis[1] = calcpointdis(pointnow,point2);
+    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
+    QPointF pointend;
+    double hdgrange = parc->GetLength()*parc->GetCurvature();
+    double hdgend = parc->GetHdg() + hdgrange;
+    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
+    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
+
+    if(parc->GetCurvature() >0)
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
+    }
+    else
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
+    }
+
+    fdis[3] = calcpointdis(pointnow,pointend);
+    int indexmin = -1;
+    double fdismin = 1000000.0;
+    if(bp1inarc)
+    {
+        indexmin = 0;fdismin = fdis[0];
+    }
+    if(bp2inarc)
+    {
+        if(indexmin == -1)
+        {
+            indexmin = 1;fdismin = fdis[1];
+        }
+        else
+        {
+            if(fdis[1]<fdismin)
+            {
+                indexmin = 1;fdismin = fdis[1];
+            }
+        }
+    }
+    if(indexmin == -1)
+    {
+        indexmin = 2;fdismin = fdis[2];
+    }
+    else
+    {
+        if(fdis[2]<fdismin)
+        {
+            indexmin = 2;fdismin = fdis[2];
+        }
+    }
+    if(fdis[3]<fdismin)
+    {
+        indexmin = 3;fdismin = fdis[3];
+    }
+
+    double hdgdiff;
+    switch (indexmin) {
+    case 0:
+        nearx = point1.x();
+        neary = point1.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
+
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
+        }
+        while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
+        while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
+        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/abs(parc->GetCurvature()));
+        if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
+        if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
+        frels = hdgdiff * R;
+        break;
+    case 1:
+        nearx = point2.x();
+        neary = point2.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
+        }
+        while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
+        while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
+        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/abs(parc->GetCurvature()));
+        if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
+        if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
+        frels = hdgdiff * R;
+        break;
+    case 2:
+        nearx = parc->GetX();
+        neary = parc->GetY();
+        nearhead = parc->GetHdg();
+        frels = 0;
+        break;
+    case 3:
+        nearx = pointend.x();
+        neary = pointend.y();
+        nearhead = hdgend;
+        frels = parc->GetLength();
+        break;
+    default:
+        std::cout<<"error in arcdis "<<std::endl;
+        break;
+    }
+
+    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
+    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
+    return fdismin;
+}
+
+double xodrfunc::GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead,double & frels)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double fdismin = 100000.0;
+    double s0 = pspiral->GetS();
+
+    frels = 0;
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+
+
+        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+            frels = s;
+        }
+        s = s+0.1;
+    }
+
+    return fdismin;
+}
+
+/**
+ * @brief GetLineDis 获得点到直线Geometry的距离。
+ * @param pline
+ * @param x
+ * @param y
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+double xodrfunc::GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
+                         double & neary,double & nearhead,double & frels)
+{
+    double fRtn = 1000.0;
+
+    double a1,a2,a3,a4,b1,b2;
+    double ratio = pline->GetHdg();
+    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
+    while(ratio<0)ratio = ratio+2.0*M_PI;
+
+    double dis1,dis2,dis3;
+    double x1,x2,x3,y1,y2,y3;
+    x1 = pline->GetX();y1=pline->GetY();
+    if((ratio == 0)||(ratio == M_PI))
+    {
+        a1 = 0;a4=0;
+        a2 = 1;b1= pline->GetY();
+        a3 = 1;b2= x;
+    }
+    else
+    {
+        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
+        {
+            a2=0;a3=0;
+            a1=1,b1=pline->GetX();
+            a4 = 1;b2 = y;
+        }
+        else
+        {
+            a1 = tan(ratio) *(-1.0);
+            a2 = 1;
+            a3 = tan(ratio+M_PI/2.0)*(-1.0);
+            a4 = 1;
+            b1 = a1*pline->GetX() + a2 * pline->GetY();
+            b2 = a3*x+a4*y;
+        }
+    }
+
+    y2 = y1 + pline->GetLength() * sin(ratio);
+    x2 = x1 + pline->GetLength() * cos(ratio);
+
+    Eigen::Matrix2d A;
+    A<<a1,a2,
+            a3,a4;
+    Eigen::Vector2d B(b1,b2);
+
+    Eigen::Vector2d opoint  = A.lu().solve(B);
+
+    x3 = opoint(0);
+    y3 = opoint(1);
+
+    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
+    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
+    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
+
+
+    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
+    {
+ //       std::cout<<" out line"<<std::endl;
+        if(dis1<dis2)
+        {
+            fRtn = dis1;
+            nearx = x1;neary=y1;nearhead = pline->GetHdg();
+        }
+        else
+        {
+            fRtn = dis2;
+            nearx = x2;neary=y2;nearhead = pline->GetHdg();
+        }
+    }
+    else
+    {
+        fRtn = dis3;
+        nearx = x3;neary=y3;nearhead = pline->GetHdg();
+    }
+
+    frels = sqrt(pow(nearx - pline->GetX(),2)+pow(neary - pline->GetY(),2));
+
+    return fRtn;
+
+
+
+}
+
+
+int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad, GeometryBlock **pgeo, double &fdis, double &nearx, double &neary, double &nearhead, const double nearthresh,double * pfs,int * pnlane)
+{
+    double dismin = std::numeric_limits<double>::infinity();
+    fdis = dismin;
+    int i;
+    *pObjRoad = 0;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh,frels;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            int nlane = 1000;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+                break;
+            case 1:
+                dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = 100000.0;
+                break;
+            case 4:
+                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+            if(dis < 100)
+            {
+                double faccuratedis;
+                faccuratedis = GetAcurateDis(x,y,proad,frels+pg->GetS(),nx,ny,nh,&nlane);
+                if(faccuratedis < dis)dis = faccuratedis;
+            }
+            if(dis < dismin)
+            {
+                dismin = dis;
+                nearx = nx;
+                neary = ny;
+                nearhead = nh;
+                fdis = dis;
+                *pObjRoad = proad;
+                *pgeo = pgb;
+                if(pfs != 0)*pfs = frels +pg->GetS();
+                if(pnlane != 0)*pnlane = nlane;
+            }
+        }
+    }
+
+
+    if(fdis > nearthresh)return -1;
+    return 0;
+}
+
+
+std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double s,const double x, const double y,const double fhdg)
+{
+    int i;
+    int nLSCount = pRoad->GetLaneSectionCount();
+    double s_section = 0;
+
+    std::vector<iv::LanePoint> xvectorlanepoint;
+    for(i=0;i<nLSCount;i++)
+    {
+//        if((pRoad->GetRoadId() == "30012")&&(s>35))
+//        {
+//            int a= 1;
+//        }
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(i<(nLSCount -1))
+        {
+            if(pRoad->GetLaneSection(i+1)->GetS()<s)
+            {
+                continue;
+            }
+        }
+        s_section = pLS->GetS();
+        int nlanecount = pLS->GetLaneCount();
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            Lane * pLane = pLS->GetLane(j);
+
+            int nlanemarktype = -1; //default no lanetype
+            int nlanetype = 2; //driving
+            int nlanecolor = 0;
+            int k;
+            double s_lane = 0;
+            for(k=0;k<pLane->GetLaneRoadMarkCount();k++)
+            {
+                 LaneRoadMark * plrm = pLane->GetLaneRoadMark(k);
+                 if(k<(pLane->GetLaneRoadMarkCount()-1))
+                 {
+                     if(pLane->GetLaneRoadMark(k+1)->GetS()<s_lane)
+                     {
+                         continue;
+                     }
+                 }
+                 if(plrm->GetType() == "solid")
+                 {
+                     nlanemarktype = 0;
+                 }
+                 if(plrm->GetType() == "broken")
+                 {
+                     nlanemarktype = 1;
+                 }
+                 if(plrm->GetType() == "solid solid")
+                 {
+                     nlanemarktype = 2;
+                 }
+                 if(plrm->GetType() == "solid broken")
+                 {
+                     nlanemarktype = 3;
+                 }
+                 if(plrm->GetType() == "broken solid")
+                 {
+                     nlanemarktype = 4;
+                 }
+                 if(plrm->GetType() == "broken broken")
+                 {
+                     nlanemarktype = 5;
+                 }
+
+                 if(plrm->GetColor() == "standard")nlanecolor = 0;
+                 if(plrm->GetColor() == "blue")nlanecolor = 1;
+                 if(plrm->GetColor() == "green")nlanecolor = 2;
+                 if(plrm->GetColor() == "red")nlanecolor = 3;
+                 if(plrm->GetColor() == "white")nlanecolor = 4;
+                 if(plrm->GetColor() == "yellow")nlanecolor = 5;
+                 if(plrm->GetColor() == "orange")nlanecolor = 6;
+                 break;
+            }
+
+            if(pLane->GetType() == "shoulder")
+            {
+                nlanetype = 0;
+            }
+            if(pLane->GetType() == "border")
+            {
+                nlanetype = 1;
+            }
+            if(pLane->GetType() == "driving")
+            {
+                nlanetype = 2;
+            }
+            if(pLane->GetType() == "none")
+            {
+                nlanetype = 4;
+            }
+            if(pLane->GetType() == "biking")
+            {
+                nlanetype = 8;
+            }
+            if(pLane->GetType() == "sidewalk")
+            {
+                nlanetype = 9;
+            }
+
+
+            if(pLane->GetId() != 0)
+            {
+
+//                        if((pRoad->GetRoadId() == "10012")&&(pLane->GetId()==1))
+//                        {
+//                            int a= 1;
+//                        }
+
+//                int k;
+//                double s_lane = 0;
+                for(k=0;k<pLane->GetLaneWidthCount();k++)
+                {
+                    if(k<(pLane->GetLaneWidthCount()-1))
+                    {
+                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
+                        {
+                            continue;
+                        }
+                    }
+                    s_lane = pLane->GetLaneWidth(k)->GetS();
+                    break;
+                }
+                LaneWidth * pLW  = pLane->GetLaneWidth(k);
+                if(pLW == 0)
+                {
+                    std::cout<<"not find LaneWidth"<<std::endl;
+                    break;
+                }
+
+
+
+                iv::LanePoint lp;
+                lp.mnlanetype = nlanetype;
+                lp.mnlanemarktype = nlanemarktype;
+                lp.mnlanecolor = nlanecolor;
+                lp.mnlane = pLane->GetId();
+                lp.mnLaneSection = i;
+                double fds = s - s_lane - s_section;
+                lp.mflanewidth = pLW->GetA() + pLW->GetB() * fds
+                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
+                lp.mflanetocenter = 0;
+                lp.mnlanetype = nlanetype;
+                lp.mfhdg = fhdg;
+                lp.mS = s;
+                xvectorlanepoint.push_back(lp);
+
+            }
+            else
+            {
+                iv::LanePoint lp;
+                lp.mnlanetype = nlanetype;
+                lp.mnlanemarktype = nlanemarktype;
+                lp.mnlanecolor = nlanecolor;
+                lp.mnlane = 0;
+                lp.mnLaneSection = i;
+                lp.mflanewidth = 0;
+                lp.mflanetocenter = 0;
+                lp.mfhdg = fhdg;
+                lp.mS = s;
+                xvectorlanepoint.push_back(lp);
+            }
+        }
+
+        for(j=0;j<xvectorlanepoint.size();j++)
+        {
+            int k;
+            for(k=0;k<xvectorlanepoint.size();k++)
+            {
+                if(abs(xvectorlanepoint[k].mnlane)>abs((xvectorlanepoint[j].mnlane)))
+                {
+                    continue;
+                }
+                if(xvectorlanepoint[k].mnlane * xvectorlanepoint[j].mnlane <= 0)
+                {
+                    continue;
+                }
+                xvectorlanepoint[j].mflanetocenter = xvectorlanepoint[j].mflanetocenter + xvectorlanepoint[k].mflanewidth;
+            }
+        }
+
+        for(j=0;j<xvectorlanepoint.size();j++)
+        {
+            if(xvectorlanepoint[j].mnlane < 0)
+            {
+                xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg-M_PI/2.0);
+                xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg-M_PI/2.0);
+            }
+            else
+            {
+                xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg+M_PI/2.0);
+                xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg+M_PI/2.0);
+            }
+        }
+
+        break;
+
+    }
+
+    std::vector<iv::LanePoint> xvectorlanepointrtn;
+    bool bIsSort = true;
+    for(i=0;i<(xvectorlanepoint.size()-1);i++)
+    {
+        if(xvectorlanepoint[i].mnlane < xvectorlanepoint[i+1].mnlane)
+        {
+            bIsSort = false;
+            break;
+        }
+    }
+    if(bIsSort == false)
+    {
+        while(xvectorlanepoint.size() > 0)
+        {
+            int nlanemin;
+            nlanemin = 0;
+            int nlanenum = xvectorlanepoint[0].mnlane;
+            for(i=1;i<xvectorlanepoint.size();i++)
+            {
+                if(xvectorlanepoint[i].mnlane >= nlanenum)
+                {
+                    nlanenum = xvectorlanepoint[i].mnlane;
+                    nlanemin = i;
+                }
+            }
+            xvectorlanepointrtn.push_back(xvectorlanepoint[nlanemin]);
+            xvectorlanepoint.erase(xvectorlanepoint.begin() + nlanemin);
+        }
+    }
+    else
+    {
+        xvectorlanepointrtn = xvectorlanepoint;
+    }
+    return xvectorlanepointrtn;
+
+}
+
+double xodrfunc::GetAcurateDis(const double x, const double y, Road *pRoad,  const double s, const double nearx, const double neary, const double nearhead,int * pnlane)
+{
+    double fdismin = 1000;
+    if(pRoad->GetLaneSectionCount() < 1)return 1000;
+
+    std::vector<iv::LanePoint> xvectorlanepoint = GetAllLanePoint(pRoad,s,nearx,neary,nearhead);
+
+    double fdistoref = sqrt(pow(x-nearx,2)+pow(y-neary,2));
+
+    int i;
+    std::vector<double> xvectordis;
+    int nsize = xvectorlanepoint.size();
+    for(i=0;i<nsize;i++)
+    {
+        double fdis = sqrt(pow(x-xvectorlanepoint[i].mfX,2)+pow(y-xvectorlanepoint[i].mfY,2));
+        xvectordis.push_back(fdis);
+        if(fdismin>fdis)fdismin = fdis;
+
+    }
+    int nlane = -1000;
+    for(i=0;i<nsize;i++)
+    {
+        if((xvectordis[i]<=xvectorlanepoint[i].mflanewidth)&&(fdistoref <= xvectorlanepoint[i].mflanetocenter))
+        {
+            nlane = xvectorlanepoint[i].mnlane;
+            fdismin = 0; //On Lane, is very near.
+            break;
+        }
+    }
+
+    if(pnlane != 0)*pnlane = nlane;
+
+    return fdismin;
+
+
+}

src/tool/map_lanetoxodr/xodrfunc.h

@@ -0,0 +1,54 @@
+#ifndef XODRFUNC_H
+#define XODRFUNC_H
+
+#include <OpenDrive/OpenDrive.h>
+
+#include <QPointF>
+
+
+namespace iv {
+struct LanePoint
+{
+    int mnlane;
+    int mnLaneSection;
+    double mS;
+    double mflanewidth;
+    double mflanetocenter;
+    double mfX;
+    double mfY;
+    double mfhdg;
+    int mnlanetype;  //0 driving 1 border 2 none 3 bike
+    int mnlanemarktype; // -1 no 0 solid 1 broken 2 solidsolid
+    int mnlanecolor;
+
+};
+
+}
+
+class xodrfunc
+{
+public:
+    xodrfunc();
+
+public:
+    static inline double calcpointdis(QPointF p1,QPointF p2);
+    static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1);
+    static double CalcHdg(QPointF p0, QPointF p1);
+    static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
+                            double & neary,double & nearhead,double & frels);
+    static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
+                            double & neary,double & nearhead,double & frels);
+    static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
+                            double & neary,double & nearhead,double & frels);
+    static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
+                             double & neary,double & nearhead,double & frels);
+    static int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & fdis,double & nearx,
+                     double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0);
+
+    static double GetAcurateDis(const double x,const double y,Road * pRoad,const  double  s,const double nearx,
+                                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);
+};
+
+#endif // XODRFUNC_H