modularization/src/tool/map_lanetoxodr/dialogparkingspacefromrtk.cpp

#include "dialogparkingspacefromrtk.h"
#include "ui_dialogparkingspacefromrtk.h"

#include "xodrfunc.h"

#include "mainwindow.h"

extern MainWindow * gw;

extern double glon0 ;
extern double glat0;


DialogParkingSpaceFromRTK::DialogParkingSpaceFromRTK(Road * pRoad,QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DialogParkingSpaceFromRTK)
{
    ui->setupUi(this);

    mpRoad = pRoad;

    mLat0 = glat0;
    mLon0 = glon0;

    setWindowTitle("ParkingSpace From RTK");
}

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

void DialogParkingSpaceFromRTK::on_pushButton_LoadData_clicked()
{
    QString strfilepath = QFileDialog::getOpenFileName(this,tr("Open RTK Data"),"",tr("RTK File(*.csv)"));
    if(strfilepath.isEmpty())return;

    QFile xFile;
    xFile.setFileName(strfilepath);

    std::vector<iv::rtkdata> xvectorrtkdata;
    xvectorrtkdata.clear();

    ui->plainTextEdit->clear();

    if(xFile.open(QIODevice::ReadWrite))
    {
        QByteArray ba;
        ba = xFile.read(xFile.size());
        QList<QByteArray> strlinedata=  ba.split('\n');
        int i;
        int nsize = strlinedata.size();
        const int nstartline = 1;
        for(i=nstartline;i<nsize;i++)
        {
            double flat,flon,fheight,frelx,frely;
            QList<QByteArray> strlistvalue = strlinedata.at(i).split(',');
            int ncount = strlistvalue.size();
            if(ncount>=4)
            {
                flon  = atof(strlistvalue.at(2).data());
                flat = atof(strlistvalue.at(1).data());
                QString strheight = strlistvalue.at(3);
                strheight = strheight.left(strheight.size() -2);
                fheight = strheight.toDouble();

                DialogAddRoadFromRTK::calcrelxy(mLon0,mLat0,flon,flat,frelx,frely);

                iv::rtkdata xdata;
                xdata.mflat = flat;
                xdata.mflon = flon;
                xdata.mfrelx = frelx;
                xdata.mfrely = frely;
                xdata.mheight = fheight;
                if(xvectorrtkdata.size() < 1)
                {
                    xdata.mfrels = 0;
                }
                else
                {
                    xdata.mfrels = xvectorrtkdata[xvectorrtkdata.size()-1].mfrels
                            +sqrt(pow(xvectorrtkdata[xvectorrtkdata.size()-1].mfrelx - frelx,2)+pow(xvectorrtkdata[xvectorrtkdata.size()-1].mfrely - frely,2));
                }
                xvectorrtkdata.push_back(xdata);

                QString strline = QString::number(flon,'f',7) + QString("\t")
                        +QString::number(flat,'f',7)+QString("\t")
                        +QString::number(fheight,'f',4)+QString("\t")
                        +QString::number(frelx,'f',2) + QString("\t")
                        +QString::number(frely,'f',2) + QString("\t");
                ui->plainTextEdit->appendPlainText(strline);
            }
        }
    }

    mvectorrtkdata = xvectorrtkdata;

    unsigned int nsize = mvectorrtkdata.size();
    ui->comboBox_Center_From->clear();
    ui->comboBox_Center_To->clear();
    ui->comboBox_Heading_From->clear();
    ui->comboBox_Heading_To->clear();
    if(nsize >= 3)
    {
        unsigned int i;
        for(i=0;i<nsize;i++)
        {
            ui->comboBox_Heading_From->addItem(QString::number(i));
            ui->comboBox_Heading_To->addItem(QString::number(i));
            ui->comboBox_Center_From->addItem(QString::number(i));
            ui->comboBox_Center_To->addItem(QString::number(i));
        }
        ui->comboBox_Heading_From->setCurrentIndex(1);
        ui->comboBox_Heading_To->setCurrentIndex(0);
        ui->comboBox_Center_From->setCurrentIndex(0);
        ui->comboBox_Center_To->setCurrentIndex(2);
    }


}

void DialogParkingSpaceFromRTK::on_pushButton_Create_clicked()
{
    if(mvectorrtkdata.size()<3)
    {
        QMessageBox::warning(this,"Warning","At Least 3 Points.",QMessageBox::YesAll);
        return;
    }

    int nHeadingFrom = ui->comboBox_Heading_From->currentIndex();
    int nHeadingTo = ui->comboBox_Heading_To->currentIndex();
    int nCenterFrom = ui->comboBox_Center_From->currentIndex();
    int nCenterTo = ui->comboBox_Center_To->currentIndex();

    double fHdg = xodrfunc::CalcHdg(QPointF(mvectorrtkdata[nHeadingFrom].mfrelx,mvectorrtkdata[nHeadingFrom].mfrely),
                                    QPointF(mvectorrtkdata[nHeadingTo].mfrelx,mvectorrtkdata[nHeadingTo].mfrely));

    double fCenter_x,fCenter_y;
    fCenter_x = (mvectorrtkdata[nCenterFrom].mfrelx + mvectorrtkdata[nCenterTo].mfrelx)/2.0;
    fCenter_y = (mvectorrtkdata[nCenterFrom].mfrely + mvectorrtkdata[nCenterTo].mfrely)/2.0;

    double fLen = sqrt(pow(mvectorrtkdata[nHeadingFrom].mfrelx - mvectorrtkdata[nHeadingTo].mfrelx,2)
                       +pow(mvectorrtkdata[nHeadingFrom].mfrely - mvectorrtkdata[nHeadingTo].mfrely,2));
    double fWidth = sqrt(pow(mvectorrtkdata[nHeadingFrom].mfrelx - mvectorrtkdata[nCenterTo].mfrelx ,2)
                         +pow(mvectorrtkdata[nHeadingFrom].mfrely - mvectorrtkdata[nCenterTo].mfrely,2));

    qDebug("fhdg:%f flen:%f fwidth:%f",fHdg,fLen,fWidth);
}