change driver_map_xodr for poly3. and change plan, but not complete.next week...

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -21,19 +21,6 @@ QMAKE_CXXFLAGS +=  -g
 
 SOURCES += main.cpp     \
     ../../include/msgtype/v2x.pb.cc \
-    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp \
-    ../../common/common/xodr/OpenDrive/OtherStructures.cpp \
-    ../../common/common/xodr/OpenDrive/OpenDrive.cpp \
-    ../../common/common/xodr/OpenDrive/Road.cpp \
-    ../../common/common/xodr/OpenDrive/Junction.cpp \
-    ../../common/common/xodr/OpenDrive/Lane.cpp \
-    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp \
-    ../../common/common/xodr/OpenDrive/ObjectSignal.cpp \
-    ../../common/common/xodr/OpenDrive/RoadGeometry.cpp \
-    ../../common/common/xodr/TinyXML/tinyxml.cpp \
-    ../../common/common/xodr/TinyXML/tinyxmlparser.cpp \
-    ../../common/common/xodr/TinyXML/tinystr.cpp \
-    ../../common/common/xodr/TinyXML/tinyxmlerror.cpp \
     fresnl.cpp \
     const.cpp \
     polevl.c \
@@ -48,17 +35,6 @@ SOURCES += main.cpp     \
 HEADERS += \
     ../../../include/ivexit.h \
     ../../include/msgtype/v2x.pb.h \
-    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.h \
-    ../../common/common/xodr/OpenDrive/RoadGeometry.h \
-    ../../common/common/xodr/OpenDrive/Road.h \
-    ../../common/common/xodr/OpenDrive/Junction.h \
-    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.h \
-    ../../common/common/xodr/OpenDrive/Lane.h \
-    ../../common/common/xodr/OpenDrive/OtherStructures.h \
-    ../../common/common/xodr/OpenDrive/OpenDrive.h \
-    ../../common/common/xodr/OpenDrive/ObjectSignal.h \
-    ../../common/common/xodr/TinyXML/tinyxml.h \
-    ../../common/common/xodr/TinyXML/tinystr.h \
     mconf.h \
     globalplan.h \
     gps_type.h \
@@ -86,6 +62,16 @@ HEADERS += \
     error( "Couldn't find the iveigen.pri file!" )
 }
 
-INCLUDEPATH += $$PWD/../../common/common/xodr
+#INCLUDEPATH += $$PWD/../../common/common/xodr
+
+!include(../../tool/map_lanetoxodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
+!include(../../tool/map_lanetoxodr/TinyXML/TinyXML.pri ) {
+    error( "Couldn't find the TinyXML.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../tool/map_lanetoxodr
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -496,6 +496,66 @@ static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow
 }
 
 
+static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+    double x,y,hdg;
+//    double s = 0.0;
+    double fdismin = 100000.0;
+ //   double s0 = ppoly->GetS();
+
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = 0.3;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+
+
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    u = u+du;
+    while(flen < ppoly->GetLength())
+    {
+        double fdis = 0;
+        v = A + B*u + C*u*u + D*u*u*u;
+        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+
+        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdisnow<fdismin)
+        {
+            fdismin = fdisnow;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+        oldx = x;
+        oldy = y;
+    }
+
+    return fdismin;
+}
+
+
 /**
   * @brief GetNearPoint
   * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
@@ -542,7 +602,7 @@ int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoa
                 break;
 
             case 3:
-                dis = 100000.0;
+                dis = GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh);
                 break;
             case 4:
                 dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh);
@@ -1049,6 +1109,65 @@ static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral)
     return xvectorPP;
 }
 
+static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly)
+{
+    double x,y;
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = 0.1;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+    pp.x = xstart;
+    pp.y = ystart;
+    pp.hdg = hdgstart;
+    pp.dis = 0;
+    pp.mS = ppoly->GetS();
+    xvectorPP.push_back(pp);
+    while(flen < ppoly->GetLength())
+    {
+        double fdis = 0;
+        v = A + B*u + C*u*u + D*u*u*u;
+        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
+        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
+        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
+        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+        oldx = x;
+        oldy = y;
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+
+ //       s = s+ dt;
+        pp.dis = flen;;
+        pp.mS = ppoly->GetS() + flen;
+        xvectorPP.push_back(pp);
+    }
+
+    return xvectorPP;
+
+}
+
 static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc)
 {
     double s = 0.1;
@@ -1147,6 +1266,7 @@ std::vector<PlanPoint> GetPoint(Road * pRoad)
 
             break;
         case 3:
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
             break;
         case 4:
             xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
@@ -1341,6 +1461,7 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
     double off = 0;
     double a,b,c,d;
     if(xvectorIndex.size() == 0)return off;
+
     for(i=0;i<(xvectorIndex.size()-1);i++)
     {
 
@@ -1364,7 +1485,8 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
     d = xvectorLWA[xvectorIndex[i]].D;
     off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
     if(nlane < 0) off = off*(-1.0);
-//    std::cout<<"s is "<<s<<std::endl;
+    std::cout<<"s is "<<s<<std::endl;
+    std::cout<<" off is "<<off<<std::endl;
     return off;
 
 }
@@ -1606,6 +1728,84 @@ void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const do
 
 }
 
+
+double getoff(Road * pRoad,int nlane,const double s)
+{
+    int i;
+    int nLSCount = pRoad->GetLaneSectionCount();
+    double s_section = 0;
+
+    double foff = 0;
+
+    for(i=0;i<nLSCount;i++)
+    {
+
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(i<(nLSCount -1))
+        {
+            if(pRoad->GetLaneSection(i+1)->GetS()<s)
+            {
+                continue;
+            }
+        }
+        s_section = pLS->GetS();
+        int nlanecount = pLS->GetLaneCount();
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            Lane * pLane = pLS->GetLane(j);
+
+            int k;
+            double s_lane = s-s_section;
+
+
+            if(pLane->GetId() != 0)
+            {
+
+                for(k=0;k<pLane->GetLaneWidthCount();k++)
+                {
+                    if(k<(pLane->GetLaneWidthCount()-1))
+                    {
+                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
+                        {
+                            continue;
+                        }
+                    }
+                    s_lane = pLane->GetLaneWidth(k)->GetS();
+                    break;
+                }
+                LaneWidth * pLW  = pLane->GetLaneWidth(k);
+                if(pLW == 0)
+                {
+                    std::cout<<"not find LaneWidth"<<std::endl;
+                    break;
+                }
+
+                double fds = s - s_lane - s_section;
+                double fwidth= pLW->GetA() + pLW->GetB() * fds
+                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
+                if(nlane == pLane->GetId())
+                {
+                    foff = foff + fwidth/2.0;
+                }
+                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
+                {
+                    foff = foff + fwidth;
+                }
+
+            }
+
+        }
+
+
+        break;
+
+    }
+
+    if(nlane<0)foff = foff*(-1);
+    return foff;
+}
+
 //暂不考虑多个LaneSection的情况，不考虑车道宽度变化
 std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
 {
@@ -1681,7 +1881,9 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
         {
 
             PlanPoint pp = xvPP.at(i);
-            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
             pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
 
@@ -1699,8 +1901,10 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
         {
 
             PlanPoint pp = xvPP.at(i);
-            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
+            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
             double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
             pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
@@ -1752,7 +1956,9 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
         for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
         {
             PlanPoint pp = xvPP.at(i);
-            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+ //           off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+
+            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
             pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
 
@@ -1770,8 +1976,11 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
         {
 
             PlanPoint pp = xvPP.at(i);
-            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+
+            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
+            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
             double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
             pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
@@ -1823,7 +2032,8 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
         for(i=(nchange2 + nchangepoint/2);i<nsize;i++)
         {
             PlanPoint pp = xvPP.at(i);
-            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+    //        off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
             pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
 

src/driver/driver_map_xodrload/main.cpp

@@ -81,7 +81,6 @@ bool LoadXODR(std::string strpath)
     }
 
 
-
     xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
     gpxd = pxd;
 //    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);

src/driver/driver_map_xodrload/xodrdijkstra.cpp

@@ -939,6 +939,35 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
     int nvertexnum = xvalidvertex.size(); //vertex num
     mvertexnum = nvertexnum;
 
+    mvectorvertexedge.clear();
+
+    int nmaxvertex = 0;
+    for(i=0;i<nvertexnum;i++)
+    {
+        if(xvalidvertex[i]>nmaxvertex)nmaxvertex = xvalidvertex[i];
+    }
+
+//    for(i=0;i<mroadedge.size();i++)
+//    {
+//        if(mroadedge[i].mvertexstart>nmaxvertex)nmaxvertex = mroadedge[i].mvertexstart;
+//    }
+
+    for(i=0;i<=nmaxvertex;i++)
+    {
+        vertexedge x;
+        mvectorvertexedge.push_back(x);
+    }
+
+    for(i=0;i<mroadedge.size();i++)
+    {
+        mvectorvertexedge[mroadedge[i].mvertexstart].mvectorroadedge.push_back(&mroadedge[i]);
+    }
+
+    std::cout<<" max is "<<nmaxvertex<<" num is "<<nvertexnum<<std::endl;
+    nmaxvertex = nmaxvertex + 0;
+
+
+
 
 }
 
@@ -969,6 +998,17 @@ inline double xodrdijkstra::getedgedis(int vs, int vd)
 
     std::vector<roadedge > * proadedge = &mroadedge;
     double dis = std::numeric_limits<double>::max();
+
+    for(i=0;i<mvectorvertexedge[vs].mvectorroadedge.size();i++)
+    {
+        if((vs == mvectorvertexedge[vs].mvectorroadedge[i]->mvertexstart)&&(vd == mvectorvertexedge[vs].mvectorroadedge[i]->mvertexend))
+        {
+            dis = mroadedge[i].mlen;
+            return dis;
+        }
+    }
+    return dis;
+
     for(i=0;i<mroadedge.size();i++)
     {
         if((vs == mroadedge[i].mvertexstart)&&(vd == mroadedge[i].mvertexend))
@@ -1026,6 +1066,7 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
     // 遍历G.vexnum-1次；每次找出一个顶点的最短路径。
     for (i = 1; i < nvertexnum; i++)
     {
+ //       qDebug("i = %d",i);
         int k,j;
         // 寻找当前最小的路径；
         // 即，在未获取最短路径的顶点中，找到离vs最近的顶点(k)。

src/driver/driver_map_xodrload/xodrdijkstra.h

@@ -71,6 +71,13 @@ public:
 };
 
 
+class vertexedge
+{
+public:
+    std::vector<roadedge *> mvectorroadedge;
+};
+
+
 class lanenetunit
 {
   public:
@@ -137,6 +144,7 @@ private:
 
 
 
+    std::vector<vertexedge> mvectorvertexedge;
 
 
 //private:

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -403,6 +403,8 @@ void MainWindow::ExecPainter()
             GaussProjCal(lon,lat,&x,&y);
             x = x-x0;
             y= y-y0;
+            x = x + mfViewMoveX;
+            y = y + mfViewMoveY;
             painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
         }
     }

src/tool/tool_xodrobj/mainwindow.cpp

@@ -533,6 +533,9 @@ void MainWindow::ExecPainter()
             GaussProjCal(m_navigation_data.at(i)->gps_lng,m_navigation_data.at(i)->gps_lat,&x,&y);
             x = x-x0;
             y= y-y0;
+
+            x = x + mfViewMoveX;
+            y = y + mfViewMoveY;
             painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
         }
         mMutexNavi.unlock();