modularization/src/tool/map_lanetoxodr/dialogroadmerge.cpp

#include "dialogroadmerge.h"
#include "ui_dialogroadmerge.h"

#include "xodrfunc.h"

#include <math.h>

DialogRoadMerge::DialogRoadMerge(OpenDrive * pxodr,QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DialogRoadMerge)
{
    ui->setupUi(this);

    mpxodr = pxodr;
}

DialogRoadMerge::~DialogRoadMerge()
{
    delete ui;
}


int DialogRoadMerge::MergeGeometry(Road *pRoad1, Road *pRoad2)
{
    double froad1len = pRoad1->GetRoadLength();
    int nroad2geocount = pRoad2->GetGeometryBlockCount();
    int i;
    GeometryBlock * pnewgb;
    for(i=0;i<nroad2geocount;i++)
    {


        GeometryBlock * pgb = pRoad2->GetGeometryBlock(i);

        RoadGeometry * pgeb = pgb->GetGeometryAt(0);
        if(pgeb == 0)
        {
            continue;
        }
        pRoad1->AddGeometryBlock();
        pnewgb = pRoad1->GetLastAddedGeometryBlock();
        switch (pgeb->GetGeomType()) {
        case 0:
            pnewgb->AddGeometryLine(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),pgeb->GetHdg(),pgeb->GetLength());
            break;
        case 1:
        {
            GeometrySpiral * pspiral = (GeometrySpiral *)pgeb;
            pnewgb->AddGeometrySpiral(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),pgeb->GetHdg(),pgeb->GetLength(),
                                      pspiral->GetCurvatureStart(),pspiral->GetCurvatureEnd());
        }
            break;
        case 2:
        {
            GeometryArc * parc = (GeometryArc *)pgeb;
            pnewgb->AddGeometryArc(pgeb->GetS() + froad1len,parc->GetX(),parc->GetY(),
                                   parc->GetHdg(),parc->GetLength(),parc->GetCurvature());
        }
            break;
        case 4:
        {
            GeometryParamPoly3 * pparmp3d = (GeometryParamPoly3 *)pgeb;
            pnewgb->AddGeometryParamPoly3(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),
                                          pgeb->GetHdg(),pgeb->GetLength(),pparmp3d->GetuA(),pparmp3d->GetuB(),
                                          pparmp3d->GetuC(),pparmp3d->GetuD(),pparmp3d->GetvA(),
                                          pparmp3d->GetvB(),pparmp3d->GetvC(),pparmp3d->GetvD(),pparmp3d->GetNormal());
        }
        default:
            break;
        }


    }
    return 0;
}

int DialogRoadMerge::MergeElevation(Road *pRoad1, Road *pRoad2)
{
    double froad1len = pRoad1->GetRoadLength();
    int i;
    int nroad2elecount = pRoad2->GetElevationCount();
    for(i=0;i<nroad2elecount;i++)
    {
        Elevation * pele = pRoad2->GetElevation(i);
        pRoad1->AddElevation(pele->GetS() + froad1len,pele->GetA(),pele->GetB(),
                             pele->GetC(),pele->GetD());
    }
    return 0;
}

int DialogRoadMerge::MergeLaneSection(Road *pRoad1, Road *pRoad2)
{
    double froad1len = pRoad1->GetRoadLength();
    int i;
    bool bAddRoad2LaneSection = true;
    if((pRoad1->GetLaneSectionCount() == pRoad2->GetLaneSectionCount())&&(pRoad1->GetLaneSectionCount() == 1))
    {
        if(pRoad1->GetLaneSection(0)->GetLaneCount() == pRoad2->GetLaneSection(0)->GetLaneCount())
        {
            LaneSection * pLS1 = pRoad1->GetLaneSection(0);
            LaneSection * pLS2 = pRoad2->GetLaneSection(0);
            Lane * pLane1,*pLane2;
            pLane1 = 0;
            pLane2 = 0;
            unsigned int j;
            for(j=0;j<pLS1->GetLaneCount();j++)
            {
                if(pLS1->GetLane(j)->GetId() != 0)
                {
                    pLane1 = pLS1->GetLane(j);
                    break;
                }
            }
            for(j=0;j<pLS2->GetLaneCount();j++)
            {
                if(pLS2->GetLane(j)->GetId() != 0)
                {
                    pLane2 = pLS2->GetLane(j);
                    break;
                }
            }
            if(pLane1 == pLane2)
            {
                bAddRoad2LaneSection = false;
            }
            if((pLane1 != 0) && (pLane2 != 0))
            {
                if((pLane1->GetLaneWidthCount() == pLane2->GetLaneWidthCount())&&(pLane1->GetLaneWidthCount()>0))
                {
                    LaneWidth * pLW1 = pLane1->GetLaneWidth(0);
                    LaneWidth * pLW2 = pLane2->GetLaneWidth(0);
                    if((pLW1->GetA() == pLW2->GetA())&&(pLW1->GetB() == pLW2->GetB())&&(pLW1->GetC() == pLW2->GetC())&&(pLW1->GetD() == pLW2->GetD()))
                    {
                        bAddRoad2LaneSection = false;
                    }
                }
            }
        }
    }

    if(bAddRoad2LaneSection == true)
    {
        int nroad2lanesectioncount = pRoad2->GetLaneSectionCount();
        for(i=0;i<nroad2lanesectioncount;i++)
        {
            LaneSection ls = *(pRoad2->GetLaneSection(i));
            ls.SetS(ls.GetS() + froad1len);
            pRoad1->GetLaneSectionVector()->push_back(ls);
        }
    }

    return 0;
}

int DialogRoadMerge::MergeSignal(Road *pRoad1, Road *pRoad2)
{
    double froad1len = pRoad1->GetRoadLength();
    int i;
    int nroad2sigcount = pRoad2->GetSignalCount();
    for(i=0;i<nroad2sigcount;i++)
    {
        Signal xs = *(pRoad2->GetSignal(i));
        xs.Sets(xs.Gets() + froad1len);
        pRoad1->GetSignalVector()->push_back(xs);
    }
}

void DialogRoadMerge::on_pushButton_Merge_clicked()
{
    std::string strroad1 = ui->lineEdit_Road1->text().toStdString();
    std::string strroad2 = ui->lineEdit_Road2->text().toStdString();
    Road * pRoad1 = xodrfunc::GetRoadByID(mpxodr,strroad1);
    if(pRoad1 == 0)
    {
        QMessageBox::warning(this,"Warning","Not found road1",QMessageBox::YesAll);
        return;
    }
    Road * pRoad2 = xodrfunc::GetRoadByID(mpxodr,strroad2);
    if(pRoad2 == 0)
    {
        QMessageBox::warning(this,"Warning","Not found road2",QMessageBox::YesAll);
        return;
    }

    double x1,y1,hdg1;
    double x2,y2,hdg2;
    xodrfunc::GetRoadXYByS(pRoad1,pRoad1->GetRoadLength(),x1,y1,hdg1);
    xodrfunc::GetRoadXYByS(pRoad2,0,x2,y2,hdg2);

    double fdis = sqrt(pow(x1-x2,2)+pow(y1-y2,2));

    if(fdis > 1.0)
    {
        char strout[256];
        snprintf(strout,256,"Road dis very fast. Dis is %6.3f",fdis);
        QMessageBox::warning(this,"Warning",QString(strout),QMessageBox::YesAll);
        return;
    }


    //Merge Geometry
    Road xroad;
    xroad = *pRoad1;
    double froad1len = pRoad1->GetRoadLength();

    MergeGeometry(pRoad1,pRoad2);
//    int nroad2geocount = pRoad2->GetGeometryBlockCount();
//    int i;
//    GeometryBlock * pnewgb;
//    for(i=0;i<nroad2geocount;i++)
//    {


//        GeometryBlock * pgb = pRoad2->GetGeometryBlock(i);

//        RoadGeometry * pgeb = pgb->GetGeometryAt(0);
//        if(pgeb == 0)
//        {
//            continue;
//        }
//        pRoad1->AddGeometryBlock();
//        pnewgb = pRoad1->GetLastAddedGeometryBlock();
//        switch (pgeb->GetGeomType()) {
//        case 0:
//            pnewgb->AddGeometryLine(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),pgeb->GetHdg(),pgeb->GetLength());
//            break;
//        case 1:
//        {
//            GeometrySpiral * pspiral = (GeometrySpiral *)pgeb;
//            pnewgb->AddGeometrySpiral(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),pgeb->GetHdg(),pgeb->GetLength(),
//                                      pspiral->GetCurvatureStart(),pspiral->GetCurvatureEnd());
//        }
//            break;
//        case 2:
//        {
//            GeometryArc * parc = (GeometryArc *)pgeb;
//            pnewgb->AddGeometryArc(pgeb->GetS() + froad1len,parc->GetX(),parc->GetY(),
//                                   parc->GetHdg(),parc->GetLength(),parc->GetCurvature());
//        }
//            break;
//        case 4:
//        {
//            GeometryParamPoly3 * pparmp3d = (GeometryParamPoly3 *)pgeb;
//            pnewgb->AddGeometryParamPoly3(pgeb->GetS() + froad1len,pgeb->GetX(),pgeb->GetY(),
//                                          pgeb->GetHdg(),pgeb->GetLength(),pparmp3d->GetuA(),pparmp3d->GetuB(),
//                                          pparmp3d->GetuC(),pparmp3d->GetuD(),pparmp3d->GetvA(),
//                                          pparmp3d->GetvB(),pparmp3d->GetvC(),pparmp3d->GetvD());
//        }
//        default:
//            break;
//        }


//    }


    //Merge ELevation
    MergeElevation(pRoad1,pRoad2);

    //Merge LaneSection

    MergeLaneSection(pRoad1,pRoad2);

    //Merge Signal
    MergeSignal(pRoad1,pRoad2);


    pRoad1->SetRoadLength(pRoad1->GetRoadLength() + pRoad2->GetRoadLength());
     mpxodr->DeleteRoad(xodrfunc::GetRoadIndex(mpxodr,pRoad2));

     QMessageBox::information(this,"Merge Road","Merge Road Successfully.",QMessageBox::YesAll);

     this->accept();
}