|
|
@@ -22,6 +22,10 @@ MainWindow::MainWindow(QWidget *parent)
|
|
|
ui->lineEdit_NowY->setText("3.0");
|
|
|
ui->lineEdit_NowHdg->setText("0.0");
|
|
|
|
|
|
+ ui->lineEdit_MaxWheel->setText("430.0");
|
|
|
+ ui->lineEdit_MinRadius->setText("4.6");
|
|
|
+ ui->lineEdit_StraightDis->setText("0.3");
|
|
|
+
|
|
|
myview = new MyView(this);
|
|
|
myview->setObjectName(QStringLiteral("graphicsView"));
|
|
|
myview->setGeometry(QRect(400, 100,1350, 830));
|
|
|
@@ -38,7 +42,7 @@ MainWindow::MainWindow(QWidget *parent)
|
|
|
QGraphicsLineItem * pliney = new QGraphicsLineItem(RATIO * X_MOVE,0,RATIO * X_MOVE,VIEW_HEIGHT);
|
|
|
mpscene->addItem(pliney);
|
|
|
|
|
|
-
|
|
|
+ setWindowTitle("Calc Park (if HDG>0.5236 Vertical Park , else Side Park)");
|
|
|
}
|
|
|
|
|
|
MainWindow::~MainWindow()
|
|
|
@@ -47,47 +51,9 @@ MainWindow::~MainWindow()
|
|
|
}
|
|
|
|
|
|
|
|
|
-void MainWindow::on_pushButton_Calc_clicked()
|
|
|
+void MainWindow::SidePark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis)
|
|
|
{
|
|
|
- double x = ui->lineEdit_NowX->text().toDouble();
|
|
|
- double y = ui->lineEdit_NowY->text().toDouble();
|
|
|
- double hdg = ui->lineEdit_NowHdg->text().toDouble();
|
|
|
-
|
|
|
- if(mvectoritem.size()>0)
|
|
|
- {
|
|
|
- int i;
|
|
|
- int nsize = static_cast<int>(mvectoritem.size());
|
|
|
- for(i=0;i<nsize;i++)
|
|
|
- {
|
|
|
- mpscene->removeItem(mvectoritem[i]);
|
|
|
- delete mvectoritem[i];
|
|
|
- }
|
|
|
- mvectoritem.clear();
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- QGraphicsEllipseItem * pmainitem;
|
|
|
- pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
|
|
|
- pmainitem->setBrush(Qt::green);
|
|
|
- pmainitem->setPen(Qt::NoPen);
|
|
|
- double x_main = x * RATIO;
|
|
|
- double y_main = y * RATIO;
|
|
|
- double fscale = 1.0;
|
|
|
- pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
- pmainitem->setScale(fscale);
|
|
|
- mpscene->addItem(pmainitem);
|
|
|
- mvectoritem.push_back(pmainitem);
|
|
|
|
|
|
- pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
|
|
|
- pmainitem->setBrush(Qt::green);
|
|
|
- pmainitem->setPen(Qt::NoPen);
|
|
|
- x_main = 0;
|
|
|
- y_main = 0;
|
|
|
- fscale = 1.0;
|
|
|
- pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
- pmainitem->setScale(fscale);
|
|
|
- mpscene->addItem(pmainitem);
|
|
|
- mvectoritem.push_back(pmainitem);
|
|
|
|
|
|
SideParkCalc xPark(x,y,hdg);
|
|
|
xPark.CalcPark();
|
|
|
@@ -107,7 +73,7 @@ void MainWindow::on_pushButton_Calc_clicked()
|
|
|
{
|
|
|
char strout[3000];
|
|
|
char strtemp[1000];
|
|
|
- snprintf(strout,3000,"Five Step:\n");
|
|
|
+ snprintf(strout,3000,"SidePark:\nFive Step:\n");
|
|
|
snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
|
|
|
int i = 0;
|
|
|
int npoint =4;
|
|
|
@@ -311,7 +277,7 @@ void MainWindow::on_pushButton_Calc_clicked()
|
|
|
{
|
|
|
char strout[3000];
|
|
|
char strtemp[1000];
|
|
|
- snprintf(strout,3000,"Two Step:\n");
|
|
|
+ snprintf(strout,3000,"SidePark:\nTwo Step:\n");
|
|
|
snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
|
|
|
int i = 0;
|
|
|
int npoint =1;
|
|
|
@@ -423,3 +389,216 @@ void MainWindow::on_pushButton_Calc_clicked()
|
|
|
mvectorwheel = xvectorwheel;
|
|
|
}
|
|
|
|
|
|
+void MainWindow::VerticalPark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis)
|
|
|
+{
|
|
|
+ VerticalParkCalc xPark(x,y,hdg,fMinRadius,fMaxWheel,45,fStraightDis);
|
|
|
+ xPark.CalcPark();
|
|
|
+
|
|
|
+ std::vector<iv::VerticalParkPoint> xvectorverticalparkpoint;
|
|
|
+ std::vector<double> xvectorwheel;
|
|
|
+ std::vector<double> xvectorlen;
|
|
|
+ double fTotalLen = 0;
|
|
|
+ VerticalParkType xtype = xPark.GetSolution(xvectorverticalparkpoint,xvectorwheel,xvectorlen,fTotalLen);
|
|
|
+
|
|
|
+ if(xtype == VerticalParkType::NoVSolution)
|
|
|
+ {
|
|
|
+ ui->plainTextEdit->setPlainText("No Solution.");
|
|
|
+ }
|
|
|
+
|
|
|
+ if(xtype == VerticalParkType::ThreeStep)
|
|
|
+ {
|
|
|
+ char strout[3000];
|
|
|
+ char strtemp[1000];
|
|
|
+ snprintf(strout,3000,"VerticalPark:\nThree Steps:\n");
|
|
|
+ snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
|
|
|
+ int i = 0;
|
|
|
+ int npoint =2;
|
|
|
+ for(i=0;i<npoint;i++)
|
|
|
+ {
|
|
|
+ snprintf(strtemp,1000,"Point %d x:%6.3f y:%6.3f hdg:%6.3f \n",
|
|
|
+ i,xvectorverticalparkpoint[i].mx,xvectorverticalparkpoint[i].my,xvectorverticalparkpoint[i].mhdg);
|
|
|
+ strncat(strout,strtemp,3000);
|
|
|
+
|
|
|
+
|
|
|
+ QGraphicsEllipseItem * pmainitem;
|
|
|
+ pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
|
|
|
+ pmainitem->setBrush(Qt::red);
|
|
|
+ pmainitem->setPen(Qt::NoPen);
|
|
|
+ double x_main = xvectorverticalparkpoint[i].mx * RATIO;
|
|
|
+ double y_main = xvectorverticalparkpoint[i].my * RATIO;
|
|
|
+ double fscale = 1.0;
|
|
|
+ pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
+ pmainitem->setScale(fscale);
|
|
|
+ mpscene->addItem(pmainitem);
|
|
|
+ mvectoritem.push_back(pmainitem);
|
|
|
+ }
|
|
|
+
|
|
|
+ if(xvectorlen[0]>0)
|
|
|
+ {
|
|
|
+ double x_point,y_point;
|
|
|
+ double fs;
|
|
|
+ fs =0.01;
|
|
|
+ while(fs<xvectorlen[0])
|
|
|
+ {
|
|
|
+ x_point = x + cos(hdg+M_PI)*fs;
|
|
|
+ y_point = y + sin(hdg+M_PI)*fs;
|
|
|
+ fs = fs+ 0.01;
|
|
|
+ QGraphicsEllipseItem * ppointitem;
|
|
|
+ ppointitem = new QGraphicsEllipseItem(0-mfPointSize/2.0,0*(-1)-mfPointSize/2.0,mfPointSize,mfPointSize);
|
|
|
+ ppointitem->setBrush(Qt::green);
|
|
|
+ ppointitem->setPen(Qt::NoPen);
|
|
|
+ double x_main = x_point * RATIO;
|
|
|
+ double y_main = y_point * RATIO;
|
|
|
+ double fscale = 1.0;
|
|
|
+ ppointitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
+ ppointitem->setScale(fscale);
|
|
|
+ mpscene->addItem(ppointitem);
|
|
|
+ mvectoritem.push_back(ppointitem);
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(xvectorlen[1]>0)
|
|
|
+ {
|
|
|
+ double x_point,y_point;
|
|
|
+ double fs;
|
|
|
+ fs =0.01;
|
|
|
+ double x_center,y_center;
|
|
|
+ if(xvectorwheel[1]>0)
|
|
|
+ {
|
|
|
+ x_center = xvectorverticalparkpoint[0].mx + mfRadius * cos(xvectorverticalparkpoint[0].mhdg + M_PI/2.0);
|
|
|
+ y_center = xvectorverticalparkpoint[0].my + mfRadius * sin(xvectorverticalparkpoint[0].mhdg + M_PI/2.0);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_center = xvectorverticalparkpoint[0].mx + mfRadius * cos(xvectorverticalparkpoint[0].mhdg - M_PI/2.0);
|
|
|
+ y_center = xvectorverticalparkpoint[0].my + mfRadius * sin(xvectorverticalparkpoint[0].mhdg - M_PI/2.0);
|
|
|
+ }
|
|
|
+ while(fs<xvectorlen[1])
|
|
|
+ {
|
|
|
+ double alpha = xvectorverticalparkpoint[0].mhdg + M_PI/2.0 + fs/mfRadius;
|
|
|
+ if(xvectorwheel[1] >0)
|
|
|
+ {
|
|
|
+ alpha = xvectorverticalparkpoint[0].mhdg - M_PI/2.0 - fs/mfRadius;
|
|
|
+ }
|
|
|
+
|
|
|
+ x_point = x_center + cos(alpha)*mfRadius;
|
|
|
+ y_point = y_center + sin(alpha)*mfRadius;
|
|
|
+ fs = fs+ 0.01;
|
|
|
+
|
|
|
+ QGraphicsEllipseItem * ppointitem;
|
|
|
+ ppointitem = new QGraphicsEllipseItem(0-mfPointSize/2.0,0*(-1)-mfPointSize/2.0,mfPointSize,mfPointSize);
|
|
|
+ ppointitem->setBrush(Qt::blue);
|
|
|
+ ppointitem->setPen(Qt::NoPen);
|
|
|
+ double x_main = x_point * RATIO;
|
|
|
+ double y_main = y_point * RATIO;
|
|
|
+ double fscale = 1.0;
|
|
|
+ ppointitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
+ ppointitem->setScale(fscale);
|
|
|
+ mpscene->addItem(ppointitem);
|
|
|
+ mvectoritem.push_back(ppointitem);
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(xvectorlen[2]>0)
|
|
|
+ {
|
|
|
+ double x_point,y_point;
|
|
|
+ double fs;
|
|
|
+ fs =0.01;
|
|
|
+ while(fs<xvectorlen[2])
|
|
|
+ {
|
|
|
+ x_point = xvectorverticalparkpoint[1].mx + cos(xvectorverticalparkpoint[1].mhdg+M_PI)*fs;
|
|
|
+ y_point = xvectorverticalparkpoint[1].my + sin(xvectorverticalparkpoint[1].mhdg+M_PI)*fs;
|
|
|
+ fs = fs+ 0.01;
|
|
|
+ QGraphicsEllipseItem * ppointitem;
|
|
|
+ ppointitem = new QGraphicsEllipseItem(0-mfPointSize/2.0,0*(-1)-mfPointSize/2.0,mfPointSize,mfPointSize);
|
|
|
+ ppointitem->setBrush(Qt::green);
|
|
|
+ ppointitem->setPen(Qt::NoPen);
|
|
|
+ double x_main = x_point * RATIO;
|
|
|
+ double y_main = y_point * RATIO;
|
|
|
+ double fscale = 1.0;
|
|
|
+ ppointitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
+ ppointitem->setScale(fscale);
|
|
|
+ mpscene->addItem(ppointitem);
|
|
|
+ mvectoritem.push_back(ppointitem);
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ snprintf(strtemp,1000,"\nWheel & Len:\n");strncat(strout,strtemp,3000);
|
|
|
+ for(i=0;i<3;i++)
|
|
|
+ {
|
|
|
+ snprintf(strtemp,1000,"Step %d Wheel:%6.3f Length:%6.3f \n",
|
|
|
+ i,xvectorwheel[i],xvectorlen[i]);
|
|
|
+ strncat(strout,strtemp,3000);
|
|
|
+ }
|
|
|
+ snprintf(strtemp,1000,"\nTotal Length:%6.3f\n",fTotalLen);strncat(strout,strtemp,3000);
|
|
|
+ ui->plainTextEdit->setPlainText(strout);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void MainWindow::on_pushButton_Calc_clicked()
|
|
|
+{
|
|
|
+ double x = ui->lineEdit_NowX->text().toDouble();
|
|
|
+ double y = ui->lineEdit_NowY->text().toDouble();
|
|
|
+ double hdg = ui->lineEdit_NowHdg->text().toDouble();
|
|
|
+
|
|
|
+ double fMinRadius = ui->lineEdit_MinRadius->text().toDouble();
|
|
|
+ mfMaxWheel = ui->lineEdit_MaxWheel->text().toDouble();
|
|
|
+ double fStraightDis = ui->lineEdit_StraightDis->text().toDouble();
|
|
|
+ double fMaxWheel = mfMaxWheel;
|
|
|
+ mfRadius = fMinRadius;
|
|
|
+
|
|
|
+ while(hdg>M_PI)hdg = hdg -2.0*M_PI;
|
|
|
+ while(hdg<=(-M_PI))hdg = hdg + 2.0*M_PI;
|
|
|
+
|
|
|
+
|
|
|
+ if(mvectoritem.size()>0)
|
|
|
+ {
|
|
|
+ int i;
|
|
|
+ int nsize = static_cast<int>(mvectoritem.size());
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ mpscene->removeItem(mvectoritem[i]);
|
|
|
+ delete mvectoritem[i];
|
|
|
+ }
|
|
|
+ mvectoritem.clear();
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ QGraphicsEllipseItem * pmainitem;
|
|
|
+ pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
|
|
|
+ pmainitem->setBrush(Qt::green);
|
|
|
+ pmainitem->setPen(Qt::NoPen);
|
|
|
+ double x_main = x * RATIO;
|
|
|
+ double y_main = y * RATIO;
|
|
|
+ double fscale = 1.0;
|
|
|
+ pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
+ pmainitem->setScale(fscale);
|
|
|
+ mpscene->addItem(pmainitem);
|
|
|
+ mvectoritem.push_back(pmainitem);
|
|
|
+
|
|
|
+ pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
|
|
|
+ pmainitem->setBrush(Qt::green);
|
|
|
+ pmainitem->setPen(Qt::NoPen);
|
|
|
+ x_main = 0;
|
|
|
+ y_main = 0;
|
|
|
+ fscale = 1.0;
|
|
|
+ pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
|
|
|
+ pmainitem->setScale(fscale);
|
|
|
+ mpscene->addItem(pmainitem);
|
|
|
+ mvectoritem.push_back(pmainitem);
|
|
|
+
|
|
|
+ if(fabs(hdg)<(M_PI/6.0))
|
|
|
+ {
|
|
|
+ SidePark(x,y,hdg,fMinRadius,fMaxWheel,fStraightDis);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ VerticalPark(x,y,hdg,fMinRadius,fMaxWheel,fStraightDis);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
dongjunhong