change driver/driver_map_xodrload. fix parampoly3 plan.

src/driver/driver_map_xodrload/globalplan.cpp

@@ -467,17 +467,13 @@ static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow
         neary = parc->GetY();
     }
 
+    double s_start = parc->GetS();
+
     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));
+        double x, y,hdg;//xtem,ytem;
+        parc->GetCoords(s_start+s,x,y,hdg);
         if(fdis<fdismin)
         {
             fdismin = fdis;
@@ -486,8 +482,8 @@ static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow
             neary = y;
         }
 
-        xold = x;
-        yold = y;
+//        xold = x;
+//        yold = y;
         s = s+ 0.1;
     }
 
@@ -1211,46 +1207,18 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,cons
     ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
     va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
 
-    double a = parc->GetS();
+    double s_start = parc->GetS();
     while(s < parc->GetLength())
-//    while(s<1.0)
     {
-
-        double len = parc->GetLength();
-        double x, y,xtem,ytem;
-//        xtem = parc->GetuA() + parc->GetuB() * s * len + parc->GetuC() * s*s *pow(len,2) + parc->GetuD() * s*s*s *pow(len,3);
-//        ytem = parc->GetvA() + parc->GetvB() * s* len + parc->GetvC() * s*s *pow(len,2) + parc->GetvD() * s*s*s *pow(len,3);
-        xtem = ua + ub * s  + uc * s*s  + ud * s*s*s ;
-        ytem = va + vb * s + vc * s*s  + vd * s*s*s ;
-        x = xtem*cos(hdg0) - ytem * sin(hdg0) + parc->GetX();
-        y = xtem*sin(hdg0) + ytem * cos(hdg0) + parc->GetY();
-
-
-//        x= xtem + parc->GetX();
-//        y = ytem + parc->GetY();
-
-//        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 x, y,hdg;
+        parc->GetCoords(s_start+s,x,y,hdg);
         pp.x = x;
         pp.y = y;
         pp.hdg = hdg;
-
-        double disx = sqrt(pow(x-xold,2)+ pow(y-yold,2));
-
-/*        if(disx > 0.1)s = s+ disx;
-        else s = s+ 0.5*/;
         s = s+ fspace;
-
-//        s = s+ sqrt(pow(x-xold,2)+ pow(y-yold,2));
-        xold = x;
-        yold = y;
- //       s = s+ dt;
-        pp.dis = pp.dis + disx;;
-        pp.mS = parc->GetS() + s;
+        pp.dis = pp.dis + fspace;;
+        pp.mS = s_start + s;
         xvectorPP.push_back(pp);
-//        std::cout<<" s is "<<s<<std::endl;
     }
     return xvectorPP;
 }