|
|
@@ -0,0 +1,810 @@
|
|
|
+#include "xodrfunc.h"
|
|
|
+
|
|
|
+#include "limits"
|
|
|
+#include <iostream>
|
|
|
+
|
|
|
+#include <Eigen/Dense>
|
|
|
+#include <Eigen/Cholesky>
|
|
|
+#include <Eigen/LU>
|
|
|
+#include <Eigen/QR>
|
|
|
+#include <Eigen/SVD>
|
|
|
+
|
|
|
+#include <QDebug>
|
|
|
+#include <QPointF>
|
|
|
+
|
|
|
+xodrfunc::xodrfunc()
|
|
|
+{
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+inline double xodrfunc::calcpointdis(QPointF p1,QPointF p2)
|
|
|
+{
|
|
|
+ return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
|
|
|
+}
|
|
|
+
|
|
|
+bool xodrfunc::pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
|
|
|
+{
|
|
|
+ double hdg = CalcHdg(poingarc,point1);
|
|
|
+ if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
|
|
|
+ else hdg = hdg - M_PI/2.0;
|
|
|
+ if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
|
|
|
+ if(hdg < 0)hdg = hdg + 2.0*M_PI;
|
|
|
+
|
|
|
+ double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
|
|
|
+
|
|
|
+ double hdgdiff = hdg - parc->GetHdg();
|
|
|
+
|
|
|
+ if(hdgrange >= 0 )
|
|
|
+ {
|
|
|
+ if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(fabs(hdgdiff ) < fabs(hdgrange))return true;
|
|
|
+ return false;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief CalcHdg
|
|
|
+ * 计算点0到点1的航向
|
|
|
+ * @param p0 Point 0
|
|
|
+ * @param p1 Point 1
|
|
|
+**/
|
|
|
+double xodrfunc::CalcHdg(QPointF p0, QPointF p1)
|
|
|
+{
|
|
|
+ double x0,y0,x1,y1;
|
|
|
+ x0 = p0.x();
|
|
|
+ y0 = p0.y();
|
|
|
+ x1 = p1.x();
|
|
|
+ y1 = p1.y();
|
|
|
+ if(x0 == x1)
|
|
|
+ {
|
|
|
+ if(y0 < y1)
|
|
|
+ {
|
|
|
+ return M_PI/2.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ return M_PI*3.0/2.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ double ratio = (y1-y0)/(x1-x0);
|
|
|
+
|
|
|
+ double hdg = atan(ratio);
|
|
|
+
|
|
|
+ if(ratio > 0)
|
|
|
+ {
|
|
|
+ if(y1 > y0)
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ hdg = hdg + M_PI;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(y1 > y0)
|
|
|
+ {
|
|
|
+ hdg = hdg + M_PI;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ hdg = hdg + 2.0*M_PI;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return hdg;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
|
|
|
+ * @param parc
|
|
|
+ * @param xnow
|
|
|
+ * @param ynow
|
|
|
+ * @param nearx
|
|
|
+ * @param neary
|
|
|
+ * @param nearhead
|
|
|
+ * @return
|
|
|
+ */
|
|
|
+double xodrfunc::GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
|
|
|
+ double & neary,double & nearhead,double & frels)
|
|
|
+{
|
|
|
+
|
|
|
+ double s = 0.1;
|
|
|
+ double fdismin = 100000.0;
|
|
|
+ frels = 0;
|
|
|
+
|
|
|
+ double xold,yold;
|
|
|
+ xold = parc->GetX();
|
|
|
+ yold = parc->GetY();
|
|
|
+
|
|
|
+ double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
|
|
|
+ if(fdis<fdismin)
|
|
|
+ {
|
|
|
+ fdismin = fdis;
|
|
|
+ nearhead = parc->GetHdg();
|
|
|
+ nearx = parc->GetX();
|
|
|
+ neary = parc->GetY();
|
|
|
+ }
|
|
|
+
|
|
|
+ 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));
|
|
|
+ if(fdis<fdismin)
|
|
|
+ {
|
|
|
+ fdismin = fdis;
|
|
|
+ nearhead = hdg;
|
|
|
+ nearx = x;
|
|
|
+ neary = y;
|
|
|
+ frels = s;
|
|
|
+ }
|
|
|
+
|
|
|
+ xold = x;
|
|
|
+ yold = y;
|
|
|
+ s = s+ 0.1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return fdismin;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief GetArcDis
|
|
|
+ * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
|
|
|
+ * @param parc pointer to a arc geomery
|
|
|
+ * @param x current x
|
|
|
+ * @param y current y
|
|
|
+ * @param nearx near x
|
|
|
+ * @param neary near y
|
|
|
+ * @param nearhead nearhead
|
|
|
+**/
|
|
|
+
|
|
|
+double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
|
|
|
+ double & neary,double & nearhead,double & frels)
|
|
|
+{
|
|
|
+
|
|
|
+ if((parc->GetS()>370)&&(parc->GetS()<370.1))
|
|
|
+ {
|
|
|
+ int a = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(parc->GetCurvature() == 0.0)return 1000.0;
|
|
|
+
|
|
|
+ double R = fabs(1.0/parc->GetCurvature());
|
|
|
+
|
|
|
+ frels = 0.0;
|
|
|
+
|
|
|
+ //calculate arc center
|
|
|
+ double x_center,y_center;
|
|
|
+ if(parc->GetCurvature() > 0)
|
|
|
+ {
|
|
|
+ x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
|
|
|
+ y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
|
|
|
+ y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
|
|
|
+ }
|
|
|
+
|
|
|
+ double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
|
|
|
+
|
|
|
+
|
|
|
+ QPointF arcpoint;
|
|
|
+ arcpoint.setX(x_center);arcpoint.setY(y_center);
|
|
|
+
|
|
|
+ QPointF pointnow;
|
|
|
+ pointnow.setX(x);pointnow.setY(y);
|
|
|
+ QPointF point1,point2;
|
|
|
+ point1.setX(x_center + (R * cos(hdgltoa)));
|
|
|
+ point1.setY(y_center + (R * sin(hdgltoa)));
|
|
|
+ point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
|
|
|
+ point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
|
|
|
+
|
|
|
+ //calculat dis
|
|
|
+ bool bp1inarc,bp2inarc;
|
|
|
+ bp1inarc =pointinarc(parc,arcpoint,point1);
|
|
|
+ bp2inarc =pointinarc(parc,arcpoint,point2);
|
|
|
+ double fdis[4];
|
|
|
+ fdis[0] = calcpointdis(pointnow,point1);
|
|
|
+ fdis[1] = calcpointdis(pointnow,point2);
|
|
|
+ fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
|
|
|
+ QPointF pointend;
|
|
|
+ double hdgrange = parc->GetLength()*parc->GetCurvature();
|
|
|
+ double hdgend = parc->GetHdg() + hdgrange;
|
|
|
+ while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
|
|
|
+ while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
|
|
|
+
|
|
|
+ if(parc->GetCurvature() >0)
|
|
|
+ {
|
|
|
+ pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
|
|
|
+ pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
|
|
|
+ pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
|
|
|
+ }
|
|
|
+
|
|
|
+ fdis[3] = calcpointdis(pointnow,pointend);
|
|
|
+ int indexmin = -1;
|
|
|
+ double fdismin = 1000000.0;
|
|
|
+ if(bp1inarc)
|
|
|
+ {
|
|
|
+ indexmin = 0;fdismin = fdis[0];
|
|
|
+ }
|
|
|
+ if(bp2inarc)
|
|
|
+ {
|
|
|
+ if(indexmin == -1)
|
|
|
+ {
|
|
|
+ indexmin = 1;fdismin = fdis[1];
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(fdis[1]<fdismin)
|
|
|
+ {
|
|
|
+ indexmin = 1;fdismin = fdis[1];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(indexmin == -1)
|
|
|
+ {
|
|
|
+ indexmin = 2;fdismin = fdis[2];
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(fdis[2]<fdismin)
|
|
|
+ {
|
|
|
+ indexmin = 2;fdismin = fdis[2];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(fdis[3]<fdismin)
|
|
|
+ {
|
|
|
+ indexmin = 3;fdismin = fdis[3];
|
|
|
+ }
|
|
|
+
|
|
|
+ double hdgdiff;
|
|
|
+ switch (indexmin) {
|
|
|
+ case 0:
|
|
|
+ nearx = point1.x();
|
|
|
+ neary = point1.y();
|
|
|
+ if(parc->GetCurvature()<0)
|
|
|
+ {
|
|
|
+ nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
|
|
|
+ }
|
|
|
+ while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
|
|
|
+ while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
|
|
|
+ hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/abs(parc->GetCurvature()));
|
|
|
+ if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
|
|
|
+ if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
|
|
|
+ frels = hdgdiff * R;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ nearx = point2.x();
|
|
|
+ neary = point2.y();
|
|
|
+ if(parc->GetCurvature()<0)
|
|
|
+ {
|
|
|
+ nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
|
|
|
+ }
|
|
|
+ while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
|
|
|
+ while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
|
|
|
+ hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/abs(parc->GetCurvature()));
|
|
|
+ if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
|
|
|
+ if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
|
|
|
+ frels = hdgdiff * R;
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ nearx = parc->GetX();
|
|
|
+ neary = parc->GetY();
|
|
|
+ nearhead = parc->GetHdg();
|
|
|
+ frels = 0;
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ nearx = pointend.x();
|
|
|
+ neary = pointend.y();
|
|
|
+ nearhead = hdgend;
|
|
|
+ frels = parc->GetLength();
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ std::cout<<"error in arcdis "<<std::endl;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
|
|
|
+ while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
|
|
|
+ return fdismin;
|
|
|
+}
|
|
|
+
|
|
|
+double xodrfunc::GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
|
|
|
+ double & neary,double & nearhead,double & frels)
|
|
|
+{
|
|
|
+
|
|
|
+ double x,y,hdg;
|
|
|
+ double s = 0.0;
|
|
|
+ double fdismin = 100000.0;
|
|
|
+ double s0 = pspiral->GetS();
|
|
|
+
|
|
|
+ frels = 0;
|
|
|
+ while(s<pspiral->GetLength())
|
|
|
+ {
|
|
|
+ pspiral->GetCoords(s0+s,x,y,hdg);
|
|
|
+
|
|
|
+
|
|
|
+ double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
|
|
|
+ if(fdis<fdismin)
|
|
|
+ {
|
|
|
+ fdismin = fdis;
|
|
|
+ nearhead = hdg;
|
|
|
+ nearx = x;
|
|
|
+ neary = y;
|
|
|
+ frels = s;
|
|
|
+ }
|
|
|
+ s = s+0.1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return fdismin;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief GetLineDis 获得点到直线Geometry的距离。
|
|
|
+ * @param pline
|
|
|
+ * @param x
|
|
|
+ * @param y
|
|
|
+ * @param nearx
|
|
|
+ * @param neary
|
|
|
+ * @param nearhead
|
|
|
+ * @return
|
|
|
+ */
|
|
|
+double xodrfunc::GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
|
|
|
+ double & neary,double & nearhead,double & frels)
|
|
|
+{
|
|
|
+ double fRtn = 1000.0;
|
|
|
+
|
|
|
+ double a1,a2,a3,a4,b1,b2;
|
|
|
+ double ratio = pline->GetHdg();
|
|
|
+ while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
|
|
|
+ while(ratio<0)ratio = ratio+2.0*M_PI;
|
|
|
+
|
|
|
+ double dis1,dis2,dis3;
|
|
|
+ double x1,x2,x3,y1,y2,y3;
|
|
|
+ x1 = pline->GetX();y1=pline->GetY();
|
|
|
+ if((ratio == 0)||(ratio == M_PI))
|
|
|
+ {
|
|
|
+ a1 = 0;a4=0;
|
|
|
+ a2 = 1;b1= pline->GetY();
|
|
|
+ a3 = 1;b2= x;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
|
|
|
+ {
|
|
|
+ a2=0;a3=0;
|
|
|
+ a1=1,b1=pline->GetX();
|
|
|
+ a4 = 1;b2 = y;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ a1 = tan(ratio) *(-1.0);
|
|
|
+ a2 = 1;
|
|
|
+ a3 = tan(ratio+M_PI/2.0)*(-1.0);
|
|
|
+ a4 = 1;
|
|
|
+ b1 = a1*pline->GetX() + a2 * pline->GetY();
|
|
|
+ b2 = a3*x+a4*y;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ y2 = y1 + pline->GetLength() * sin(ratio);
|
|
|
+ x2 = x1 + pline->GetLength() * cos(ratio);
|
|
|
+
|
|
|
+ Eigen::Matrix2d A;
|
|
|
+ A<<a1,a2,
|
|
|
+ a3,a4;
|
|
|
+ Eigen::Vector2d B(b1,b2);
|
|
|
+
|
|
|
+ Eigen::Vector2d opoint = A.lu().solve(B);
|
|
|
+
|
|
|
+ x3 = opoint(0);
|
|
|
+ y3 = opoint(1);
|
|
|
+
|
|
|
+ dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
|
|
|
+ dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
|
|
|
+ dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
|
|
|
+
|
|
|
+
|
|
|
+ if((dis1>pline->GetLength())||(dis2>pline->GetLength())) //Outoff line
|
|
|
+ {
|
|
|
+ // std::cout<<" out line"<<std::endl;
|
|
|
+ if(dis1<dis2)
|
|
|
+ {
|
|
|
+ fRtn = dis1;
|
|
|
+ nearx = x1;neary=y1;nearhead = pline->GetHdg();
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ fRtn = dis2;
|
|
|
+ nearx = x2;neary=y2;nearhead = pline->GetHdg();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ fRtn = dis3;
|
|
|
+ nearx = x3;neary=y3;nearhead = pline->GetHdg();
|
|
|
+ }
|
|
|
+
|
|
|
+ frels = sqrt(pow(nearx - pline->GetX(),2)+pow(neary - pline->GetY(),2));
|
|
|
+
|
|
|
+ return fRtn;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+int xodrfunc::GetNearPoint(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad, GeometryBlock **pgeo, double &fdis, double &nearx, double &neary, double &nearhead, const double nearthresh,double * pfs,int * pnlane)
|
|
|
+{
|
|
|
+ double dismin = std::numeric_limits<double>::infinity();
|
|
|
+ fdis = dismin;
|
|
|
+ int i;
|
|
|
+ *pObjRoad = 0;
|
|
|
+ for(i=0;i<pxodr->GetRoadCount();i++)
|
|
|
+ {
|
|
|
+ int j;
|
|
|
+ Road * proad = pxodr->GetRoad(i);
|
|
|
+ double nx,ny,nh,frels;
|
|
|
+
|
|
|
+ for(j=0;j<proad->GetGeometryBlockCount();j++)
|
|
|
+ {
|
|
|
+ GeometryBlock * pgb = proad->GetGeometryBlock(j);
|
|
|
+ double dis;
|
|
|
+ RoadGeometry * pg;
|
|
|
+ int nlane = 1000;
|
|
|
+ pg = pgb->GetGeometryAt(0);
|
|
|
+
|
|
|
+ switch (pg->GetGeomType()) {
|
|
|
+ case 0: //line
|
|
|
+ dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
|
|
|
+ break;
|
|
|
+ case 2: //arc
|
|
|
+ dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
|
|
|
+ break;
|
|
|
+
|
|
|
+ case 3:
|
|
|
+ dis = 100000.0;
|
|
|
+ break;
|
|
|
+ case 4:
|
|
|
+ dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ dis = 100000.0;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(dis < 100)
|
|
|
+ {
|
|
|
+ double faccuratedis;
|
|
|
+ faccuratedis = GetAcurateDis(x,y,proad,frels+pg->GetS(),nx,ny,nh,&nlane);
|
|
|
+ if(faccuratedis < dis)dis = faccuratedis;
|
|
|
+ }
|
|
|
+ if(dis < dismin)
|
|
|
+ {
|
|
|
+ dismin = dis;
|
|
|
+ nearx = nx;
|
|
|
+ neary = ny;
|
|
|
+ nearhead = nh;
|
|
|
+ fdis = dis;
|
|
|
+ *pObjRoad = proad;
|
|
|
+ *pgeo = pgb;
|
|
|
+ if(pfs != 0)*pfs = frels +pg->GetS();
|
|
|
+ if(pnlane != 0)*pnlane = nlane;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(fdis > nearthresh)return -1;
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad, const double s,const double x, const double y,const double fhdg)
|
|
|
+{
|
|
|
+ int i;
|
|
|
+ int nLSCount = pRoad->GetLaneSectionCount();
|
|
|
+ double s_section = 0;
|
|
|
+
|
|
|
+ std::vector<iv::LanePoint> xvectorlanepoint;
|
|
|
+ for(i=0;i<nLSCount;i++)
|
|
|
+ {
|
|
|
+// if((pRoad->GetRoadId() == "30012")&&(s>35))
|
|
|
+// {
|
|
|
+// int a= 1;
|
|
|
+// }
|
|
|
+ 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 nlanemarktype = -1; //default no lanetype
|
|
|
+ int nlanetype = 2; //driving
|
|
|
+ int nlanecolor = 0;
|
|
|
+ int k;
|
|
|
+ double s_lane = 0;
|
|
|
+ for(k=0;k<pLane->GetLaneRoadMarkCount();k++)
|
|
|
+ {
|
|
|
+ LaneRoadMark * plrm = pLane->GetLaneRoadMark(k);
|
|
|
+ if(k<(pLane->GetLaneRoadMarkCount()-1))
|
|
|
+ {
|
|
|
+ if(pLane->GetLaneRoadMark(k+1)->GetS()<s_lane)
|
|
|
+ {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(plrm->GetType() == "solid")
|
|
|
+ {
|
|
|
+ nlanemarktype = 0;
|
|
|
+ }
|
|
|
+ if(plrm->GetType() == "broken")
|
|
|
+ {
|
|
|
+ nlanemarktype = 1;
|
|
|
+ }
|
|
|
+ if(plrm->GetType() == "solid solid")
|
|
|
+ {
|
|
|
+ nlanemarktype = 2;
|
|
|
+ }
|
|
|
+ if(plrm->GetType() == "solid broken")
|
|
|
+ {
|
|
|
+ nlanemarktype = 3;
|
|
|
+ }
|
|
|
+ if(plrm->GetType() == "broken solid")
|
|
|
+ {
|
|
|
+ nlanemarktype = 4;
|
|
|
+ }
|
|
|
+ if(plrm->GetType() == "broken broken")
|
|
|
+ {
|
|
|
+ nlanemarktype = 5;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(plrm->GetColor() == "standard")nlanecolor = 0;
|
|
|
+ if(plrm->GetColor() == "blue")nlanecolor = 1;
|
|
|
+ if(plrm->GetColor() == "green")nlanecolor = 2;
|
|
|
+ if(plrm->GetColor() == "red")nlanecolor = 3;
|
|
|
+ if(plrm->GetColor() == "white")nlanecolor = 4;
|
|
|
+ if(plrm->GetColor() == "yellow")nlanecolor = 5;
|
|
|
+ if(plrm->GetColor() == "orange")nlanecolor = 6;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(pLane->GetType() == "shoulder")
|
|
|
+ {
|
|
|
+ nlanetype = 0;
|
|
|
+ }
|
|
|
+ if(pLane->GetType() == "border")
|
|
|
+ {
|
|
|
+ nlanetype = 1;
|
|
|
+ }
|
|
|
+ if(pLane->GetType() == "driving")
|
|
|
+ {
|
|
|
+ nlanetype = 2;
|
|
|
+ }
|
|
|
+ if(pLane->GetType() == "none")
|
|
|
+ {
|
|
|
+ nlanetype = 4;
|
|
|
+ }
|
|
|
+ if(pLane->GetType() == "biking")
|
|
|
+ {
|
|
|
+ nlanetype = 8;
|
|
|
+ }
|
|
|
+ if(pLane->GetType() == "sidewalk")
|
|
|
+ {
|
|
|
+ nlanetype = 9;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(pLane->GetId() != 0)
|
|
|
+ {
|
|
|
+
|
|
|
+// if((pRoad->GetRoadId() == "10012")&&(pLane->GetId()==1))
|
|
|
+// {
|
|
|
+// int a= 1;
|
|
|
+// }
|
|
|
+
|
|
|
+// int k;
|
|
|
+// double s_lane = 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;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ iv::LanePoint lp;
|
|
|
+ lp.mnlanetype = nlanetype;
|
|
|
+ lp.mnlanemarktype = nlanemarktype;
|
|
|
+ lp.mnlanecolor = nlanecolor;
|
|
|
+ lp.mnlane = pLane->GetId();
|
|
|
+ lp.mnLaneSection = i;
|
|
|
+ double fds = s - s_lane - s_section;
|
|
|
+ lp.mflanewidth = pLW->GetA() + pLW->GetB() * fds
|
|
|
+ +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
|
|
|
+ lp.mflanetocenter = 0;
|
|
|
+ lp.mnlanetype = nlanetype;
|
|
|
+ lp.mfhdg = fhdg;
|
|
|
+ lp.mS = s;
|
|
|
+ xvectorlanepoint.push_back(lp);
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ iv::LanePoint lp;
|
|
|
+ lp.mnlanetype = nlanetype;
|
|
|
+ lp.mnlanemarktype = nlanemarktype;
|
|
|
+ lp.mnlanecolor = nlanecolor;
|
|
|
+ lp.mnlane = 0;
|
|
|
+ lp.mnLaneSection = i;
|
|
|
+ lp.mflanewidth = 0;
|
|
|
+ lp.mflanetocenter = 0;
|
|
|
+ lp.mfhdg = fhdg;
|
|
|
+ lp.mS = s;
|
|
|
+ xvectorlanepoint.push_back(lp);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for(j=0;j<xvectorlanepoint.size();j++)
|
|
|
+ {
|
|
|
+ int k;
|
|
|
+ for(k=0;k<xvectorlanepoint.size();k++)
|
|
|
+ {
|
|
|
+ if(abs(xvectorlanepoint[k].mnlane)>abs((xvectorlanepoint[j].mnlane)))
|
|
|
+ {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ if(xvectorlanepoint[k].mnlane * xvectorlanepoint[j].mnlane <= 0)
|
|
|
+ {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ xvectorlanepoint[j].mflanetocenter = xvectorlanepoint[j].mflanetocenter + xvectorlanepoint[k].mflanewidth;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for(j=0;j<xvectorlanepoint.size();j++)
|
|
|
+ {
|
|
|
+ if(xvectorlanepoint[j].mnlane < 0)
|
|
|
+ {
|
|
|
+ xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg-M_PI/2.0);
|
|
|
+ xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg-M_PI/2.0);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg+M_PI/2.0);
|
|
|
+ xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg+M_PI/2.0);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ break;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<iv::LanePoint> xvectorlanepointrtn;
|
|
|
+ bool bIsSort = true;
|
|
|
+ for(i=0;i<(xvectorlanepoint.size()-1);i++)
|
|
|
+ {
|
|
|
+ if(xvectorlanepoint[i].mnlane < xvectorlanepoint[i+1].mnlane)
|
|
|
+ {
|
|
|
+ bIsSort = false;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(bIsSort == false)
|
|
|
+ {
|
|
|
+ while(xvectorlanepoint.size() > 0)
|
|
|
+ {
|
|
|
+ int nlanemin;
|
|
|
+ nlanemin = 0;
|
|
|
+ int nlanenum = xvectorlanepoint[0].mnlane;
|
|
|
+ for(i=1;i<xvectorlanepoint.size();i++)
|
|
|
+ {
|
|
|
+ if(xvectorlanepoint[i].mnlane >= nlanenum)
|
|
|
+ {
|
|
|
+ nlanenum = xvectorlanepoint[i].mnlane;
|
|
|
+ nlanemin = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ xvectorlanepointrtn.push_back(xvectorlanepoint[nlanemin]);
|
|
|
+ xvectorlanepoint.erase(xvectorlanepoint.begin() + nlanemin);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ xvectorlanepointrtn = xvectorlanepoint;
|
|
|
+ }
|
|
|
+ return xvectorlanepointrtn;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+double xodrfunc::GetAcurateDis(const double x, const double y, Road *pRoad, const double s, const double nearx, const double neary, const double nearhead,int * pnlane)
|
|
|
+{
|
|
|
+ double fdismin = 1000;
|
|
|
+ if(pRoad->GetLaneSectionCount() < 1)return 1000;
|
|
|
+
|
|
|
+ std::vector<iv::LanePoint> xvectorlanepoint = GetAllLanePoint(pRoad,s,nearx,neary,nearhead);
|
|
|
+
|
|
|
+ double fdistoref = sqrt(pow(x-nearx,2)+pow(y-neary,2));
|
|
|
+
|
|
|
+ int i;
|
|
|
+ std::vector<double> xvectordis;
|
|
|
+ int nsize = xvectorlanepoint.size();
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ double fdis = sqrt(pow(x-xvectorlanepoint[i].mfX,2)+pow(y-xvectorlanepoint[i].mfY,2));
|
|
|
+ xvectordis.push_back(fdis);
|
|
|
+ if(fdismin>fdis)fdismin = fdis;
|
|
|
+
|
|
|
+ }
|
|
|
+ int nlane = -1000;
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ if((xvectordis[i]<=xvectorlanepoint[i].mflanewidth)&&(fdistoref <= xvectorlanepoint[i].mflanetocenter))
|
|
|
+ {
|
|
|
+ nlane = xvectorlanepoint[i].mnlane;
|
|
|
+ fdismin = 0; //On Lane, is very near.
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(pnlane != 0)*pnlane = nlane;
|
|
|
+
|
|
|
+ return fdismin;
|
|
|
+
|
|
|
+
|
|
|
+}
|
