change driver_map_xodrload.

src/common/common/xodr/OpenDrive/Road.cpp

@@ -2008,26 +2008,199 @@ void Road::ResetPriority()
 
 double Road::GetDis(const double x,const double y, const double hdg, double & fRefDis, double & fHdgDiff, int & nlane,double & s,double & refx,double & refy,double & refhdg)
 {
-//    unsigned int ngbsize = static_cast<unsigned int >(mGeometryBlockVector.size());
-//    unsigned int i;
-//    double fdismin = 1000000.0;
-//    for(i=0;i<ngbsize;i++)
-//    {
-//        GeometryBlock * pgb = GetGeometryBlock(i);
-//        RoadGeometry * pg;
-//        pg = pgb->GetGeometryAt(0);
-//        double xstart,ystart;
-//        xstart = pg->GetX();
-//        ystart = pg->GetY();
-//        double fdisstart = sqrt(pow(xstart - x,2) + pow(ystart - y,2));
-
-//        if(fdis)
-
-//        if(fdisstart<(100.0 + pg->GetLength()))
-//        {
-
-//        }
-//    }
+    unsigned int ngbsize = static_cast<unsigned int >(mGeometryBlockVector.size());
+    unsigned int i;
+    double fdismin = 1000000.0;
+    for(i=0;i<ngbsize;i++)
+    {
+        GeometryBlock * pgb = GetGeometryBlock(i);
+        RoadGeometry * pg;
+        pg = pgb->GetGeometryAt(0);
+        double xstart,ystart;
+        xstart = pg->GetX();
+        ystart = pg->GetY();
+        double fdisstart = sqrt(pow(xstart - x,2) + pow(ystart - y,2));
+        double sstart = pg->GetS();
+        double geolen = pg->GetLength();
+
+        if(fdismin > fdisstart)
+        {
+            fdismin = fdisstart;
+            fRefDis = fdismin;
+            nlane = 1000;
+            s = sstart;
+            refx = xstart;
+            refy = ystart;
+            refhdg = pg->GetHdg();
+            fHdgDiff = hdg - refhdg;
+            if(fHdgDiff<(-M_PI))fHdgDiff = fHdgDiff + 2.0*M_PI;
+            if(fHdgDiff>M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+        }
+
+        if(fdisstart<(100.0 + geolen))
+        {
+            double fstep = 0.1;
+            double fsnow = 0.1;
+            while(fsnow<=geolen)
+            {
+                double xnow,ynow,hdgnow;
+                GetGeometryCoords(fsnow,xnow,ynow,hdgnow);
+                fsnow = fsnow + fstep;
+                double fdisnow = sqrt(pow(xnow - x,2)+pow(ynow - y,2));
+                if(fdisnow < fdismin)
+                {
+                    fdismin = fdisnow;;
+                    fRefDis = fdismin;
+                    nlane = 1000;
+                    s = fsnow;
+                    refx = xnow;
+                    refy = ynow;
+                    refhdg = hdgnow;
+                    fHdgDiff = hdg - hdgnow;
+                    if(fHdgDiff<(-M_PI))fHdgDiff = fHdgDiff + 2.0*M_PI;
+                    if(fHdgDiff>M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+                }
+            }
+        }
+    }
+
+    double frealdis = fRefDis;
+
+    if(fdismin < 100.0)
+    {
+        double fOffset = GetLaneOffsetValue(s);
+        double realx,realy,realhdg;
+        realhdg = refhdg;
+        if(fabs(fOffset)<0.00001)
+        {
+            realx = refx;
+            realy = refy;
+        }
+        else
+        {
+            realx = refx + fOffset * cos(refhdg + M_PI/2.0);
+            realy = refy + fOffset * sin(refhdg + M_PI/2.0);
+        }
+
+        if((fabs(realx - x)<0.001)&&(fabs(realy - y)<0.001))
+        {
+            if(GetRightDrivingLaneCount(s)>0)
+            {
+                nlane = -1;
+            }
+            else
+            {
+                if(GetLeftDrivingLaneCount(s)>0)
+                {
+                    nlane = 1;
+                }
+                else
+                {
+                    nlane = 0;
+                }
+            }
+        }
+        else
+        {
+            double x0,y0,x1,y1;
+            x0 = realx;
+            y0 = realy;
+            x1 = x;
+            y1 = y;
+            double hdgpoint;
+            if(x0 == x1)
+            {
+                if(y0 < y1)
+                {
+                    hdgpoint =  M_PI/2.0;
+                }
+                else
+                    hdgpoint =  M_PI*3.0/2.0;
+            }
+            else
+            {
+                double ratio = (y1-y0)/(x1-x0);
+                double hdgpoint = atan(ratio);
+                if(ratio > 0)
+                {
+                    if(y1 > y0)
+                    {
+
+                    }
+                    else
+                    {
+                        hdgpoint = hdg + M_PI;
+                    }
+                }
+                else
+                {
+                    if(y1 > y0)
+                    {
+                        hdgpoint = hdg + M_PI;
+                    }
+                    else
+                    {
+                        hdgpoint = hdg + 2.0*M_PI;
+                    }
+                }
+
+            }
+
+
+            hdgpoint = hdgpoint - refhdg;
+
+            while(hdgpoint>=M_PI)hdgpoint = hdgpoint -2.0*M_PI;
+            while(hdgpoint <(-M_PI))hdgpoint = hdgpoint +2.0*M_PI;
+
+            frealdis = sqrt(pow(realx - x,2)+pow(realy - y,2));
+
+            if(hdgpoint > 0)
+            {
+                if(GetLeftDrivingLaneCount(s)>0)
+                {
+                    std::vector<double> fvectorwidth =  GetDrivingLaneWidthVector(s,1);
+                    if(fvectorwidth.size()>0)
+                    {
+                        unsigned int i;
+                        unsigned int nwidthsize = fvectorwidth.size();
+                        for(i=0;i<nwidthsize;i++)
+                        {
+                            frealdis = frealdis - fvectorwidth[i];
+                            if(frealdis<=0)
+                            {
+                                nlane = static_cast<int>(i+1);
+                                frealdis = 0;
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+            else
+            {
+                if(GetRightDrivingLaneCount(s)>0)
+                {
+                    std::vector<double> fvectorwidth =  GetDrivingLaneWidthVector(s,2);
+                    if(fvectorwidth.size()>0)
+                    {
+                        unsigned int i;
+                        unsigned int nwidthsize = fvectorwidth.size();
+                        for(i=0;i<nwidthsize;i++)
+                        {
+                            frealdis = frealdis - fvectorwidth[i];
+                            if(frealdis<=0)
+                            {
+                                nlane = (static_cast<int>(i+1))*(-1);
+                                frealdis = 0;
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+    return frealdis;
 }
 
 

src/common/common/xodr/xodrfunc/xodrdijkstra.cpp

@@ -1999,9 +1999,9 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                 }
                 else
                 {
-                    if(pLane2->IsSuccessorSet())
+                    if(pLane2->IsPredecessorSet())
                     {
-                        xpathsection[i].secondsel = pLane2->GetSuccessor();
+                        xpathsection[i].secondsel = pLane2->GetPredecessor();
                     }
 //                    if(pLane2->IsPredecessorSet())
 //                    {

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -655,6 +655,74 @@ int xodrfunc::GetNearPointAtRoad(const double x, const double y, Road *pRoad, Ge
 }
 
 
+int xodrfunc::GetNearPointWithHide2(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad,
+                                    double &fdis, double &nearx, double &neary,
+                                   double &nearhead, const double nearthresh,
+                                   std::vector<int> &xvectorhideroad, double *pfs,
+                                   int *pnlane, bool bnotuselane)
+{
+    double dismin = std::numeric_limits<double>::infinity();
+    fdis = dismin;
+    unsigned int i;
+    *pObjRoad = 0;
+    std::vector<iv::nearoption> xvectornearopt;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        unsigned int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh,frels;
+
+        if(xvectorhideroad.size() > 0)
+        {
+            int nroadid = atoi(proad->GetRoadId().data());
+            unsigned int k;
+            bool bIsHiden = false;
+            for(k = 0;k<xvectorhideroad.size();k++)
+            {
+                if(xvectorhideroad[k] == nroadid)
+                {
+                    bIsHiden = true;
+                    break;
+                }
+            }
+            if(bIsHiden)continue;
+        }
+
+        double fRefDis,fHdgDiff,s,refx,refy,refhdg;
+        int nlane = 1000;
+        double fdisroad = proad->GetDis(x,y,0,fRefDis,fHdgDiff,nlane,s,refx,refy,refhdg);
+
+        if(nlane != 1000)
+        {
+            fdis = fdisroad;
+            nearx = refx;
+            neary = refy;
+            nearhead = refhdg;
+            *pObjRoad = proad;
+            if(pfs != 0)*pfs = s;
+            if(pnlane != 0)*pnlane = nlane;
+             return 0;
+        }
+
+        if(fdisroad < fdis)
+        {
+            fdis = fdisroad;
+            nearx = refx;
+            neary = refy;
+            nearhead = refhdg;
+            *pObjRoad = proad;
+            if(pfs != 0)*pfs = s;
+            if(pnlane != 0)*pnlane = nlane;
+        }
+    }
+
+
+    if(fdis > nearthresh)return -1;
+    return 0;
+}
+
+
+
 int xodrfunc::GetNearPointWithHide(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad,
                                    GeometryBlock **pgeo, double &fdis, double &nearx, double &neary,
                                    double &nearhead, const double nearthresh,

src/common/common/xodr/xodrfunc/xodrfunc.h

@@ -53,6 +53,10 @@ public:
                      double & neary,double & nearhead,const double nearthresh,std::vector<int> & xvectorhideroad,
                                     double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
 
+    static int GetNearPointWithHide2(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,double & fdis,double & nearx,
+                     double & neary,double & nearhead,const double nearthresh,std::vector<int> & xvectorhideroad,
+                                    double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
+
     static int GetNearPointAtRoad(const double x,const double y,Road *pRoad,GeometryBlock ** pgeo, double & fdis,double & nearx,
                      double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
 

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -1300,6 +1300,7 @@ void MainWindow::onClickXY(double x, double y)
     double fdis,nearx,neary,hdg;
     double fs;
     int nlane;
+ //   if(xodrfunc::GetNearPointWithHide2(rel_x,rel_y,&mxodr,&pRoad,fdis,nearx,neary,hdg,50,mvectorhideroadid,&fs,&nlane) == 0)
     if(xodrfunc::GetNearPointWithHide(rel_x,rel_y,&mxodr,&pRoad,&pgeob,fdis,nearx,neary,hdg,50,mvectorhideroadid,&fs,&nlane) == 0)
     {
         qDebug("s:%f dis is %f nlane is %d",fs,fdis,nlane);