change tool/map_lanetoxodr. solve autoconnect when have laneoffset.

src/tool/map_lanetoxodr/OpenDrive/OpenDriveXmlWriter.cpp

@@ -161,6 +161,8 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
 	WriteLanes(nodeRoad, road);
 
 
+
+
 	//Proceed to Objects
 	WriteObjects(nodeRoad, road);
 
@@ -697,21 +699,29 @@ bool OpenDriveXmlWriter::WriteLaneOffset(TiXmlElement *node, LaneOffset *laneOff
     TiXmlElement* nodeLaneSection = new TiXmlElement("laneOffset");
     node->LinkEndChild(nodeLaneSection);
 
-    std::stringstream ss;
-    ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
-    nodeLaneSection->SetAttribute("s",ss.str());
+    nodeLaneSection->SetDoubleAttribute("s",laneOffset->GetS());
+    nodeLaneSection->SetDoubleAttribute("a",laneOffset->Geta());
+    nodeLaneSection->SetDoubleAttribute("b",laneOffset->Getb());
+    nodeLaneSection->SetDoubleAttribute("c",laneOffset->Getc());
+    nodeLaneSection->SetDoubleAttribute("d",laneOffset->Getd());
+
+//    std::stringstream ss;
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
+//    nodeLaneSection->SetAttribute("s",ss.str());
+
+
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Geta();
+//    nodeLaneSection->SetAttribute("a",ss.str());
 
-    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Geta();
-    nodeLaneSection->SetAttribute("a",ss.str());
 
-    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getb();
-    nodeLaneSection->SetAttribute("b",ss.str());
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getb();
+//    nodeLaneSection->SetAttribute("b",ss.str());
 
-    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getc();
-    nodeLaneSection->SetAttribute("c",ss.str());
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getc();
+//    nodeLaneSection->SetAttribute("c",ss.str());
 
-    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getd();
-    nodeLaneSection->SetAttribute("d",ss.str());
+//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getd();
+//    nodeLaneSection->SetAttribute("d",ss.str());
 
     return true;
 }

src/tool/map_lanetoxodr/autoconnect.cpp

@@ -266,8 +266,17 @@ static std::vector<iv::roadpoint> getstartpoint(Road * p1)
         startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
         starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
 
-        startx = startx + off1 * cos(starthdg +M_PI/2.0);
-        starty = starty + off1 * sin(starthdg +M_PI/2.0);
+
+        LaneOffset * pLO = p1->GetLaneOffset(0);
+        double fofflane = 0.0;
+        if(pLO != NULL)
+        {
+            double s_tem = 0;
+            fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
+                    +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
+        }
+        startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
+        starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
         starthgt = 0;
         if(p1->GetElevationCount()>0)
         {
@@ -341,11 +350,14 @@ int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
         ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
         x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
         y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
-        s = ppoly3->GetLength()*0.99;
+        s = ppoly3->GetLength()*0.999;
+ //       double fgeolen;
+        double fr = 1.0;
         if(s>0)
         {
-            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
-            ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
+            fr = 1.0;
+            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * fr  + ppoly3->GetuC() * fr*fr  + ppoly3->GetuD() * fr*fr*fr ;
+            ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * fr + ppoly3->GetvC() * fr*fr  + ppoly3->GetvD() * fr*fr*fr ;
             x1 = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
             y1 = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
             hdg = geofit::CalcHdg(xtem1,ytem1,x1,y1);
@@ -391,9 +403,17 @@ static std::vector<iv::roadpoint> getendpoint(Road * p1)
 
         off1 = getoff(p1,nlane,false);
 
+        LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);
 
-        startx = startx + off1 * cos(starthdg +M_PI/2.0);
-        starty = starty + off1 * sin(starthdg +M_PI/2.0);
+        double fofflane = 0.0;
+        if(pLO!= NULL)
+        {
+            double s_tem = p1->GetRoadLength() - pLO->GetS();
+            fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
+                    +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
+        }
+        startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
+        starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
         starthgt = 0;
         if(p1->GetElevationCount()>0)
         {