modularization/src/tool/map_lanetoxodr/xodrfunc.cpp

#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::GetPoly3Dis(GeometryPoly3 * ppoly,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 = 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;

    frels = 0;

    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 = xodrfunc::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;
            frels = flen;
        }

        oldx = x;
        oldy = y;
    }

    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;



}


namespace iv {
struct nearoption
{
    Road * pRoad;
    GeometryBlock * pgeob;
    double fdis;
    double nearx;
    double neary;
    double nearhead;
    double fs;
    int nlane;
    double fgeodis;
};
}

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,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;

        for(j=0;j<proad->GetGeometryBlockCount();j++)
        {
            GeometryBlock * pgb = proad->GetGeometryBlock(j);
            double dis;
            RoadGeometry * pg;
            int nlane = 1000;
            pg = pgb->GetGeometryAt(0);

            if((sqrt(pow(x-pg->GetX(),2)+pow(y- pg->GetY(),2))-pg->GetLength())>nearthresh)
            {
                continue;
            }

            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 = GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
                break;
            case 4:
                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
                break;
            default:
                dis = 100000.0;
                break;
            }

            double fgeodis;

            fgeodis = dis;
            if((dis < 100)&&(bnotuselane == false))
            {
                double faccuratedis;
                faccuratedis = GetAcurateDis(x,y,proad,frels+pg->GetS(),nx,ny,nh,&nlane);
                if(faccuratedis < dis)dis = faccuratedis;

            }

            if(dis == 0)
            {
                iv::nearoption xopt;
                xopt.fdis = dis;
                xopt.fgeodis = fgeodis;
                xopt.fs = frels +pg->GetS();
                xopt.nearhead = nh;
                xopt.nearx = nx;
                xopt.neary = ny;
                xopt.nlane = nlane;
                xopt.pgeob = pgb;
                xopt.pRoad = proad;
                xvectornearopt.push_back(xopt);
            }

            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(xvectornearopt.size() > 1)
    {
        double fgeodismin = 1000;
        int nindex = 0;
        for(i=0;i<xvectornearopt.size();i++)
        {
            if(xvectornearopt.at(i).fgeodis < fgeodismin)
            {
                fgeodismin = xvectornearopt.at(i).fgeodis;
                nindex = i;
            }
        }
        dismin = xvectornearopt.at(nindex).fdis;
        nearx = xvectornearopt.at(nindex).nearx;
        neary = xvectornearopt.at(nindex).neary;
        nearhead = xvectornearopt.at(nindex).nearhead;
        fdis = dismin;
        *pObjRoad = xvectornearopt.at(nindex).pRoad;
        *pgeo = xvectornearopt.at(nindex).pgeob;
        if(pfs != 0)*pfs = xvectornearopt.at(nindex).fs;
        if(pnlane != 0)*pnlane = xvectornearopt.at(nindex).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 = s-s_section;
            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;
                     }
                 }
                 else
                 {
                     if(s_lane<plrm->GetS())
                     {
                         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;
                lp.mfGeoX = x;
                lp.mfGeoY = y;
                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;
                lp.mfGeoX = x;
                lp.mfGeoY = y;
                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;


}



int xodrfunc::GetLineXY(GeometryLine *pline, double soff, double &x, double &y, double &hdg)
{
    if(soff<0)return -1;

    hdg = pline->GetHdg();
    x = pline->GetX() + soff*cos(hdg);
    y = pline->GetY() + soff*sin(hdg);
    return 0;
}

Road * xodrfunc::GetRoadByID(OpenDrive * pxodr,std::string strroadid)
{
    Road * pRoad = 0;
    int nroadcount = pxodr->GetRoadCount();
    int i;
    for(i=0;i<nroadcount;i++)
    {
        if(pxodr->GetRoad(i)->GetRoadId() == strroadid)
        {
            pRoad = pxodr->GetRoad(i);
            break;
        }
    }
    return pRoad;
}

int xodrfunc::GetSpiralXY(GeometrySpiral *pspira, double soff, double &x, double &y, double &hdg)
{
    pspira->GetCoords(pspira->GetS() + soff,x,y,hdg);
    return 0;
}

int xodrfunc::GetArcXY(GeometryArc *parc, double soff, double &x, double &y, double &hdg)
{
    if(parc->GetCurvature() == 0)return -1;
    double R = fabs(1.0/parc->GetCurvature());

    //calculate arc center
    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);

    double arcdiff = soff/R;

    if(parc->GetCurvature() > 0)
    {
        x = x_center + R * cos(parc->GetHdg() + arcdiff - M_PI/2.0);
        y = y_center + R * sin(parc->GetHdg() + arcdiff - M_PI/2.0);
        hdg = parc->GetHdg() + arcdiff;
    }
    else
    {
        x = x_center + R * cos(parc->GetHdg() -arcdiff + M_PI/2.0);
        y = y_center + R * sin(parc->GetHdg() -arcdiff + M_PI/2.0);
        hdg = parc->GetHdg() - arcdiff;
    }



    return 0;
}

int xodrfunc::GetParamPoly3XY(GeometryParamPoly3 *pparam3d, double soff, double &x, double &y, double &hdg)
{
    double xtem,ytem;
    double ua,ub,uc,ud,va,vb,vc,vd;
    ua = pparam3d->GetuA();ub= pparam3d->GetuB();uc= pparam3d->GetuC();ud = pparam3d->GetuD();
    va = pparam3d->GetvA();vb= pparam3d->GetvB();vc= pparam3d->GetvC();vd = pparam3d->GetvD();
//        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 * soff  + uc * soff * soff  + ud * soff *soff*soff ;
    ytem = va + vb * soff + vc * soff*soff  + vd * soff*soff*soff ;
    x = xtem*cos(pparam3d->GetHdg()) - ytem * sin(pparam3d->GetHdg()) + pparam3d->GetX();
    y = xtem*sin(pparam3d->GetHdg()) + ytem * cos(pparam3d->GetHdg()) + pparam3d->GetY();
    if(soff<0.3)hdg = pparam3d->GetHdg();
    else
    {
        double soff1 = soff - 0.1;
        double x1,y1;
        xtem = ua + ub * soff1  + uc * soff1 * soff1  + ud * soff1 *soff1*soff1 ;
        ytem = va + vb * soff1 + vc * soff1*soff1  + vd * soff1*soff1*soff1 ;
        x1 = xtem*cos(pparam3d->GetHdg()) - ytem * sin(pparam3d->GetHdg()) + pparam3d->GetX();
        y1 = xtem*sin(pparam3d->GetHdg()) + ytem * cos(pparam3d->GetHdg()) + pparam3d->GetY();
        hdg = CalcHdg(QPointF(x1,y1),QPointF(x,y));
    }
    return 0;
}

int xodrfunc::GetRoadXYByS(Road *pRoad, const double s, double &x, double &y, double &hdg)
{
    if(s<0)return -1;
    if(s>(pRoad->GetRoadLength()+0.1))return -2;
    if(pRoad == 0)return -3;
    if(pRoad->GetGeometryBlockCount()<1)return -4;
    int i;
    int nroadgeosize = pRoad->GetGeometryBlockCount();
    RoadGeometry * pgeosel = pRoad->GetGeometryBlock(nroadgeosize -1)->GetGeometryAt(0);
    for(i=0;i<(nroadgeosize-1);i++)
    {
        if(s<pRoad->GetGeometryBlock(i+1)->GetGeometryAt(0)->GetS())
        {

            pgeosel = pRoad->GetGeometryBlock(0)->GetGeometryAt(0);
            break;
        }
    }


    switch (pgeosel->GetGeomType()) {
    case 0:
        return GetLineXY((GeometryLine *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
        break;
    case 1:
        return GetSpiralXY((GeometrySpiral *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
        break;
    case 2:
        return GetArcXY((GeometryArc *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);

        break;
    case 3:
        break;
    case 4:
        return GetParamPoly3XY((GeometryParamPoly3 *)pgeosel,(s-pgeosel->GetS()),x,y,hdg);
        break;
    default:
        break;
    }
    return -5;
}

int xodrfunc::GetRoadIndex(OpenDrive * pxodr, Road *pRoad)
{
    int nroadcount = pxodr->GetRoadCount();
    int i;
    for(i=0;i<nroadcount;i++)
    {
        if(pxodr->GetRoad(i) == pRoad)
        {
            return i;
        }
    }
    return -1;
}