#include "dialogroadoptimize.h" #include "ui_dialogroadoptimize.h" #include #include #include "geofit.h" DialogRoadOptimize::DialogRoadOptimize(Road * pRoad,OpenDrive * pxodr,QWidget *parent) : QDialog(parent), ui(new Ui::DialogRoadOptimize) { ui->setupUi(this); ui->lineEdit_disthresh->setText("0.1"); ui->lineEdit_hdgthresh->setText("0.01"); ui->lineEdit_arcbezel->setText("3.0"); ui->lineEdit_linebezel->setText("10.0"); ui->lineEdit_roadoffthresh->setText("0.5"); mpRoad = pRoad; mpxodr = pxodr; std::vector fvectordis,fvectorhdg; int nrtn = CheckRoadQuality(pRoad,fvectordis,fvectorhdg); const int noutbufsize = 100000; std::shared_ptr pstrout_ptr = std::shared_ptr(new char[noutbufsize]); char strline[1000]; snprintf(pstrout_ptr.get(),noutbufsize,"Check point:%d\n",nrtn); if(nrtn > 0) { int i; for(i=0;iGetGeometryBlock(i)->GetGeometryAt(0); snprintf(strline,1000,"%d distance:%f hdgdiff:%f geo type:%d s:%f length %f\n", i,fvectordis[i],fvectorhdg[i],pg->GetGeomType(),pg->GetS(),pg->GetLength()); strncat(pstrout_ptr.get(),strline,noutbufsize); } } ui->plainTextEdit->setPlainText(pstrout_ptr.get()); setWindowTitle("Road Optimize"); } DialogRoadOptimize::~DialogRoadOptimize() { delete ui; } int DialogRoadOptimize::CheckRoadQuality(Road *pRoad, std::vector & fvectordis, std::vector & fvectorhdg) { if(pRoad->GetGeometryBlockCount() <=1) { return 0; } int nrtn = 0; unsigned int i; for(i=0;i<(pRoad->GetGeometryBlockCount()-1);i++) { GeometryBlock * pgeob1 = pRoad->GetGeometryBlock(i); GeometryBlock * pgeob2 = pRoad->GetGeometryBlock(i+1); double x1,y1,hdg1; double x2,y2,hdg2; pgeob1->GetGeometryAt(0)->GetCoords(pgeob1->GetGeometryAt(0)->GetS() + pgeob1->GetGeometryAt(0)->GetLength(), x1,y1,hdg1); pgeob2->GetGeometryAt(0)->GetCoords(pgeob2->GetGeometryAt(0)->GetS() , x2,y2,hdg2); double fdis = sqrt(pow(x2-x1,2)+pow(y2-y1,2)); double fhdgdiff = hdg2 - hdg1; while (fhdgdiff>M_PI)fhdgdiff = fhdgdiff -2.0*M_PI; while(fhdgdiff<=(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI; fvectordis.push_back(fdis); fvectorhdg.push_back(fhdgdiff); nrtn++; } return nrtn; } void DialogRoadOptimize::on_pushButton_Optimize_clicked() { double fdisthresh = ui->lineEdit_disthresh->text().toDouble(); double fhdgthresh = ui->lineEdit_hdgthresh->text().toDouble(); double flinebezel = ui->lineEdit_linebezel->text().toDouble(); double farcbezel = ui->lineEdit_arcbezel->text().toDouble(); double froadoffthresh = ui->lineEdit_roadoffthresh->text().toDouble(); if(fdisthresh<0.00001) { fdisthresh = 0.00001; std::cout<<"fdisthresh change to "< fvectordis,fvectorhdg; Road * pRoad = mpRoad; int nrtn = CheckRoadQuality(pRoad,fvectordis,fvectorhdg); if(nrtn == 0) { QMessageBox::information(this,"Info","Not Need Optimize",QMessageBox::YesAll); return; } int nmsgbufsize = 100000; std::shared_ptr strmsg_ptr = std::shared_ptr(new char[nmsgbufsize]); snprintf(strmsg_ptr.get(),nmsgbufsize,"Change pos: \n"); int nchange = 0; Road * pnewroad = mpRoad; int i; int nowgeobpos = 0; GeometryBlock * pgeobnow = pnewroad->GetGeometryBlock(0); std::vector * pvectorgeo = pnewroad->GetGeometryBlockVector(); for(i=0;ifdisthresh)||(fabs(fvectorhdg[i])>fhdgthresh)) { pgeobnow = pnewroad->GetGeometryBlock(nowgeobpos); RoadGeometry * pgeo = pgeobnow->GetGeometryAt(0); int ngeotype = pgeo->GetGeomType(); std::cout<<"geo type : "<GetGeomType() ==0) ||(pgeo->GetGeomType() == 2)) { double fthresh = flinebezel; if(ngeotype == 2)fthresh = farcbezel; if(pgeo->GetLength()>(fthresh+1.0)) { double xkeeplen = pgeo->GetLength() - fthresh; double x_start,y_start,hdg_start,x_end,y_end,hdg_end; pgeo->GetCoords(pgeo->GetS()+xkeeplen,x_start,y_start,hdg_start); pnewroad->GetGeometryBlock(nowgeobpos+1)->GetGeometryAt(0)->GetCoords(pgeo->GetS() + pgeo->GetLength(), x_end,y_end,hdg_end); std::vector xvectorgeo = geofit::CreateBezierGeo(x_start,y_start, hdg_start,x_end,y_end,hdg_end); GeometryBlock xgb; qDebug("be len: %f ",xvectorgeo[0].mfLen); xgb.AddGeometryParamPoly3(pgeo->GetS() + xkeeplen,x_start,y_start,hdg_start,xvectorgeo[0].mfLen, xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1], xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3], xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1], xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3],false); double fdisgeo = GetGeo1toGeo2MaxDis(&xgb,&(pvectorgeo->at(nowgeobpos))); if(fdisgeo<= froadoffthresh) { pgeo->SetLength(xkeeplen); pvectorgeo->insert(pvectorgeo->begin() + nowgeobpos+1,xgb); nowgeobpos = nowgeobpos+2; char strout[1000]; snprintf(strout,1000,"geo %d to geo %d. Insert a bezel.geo off:%f.\n",i,i+1,fdisgeo); strncat(strmsg_ptr.get(),strout,nmsgbufsize); nchange++; } else { char strout[1000]; snprintf(strout,1000,"geo %d to geo %d. Optimize off %f exceed thresh.\n",i,i+1,fdisgeo); strncat(strmsg_ptr.get(),strout,nmsgbufsize); nchange++; nowgeobpos++; } } else { double x_start,y_start,hdg_start,x_end,y_end,hdg_end; pgeo->GetCoords(pgeo->GetS(),x_start,y_start,hdg_start); pnewroad->GetGeometryBlock(nowgeobpos+1)->GetGeometryAt(0)->GetCoords(pgeo->GetS() + pgeo->GetLength(), x_end,y_end,hdg_end); std::vector xvectorgeo = geofit::CreateBezierGeo(x_start,y_start, hdg_start,x_end,y_end,hdg_end); qDebug("be len: %f ",xvectorgeo[0].mfLen); GeometryBlock xgb; xgb.AddGeometryParamPoly3(pgeo->GetS(),x_start,y_start,hdg_start,xvectorgeo[0].mfLen, xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1], xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3], xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1], xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3],false); double fdisgeo = GetGeo1toGeo2MaxDis(&xgb,&(pvectorgeo->at(nowgeobpos))); if(fdisgeo <= froadoffthresh) { pvectorgeo->erase(pvectorgeo->begin()+nowgeobpos); pvectorgeo->insert(pvectorgeo->begin()+nowgeobpos,xgb); nowgeobpos = nowgeobpos+1; char strout[1000]; snprintf(strout,1000,"geo %d to geo %d. geo %d replace by a bezel. geo off:%f.\n",i,i+1,i,fdisgeo); strncat(strmsg_ptr.get(),strout,nmsgbufsize); nchange++; } else { char strout[1000]; snprintf(strout,1000,"geo %d to geo %d. Optimize off %f exceed thresh.\n",i,i+1,fdisgeo); strncat(strmsg_ptr.get(),strout,nmsgbufsize); nchange++; nowgeobpos++; } // break; } unsigned int j; for(j=1;jsize();j++) { pvectorgeo->at(j).GetGeometryAt(0)->SetS(pvectorgeo->at(j-1).GetGeometryAt(0)->GetS() +pvectorgeo->at(j-1).GetGeometryAt(0)->GetLength()); } } else { nowgeobpos++; } } else { char strout[1000]; snprintf(strout,1000,"geo %d to geo %d. Not Need Optimize.\n",i,i+1); strncat(strmsg_ptr.get(),strout,nmsgbufsize); nowgeobpos++; } } if(nchange > 0) { RoadGeometry * pgeolast = pvectorgeo->at(pvectorgeo->size()-1).GetGeometryAt(0); pnewroad->SetRoadLength(pgeolast->GetS() + pgeolast->GetLength()); } char strout[1000]; snprintf(strout,1000,"Optimize %d position.",nchange); strncat(strmsg_ptr.get(),strout,nmsgbufsize); QMessageBox::information(this,"Optimize Result",strmsg_ptr.get(),QMessageBox::YesAll); } double DialogRoadOptimize::GetGeo1toGeo2MaxDis(GeometryBlock *pgeob1, GeometryBlock *pgeob2) { RoadGeometry * prg1,*prg2; prg1 = pgeob1->GetGeometryAt(0); prg2 = pgeob2->GetGeometryAt(0); double fstep = 0.1; int nCount = (int)(prg1->GetLength()/fstep); if(nCount == 0) { return sqrt(pow(prg1->GetX() - prg2->GetX(),2)+pow(prg1->GetY() - prg2->GetY(),2)); } int i; double rg1_s = prg1->GetS(); double rg2_s = prg2->GetS(); // double rg1_len = prg1->GetLength(); double rg2_len = prg2->GetLength(); double fdismax = 0; double fsmax = 0; for(i=0;iGetCoords(rg1_s+s,x1,y1,hdg1); int nCount2 =(int)( rg2_len/fstep2); double snear = 0; double sdis = 1000.0; if(nCount2 == 0) { snear = 0; } else { int j; for(j=0;jGetCoords(rg2_s +s2,x2,y2,hdg2); double fdis = sqrt(pow(x2-x1,2)+pow(y2-y1,2)); if(fdis rg2_len)snear_high = rg2_len; double s3 = snear_low; while(s3<=snear_high) { double x3,y3,hdg3; prg2->GetCoords(rg2_s+s3,x3,y3,hdg3); double fdis = sqrt(pow(x3-x1,2)+pow(y3-y1,2)); if(fdis < sdis) { snear = s3; sdis = fdis; } s3 = s3 +0.01; } if(sdis > fdismax) { fdismax = sdis; fsmax = snear; } } // std::cout<<"dismax : "<