change tool/map_lanetoxodr. change ParamPoly3 GetCoords heading calculation.

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

@@ -867,19 +867,81 @@ void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, d
     {
         if(mbNormal)
         {
-            pRange = (s_check -mS-0.001)/mLength;
+            pRange = (s_check -mS)/mLength;
             if(pRange<0)pRange = 0.0;
             if(pRange>1.0)pRange = 1.0;
         }
         else
         {
-            pRange = s_check-mS - 0.001;
+            pRange = s_check-mS;
         }
-        xtem = muA + muB * pRange + muC * pRange*pRange + muD * pRange*pRange*pRange;
-        ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;
-        double x = xtem*cos(mHdg) - ytem * sin(mHdg) + mX;
-        double y = xtem*sin(mHdg) + ytem * cos(mHdg) + mY;
-        retHDG = CalcHdg(x,y,retX,retY);
+
+        double a,b;
+        a = muB + 2 * muC*pRange + 3*muD*pRange*pRange;
+        b = mvB + 2 * mvC*pRange + 3*mvD*pRange*pRange;
+
+
+        if(a == 0)
+        {
+            if(b>0)
+            {
+                retHDG = M_PI/2.0;
+            }
+            else
+            {
+                retHDG = M_PI*3.0/2.0;
+            }
+        }
+        else
+        {
+            double ratio = b/a;
+            double hdg = atan(ratio);
+            if(ratio > 0)
+            {
+                if(b>0)
+                {
+
+                }
+                else
+                {
+                    hdg = hdg + M_PI;
+                }
+            }
+            else
+            {
+                if(b>0)
+                {
+                    hdg = hdg + M_PI;
+                }
+                else
+                {
+                    hdg = hdg + 2.0*M_PI;
+                }
+            }
+            retHDG = hdg;
+        }
+
+        retHDG = retHDG + mHdg;
+        while(retHDG<0)retHDG = retHDG + 2.0*M_PI;
+        while(retHDG>=2.0*M_PI)retHDG = retHDG - 2.0*M_PI;
+
+
+
+//        if(mbNormal)
+//        {
+//            pRange = (s_check -mS-0.001)/mLength;
+//            if(pRange<0)pRange = 0.0;
+//            if(pRange>1.0)pRange = 1.0;
+//        }
+//        else
+//        {
+//            pRange = s_check-mS - 0.001;
+//        }
+//        xtem = muA + muB * pRange + muC * pRange*pRange + muD * pRange*pRange*pRange;
+//        ytem = mvA + mvB * pRange + mvC * pRange*pRange + mvD * pRange*pRange*pRange;
+//        double x = xtem*cos(mHdg) - ytem * sin(mHdg) + mX;
+//        double y = xtem*sin(mHdg) + ytem * cos(mHdg) + mY;
+//        retHDG = CalcHdg(x,y,retX,retY);
     }
 }