|
@@ -2,21 +2,25 @@
|
|
|
#include "ui_dialogdrawroad.h"
|
|
|
|
|
|
#include "mainwindow.h"
|
|
|
+#include "geofit.h"
|
|
|
|
|
|
extern MainWindow * gw;
|
|
|
|
|
|
-DialogDrawRoad::DialogDrawRoad(QWidget *parent) :
|
|
|
+DialogDrawRoad::DialogDrawRoad(OpenDrive * pxodr,QWidget *parent) :
|
|
|
QDialog(parent),
|
|
|
ui(new Ui::DialogDrawRoad)
|
|
|
{
|
|
|
ui->setupUi(this);
|
|
|
|
|
|
+ mpxodr = pxodr;
|
|
|
+
|
|
|
ui->comboBox_Type->addItem("两点创建Line");
|
|
|
ui->comboBox_Type->addItem("航向和长度创建Line");
|
|
|
ui->comboBox_Type->addItem("两点和半径创建Arc");
|
|
|
ui->comboBox_Type->addItem("点和半径创建Arc");
|
|
|
|
|
|
ui->comboBox_Type->setCurrentIndex(0);
|
|
|
+ ui->lineEdit_Height->setText("0.0");
|
|
|
|
|
|
connect(gw,SIGNAL(CurrentPosition(double,double)),this,SLOT(onCurrentPos(double,double)));
|
|
|
|
|
@@ -42,12 +46,13 @@ void DialogDrawRoad::on_comboBox_Type_currentIndexChanged(int index)
|
|
|
ui->label_Radius->setVisible(false);
|
|
|
ui->lineEdit_Radius->setVisible(false);
|
|
|
ui->lineEdit_hdgrange->setVisible(false);
|
|
|
+ ui->lineEdit_starthdg->setVisible(false);
|
|
|
}
|
|
|
break;
|
|
|
case 1:
|
|
|
{
|
|
|
ui->label_Point1->setText("Point(X Y)");
|
|
|
- ui->label_Point2->setText("hdg&Len(hdg len)");
|
|
|
+ ui->label_Point2->setText("Len&hdg(len hdg)");
|
|
|
ui->checkBox_point2->setVisible(false);
|
|
|
ui->label_Point2->setVisible(true);
|
|
|
ui->lineEdit_point2x->setVisible(true);
|
|
@@ -55,6 +60,7 @@ void DialogDrawRoad::on_comboBox_Type_currentIndexChanged(int index)
|
|
|
ui->label_Radius->setVisible(false);
|
|
|
ui->lineEdit_Radius->setVisible(false);
|
|
|
ui->lineEdit_hdgrange->setVisible(false);
|
|
|
+ ui->lineEdit_starthdg->setVisible(false);
|
|
|
}
|
|
|
break;
|
|
|
case 2:
|
|
@@ -69,12 +75,13 @@ void DialogDrawRoad::on_comboBox_Type_currentIndexChanged(int index)
|
|
|
ui->label_Radius->setVisible(true);
|
|
|
ui->lineEdit_Radius->setVisible(true);
|
|
|
ui->lineEdit_hdgrange->setVisible(false);
|
|
|
+ ui->lineEdit_starthdg->setVisible(false);
|
|
|
}
|
|
|
break;
|
|
|
case 3:
|
|
|
{
|
|
|
- ui->label_Point1->setText("Point(X Y");
|
|
|
- ui->label_Radius->setText("Radius&Range");
|
|
|
+ ui->label_Point1->setText("Point(X Y)");
|
|
|
+ ui->label_Radius->setText("Radius&Range&StartHdg");
|
|
|
ui->checkBox_point2->setVisible(false);
|
|
|
ui->label_Point2->setVisible(false);
|
|
|
ui->lineEdit_point2x->setVisible(false);
|
|
@@ -82,6 +89,7 @@ void DialogDrawRoad::on_comboBox_Type_currentIndexChanged(int index)
|
|
|
ui->label_Radius->setVisible(true);
|
|
|
ui->lineEdit_Radius->setVisible(true);
|
|
|
ui->lineEdit_hdgrange->setVisible(true);
|
|
|
+ ui->lineEdit_starthdg->setVisible(true);
|
|
|
}
|
|
|
break;
|
|
|
default:
|
|
@@ -106,3 +114,493 @@ void DialogDrawRoad::onCurrentPos(double x, double y)
|
|
|
ui->lineEdit_newpointx->setText(QString::number(x));
|
|
|
ui->lineEdit_newpointy->setText(QString::number(y));
|
|
|
}
|
|
|
+
|
|
|
+void DialogDrawRoad::on_pushButton_Create_clicked()
|
|
|
+{
|
|
|
+ int index = ui->comboBox_Type->currentIndex();
|
|
|
+ switch (index) {
|
|
|
+ case 0:
|
|
|
+ CreateLineWith2Point();
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ CreateLineWithPointLenHdg();
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ CreateArcWith2Point();
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ CreateArcWithRadiusLenHdg();
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void DialogDrawRoad::CreateLineWith2Point()
|
|
|
+{
|
|
|
+ double point1_x,point1_y,point2_x,point2_y;
|
|
|
+ if(ui->lineEdit_point1x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 X not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point1y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 Y not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point2x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point2 X not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point2y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point2 Y not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ point1_x = ui->lineEdit_point1x->text().toDouble();
|
|
|
+ point1_y = ui->lineEdit_point1y->text().toDouble();
|
|
|
+ point2_x = ui->lineEdit_point2x->text().toDouble();
|
|
|
+ point2_y = ui->lineEdit_point2y->text().toDouble();
|
|
|
+
|
|
|
+ double len = sqrt(pow(point1_x - point2_x,2)+pow(point1_y-point2_y,2));
|
|
|
+ if(len == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Road len is zero.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ double hdg = geofit::CalcHdg(point1_x,point1_y,point2_x,point2_y);
|
|
|
+ double height = ui->lineEdit_Height->text().toDouble();
|
|
|
+
|
|
|
+ int nrtn = CreateLineRoad(point1_x,point1_y,hdg,len,height);
|
|
|
+
|
|
|
+ if(nrtn != 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","CreateLineRoad Fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void DialogDrawRoad::CreateLineWithPointLenHdg()
|
|
|
+{
|
|
|
+ double point1_x,point1_y,len,hdg;
|
|
|
+ if(ui->lineEdit_point1x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 X not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point1y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 Y not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point2x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Length not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point2y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Hdg not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ point1_x = ui->lineEdit_point1x->text().toDouble();
|
|
|
+ point1_y = ui->lineEdit_point1y->text().toDouble();
|
|
|
+ len = ui->lineEdit_point2x->text().toDouble();
|
|
|
+ hdg = ui->lineEdit_point2y->text().toDouble();
|
|
|
+
|
|
|
+ if(len == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Road len is zero.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if((hdg<0)||(hdg>2.0*M_PI))
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","hdg must between 0.0 to 6.28",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double height = ui->lineEdit_Height->text().toDouble();
|
|
|
+
|
|
|
+ int nrtn = CreateLineRoad(point1_x,point1_y,hdg,len,height);
|
|
|
+
|
|
|
+ if(nrtn != 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","CreateLineRoad Fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void DialogDrawRoad::CreateArcWith2Point()
|
|
|
+{
|
|
|
+ double point1_x,point1_y,point2_x,point2_y;
|
|
|
+ if(ui->lineEdit_point1x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 X not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point1y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 Y not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point2x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point2 X not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point2y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point2 Y not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ point1_x = ui->lineEdit_point1x->text().toDouble();
|
|
|
+ point1_y = ui->lineEdit_point1y->text().toDouble();
|
|
|
+ point2_x = ui->lineEdit_point2x->text().toDouble();
|
|
|
+ point2_y = ui->lineEdit_point2y->text().toDouble();
|
|
|
+
|
|
|
+ double dis = sqrt(pow(point1_x - point2_x,2)+pow(point1_y-point2_y,2));
|
|
|
+ if(dis == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point dis is zero.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double xc,yc;
|
|
|
+ int nrtn = 0;
|
|
|
+ double R = ui->lineEdit_Radius->text().toDouble();
|
|
|
+ if(R == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Radius is not zero.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ nrtn = YuanXin(point1_x,point1_y,point2_x,point2_y,R,xc,yc);
|
|
|
+ if(nrtn != 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Calc Arc Center Point Fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double hdg1 = geofit::CalcHdg(xc,yc,point1_x,point1_y);
|
|
|
+ double hdg2 = geofit::CalcHdg(xc,yc,point2_x,point2_y);
|
|
|
+ if(R>0)
|
|
|
+ {
|
|
|
+ hdg1 = hdg1 + M_PI/2.0;
|
|
|
+ hdg2 = hdg2 + M_PI/2.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ hdg1 = hdg1 - M_PI/2.0;
|
|
|
+ hdg2 = hdg2 - M_PI/2.0;
|
|
|
+ }
|
|
|
+ normalhdg(hdg1);
|
|
|
+ normalhdg(hdg2);
|
|
|
+
|
|
|
+ double hdgdiff = hdg2 - hdg1;
|
|
|
+ if(R>0)
|
|
|
+ {
|
|
|
+ if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(hdgdiff>0)hdgdiff = hdgdiff - 2.0*M_PI;
|
|
|
+ }
|
|
|
+
|
|
|
+ double arclen = hdgdiff * R;
|
|
|
+ double curv = 1.0/R;
|
|
|
+
|
|
|
+ double height = ui->lineEdit_Height->text().toDouble();
|
|
|
+
|
|
|
+ nrtn = CreateArcRoad(point1_x,point1_y,hdg1,arclen,height,curv);
|
|
|
+
|
|
|
+ if(nrtn != 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","CreateLineRoad Fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void DialogDrawRoad::CreateArcWithRadiusLenHdg()
|
|
|
+{
|
|
|
+ double point1_x,point1_y,point2_x,point2_y;
|
|
|
+ if(ui->lineEdit_point1x->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 X not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if(ui->lineEdit_point1y->text().length() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Point1 Y not set.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ point1_x = ui->lineEdit_point1x->text().toDouble();
|
|
|
+ point1_y = ui->lineEdit_point1y->text().toDouble();
|
|
|
+
|
|
|
+ double R = ui->lineEdit_Radius->text().toDouble();
|
|
|
+ if(R == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Radius is not zero.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double hdgrange = ui->lineEdit_hdgrange->text().toDouble();
|
|
|
+
|
|
|
+ if((hdgrange<=0)||(hdgrange>2.0*M_PI))
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","hdg range must between 0.0 to 6.28",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double hdgstart = ui->lineEdit_starthdg->text().toDouble();
|
|
|
+ if((hdgstart<0)||(hdgstart>=2.0*M_PI))
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","hdg must between 0.0 to 6.28",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ double arclen = hdgrange * fabs(R);
|
|
|
+ double curv = 1.0/R;
|
|
|
+
|
|
|
+ double height = ui->lineEdit_Height->text().toDouble();
|
|
|
+
|
|
|
+ int nrtn = CreateArcRoad(point1_x,point1_y,hdgstart,arclen,height,curv);
|
|
|
+
|
|
|
+ if(nrtn != 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","CreateLineRoad Fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int DialogDrawRoad::CreateLineRoad(double startx, double starty, double hdg, double len, double height)
|
|
|
+{
|
|
|
+ int roadid = gw->CreateRoadID();
|
|
|
+ mpxodr->AddRoad("",len,QString::number(roadid).toStdString(),"-1");
|
|
|
+ Road * pRoad = mpxodr->GetLastAddedRoad();
|
|
|
+ if(pRoad == 0)
|
|
|
+ {
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ pRoad->AddGeometryBlock();
|
|
|
+ GeometryBlock * pgb = pRoad->GetLastAddedGeometryBlock();
|
|
|
+ pgb->AddGeometryLine(0,startx,starty,hdg,len);
|
|
|
+
|
|
|
+ pRoad->AddElevation(0,height,0,0,0);
|
|
|
+
|
|
|
+ pRoad->AddLaneSection(0);
|
|
|
+ LaneSection * pLS = pRoad->GetLastAddedLaneSection();
|
|
|
+ Lane * pLane;
|
|
|
+ pLS->AddLane(0,0,"none",false);
|
|
|
+ pLane = pLS->GetLastAddedLane();
|
|
|
+ pLane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"false");
|
|
|
+ pLS->AddLane(-1,-1,"driving",false);
|
|
|
+ pLane = pLS->GetLastAddedLane();
|
|
|
+ pLane->AddWidthRecord(0,3.5,0,0,0);
|
|
|
+ pLane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
|
|
|
+ pLS->AddLane(1,1,"driving",false);
|
|
|
+ pLane = pLS->GetLastAddedLane();
|
|
|
+ pLane->AddWidthRecord(0,3.5,0,0,0);
|
|
|
+ pLane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
|
|
|
+
|
|
|
+ ui->lineEdit_RoadID->setText(QString::number(roadid));
|
|
|
+
|
|
|
+ emit drawnewroad();
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+int DialogDrawRoad::CreateArcRoad(double startx, double starty, double hdg, double len, double height, double curv)
|
|
|
+{
|
|
|
+ int roadid = gw->CreateRoadID();
|
|
|
+ mpxodr->AddRoad("",len,QString::number(roadid).toStdString(),"-1");
|
|
|
+ Road * pRoad = mpxodr->GetLastAddedRoad();
|
|
|
+ if(pRoad == 0)
|
|
|
+ {
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ pRoad->AddGeometryBlock();
|
|
|
+ GeometryBlock * pgb = pRoad->GetLastAddedGeometryBlock();
|
|
|
+ pgb->AddGeometryArc(0,startx,starty,hdg,len,curv);
|
|
|
+
|
|
|
+ pRoad->AddElevation(0,height,0,0,0);
|
|
|
+
|
|
|
+ pRoad->AddLaneSection(0);
|
|
|
+ LaneSection * pLS = pRoad->GetLastAddedLaneSection();
|
|
|
+ Lane * pLane;
|
|
|
+ pLS->AddLane(0,0,"none",false);
|
|
|
+ pLane = pLS->GetLastAddedLane();
|
|
|
+ pLane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"false");
|
|
|
+ pLS->AddLane(-1,-1,"driving",false);
|
|
|
+ pLane = pLS->GetLastAddedLane();
|
|
|
+ pLane->AddWidthRecord(0,3.5,0,0,0);
|
|
|
+ pLane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
|
|
|
+ pLS->AddLane(1,1,"driving",false);
|
|
|
+ pLane = pLS->GetLastAddedLane();
|
|
|
+ pLane->AddWidthRecord(0,3.5,0,0,0);
|
|
|
+ pLane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
|
|
|
+
|
|
|
+ ui->lineEdit_RoadID->setText(QString::number(roadid));
|
|
|
+
|
|
|
+ emit drawnewroad();
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+// >0 left 0 on line <0 right
|
|
|
+double DialogDrawRoad::LeftRight(double x,double y,double x1,double y1,double x2,double y2)
|
|
|
+{
|
|
|
+ return (y1-y2)*x + (x2-x1)*y + x1*y2 -x2*y1;
|
|
|
+}
|
|
|
+
|
|
|
+int DialogDrawRoad::YuanXin(double x1,double y1,double x2,double y2,double R,double &x,double &y)
|
|
|
+{
|
|
|
+ double xp,yp,xn,yn;
|
|
|
+ double c1,c2,A,B,C;
|
|
|
+
|
|
|
+ double fdis = sqrt(pow(x2-x1,2) + pow(y2 - y1,2));
|
|
|
+ if(fdis>(2.0*fabs(R)))
|
|
|
+ {
|
|
|
+ return -2;
|
|
|
+ }
|
|
|
+ if(x2 == x1)
|
|
|
+ {
|
|
|
+ if(y2 == y1)return -1;
|
|
|
+ yp = (y1*y1 - y2*y2)/(-2.0*(y2-y1));
|
|
|
+ yn = yp;
|
|
|
+
|
|
|
+ xp = x1 - sqrt(pow(R,2)-pow(y1-yp,2));
|
|
|
+
|
|
|
+ xn = x1 + sqrt(pow(R,2)-pow(y1-yn,2));
|
|
|
+
|
|
|
+ goto YuanDecide;
|
|
|
+ }
|
|
|
+ c1 = (x2*x2 - x1*x1 + y2*y2 - y1*y1) / (2 *(x2 - x1));
|
|
|
+ c2 = (y2 - y1) / (x2 - x1);
|
|
|
+ A = (c2*c2 + 1);
|
|
|
+ B = (2 * x1*c2 - 2 * c1*c2 - 2 * y1);
|
|
|
+ C = x1*x1 - 2 * x1*c1 + c1*c1 + y1*y1 - R*R;
|
|
|
+ yp = (-B + sqrt(B*B - 4 * A*C)) / (2 * A);
|
|
|
+ yn = (-B - sqrt(B*B - 4 * A*C)) / (2 * A);
|
|
|
+ xp = c1 - c2 * yp;
|
|
|
+ xn = c1 - c2 * yn;
|
|
|
+YuanDecide:
|
|
|
+
|
|
|
+ double lf = LeftRight(xp,yp,x1,y1,x2,y2);
|
|
|
+ double xr = lf * R;
|
|
|
+ if(xr > 0)
|
|
|
+ {
|
|
|
+ x = xp;
|
|
|
+ y = yp;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x = xn;
|
|
|
+ y = yn;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void DialogDrawRoad::normalhdg(double &hdg)
|
|
|
+{
|
|
|
+ while(hdg>= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
|
|
|
+ while(hdg<0)hdg = hdg + 2.0*M_PI;
|
|
|
+}
|
|
|
+
|
|
|
+void DialogDrawRoad::on_pushButton_Draw_clicked()
|
|
|
+{
|
|
|
+ Road * pRoad = 0;
|
|
|
+ int nroadid = ui->lineEdit_RoadID->text().toInt();
|
|
|
+ unsigned int i;
|
|
|
+ for(i=0;i<mpxodr->GetRoadCount();i++)
|
|
|
+ {
|
|
|
+ if(atoi(mpxodr->GetRoad(i)->GetRoadId().data()) == nroadid)
|
|
|
+ {
|
|
|
+ pRoad = mpxodr->GetRoad(i);
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(pRoad == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warnig","Can't find this road.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ double point_x,point_y;
|
|
|
+ point_x = ui->lineEdit_newpointx->text().toDouble();
|
|
|
+ point_y = ui->lineEdit_newpointy->text().toDouble();
|
|
|
+ double newhdg;
|
|
|
+ newhdg = ui->lineEdit_newpointhdg->text().toDouble();
|
|
|
+
|
|
|
+ double endx,endy,endhdg;
|
|
|
+ int nrtn;
|
|
|
+ nrtn = pRoad->GetGeometryCoords(pRoad->GetRoadLength(),endx,endy,endhdg);
|
|
|
+ if(nrtn<0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Find Road End Point errror.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double dis = sqrt(pow(endx - point_x,2)+pow(endy - point_y,2));
|
|
|
+ if(dis < 1.0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","dis is small",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double calchdg = geofit::CalcHdg(endx,endy,point_x,point_y);
|
|
|
+ if(ui->checkBox_newpointusehdg->isChecked() == false)
|
|
|
+ {
|
|
|
+ newhdg = calchdg;
|
|
|
+ }
|
|
|
+ std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(endx,endy,endhdg,
|
|
|
+ point_x,point_y,newhdg);
|
|
|
+
|
|
|
+ if(xvectorgeo.size() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","Create Bezier fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ geobase * pline;
|
|
|
+ geobase * pbez;
|
|
|
+ geobase * parc;
|
|
|
+ pRoad->AddGeometryBlock();
|
|
|
+ GeometryBlock * pgb = pRoad->GetLastAddedGeometryBlock();
|
|
|
+ if(pgb == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,"Warning","GetLastAddedGeometryBlock fail.",QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ double s = pRoad->GetRoadLength();
|
|
|
+ switch (xvectorgeo[0].mnType) {
|
|
|
+ case 0:
|
|
|
+ pline = &xvectorgeo[0];
|
|
|
+ pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ parc = &xvectorgeo[0];
|
|
|
+ pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ pbez = &xvectorgeo[0];
|
|
|
+ pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
|
|
|
+ pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
|
|
|
+ pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
|
|
|
+ pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ double newroadlen = s + xvectorgeo[0].mfLen;
|
|
|
+ pRoad->SetRoadLength(newroadlen);
|
|
|
+
|
|
|
+ emit drawnewroad();
|
|
|
+
|
|
|
+
|
|
|
+}
|