change tool/map_lanetoxodr. change paramPoly3 support.

src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -547,6 +547,8 @@ bool OpenDriveXmlParser::ReadGeometry(GeometryBlock* geomBlock, TiXmlElement *no
     case 4:		//parampoly3,add by Yuchuli 2019.11.1
         checker=TIXML_SUCCESS;
         double ua,ub,uc,ud,va,vb,vc,vd;
+        bool bNormal = true;
+        std::string strpRange;
         checker+=subNode->QueryDoubleAttribute("aU",&ua);
         checker+=subNode->QueryDoubleAttribute("bU",&ub);
         checker+=subNode->QueryDoubleAttribute("cU",&uc);
@@ -561,7 +563,14 @@ bool OpenDriveXmlParser::ReadGeometry(GeometryBlock* geomBlock, TiXmlElement *no
             return false;
         }
 
-        geomBlock->AddGeometryParamPoly3(s,x,y,hdg,length,ua,ub,uc,ud,va,vb,vc,vd);
+        if(subNode->QueryStringAttribute("pRange",&strpRange) == TIXML_SUCCESS)
+        {
+            if(strpRange == "arcLength")
+            {
+                bNormal = false;
+            }
+        }
+        geomBlock->AddGeometryParamPoly3(s,x,y,hdg,length,ua,ub,uc,ud,va,vb,vc,vd,bNormal);
         break;
 	}
 

src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp

@@ -435,7 +435,19 @@ bool OpenDriveXmlWriter::WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeo
 			break;
 		}
 	case 3:
-		{		
+        {
+            double ua,ub,uc,ud;
+            GeometryPoly3 * lp3 = static_cast<GeometryPoly3 *>(roadGeometry);
+            ua = lp3->GetA();
+            ub = lp3->GetB();
+            uc = lp3->GetC();
+            ud = lp3->GetD();
+            TiXmlElement *nodePoly3 = new TiXmlElement("poly3");
+            nodeGeometry->LinkEndChild(nodePoly3);
+            nodePoly3->SetDoubleAttribute("a",ua);
+            nodePoly3->SetDoubleAttribute("b",ub);
+            nodePoly3->SetDoubleAttribute("c",uc);
+            nodePoly3->SetDoubleAttribute("d",ud);
 			//poly3
 			break;
 		}
@@ -443,6 +455,7 @@ bool OpenDriveXmlWriter::WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeo
     case 4:
         {   //paramPoly3 add by Yu Chuli. 2019.11.1
             double ua,ub,uc,ud,va,vb,vc,vd;
+            bool bNormal;
             GeometryParamPoly3 *lpp3=static_cast<GeometryParamPoly3 *>(roadGeometry);
 
             ua = lpp3->GetuA();
@@ -453,6 +466,7 @@ bool OpenDriveXmlWriter::WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeo
             vb = lpp3->GetvB();
             vc = lpp3->GetvC();
             vd = lpp3->GetvD();
+            bNormal = lpp3->GetNormal();
 
             TiXmlElement *nodeParamPoly3 = new TiXmlElement("paramPoly3");
             nodeGeometry->LinkEndChild(nodeParamPoly3);
@@ -488,6 +502,11 @@ bool OpenDriveXmlWriter::WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeo
             std::stringstream svd;
             svd << setprecision(16) << setiosflags (ios_base::scientific) << vd;
             nodeParamPoly3->SetAttribute("dV",svd.str());
+
+            if(bNormal == false)
+            {
+                nodeParamPoly3->SetAttribute("pRange","arcLength");
+            }
             break;
 
         }

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

@@ -788,9 +788,9 @@ void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double
 /**
  * Constructor that initializes the base properties of the record
  */
-GeometryParamPoly3::GeometryParamPoly3 (double s, double x, double y, double hdg, double length,double ua,double ub,double uc,double ud,double va, double vb, double vc,double vd  ):	RoadGeometry(s, x, y, hdg, length)
+GeometryParamPoly3::GeometryParamPoly3 (double s, double x, double y, double hdg, double length,double ua,double ub,double uc,double ud,double va, double vb, double vc,double vd,bool bNormal  ):	RoadGeometry(s, x, y, hdg, length)
 {
-    SetGeomType(4); muA=ua; muB=ub; muC=uc; muD=ud;mvA=va; mvB=vb; mvC=vc; mvD=vd;
+    SetGeomType(4); muA=ua; muB=ub; muC=uc; muD=ud;mvA=va; mvB=vb; mvC=vc; mvD=vd;mbNormal = bNormal;
 }
 
 /**
@@ -798,7 +798,7 @@ GeometryParamPoly3::GeometryParamPoly3 (double s, double x, double y, double hdg
  */
 RoadGeometry* GeometryParamPoly3::Clone() const
 {
-    GeometryParamPoly3* ret=new GeometryParamPoly3(mS,mX,mY, mHdg, mLength, muA, muB, muC, muD,mvA,mvB,mvC,mvD);
+    GeometryParamPoly3* ret=new GeometryParamPoly3(mS,mX,mY, mHdg, mLength, muA, muB, muC, muD,mvA,mvB,mvC,mvD,mbNormal);
     return ret;
 }
 
@@ -832,6 +832,7 @@ double GeometryParamPoly3::GetvA(){return mvA;}
 double GeometryParamPoly3::GetvB(){return mvB;}
 double GeometryParamPoly3::GetvC(){return mvC;}
 double GeometryParamPoly3::GetvD(){return mvD;}
+bool GeometryParamPoly3::GetNormal(){return mbNormal;}
 
 void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
 {
@@ -961,9 +962,9 @@ void GeometryBlock::AddGeometryPoly3(double s, double x, double y, double hdg, d
 {	
 	mGeometryBlockElement.push_back(new GeometryPoly3(s, x, y, hdg, length, a, b, c, d));	
 }
-void GeometryBlock::AddGeometryParamPoly3(double s, double x, double y, double hdg, double length, double ua, double ub, double uc, double ud, double va, double vb, double vc, double vd)
+void GeometryBlock::AddGeometryParamPoly3(double s, double x, double y, double hdg, double length, double ua, double ub, double uc, double ud, double va, double vb, double vc, double vd,bool bNormal)
 {
-    mGeometryBlockElement.push_back(new GeometryParamPoly3(s,x,y,hdg,length,ua,ub,uc,ud,va,vb,vc,vd));
+    mGeometryBlockElement.push_back(new GeometryParamPoly3(s,x,y,hdg,length,ua,ub,uc,ud,va,vb,vc,vd,bNormal));
 }
 
 //-------------------------------------------------

src/common/common/xodr/OpenDrive/RoadGeometry.h

@@ -356,7 +356,7 @@ public:
     /**
      * Constructor that initializes the base properties of the record
      */
-    GeometryParamPoly3 (double s, double x, double y, double hdg, double length, double ua,double ub,double uc,double ud,double va, double vb, double vc,double vd );
+    GeometryParamPoly3 (double s, double x, double y, double hdg, double length, double ua,double ub,double uc,double ud,double va, double vb, double vc,double vd,bool bNormal = true );
 
     /**
      * Clones and returns the new geometry record
@@ -379,6 +379,8 @@ public:
     double GetvC();
     double GetvD();
 
+    bool GetNormal();
+
     void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
 };
 
@@ -423,7 +425,7 @@ public:
 	void AddGeometryArc(double s, double x, double y, double hdg, double length, double curvature);
 	void AddGeometrySpiral(double s, double x, double y, double hdg, double length, double curvatureStart,double curvatureEnd);
 	void AddGeometryPoly3(double s, double x, double y, double hdg, double length, double a,double b,double c,double d);
-    void AddGeometryParamPoly3(double s, double x, double y, double hdg, double length, double ua,double ub,double uc,double ud,double va,double vb,double vc,double vd);
+    void AddGeometryParamPoly3(double s, double x, double y, double hdg, double length, double ua,double ub,double uc,double ud,double va,double vb,double vc,double vd,bool bNormal = true);
 	
 	//-------------------------------------------------
 

src/tool/map_lanetoxodr/dialogparkingspaceedit.ui

@@ -86,7 +86,7 @@
    <property name="geometry">
     <rect>
      <x>120</x>
-     <y>220</y>
+     <y>221</y>
      <width>121</width>
      <height>31</height>
     </rect>
@@ -132,7 +132,7 @@
    <property name="geometry">
     <rect>
      <x>710</x>
-     <y>220</y>
+     <y>221</y>
      <width>121</width>
      <height>31</height>
     </rect>
@@ -259,6 +259,98 @@
     </rect>
    </property>
   </widget>
+  <widget class="QLabel" name="label_10">
+   <property name="geometry">
+    <rect>
+     <x>630</x>
+     <y>276</y>
+     <width>67</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Type</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox">
+   <property name="geometry">
+    <rect>
+     <x>710</x>
+     <y>280</y>
+     <width>181</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_11">
+   <property name="geometry">
+    <rect>
+     <x>300</x>
+     <y>337</y>
+     <width>131</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>orientation</string>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_2">
+   <property name="geometry">
+    <rect>
+     <x>430</x>
+     <y>340</y>
+     <width>121</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_12">
+   <property name="geometry">
+    <rect>
+     <x>616</x>
+     <y>336</y>
+     <width>81</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>subtype</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_width_3">
+   <property name="geometry">
+    <rect>
+     <x>710</x>
+     <y>340</y>
+     <width>121</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QComboBox" name="comboBox_3">
+   <property name="geometry">
+    <rect>
+     <x>120</x>
+     <y>403</y>
+     <width>121</width>
+     <height>31</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_13">
+   <property name="geometry">
+    <rect>
+     <x>14</x>
+     <y>400</y>
+     <width>91</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>dynamic</string>
+   </property>
+  </widget>
  </widget>
  <resources/>
  <connections/>

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -352,6 +352,7 @@ void MainWindow::ExecPainter()
                 ppp3 = (GeometryParamPoly3 * )pg;
                 int ncount = ppp3->GetLength()* mnfac;
                 double sstep;
+                bool bNormal = ppp3->GetNormal();
                 double arclength = ppp3->GetLength();
                 if(ncount > 0)sstep = ppp3->GetLength()/ncount;
 
@@ -360,7 +361,8 @@ void MainWindow::ExecPainter()
                 while(s < ppp3->GetLength())
                 {
                     double xtem,ytem;
-                    double pRange = s/arclength;
+                    double pRange =s;
+                    if(bNormal && (arclength>0))pRange = s/arclength;
                     xtem = ppp3->GetuA() +  ppp3->GetuB() * pRange +  ppp3->GetuC() * pRange*pRange +  ppp3->GetuD() * pRange*pRange*pRange;
                     ytem = ppp3->GetvA() + ppp3->GetvB() * pRange + ppp3->GetvC() * pRange*pRange + ppp3->GetvD() * pRange*pRange*pRange;
                     x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();

src/tool/map_lanetoxodr/roaddigit.cpp

@@ -314,7 +314,8 @@ void RoadDigit::CalcLine(double fspace)
             else sstep = 10000.0;
             double s = 0;
             double xtem,ytem;
-            double pr;
+ //           double pr;
+            bool bNormal = ppp3->GetNormal();
 
             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;
@@ -332,8 +333,8 @@ void RoadDigit::CalcLine(double fspace)
             double flasty = pg->GetY();
             while(s <= ppp3->GetLength())
             {
-                double pr= 1.0;
-                if(arclength > 0)pr = s/arclength;
+                double pr=s;
+                if((arclength > 0)&&(bNormal))pr = s/arclength;
                 xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
                 ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
                 x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
@@ -348,6 +349,21 @@ void RoadDigit::CalcLine(double fspace)
                 s = s+ sstep;
 
 
+            }
+            if(s != ppp3->GetLength())
+            {
+                double pr=s;
+                if((arclength > 0)&&(bNormal))pr = s/arclength;
+                xtem = ppp3->GetuA() +  ppp3->GetuB() * pr +  ppp3->GetuC() * pr*pr +  ppp3->GetuD() * pr*pr*pr;
+                ytem = ppp3->GetvA() + ppp3->GetvB() * pr + ppp3->GetvC() * pr*pr + ppp3->GetvD() * pr*pr*pr;
+                x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
+                y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
+
+                rdu.mS = pg->GetS() + s;
+                rdu.mX = x;
+                rdu.mY = y;
+                rdu.mfHdg = xodrfunc::CalcHdg(QPointF(flastx,flasty),QPointF(x,y));
+                mvectorRDU.push_back(rdu);
             }
             }
             break;

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -120,6 +120,8 @@ double xodrfunc::GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double y
     double fdismin = 100000.0;
     frels = 0;
 
+    bParamNormal = parc->GetNormal();
+
     double xold,yold;
     xold = parc->GetX();
     yold = parc->GetY();