change tool/map_lanetoxodr. add Geometry poly3 sample point calcution.

src/tool/map_lanetoxodr/OpenDrive/RoadGeometry.cpp

@@ -43,6 +43,48 @@ void RoadGeometry::SetGeomType(short int geomType)
 }
 
 
+double RoadGeometry::CalcHdg(double x0, double y0, double x1, double y1)
+{
+    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;
+}
+
 /**
  * Setter for the base properties
  */
@@ -494,6 +536,55 @@ void GeometrySpiral::GetCoords(double s_check, double &retX, double &retY, doubl
 GeometryPoly3::GeometryPoly3 (double s, double x, double y, double hdg, double length, double a, double b,double c, double d ):	RoadGeometry(s, x, y, hdg, length)
 {	
 	SetGeomType(3); mA=a; mB=b; mC=c; mD=d;	
+    UpdateSamplePoint();
+}
+
+void GeometryPoly3::UpdateSamplePoint()
+{
+    double u = 0.0;
+    double du = 0.1;
+    geosamplepoint gsp;
+    gsp.s = 0;
+    gsp.x = mA;
+    gsp.y = 0.0;
+    mvectorgeosample.push_back(gsp);
+    u = du;
+    double v;
+    double flen = 0.0;
+    double oldu,oldv;
+    oldu = mvectorgeosample[0].x;
+    oldv = mvectorgeosample[0].y;
+    while(flen <= mLength)
+    {
+        double fdis = 0;
+        v = mA + mB*u + mC*u*u + mD*u*u*u;
+        fdis = sqrt(pow(u- oldu,2)+pow(v-oldv,2));
+        oldu = u;
+        oldv = v;
+        flen = flen + fdis;
+
+        gsp.s = flen;
+        gsp.x = u;
+        gsp.y = v;
+        mvectorgeosample.push_back(gsp);
+
+        if(fdis < 0.05)
+        {
+            if(fdis > 0)
+            {
+                du = du * 0.1/fdis;
+            }
+        }
+        if(fdis > 0.2)
+        {
+            du = du * 0.1/fdis;
+        }
+        u = u + du;
+
+
+    }
+    mbHaveSample = true;
+
 }
 
 /**
@@ -517,6 +608,7 @@ void GeometryPoly3::SetAll(double s, double x, double y, double hdg, double leng
 	mC=c;
 	mD=d;
 	ComputeVars();
+    UpdateSamplePoint();
 }
 
 //GetA to GetD, Added by Yuchuli
@@ -540,7 +632,6 @@ double GeometryPoly3::GetD()
     return mD;
 }
 
-#include "xodrfunc.h"
 
 void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
 {
@@ -553,6 +644,7 @@ void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double
     oldx = mX;
     oldy = mY;
     double du =0.1;
+    retHDG = mHdg;
     if(currentLength<du)
     {
         retX = mX;
@@ -572,7 +664,7 @@ void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double
         oldy = y;
         flen = flen + fdis;
         u = u + du;
-        retHDG = xodrfunc::CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+        retHDG = CalcHdg(oldx,oldy,x,y);
 
     }
 }

src/tool/map_lanetoxodr/OpenDrive/RoadGeometry.h

@@ -14,6 +14,15 @@ using std::vector;
 using std::string;
 
 
+
+class geosamplepoint
+{
+public:
+    double s;
+    double x;
+    double y;
+};
+
 /**
  * RoadGeometry class is responsible for storing the basic chordline geometry properties
  *
@@ -31,6 +40,11 @@ protected:
 	double mLength;
 	double mS2;
     short int mGeomType;	//0-line, 2-arc, 1-spiral 3-poly3 4-parampoly3
+
+
+public:
+    static double CalcHdg(double x0,double y0,double x1,double y1);
+
 public:
 	/**
 	 * Constructor that initializes the base properties of teh record
@@ -277,6 +291,11 @@ private:
 	double mB;
 	double mC;
 	double mD;
+
+private:
+    vector<geosamplepoint> mvectorgeosample;
+    bool mbHaveSample = false;
+    void UpdateSamplePoint();
 public:
 	/**
 	 * Constructor that initializes the base properties of the record

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -3659,6 +3659,7 @@ int MainWindow::GetEndPoint(Road *proad, double &x, double &y, double &hdg)
 
     RoadGeometry * pgeo = pblock->GetLastGeometry();
 
+
     //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
     switch (pgeo->GetGeomType()) {
     case 0:
@@ -3698,15 +3699,20 @@ int MainWindow::GetEndPoint(Road *proad, double &x, double &y, double &hdg)
         double xtem1,ytem1,x1,y1;
         GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
         double s = ppoly3->GetLength();
-        xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
-        ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
+//        xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
+//        ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
+        xtem = ppoly3->GetuA() + ppoly3->GetuB()  + ppoly3->GetuC()   + ppoly3->GetuD()  ;
+        ytem = ppoly3->GetvA() + ppoly3->GetvB() + ppoly3->GetvC()   + ppoly3->GetvD() ;
         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;
         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 ;
+            double frel = 0.99;
+ //           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 ;
+            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * frel + ppoly3->GetuC() *frel*frel   + ppoly3->GetuD()*frel*frel*frel  ;
+            ytem1 = ppoly3->GetvA() + ppoly3->GetvB()*frel + ppoly3->GetvC()*frel*frel   + ppoly3->GetvD()*frel*frel*frel ;
             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);