modularization/src/tool/map_lanetoxodr/autoconnect.cpp

#include "autoconnect.h"

#include <vector>

#include <math.h>
#include <iostream>

namespace iv {


struct laneconnect
{
    unsigned int nroadid1;
    unsigned int nroadid2;
    int nlaneid1;
    int nlaneid2;
    unsigned int nstart1; //1 start 2 end;
    unsigned int nstart2; //1 start 2 end;

};

struct prenextlane
{
    unsigned int nroadid;
    int nlaneidcur;
    int nlaneidoth;
    int nconnect; //1 start 2 end

};


struct ivjunctionlanelink
{
    int incominglane;
    int connectlane;
};

struct ivjunctionlink
{
    int incommingroad;
    int connectroad;
    int connectpoint; //1 start 2 end
    int nid;
    std::vector<ivjunctionlanelink> xvectorlanelink;
};

struct ivjunction
{
    std::vector<ivjunctionlink> xvectorjunctionlink;
    int nid;
    int nopid = -1;
//    int nsameid = -1;  //is same as from id .used for merge same.
};


struct roadconnect
{
    unsigned int nroadid;
    std::vector<iv::prenextlane> xvectorpre;
    std::vector<iv::prenextlane> xvectornxt;
    bool mbJunctionpre = false;
    bool mbJunctionnxt = false;
    bool mbInJunction = false;
    int nprejunctionindex;
    int nnxtjunctionindex;
};

struct roadpoint
{
    unsigned int nRoadID;
    unsigned int nstart; //1 start 2 end;
    int nlane;
    double x;
    double y;
    double z;
    double mfhdg;
    bool bIsEnterRoad = false; //true enter this road, false out this road.
};






}


namespace  iv {

struct lanewidthabcd
{
    int nlane;
    double A;
    double B;
    double C;
    double D;
};
}

double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
{
    double frtn = 0;


    int i;
    int nlanecount = xvectorlwa.size();
    for(i=0;i<nlanecount;i++)
    {
        if(nlane == xvectorlwa.at(i).nlane)
        {
            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
            break;
        }
    }
    return frtn;
}

std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
{
    std::vector<iv::lanewidthabcd> xvectorlwa;
    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
    int i;
    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
    for(i=0;i<nlanecount;i++)
    {
        iv::lanewidthabcd xlwa;


        LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
        xlwa.nlane = pRoad->GetLaneSection(0)->GetLane(i)->GetId();
        if(pLW != 0)
        {

            xlwa.A = pLW->GetA();
            xlwa.B = pLW->GetB();
            xlwa.C = pLW->GetC();
            xlwa.D = pLW->GetD();

        }
        else
        {
            xlwa.A = 0;
            xlwa.B = 0;
            xlwa.C = 0;
            xlwa.D = 0;
        }


        xvectorlwa.push_back(xlwa);

    }
    return xvectorlwa;

}


inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
{
    int i;
    double off = 0;
    double a,b,c,d;
    if(xvectorIndex.size() == 0)return off;
    for(i=0;i<(xvectorIndex.size()-1);i++)
    {

        a = xvectorLWA[xvectorIndex[i]].A;
        b = xvectorLWA[xvectorIndex[i]].B;
        c = xvectorLWA[xvectorIndex[i]].C;
        d = xvectorLWA[xvectorIndex[i]].D;
        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
        {
            off = off + a + b*s + c*s*s + d*s*s*s;
        }
        else
        {
            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
        }
    }
    i = xvectorIndex.size()-1;
    a = xvectorLWA[xvectorIndex[i]].A;
    b = xvectorLWA[xvectorIndex[i]].B;
    c = xvectorLWA[xvectorIndex[i]].C;
    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;
    return off;

}

std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
{
    std::vector<int> xvectorindex;
    int i;
    if(nlane >= 0)
    {
    for(i=0;i<=nlane;i++)
    {
       int j;
       int nsize = xvectorLWA.size();
       for(j=0;j<nsize;j++)
       {
           if(i == xvectorLWA.at(j).nlane )
           {
               xvectorindex.push_back(j);
               break;
           }
       }
    }
    }
    else
    {
        for(i=0;i>=nlane;i--)
        {
           int j;
           int nsize = xvectorLWA.size();
           for(j=0;j<nsize;j++)
           {
               if(i == xvectorLWA.at(j).nlane )
               {
                   xvectorindex.push_back(j);
                   break;
               }
           }
        }
    }
    return xvectorindex;
}


static double getoff(Road *p, int nlane, bool bstart)
{
    double off = 0;
    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(p);
    std::vector<int> xvectorindex1;
    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
    if(bstart)
    {
        off = getoff(nlane,0,xvectorLWA,xvectorindex1);
    }
    else
    {
        off = getoff(nlane,p->GetRoadLength(),xvectorLWA,xvectorindex1);
    }

    return off;
}


static std::vector<iv::roadpoint> getstartpoint(Road * p1)
{
    double startx,starty,starthdg,starthgt;
    double off1;
    int nlane;

    std::vector<iv::roadpoint> xvectorroadpoint;
    if(p1->GetLaneSectionCount() == 0)return xvectorroadpoint;
    int i;
    for(i=0;i<p1->GetLaneSection(0)->GetLaneCount();i++)
    {
        nlane = p1->GetLaneSection(0)->GetLane(i)->GetId();
        if(nlane == 0)continue;
        starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
        off1 = getoff(p1,nlane,true);
        startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
        starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();


        LaneOffset * pLO = p1->GetLaneOffset(0);
        double fofflane = 0.0;
        if(pLO != NULL)
        {
            double s_tem = 0;
            fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
                    +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
        }
        startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
        starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
        starthgt = 0;
        if(p1->GetElevationCount()>0)
        {
            starthgt = p1->GetElevation(0)->GetA();
        }
        if(nlane<0)
        {
            starthdg = starthdg + M_PI;
            if(starthdg >= 2.0*M_PI)starthdg = starthdg - 2.0*M_PI;
        }
        iv::roadpoint rp;
        rp.nRoadID = atoi(p1->GetRoadId().data());
        rp.nlane = nlane;
        rp.nstart = 1;
        rp.mfhdg = starthdg;
        rp.x = startx;
        rp.y = starty;
        rp.z = starthgt;
        if(nlane < 0)rp.bIsEnterRoad = true;
        if(nlane > 0) rp.bIsEnterRoad = false;
        xvectorroadpoint.push_back(rp);
    }
    return xvectorroadpoint;
}

int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
{
    proad->GetGeometryCoords(proad->GetRoadLength(),x,y,hdg);
    return 0;
    GeometryBlock * pblock = proad->GetLastGeometryBlock();

    RoadGeometry * pgeo = pblock->GetLastGeometry();

    //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
    switch (pgeo->GetGeomType()) {
    case 0:
        {
        GeometryLine * pline = (GeometryLine *)pgeo;
        x = pline->GetX() + pline->GetLength() * cos(pline->GetHdg());
        y = pline->GetY() + pline->GetLength() * sin(pline->GetHdg());
        hdg = pline->GetHdg();
        }
        return 0;
        break;
    case 1:
        {
        GeometryArc * parc = (GeometryArc *)pgeo;
        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);
        x = x_center + fabs(1.0/parc->GetCurvature()) * cos(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
        y = y_center + fabs(1.0/parc->GetCurvature()) * sin(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
        hdg = parc->GetHdg() + parc->GetLength() * parc->GetCurvature();
        return 0;
        }
        break;
    case 2:
        {
        GeometrySpiral * pspiral = (GeometrySpiral *)pgeo;
        pspiral->GetCoords(pspiral->GetS()+pspiral->GetLength(),x,y,hdg);
        return 0;
        }
        break;
    case 3:
 //       QMessageBox::warning(this,"warn","type not supported.");
        break;
    case 4:
        {
        double xtem,ytem;
        double xtem1,ytem1,x1,y1;
        GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
        double s = ppoly3->GetLength();
        if(ppoly3->GetNormal())
        {
            xtem = ppoly3->GetuA() + ppoly3->GetuB()  + ppoly3->GetuC()   + ppoly3->GetuD()  ;
            ytem = ppoly3->GetvA() + ppoly3->GetvB() + ppoly3->GetvC()   + ppoly3->GetvD() ;
        }
        else
        {
            xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
            ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
        }
//        xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
//        ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
        x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
        y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
        s = ppoly3->GetLength()*0.999;
 //       double fgeolen;
        double fr = 1.0;
        if(s>0)
        {
            double frel = 0.99;
            if(ppoly3->GetNormal() == false)
            {
                xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
                ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
            }
            else
            {
                xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * frel + ppoly3->GetuC() *frel*frel   + ppoly3->GetuD()*frel*frel*frel  ;
                ytem1 = ppoly3->GetvA() + ppoly3->GetvB()*frel + ppoly3->GetvC()*frel*frel   + ppoly3->GetvD()*frel*frel*frel ;
            }
            fr = 1.0;
//            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * fr  + ppoly3->GetuC() * fr*fr  + ppoly3->GetuD() * fr*fr*fr ;
//            ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * fr + ppoly3->GetvC() * fr*fr  + ppoly3->GetvD() * fr*fr*fr ;
            x1 = xtem1*cos(ppoly3->GetHdg()) - ytem1 * sin(ppoly3->GetHdg()) + ppoly3->GetX();
            y1 = xtem1*sin(ppoly3->GetHdg()) + ytem1 * cos(ppoly3->GetHdg()) + ppoly3->GetY();
            hdg = geofit::CalcHdg(x1,y1,x,y);
        }
        else
        {
            hdg = 0;
        }
        return 0;
        }
        break;
    default:
 //       QMessageBox::warning(this,"warn","type not supported.");
        break;
    }


    return -1;
}


static std::vector<iv::roadpoint> getendpoint(Road * p1)
{

    double startx,starty,starthdg,starthgt;
    double off1;
    int nlane;

    std::vector<iv::roadpoint> xvectorroadpoint;
    if(p1->GetLaneSectionCount() == 0)return xvectorroadpoint;
    int i;
    for(i=0;i<p1->GetLastLaneSection()->GetLaneCount();i++)
    {
        nlane = p1->GetLastLaneSection()->GetLane(i)->GetId();
        if(nlane == 0)continue;

        if(GetEndPoint(p1,startx,starty,starthdg) != 0)
        {
            continue;
        }



        off1 = getoff(p1,nlane,false);

        LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);

        double fofflane = 0.0;
        if(pLO!= NULL)
        {
            double s_tem = p1->GetRoadLength() - pLO->GetS();
            fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
                    +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
        }
        startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
        starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
        starthgt = 0;
        if(p1->GetElevationCount()>0)
        {
            Elevation * pe = p1->GetLastElevation();
            double s = p1->GetRoadLength();
            double a, b,c,d;
            a = pe->GetA();
            b = pe->GetB();
            c = pe->GetC();
            d = pe->GetD();
            starthgt = a + b*s + c*s*s + d*s*s*s;
        }

        if(nlane<0)
        {
            starthdg = starthdg + M_PI;
            if(starthdg >= 2.0*M_PI)starthdg = starthdg - 2.0*M_PI;
        }
        iv::roadpoint rp;
        rp.nRoadID = atoi(p1->GetRoadId().data());
        rp.nlane = nlane;
        rp.nstart = 2;
        rp.mfhdg = starthdg;
        rp.x = startx;
        rp.y = starty;
        rp.z = starthgt;
        if(nlane < 0)rp.bIsEnterRoad = false;
        if(nlane > 0) rp.bIsEnterRoad = true;
        xvectorroadpoint.push_back(rp);
    }
    return xvectorroadpoint;
}

static std::vector<iv::roadpoint> getroadpoint(OpenDrive * pxodr)
{
    std::vector<iv::roadpoint> xvectorroadpoint;
    int i;
    for(i=0;i<pxodr->GetRoadCount();i++)
    {
        Road * p1 = pxodr->GetRoad(i);
        std::vector<iv::roadpoint> x1,x2;
        x1 = getstartpoint(p1);
        x2 = getendpoint(p1);
        int j;
        for(j=0;j<x1.size();j++)xvectorroadpoint.push_back(x1.at(j));
        for(j=0;j<x2.size();j++)xvectorroadpoint.push_back(x2.at(j));
    }
    for(i=0;i<xvectorroadpoint.size();i++)
    {
        std::cout<<xvectorroadpoint[i].nRoadID<<" "<<xvectorroadpoint[i].nlane<<" "<<xvectorroadpoint[i].nstart<<" "<<xvectorroadpoint[i].x<<" "<<xvectorroadpoint[i].y<<std::endl;
    }
    return xvectorroadpoint;
}


static bool IsConn(iv::roadpoint rp1,iv::roadpoint rp2)
{
    if(rp1.nRoadID == rp2.nRoadID)
    {
        return false;
    }
    if(rp1.nstart == rp2.nstart)
    {
        if(rp1.nlane * rp2.nlane > 0)
        {
            return false;
        }
    }

    if(rp1.bIsEnterRoad == rp2.bIsEnterRoad)
    {
        return false;
    }
    double fdis;
    fdis = sqrt(pow(rp1.x-rp2.x,2)+pow(rp1.y -rp2.y,2));
    if(fdis>1.0)return false;
//    if(rp1.nRoadID == 10011)
//    {
//        int x = 1;
//        x = 2;
//    }

    double fhdgdiff = rp1.mfhdg - rp2.mfhdg;
    if(fhdgdiff>M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
    if(fhdgdiff<(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI;
    if(fabs(fhdgdiff)>1.0)return false;
    return true;
}

static std::vector<iv::laneconnect> getlaneconn(std::vector<iv::roadpoint>  xvectorroadpoint)
{
    std::vector<iv::laneconnect> xvectorroadconn;

    int i;
    for(i=0;i<(xvectorroadpoint.size()-1);i++)
    {
        iv::roadpoint rp1;
        rp1 = xvectorroadpoint[i];
        int j;
        for(j=i+1;j<xvectorroadpoint.size();j++)
        {
            iv::roadpoint rp2;
            rp2 = xvectorroadpoint[j];
            if(IsConn(rp1,rp2))
            {
                iv::laneconnect lc;
                lc.nlaneid1 = rp1.nlane;
                lc.nlaneid2 = rp2.nlane;
                lc.nroadid1 = rp1.nRoadID;
                lc.nroadid2 = rp2.nRoadID;
                lc.nstart1 = rp1.nstart;
                lc.nstart2 = rp2.nstart;
                xvectorroadconn.push_back(lc);
            }

        }
    }

    return xvectorroadconn;
}

static std::vector<iv::roadconnect> getroadconn(std::vector<iv::laneconnect> xvectorlaneconn)
{
    std::vector<iv::roadconnect> xvectorroadconn;

    //获得road连接关系
    int i;
    for(i=0;i<xvectorlaneconn.size();i++)
    {
        iv::laneconnect xlc = xvectorlaneconn.at(i);
        bool bhave1,bhave2;
        bhave1 = false;
        bhave2 = false;
        iv::roadconnect * p1, * p2;
        int np1pos,np2pos;
        int j;
        for(j=0;j<xvectorroadconn.size();j++)
        {
            if(xvectorroadconn[j].nroadid == xlc.nroadid1)
            {
                bhave1 = true;
                np1pos = j;
                p1 = &xvectorroadconn[j];
            }
            if(xvectorroadconn[j].nroadid == xlc.nroadid2)
            {
                bhave2 = true;
                np2pos = j;
                p2 = &xvectorroadconn[j];
            }
            if(bhave1 && bhave2)break;
        }
        if(bhave1 == false)
        {
            iv::roadconnect rc;
            rc.nroadid = xlc.nroadid1;
            xvectorroadconn.push_back(rc);
            np1pos = xvectorroadconn.size() -1;
            p1 = &xvectorroadconn[xvectorroadconn.size()-1];
        }
        if(bhave2 == false)
        {
            iv::roadconnect rc;
            rc.nroadid = xlc.nroadid2;
            xvectorroadconn.push_back(rc);
            np2pos = xvectorroadconn.size() -1;
            p2 = &xvectorroadconn[xvectorroadconn.size()-1];
        }
        p1 = &xvectorroadconn[np1pos];
        p2 = &xvectorroadconn[np2pos];
        iv::prenextlane xpx;
        xpx.nlaneidcur = xlc.nlaneid1;
        xpx.nlaneidoth = xlc.nlaneid2;
        xpx.nroadid = xlc.nroadid2;
        xpx.nconnect = xlc.nstart2;
        if(xlc.nstart1 == 1)
        {
            p1->xvectorpre.push_back(xpx);
        }
        else
        {
            p1->xvectornxt.push_back(xpx);
        }

        xpx.nlaneidcur = xlc.nlaneid2;
        xpx.nlaneidoth = xlc.nlaneid1;
        xpx.nroadid = xlc.nroadid1;
        xpx.nconnect = xlc.nstart1;
        if(xlc.nstart2 == 1)
        {
            p2->xvectorpre.push_back(xpx);
        }
        else
        {
            p2->xvectornxt.push_back(xpx);
        }



    }


    //判断road的连接是否是junction
    for(i=0;i<xvectorroadconn.size();i++)
    {
        iv::roadconnect * pr = &xvectorroadconn[i];
        int j;
        int nfirstid;
        if(pr->xvectorpre.size()>0)nfirstid = pr->xvectorpre[0].nroadid;
        for(j=1;j<pr->xvectorpre.size();j++)
        {
            if(pr->xvectorpre[j].nroadid != nfirstid)
            {
                pr->mbJunctionpre = true;
                break;
            }
        }

        if(pr->xvectornxt.size()>0)nfirstid = pr->xvectornxt[0].nroadid;
        for(j=1;j<pr->xvectornxt.size();j++)
        {
            if(pr->xvectornxt[j].nroadid != nfirstid)
            {
                pr->mbJunctionnxt = true;
                break;
            }
        }
    }




    //设置junction区域为juncion


    return xvectorroadconn;
}

static std::vector<iv::ivjunction> getjunction(std::vector<iv::roadconnect> * pxvectorroadconn)
{
    std::vector<iv::ivjunction> xvectorjunction;

    int i;
    for(i=0;i<pxvectorroadconn->size();i++)
    {
        iv::roadconnect * pr = &pxvectorroadconn->at(i);

        if(pr->mbJunctionpre)
        {
            int j;
            iv::ivjunction ij;
            for(j=0;j<pr->xvectorpre.size();j++)
            {
                int nroadid = pr->xvectorpre[j].nroadid;
                int k;
                iv::prenextlane * plc = &pr->xvectorpre[j];
                iv::ivjunctionlanelink ijl;
                ijl.incominglane = plc->nlaneidcur;
                ijl.connectlane = plc->nlaneidoth;

                bool bfindjunction = false;
                int nx = -1;
                for(k=0;k<ij.xvectorjunctionlink.size();k++)
                {
                    if(ij.xvectorjunctionlink[k].connectroad == nroadid)
                    {
                       nx = k;
                       bfindjunction = true;
                       break;
                    }
                }
                if(bfindjunction == false)
                {
                    iv::ivjunctionlink ijk;
                    ijk.incommingroad = pr->nroadid;
                    ijk.connectpoint = plc->nconnect;
                    ijk.connectroad = plc->nroadid;
                    ij.xvectorjunctionlink.push_back(ijk);
                    nx = ij.xvectorjunctionlink.size() - 1;
                }
                iv::ivjunctionlink * pijk = &ij.xvectorjunctionlink[nx];

                pijk->xvectorlanelink.push_back(ijl);


            }
            xvectorjunction.push_back(ij);
            pr->nprejunctionindex = xvectorjunction.size()-1;
        }


        if(pr->mbJunctionnxt)
        {
            int j;
            iv::ivjunction ij;
            for(j=0;j<pr->xvectornxt.size();j++)
            {
                int nroadid = pr->xvectornxt[j].nroadid;
                int k;
                iv::prenextlane * plc = &pr->xvectornxt[j];
                iv::ivjunctionlanelink ijl;
                ijl.incominglane = plc->nlaneidcur;
                ijl.connectlane = plc->nlaneidoth;

                bool bfindjunction = false;
                int nx = -1;
                for(k=0;k<ij.xvectorjunctionlink.size();k++)
                {
                    if(ij.xvectorjunctionlink[k].connectroad == nroadid)
                    {
                       nx = k;
                       bfindjunction = true;
                       break;
                    }
                }
                if(bfindjunction == false)
                {
                    iv::ivjunctionlink ijk;
                    ijk.incommingroad = pr->nroadid;
                    ijk.connectpoint = plc->nconnect;
                    ijk.connectroad = plc->nroadid;
                    ij.xvectorjunctionlink.push_back(ijk);
                    nx = ij.xvectorjunctionlink.size() - 1;
                }
                iv::ivjunctionlink * pijk = &ij.xvectorjunctionlink[nx];

                pijk->xvectorlanelink.push_back(ijl);

            }
            xvectorjunction.push_back(ij);
            pr->nnxtjunctionindex = xvectorjunction.size() -1;
        }


    }

    int idnow = 0;
    if(xvectorjunction.size() > 0)
    {
        xvectorjunction[0].nid = idnow;
        idnow++;
    }

    for(i=1;i<xvectorjunction.size();i++)
    {
        bool bhavejunction = false;
        int noldid = -1;
        iv::ivjunction * pij = &xvectorjunction[i];
        int j;
        for(j=0;j<i;j++)
        {
            iv::ivjunction * pijpre = &xvectorjunction[j];
            int k;
            for(k=0;k<pijpre->xvectorjunctionlink.size();k++)
            {
                iv::ivjunctionlink * pijlinkpre = &pijpre->xvectorjunctionlink[k];
                int m;
                for(m=0;m<pij->xvectorjunctionlink.size();m++)
                {
                    iv::ivjunctionlink * pijlinknow = &pij->xvectorjunctionlink[m];
                    if(pijlinknow->connectroad == pijlinkpre->connectroad)
                    {
                        noldid = pijpre->nid;
                        bhavejunction = true;
                        break;
                    }
                }
                if(bhavejunction)break;
            }
            if(bhavejunction)break;;
        }
        if(bhavejunction)
        {
            pij->nid = noldid;
        }
        else
        {
            pij->nid = idnow;
            idnow++;
        }

    }

    return xvectorjunction;
}

static int CreateJunctionID(OpenDrive * pxodr)
{
    int i;
    bool bUsed = false;
    int njunctionidstart  = 800000;

    int njunctioncount = pxodr->GetJunctionCount();
    if(njunctioncount == 0)return njunctionidstart;
    int * pjunctionid = new int[njunctioncount];
    for(i=0;i<njunctioncount;i++)
    {
        pjunctionid[i]=atoi(pxodr->GetJunction(i)->GetId().data());
    }
    do
    {
        bUsed = false;
    for(i=0;i<njunctioncount;i++)
    {
        if(pjunctionid[i] == njunctionidstart)
        {
            njunctionidstart++;
            bUsed = true;
        }
    }
    }while(bUsed);
    delete pjunctionid;
    return njunctionidstart;

}


static Junction * GetJunctioByID(OpenDrive * pxodr,int nopid)
{
    int i;
    for(i=0;i<pxodr->GetJunctionCount();i++)
    {
        if(atoi(pxodr->GetJunction(i)->GetId().data()) == nopid)
        {
            return pxodr->GetJunction(i);
        }
    }
    return 0;
}

static Road * GetRoadByID(OpenDrive * pxodr,int nroadid)
{
    int i;
    for(i=0;i<pxodr->GetRoadCount();i++)
    {
        if(atoi(pxodr->GetRoad(i)->GetRoadId().data()) == nroadid)
        {
            return pxodr->GetRoad(i);
        }
    }
    return 0;
}

static void ChangeOpenDrive(OpenDrive * pxodr,std::vector<iv::ivjunction> * pxvectorivjunction,std::vector<iv::roadconnect> * pxvectorroadconn)
{
    unsigned int i;
    for(i=0;i<pxvectorivjunction->size();i++)
    {
        bool bnewjunction = true;
        Junction * pj = 0;
        iv::ivjunction * pivjunction = &pxvectorivjunction->at(i);
        if(i==0)
        {
            bnewjunction = true;
        }
        else
        {
            int j;
            for(j=0;j<i;j++)
            {
                if(pxvectorivjunction->at(j).nid == pivjunction->nid)
                {
                    pivjunction->nopid = pxvectorivjunction->at(j).nopid;
                    bnewjunction = false;
                    pj = GetJunctioByID(pxodr,pivjunction->nopid);
                    break;
                }
            }
        }

        if(pj == 0)bnewjunction = true;

        if(bnewjunction)
        {
            int njunctionid = CreateJunctionID(pxodr);
            pxodr->AddJunction("",QString::number(njunctionid).toStdString());
            pivjunction->nopid = njunctionid;
            pj = pxodr->GetJunction(pxodr->GetJunctionCount()-1);
        }

        int j;
        JunctionConnection * pjc = 0;
        for(j=0;j<pivjunction->xvectorjunctionlink.size();j++)
        {
            iv::ivjunctionlink * pivjunctionlink = &pivjunction->xvectorjunctionlink[j];
            string id = QString::number(pj->GetJunctionConnectionCount()).toStdString();
            string strincomming = QString::number(pivjunctionlink->incommingroad).toStdString();
            string strconnecting = QString::number(pivjunctionlink->connectroad).toStdString();
            Road * proad = GetRoadByID(pxodr,pivjunctionlink->connectroad);
            if(proad != 0)
            {
                proad->SetRoadJunction(pj->GetId());
            }


            string strcontactpoint;
            if(pivjunctionlink->connectpoint == 1)
            {
                strcontactpoint = "start";
            }
            else
            {
                strcontactpoint = "end";
            }
            pj->AddJunctionConnection(id,strincomming,strconnecting,strcontactpoint);
            pjc = pj->GetJunctionConnection(pj->GetJunctionConnectionCount() - 1);
            int k;
            for(k=0;k<pivjunctionlink->xvectorlanelink.size();k++)
            {
                iv::ivjunctionlanelink * pivjunctionlanelink = &pivjunctionlink->xvectorlanelink[k];
                pjc->AddJunctionLaneLink(pivjunctionlanelink->incominglane,pivjunctionlanelink->connectlane);
            }
        }
    }

    for(i=0;i<pxvectorroadconn->size();i++)
    {
        iv::roadconnect * pr = &pxvectorroadconn->at(i);
        Road * proad = GetRoadByID(pxodr,pr->nroadid);
        if(pr->mbJunctionpre)
        {
            string strpre = QString::number(pxvectorivjunction->at(pr->nprejunctionindex).nopid).toStdString();
            proad->SetPredecessor("junction",strpre,"start");

            LaneSection * pLS = proad->GetLaneSection(0);
            unsigned int k;
            for(k=0;k<pLS->GetLaneCount();k++)
            {
                if(pLS->GetLane(k)->IsPredecessorSet())
                {
                    pLS->GetLane(k)->RemovePredecessor();
                }
            }
        }
        else
        {
            if(pr->xvectorpre.size()>0)
            {
               string strroadid = QString::number(pr->xvectorpre[0].nroadid).toStdString();
               string strcontact = "start";
               if(pr->xvectorpre[0].nconnect == 2)strcontact = "end";
               proad->SetPredecessor("road",strroadid,strcontact);
               int j;
               for(j=0;j<pr->xvectorpre.size();j++)
               {
                   int nlane = pr->xvectorpre[j].nlaneidcur;
                   int nlaneoth = pr->xvectorpre[j].nlaneidoth;
                   Lane * plane = 0;
                   proad = GetRoadByID(pxodr,pr->nroadid);
                   LaneSection * pLS = proad->GetLaneSection(0);
                   int k;
                   for(k=0;k<pLS->GetLaneCount();k++)
                   {
                       if(pLS->GetLane(k)->GetId() == nlane)
                       {
                           plane = pLS->GetLane(k);
                           break;
                       }
                   }
                   plane->SetPredecessor(nlaneoth);
               }
            }
        }
        if(pr->mbJunctionnxt)
        {
            string strnxt = QString::number(pxvectorivjunction->at(pr->nnxtjunctionindex).nopid).toStdString();
            proad->SetSuccessor("junction",strnxt,"end");

            LaneSection * pLS = proad->GetLastLaneSection();
            unsigned int k;
            for(k=0;k<pLS->GetLaneCount();k++)
            {
                if(pLS->GetLane(k)->IsSuccessorSet())
                    pLS->GetLane(k)->RemoveSuccessor();
            }
        }
        else
        {
            if(pr->xvectornxt.size()>0)
            {
               string strroadid = QString::number(pr->xvectornxt[0].nroadid).toStdString();
               string strcontact = "start";
               if(pr->xvectornxt[0].nconnect == 2)strcontact = "end";
               proad->SetSuccessor("road",strroadid,strcontact);
               int j;
               for(j=0;j<pr->xvectornxt.size();j++)
               {
                   int nlane = pr->xvectornxt[j].nlaneidcur;
                   int nlaneoth = pr->xvectornxt[j].nlaneidoth;
                   Lane * plane = 0;
                   proad = GetRoadByID(pxodr,pr->nroadid);
                   LaneSection * pLS = proad->GetLastLaneSection();
                   int k;
                   for(k=0;k<pLS->GetLaneCount();k++)
                   {
                       if(pLS->GetLane(k)->GetId() == nlane)
                       {
                           plane = pLS->GetLane(k);
                           break;
                       }
                   }
                   plane->SetSuccessor(nlaneoth);
               }
            }
        }
    }



}

AutoConnect::AutoConnect(OpenDrive * pxodr)
{
    mpxodr = pxodr;

    while(mpxodr->GetJunctionCount()>0)mpxodr->DeleteJunction(0);

    Road * proad;
    int nroadcount = mpxodr->GetRoadCount();
    int i;
    for(i=0;i<nroadcount;i++)
    {
        proad = mpxodr->GetRoad(i);
        proad->SetRoadJunction("-1");
    }

}


void AutoConnect::Connect()
{

    std::vector<iv::roadpoint> xvectorroadpoint = getroadpoint(mpxodr);

    std::vector<iv::laneconnect> xvectorlaneconn = getlaneconn(xvectorroadpoint);

    std::vector<iv::roadconnect> xvectorroadconn = getroadconn(xvectorlaneconn);

    std::vector<iv::ivjunction> xvectorivjunction = getjunction(&xvectorroadconn);

    ChangeOpenDrive(mpxodr,&xvectorivjunction,&xvectorroadconn);



}