change adcxodrview2.

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

@@ -2128,6 +2128,86 @@ std::vector<double> Road::GetLaneWidthVector(double s_check,int nlr) //Lane Widt
 
 //-------------------------------------------------
 
+double Road::GetLeftRoadWidth(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    double xrtn;
+
+    unsigned int nlanecount = pLS->GetLeftLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+        if(pLane == NULL)
+        {
+            std::cout<<" Road::GetLeftRoadWidth Fail."<<" s_check: "<<s_check<<std::endl;
+            return 0;
+        }
+       double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+       xrtn = xrtn + fwidth;
+
+
+    }
+    return xrtn;
+}
+
+//-------------------------------------------------
+
+
+double Road::GetRightRoadWidth(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    double xrtn;
+
+    unsigned int nlanecount = pLS->GetRightLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetRightLaneAt(i+1);
+        if(pLane == NULL)
+        {
+            std::cout<<" Road::GetRightRoadWidth Fail."<<" s_check: "<<s_check<<std::endl;
+            return 0;
+        }
+
+       double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+       xrtn = xrtn + fwidth;
+
+
+    }
+
+    return  xrtn;
+
+
+}
+
+//-------------------------------------------------
+
 int Road::GetLeftLaneCount(double s_check)
 {
     LaneSection * pLS = NULL;

src/common/common/xodr/OpenDrive/Road.h

@@ -474,6 +474,10 @@ public:
     int GetRightDrivingLaneCount(double s_check);
 
     std::vector<double> GetLaneWidthVector(double s_check,int nlr); //Lane Width, From Refline to side. nlr 1 left 2 right
+
+    double GetLeftRoadWidth(double s_check);
+    double GetRightRoadWidth(double s_check);
+
     int GetLeftLaneCount(double s_check);
     int GetRightLaneCount(double s_check);
 

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

@@ -17,7 +17,6 @@ RoadSample::RoadSample(Road * pRoad)
 
     std::vector<RoadPoint> xvectorRoadPoint = mvectorRoadPoint;
 
-    int n = 1;
 }
 
 int RoadSample::SampleRoad(Road * pRoad)
@@ -27,6 +26,7 @@ int RoadSample::SampleRoad(Road * pRoad)
     mstrroadname = pRoad->GetRoadName();
 
     bool bMaxMinInit = false;
+    bool bRealMaxMinInit = false;
 
     double fS = 0;
     const double fStep = 0.1;
@@ -87,6 +87,8 @@ int RoadSample::SampleRoad(Road * pRoad)
             fRefZ = pRoad->GetElevationValue(fS);
             fOffsetValue = pRoad->GetLaneOffsetValue(fS);
 
+            if(fabs(fOffsetValue) > mfMaxLaneOff)mfMaxLaneOff = fabs(fOffsetValue);
+
             if(fabs(send_Section - fS)<0.1)
             {
                 fS = send_Section;  //If Section End,Same Point
@@ -163,10 +165,6 @@ int RoadSample::SampleRoad(Road * pRoad)
                 }
             }
 
-            if(mstrroadid == "10014")
-            {
-                int a = 1;
-            }
 
             if((bInserPoint == false)&&(bHaveLast))
             {
@@ -275,6 +273,49 @@ int RoadSample::SampleRoad(Road * pRoad)
             {
                 xRP.mvectorLanePoint = xvectorLanePoint;
                 xRP.mbMarkBrokenVisable = bMarkBrokenVisable;
+                if(fabs(xRP.mfLaneOffValue)<0.01)
+                {
+                    xRP.mfCenterX = xRP.mfRefX;
+                    xRP.mfCenterY = xRP.mfRefY;
+                }
+                else
+                {
+                    xRP.mfCenterX = xRP.mfRefX + xRP.mfLaneOffValue * cos(xRP.mfHdg + M_PI/2.0);
+                    xRP.mfCenterY = xRP.mfRefY + xRP.mfLaneOffValue * sin(xRP.mfHdg + M_PI/2.0);
+                }
+
+                xRP.mfRoadWidthLeft = pRoad->GetRoadLeftWidth(fS);
+                xRP.mfRoadWidthRight = pRoad->GetRoadRightWidth(fS);
+
+                xRP.mfBorderX_Left = xRP.mfCenterX + xRP.mfRoadWidthLeft * cos(xRP.mfHdg + M_PI/2.0);
+                xRP.mfBorderY_Left = xRP.mfCenterY + xRP.mfRoadWidthLeft * sin(xRP.mfHdg + M_PI/2.0);
+
+                xRP.mfBorderX_Right = xRP.mfCenterX + xRP.mfRoadWidthRight * cos(xRP.mfHdg - M_PI/2.0);
+                xRP.mfBorderY_Right = xRP.mfCenterY + xRP.mfRoadWidthRight * sin(xRP.mfHdg - M_PI/2.0);
+
+
+                if(bRealMaxMinInit == false)
+                {
+                    mfX_max = xRP.mfBorderX_Left;
+                    mfX_min = xRP.mfBorderX_Left;
+                    mfY_max = xRP.mfBorderY_Left;
+                    mfY_min = xRP.mfBorderY_Left;
+                    bRealMaxMinInit = true;
+                }
+
+                if(bRealMaxMinInit)
+                {
+                    if(mfX_max<xRP.mfBorderX_Left)mfX_max = xRP.mfBorderX_Left;
+                    if(mfX_min>xRP.mfBorderX_Left)mfX_min = xRP.mfBorderX_Left;
+                    if(mfY_max<xRP.mfBorderY_Left)mfY_max = xRP.mfBorderY_Left;
+                    if(mfY_min>xRP.mfBorderY_Left)mfY_min = xRP.mfBorderY_Left;
+                    if(mfX_max<xRP.mfBorderX_Right)mfX_max = xRP.mfBorderX_Right;
+                    if(mfX_min>xRP.mfBorderX_Right)mfX_min = xRP.mfBorderX_Right;
+                    if(mfY_max<xRP.mfBorderY_Right)mfY_max = xRP.mfBorderY_Right;
+                    if(mfY_min>xRP.mfBorderY_Right)mfY_min = xRP.mfBorderY_Right;
+                }
+
+
                 mvectorRoadPoint.push_back(xRP);
 
                 fLastS = fS;
@@ -382,7 +423,7 @@ int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::v
         xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
         xLanePoint.mstrroadmarktype = xRoadMark.GetType();
         xLanePoint.mfLaneWidth = xRightWidth[i];
-        xLanePoint.nLane = static_cast<int>(i+1);
+        xLanePoint.nLane = static_cast<int>(i+1) *(-1);
         xLanePoint.mstrLaneType = pLS->GetRightLaneAt(i+1)->GetType();
         xvectorLanePoint.push_back(xLanePoint);
     }
@@ -548,6 +589,148 @@ double RoadSample::GetRefYMin()
     return  mfRefY_min;
 }
 
+double RoadSample::GetXMax()
+{
+    return  mfX_max;
+}
+
+double RoadSample::GetXMin()
+{
+    return  mfX_min;
+}
+
+double RoadSample::GetYMax()
+{
+    return mfY_max;
+}
+
+double RoadSample::GetYMin()
+{
+    return  mfY_min;
+}
+
+bool RoadSample::PointInRoad(const double x, const double y,  double &s,double & t,int & nlane)
+{
+    if((x<mfX_min)||(x>mfX_max))
+    {
+        return false;
+    }
+    if((y<mfY_min)||(y>mfY_max))
+    {
+        return false;
+    }
+
+    unsigned int i;
+    unsigned int nRPCount = static_cast<unsigned int >(mvectorRoadPoint.size());
+
+    for(i=0;i<(nRPCount -1);i++)
+    {
+        if(mvectorRoadPoint[i].nSecNum != mvectorRoadPoint[i+1].nSecNum)  //s same
+        {
+            continue;
+        }
+        if(PointInQuadrilateral(x,y,PointRS(mvectorRoadPoint[i].mfBorderX_Left,mvectorRoadPoint[i].mfBorderY_Left),
+                                PointRS(mvectorRoadPoint[i+1].mfBorderX_Left,mvectorRoadPoint[i+1].mfBorderY_Left),
+                                PointRS(mvectorRoadPoint[i+1].mfBorderX_Right,mvectorRoadPoint[i+1].mfBorderY_Right),
+                                PointRS(mvectorRoadPoint[i].mfBorderX_Right,mvectorRoadPoint[i].mfBorderY_Right)))
+        {
+            unsigned int j;
+            unsigned int nlanecount = static_cast<unsigned int>(mvectorRoadPoint[i].mvectorLanePoint.size()) ;
+
+            for(j=0;j<(nlanecount -1);j++)
+            {
+                iv::RoadPoint p1 = mvectorRoadPoint[i];
+                iv::RoadPoint p2 = mvectorRoadPoint[i+1];
+                if(PointInQuadrilateral(x,y,PointRS(p1.mvectorLanePoint[j].mx,p1.mvectorLanePoint[j].my),
+                                        PointRS(p2.mvectorLanePoint[j].mx,p2.mvectorLanePoint[j].my),
+                                        PointRS(p2.mvectorLanePoint[j+1].mx,p2.mvectorLanePoint[j+1].my),
+                                        PointRS(p1.mvectorLanePoint[j+1].mx,p1.mvectorLanePoint[j+1].my)))
+                {
+                    double t_off = pldis(PointRS(x,y),
+                                         PointRS(p1.mfCenterX,p1.mfCenterY),
+                                         PointRS(p2.mfCenterX,p2.mfCenterY));
+                    if(p1.mvectorLanePoint[j].nLane<=0)t_off = t_off*(-1.0);
+                    double s_off = pldis(PointRS(x,y),
+                                         PointRS(p1.mvectorLanePoint[j].mx,p1.mvectorLanePoint[j].my),
+                                         PointRS(p1.mvectorLanePoint[j+1].mx,p1.mvectorLanePoint[j+1].my));
+                    s = p1.ms + s_off;
+                    t = t_off;
+                    if(p1.mvectorLanePoint[j].nLane<=0)
+                    {
+                        nlane = p1.mvectorLanePoint[j+1].nLane;
+                    }
+                    else
+                    {
+                        nlane = p1.mvectorLanePoint[j].nLane;
+                    }
+                    return true;
+                }
+            }
+
+            std::cout<<"RoadSample::PointInRoad Warning."<<std::endl;
+        }
+    }
+
+    return false;
+}
+
+bool RoadSample::PointInQuadrilateral(double x, double y, PointRS A,PointRS B,PointRS C, PointRS D)
+{
+    double a = (B.x - A.x)*(y - A.y) - (B.y - A.y)*(x - A.x);
+    double b = (C.x - B.x)*(y - B.y) - (C.y - B.y)*(x - B.x);
+    double c = (D.x - C.x)*(y - C.y) - (D.y - C.y)*(x - C.x);
+    double d = (A.x - D.x)*(y - D.y) - (A.y - D.y)*(x - D.x);
+    if((a >= 0 && b >= 0 && c >= 0 && d >= 0) || (a <= 0 && b <= 0 && c <= 0 && d <= 0)) {
+        return true;
+    }
+    return  false;
+}
+
+
+
+//参考https://blog.csdn.net/Mr_robot_strange/article/details/123495084
+double RoadSample::pldis(iv::PointRS a,iv::PointRS b,iv::PointRS c)
+{
+
+    iv::PointRS s1(c.x-b.x,c.y-b.y);
+    iv::PointRS s2(a.x-b.x,a.y-b.y);
+    iv::PointRS s3(a.x-c.x,a.y-c.y);
+
+    double fdis_ab = sqrt(pow(a.x-b.x,2) + pow(a.y - b.y,2));
+    double fdis_ac = sqrt(pow(a.x-c.x,2) + pow(a.y - c.y,2));
+    double fdis_bc = sqrt(pow(b.x-c.x,2) + pow(b.y - c.y,2));
+
+    double cmult_bac = (a.x - b.x) * (c.y-b.y) - (c.x - b.x) * (a.y - b.y);
+
+    double pmults1s2 = s1.x*s2.x + s1.y*s2.y;
+    double pmults1s3 = s1.x*s3.x + s1.y*s3.y;
+
+    if((b.x == c.x) && (b.y == c.y))
+    {
+        return fdis_ab;
+    }
+
+    if(pmults1s2<-0.0000001)
+    {
+        return  fdis_ab;
+    }
+    else
+    {
+        if(pmults1s3>0.000001)
+        {
+            return fdis_ac;
+        }
+        else
+        {
+            return fabs(cmult_bac)/fdis_bc;
+        }
+    }
+
+    return 0;
+
+
+}
+
 }
 
 

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

@@ -9,6 +9,18 @@
 namespace iv
 {
 
+struct PointRS
+{
+public:
+    double x;
+    double y;
+    PointRS(double xset,double yset)
+    {
+        x = xset;
+        y = yset;
+    }
+};
+
 struct LaneSamplePoint
 {
 public:
@@ -42,6 +54,19 @@ public:
     double mfHdg;
     double mfLaneOffValue;
 
+    double mfCenterX;   //Because LaneOffset, the Centerline or Refline is not the real center, so add CenterX CenterY define the real.
+    double mfCenterY;
+
+    double mfBorderX_Left;  //Left Border, Right Border, use to describe the road border
+    double mfBorderY_Left;
+
+    double mfBorderX_Right;
+    double mfBorderY_Right;
+
+    double mfRoadWidthLeft;
+    double mfRoadWidthRight;
+
+
     bool mbMarkBrokenVisable = false;
 
     int nSecNum;  //If lane count change or lane roadmarkchange is new section.
@@ -64,12 +89,22 @@ public:
     double GetRefYMax();
     double GetRefYMin();
 
+    double GetXMax();
+    double GetXMin();
+    double GetYMax();
+    double GetYMin();
+
+    bool PointInRoad(const double x, const double y,  double & s, double & t, int & nlane);
+
 private:
     std::string mstrroadid;
     std::string mstrroadname;
     double mfRoadLen;
     std::vector<RoadPoint> mvectorRoadPoint;
     double mfRefX_min,mfRefX_max,mfRefY_min,mfRefY_max;
+    double mfX_min,mfX_max,mfY_min,mfY_max;
+    double mfMaxLaneOff = 0.0;
+    double mfMaxRoadWidth = 0.0;
 
 private:
     /**
@@ -90,7 +125,9 @@ private:
 
     inline double GetMaxWidthRatio(LaneSection * pLS, double fS);
 
+    bool PointInQuadrilateral(double x, double y, PointRS A,PointRS B,PointRS C, PointRS D);
 
+    double pldis(iv::PointRS a,iv::PointRS b,iv::PointRS c);
 
 };
 

src/tool/map_lanetoxodr/xvmainwindow2.cpp

@@ -295,6 +295,29 @@ void XVMainWindow::onClickXY(double x, double y)
     rel_x = selx - mfViewMoveX + VIEW_WIDTH/2 ;
     rel_y = sely - mfViewMoveY - VIEW_HEIGHT/2;
 
+    unsigned int nRoadCount = static_cast<unsigned int>(mvectorRoadSample.size());
+    unsigned int i;
+
+
+    int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
+    for(i=0;i<nRoadCount;i++)
+    {
+        double s,t;
+        int nlane;
+        if(mvectorRoadSample[i].PointInRoad(rel_x,rel_y,s,t,nlane))
+        {
+            std::cout<<" In Road"<<mvectorRoadSample[i].GetRoadID()<<" s:"<<s<<" t:"<<t<<" nlane:"<<nlane<<std::endl;
+            char strout[1000];
+            snprintf(strout,1000,"Road:%s s:%f t:%f nlane:%d",mvectorRoadSample[i].GetRoadID().data(),s,t,nlane);
+     //       mpLabel_Status->setText(strout);
+            ui->statusbar->showMessage(strout,10000);
+        }
+    }
+    int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
+    std::cout<<" find use time: "<<(time2 - time1)<<std::endl;
+
+    return;
+
 
     Road * pRoad = 0;
     GeometryBlock * pgeob;