modularization/src/tool/map_lanetoxodr/dialogroadmirror.cpp

#include "dialogroadmirror.h"
#include "ui_dialogroadmirror.h"

#include "gnss_coordinate_convert.h"

#include <math.h>

#include <mainwindow.h>

extern MainWindow * gw;

extern double glon0 ;
extern double glat0 ;

DialogRoadMirror::DialogRoadMirror(OpenDrive * pxodr,Road * pRoad,QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DialogRoadMirror)
{
    ui->setupUi(this);
    mpxodr = pxodr;
    mpRoad = pRoad;

    if(pRoad != 0)
    {
        setWindowTitle(QString(pRoad->GetRoadId().data()));
    }
}

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

void DialogRoadMirror::on_pushButton_Mirror_clicked()
{

    double x0,y0;
    double rel_x,rel_y;
    double x,y;
    double flon,flat;
    flon = ui->lineEdit_Lon->text().toDouble();
    flat = ui->lineEdit_Lat->text().toDouble();
    GaussProjCal(glon0,glat0,&x0,&y0);
    GaussProjCal(flon,flat,&x,&y);
    rel_x = x - x0;
    rel_y = y - y0;

    double hdg = (90 - ui->lineEdit_Heading->text().toDouble())*M_PI/180.0;
    while(hdg < 0)hdg = hdg + 2.0*M_PI;
    while(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;

    x0 = rel_x;
    y0 = rel_y;
    double hdg0 = hdg;

    Road newroad = *mpRoad;

    int i;
    int ngeobcount = mpRoad->GetGeometryBlockCount();
    for(i=0;i<ngeobcount;i++)
    {
        double x,y,hdg;
        RoadGeometry * pgeo = newroad.GetGeometryBlock(i)->GetGeometryAt(0);
        if(pgeo == 0)continue;
        x = newroad.GetGeometryBlock(i)->GetGeometryAt(0)->GetX();
        y = newroad.GetGeometryBlock(i)->GetGeometryAt(0)->GetY();
        hdg = newroad.GetGeometryBlock(i)->GetGeometryAt(0)->GetHdg();

        x = x-x0;
        y = y- y0;

        double s,t;
        double frotate = hdg0*(-1);
        s = x*cos(frotate) -y*sin(frotate) ;
        t = x*sin(frotate) +y*cos(frotate);
        hdg = hdg + frotate;
        while(hdg<0)hdg = hdg + 2.0*M_PI;
        while(hdg>=2.0*M_PI)hdg = hdg - 2.0*M_PI;

        x = s;
        y = t*(-1);
        hdg = hdg*(-1);

        while(hdg<0)hdg = hdg + 2.0*M_PI;
        while(hdg>=2.0*M_PI)hdg = hdg - 2.0*M_PI;

        frotate = hdg0;

        s = x*cos(frotate) -y*sin(frotate) ;
        t = x*sin(frotate) +y*cos(frotate);
        hdg = hdg + frotate;
        while(hdg<0)hdg = hdg + 2.0*M_PI;
        while(hdg>=2.0*M_PI)hdg = hdg - 2.0*M_PI;


        newroad.GetGeometryBlock(i)->GetGeometryAt(0)->SetX(s + x0);
        newroad.GetGeometryBlock(i)->GetGeometryAt(0)->SetY(t + y0);
        newroad.GetGeometryBlock(i)->GetGeometryAt(0)->SetHdg(hdg);

        if(pgeo->GetGeomType() == 1)
        {
            GeometrySpiral * pSpiral = (GeometrySpiral *)pgeo;
            double startcurv = pSpiral->GetCurvatureStart() *(-1);
            double endcurv = pSpiral->GetCurvatureEnd() *(-1);
            pSpiral->SetCurvatureStart(startcurv);
            pSpiral->SetCurvatureEnd(endcurv);
        }

        if(pgeo->GetGeomType() == 2)
        {
            GeometryArc * pArc = (GeometryArc *)pgeo;
            double curv = pArc->GetCurvature();
            pArc->SetCurvature(curv*(-1));
        }
    }

    int nnewroadid = gw->CreateRoadID();
    newroad.SetRoadId(QString::number(nnewroadid).toStdString());

    mpxodr->GetRoadVector()->push_back(newroad);

    bool bSaveOldRoad = true;
    QMessageBox::StandardButton button;
    char strout[256];
    snprintf(strout,256,"New Road id is %d. Keep the old road.",nnewroadid);
    button=QMessageBox::question(this,tr("Quest"),QString(strout),QMessageBox::Yes|QMessageBox::No);
    if(button==QMessageBox::No)
    {
        bSaveOldRoad = false;
    }
    else if(button==QMessageBox::Yes)
    {
        bSaveOldRoad = true;
    }

    if(bSaveOldRoad == false)
    {
        int nroadindex = xodrfunc::GetRoadIndex(mpxodr,mpRoad);
        if(nroadindex >= 0)
        {
            mpxodr->DeleteRoad(nroadindex);
        }
    }

    this->accept();
}