change testsideparkcalc, add draw line. change map_lanetoxodr, add use lon lat add signal.

src/test/testsideparkcalc/mainwindow.cpp

@@ -1,7 +1,16 @@
 #include "mainwindow.h"
 #include "ui_mainwindow.h"
 
-#include "sideparkcalc.h"
+
+
+#define VIEW_WIDTH 1300
+#define VIEW_HEIGHT 800
+
+#define  RATIO 50
+
+#define X_MOVE 2
+#define Y_MOVE 6
+
 
 MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
@@ -13,6 +22,22 @@ MainWindow::MainWindow(QWidget *parent)
     ui->lineEdit_NowY->setText("3.0");
     ui->lineEdit_NowHdg->setText("0.0");
 
+    myview = new MyView(this);
+    myview->setObjectName(QStringLiteral("graphicsView"));
+    myview->setGeometry(QRect(400, 100,1350, 830));
+
+
+    mpscene = new  QGraphicsScene(0,0,VIEW_WIDTH,VIEW_HEIGHT);
+    mpscene->setBackgroundBrush(Qt::white);
+    myview->setScene(mpscene);
+
+    myview->centerOn(VIEW_WIDTH/2,VIEW_HEIGHT/2);
+
+    QGraphicsLineItem * plinex = new QGraphicsLineItem(0,RATIO * Y_MOVE,VIEW_WIDTH,RATIO * Y_MOVE);
+    mpscene->addItem(plinex);
+    QGraphicsLineItem * pliney = new QGraphicsLineItem(RATIO * X_MOVE,0,RATIO * X_MOVE,VIEW_HEIGHT);
+    mpscene->addItem(pliney);
+
 
 }
 
@@ -28,6 +53,42 @@ void MainWindow::on_pushButton_Calc_clicked()
     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();
 
@@ -55,7 +116,183 @@ void MainWindow::on_pushButton_Calc_clicked()
             snprintf(strtemp,1000,"Point %d x:%6.3f y:%6.3f hdg:%6.3f \n",
                      i,xvectorsideparkpoint[i].mx,xvectorsideparkpoint[i].my,xvectorsideparkpoint[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 = xvectorsideparkpoint[i].mx * RATIO;
+            double y_main = xvectorsideparkpoint[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 = xvectorsideparkpoint[0].mx + mfRadius * cos(xvectorsideparkpoint[0].mhdg + M_PI/2.0);
+                y_center = xvectorsideparkpoint[0].my + mfRadius * sin(xvectorsideparkpoint[0].mhdg + M_PI/2.0);
+            }
+            else
+            {
+                x_center = xvectorsideparkpoint[0].mx + mfRadius * cos(xvectorsideparkpoint[0].mhdg - M_PI/2.0);
+                y_center = xvectorsideparkpoint[0].my + mfRadius * sin(xvectorsideparkpoint[0].mhdg - M_PI/2.0);
+            }
+            while(fs<xvectorlen[1])
+            {
+                double alpha = xvectorsideparkpoint[0].mhdg + M_PI/2.0 + fs/mfRadius;
+                if(xvectorwheel[1] >0)
+                {
+                    alpha = xvectorsideparkpoint[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 = xvectorsideparkpoint[1].mx + cos(xvectorsideparkpoint[1].mhdg+M_PI)*fs;
+                y_point = xvectorsideparkpoint[1].my + sin(xvectorsideparkpoint[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);
+
+            }
+        }
+
+        if(xvectorlen[3]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            double x_center,y_center;
+            if(xvectorwheel[3]>0)
+            {
+                x_center = xvectorsideparkpoint[2].mx + mfRadius * cos(xvectorsideparkpoint[2].mhdg + M_PI/2.0);
+                y_center = xvectorsideparkpoint[2].my + mfRadius * sin(xvectorsideparkpoint[2].mhdg + M_PI/2.0);
+            }
+            else
+            {
+                x_center = xvectorsideparkpoint[2].mx + mfRadius * cos(xvectorsideparkpoint[2].mhdg - M_PI/2.0);
+                y_center = xvectorsideparkpoint[2].my + mfRadius * sin(xvectorsideparkpoint[2].mhdg - M_PI/2.0);
+            }
+            while(fs<xvectorlen[3])
+            {
+                double alpha = xvectorsideparkpoint[2].mhdg + M_PI/2.0 + fs/mfRadius;
+                if(xvectorwheel[3] >0)
+                {
+                    alpha = xvectorsideparkpoint[2].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[4]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            while(fs<xvectorlen[4])
+            {
+                x_point = xvectorsideparkpoint[3].mx + cos(xvectorsideparkpoint[3].mhdg+M_PI)*fs;
+                y_point = xvectorsideparkpoint[3].my + sin(xvectorsideparkpoint[3].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<5;i++)
         {
@@ -65,6 +302,9 @@ void MainWindow::on_pushButton_Calc_clicked()
         }
         snprintf(strtemp,1000,"\nTotal Length:%6.3f\n",fTotalLen);strncat(strout,strtemp,3000);
         ui->plainTextEdit->setPlainText(strout);
+
+
+
     }
 
     if(xtype == SideParkType::TwoStep)
@@ -80,6 +320,18 @@ void MainWindow::on_pushButton_Calc_clicked()
             snprintf(strtemp,1000,"Point %d x:%6.3f y:%6.3f hdg:%6.3f \n",
                      i,xvectorsideparkpoint[i].mx,xvectorsideparkpoint[i].my,xvectorsideparkpoint[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 = xvectorsideparkpoint[i].mx * RATIO;
+            double y_main = xvectorsideparkpoint[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);
         }
         snprintf(strtemp,1000,"\nWheel & Len:\n");strncat(strout,strtemp,3000);
         for(i=0;i<2;i++)
@@ -90,6 +342,84 @@ void MainWindow::on_pushButton_Calc_clicked()
         }
         snprintf(strtemp,1000,"\nTotal Length:%6.3f\n",fTotalLen);strncat(strout,strtemp,3000);
         ui->plainTextEdit->setPlainText(strout);
+
+
+        if(xvectorlen[0]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            double x_center,y_center;
+            double fRadius = 1000.0;
+            if(xvectorwheel[0] != 0)
+                fRadius = mfRadius * mfMaxWheel / fabs(xvectorwheel[0]);
+            if(xvectorwheel[0]>0)
+            {
+                x_center = x + fRadius * cos(hdg + M_PI/2.0);
+                y_center = y + fRadius * sin(hdg + M_PI/2.0);
+            }
+            else
+            {
+                x_center = x + fRadius * cos(hdg - M_PI/2.0);
+                y_center = y + fRadius * sin(hdg - M_PI/2.0);
+            }
+            while(fs<xvectorlen[0])
+            {
+                double alpha = hdg + M_PI/2.0 + fs/fRadius;
+                if(xvectorwheel[0] >0)
+                {
+                    alpha = hdg - M_PI/2.0 - fs/fRadius;
+                }
+
+                x_point = x_center + cos(alpha)*fRadius;
+                y_point = y_center + sin(alpha)*fRadius;
+                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[1]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            while(fs<xvectorlen[1])
+            {
+                x_point = xvectorsideparkpoint[0].mx + cos(xvectorsideparkpoint[0].mhdg+M_PI)*fs;
+                y_point = xvectorsideparkpoint[0].my + sin(xvectorsideparkpoint[0].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);
+
+            }
+        }
+
     }
+
+    mtype = xtype;
+    mvectorlen = xvectorlen;
+    mvectorsideparkpoint = xvectorsideparkpoint;
+    mvectorwheel = xvectorwheel;
 }
 

src/test/testsideparkcalc/mainwindow.h

@@ -3,6 +3,10 @@
 
 #include <QMainWindow>
 
+#include "myview.h"
+
+#include "sideparkcalc.h"
+
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
 QT_END_NAMESPACE
@@ -20,7 +24,25 @@ public:
 private slots:
     void on_pushButton_Calc_clicked();
 
+private:
+    double mfRadius = 5.0;
+    double mfMaxWheel = 430.0;
+    std::vector<iv::SideParkPoint> mvectorsideparkpoint;
+    std::vector<double> mvectorwheel;
+    std::vector<double> mvectorlen;
+    double mTotalLen = 0;
+    SideParkType mtype;
+
+    const double mfMainSize = 6.0;
+    const double mfPointSize = 3.0;
+
+    std::vector<QGraphicsEllipseItem *> mvectoritem;
+
 private:
     Ui::MainWindow *ui;
+
+    MyView *myview;
+
+    QGraphicsScene * mpscene;
 };
 #endif // MAINWINDOW_H

src/test/testsideparkcalc/mainwindow.ui

@@ -107,8 +107,8 @@
      <rect>
       <x>40</x>
       <y>100</y>
-      <width>371</width>
-      <height>491</height>
+      <width>350</width>
+      <height>621</height>
      </rect>
     </property>
    </widget>

src/test/testsideparkcalc/myview.cpp

@@ -0,0 +1,139 @@
+#include "myview.h"
+#include <QScrollBar>
+#include <iostream>
+#define VIEW_CENTER viewport()->rect().center()
+//const double PI = 3.1415926535898;
+
+MyView::MyView(QWidget *parent) :
+     QGraphicsView(parent),
+    beishu(1.00000)
+{
+    setDragMode(QGraphicsView::ScrollHandDrag);
+}
+
+void MyView::mousePressEvent(QMouseEvent *event)
+{
+//    qDebug("x is %d",event->pos().x());
+    bottonstatus = true;
+    QGraphicsView::mousePressEvent(event);
+}
+void MyView::mouseMoveEvent(QMouseEvent *event)
+{
+    QGraphicsView::mouseMoveEvent(event);
+
+//    QScrollBar * ps = verticalScrollBar();
+//    std::cout<<" size is "<<ps->size().height()<<" v = "<<ps->value()<<std::endl;
+//    QScrollBar * ps2= horizontalScrollBar();
+//     std::cout<<" size is "<<ps2->size().width()<<" h = "<<ps2->value()<<std::endl;
+}
+void MyView::mouseReleaseEvent(QMouseEvent *event)
+{
+    bottonstatus = false;
+    QGraphicsView::mouseReleaseEvent(event);
+}
+
+// 放大/缩小
+void MyView::wheelEvent(QWheelEvent *event)
+{
+    // 滚轮的滚动量
+    QPoint scrollAmount = event->angleDelta();
+    // 正值表示滚轮远离使用者（放大），负值表示朝向使用者（缩小）
+    scrollAmount.y() > 0 ? zoomIn() : zoomOut();
+}
+
+
+// 放大
+void MyView::zoomIn()
+{
+
+    int width,hgt;
+    width = sceneRect().width();
+    hgt = sceneRect().height();
+    QScrollBar * psV = verticalScrollBar();
+    QScrollBar * psH = horizontalScrollBar();
+
+
+    qDebug("%d %d ",width,hgt);
+
+
+    int centery = (psV->value() + psV->size().height()/2)/beishu;
+    int centerx = (psH->value() + psH->size().width()/2)/beishu;
+
+    scale(1.1, 1.1);
+    beishu *= 1.1;
+ //   centerOn(450, 700 - (200 / beishu));
+
+
+
+    qDebug("beishu is %f",beishu);
+
+    centerOn(centerx,centery);
+
+
+
+//    QPoint x = viewport()->rect().center();
+
+
+//    std::cout<<" x is"<<sceneRect().bottom()<<" y is "<<sceneRect().y()<<std::endl;
+//    QScrollBar * ps = verticalScrollBar();
+//    std::cout<<" size is "<<ps->size().height()<<" v = "<<ps->value()<<std::endl;
+
+}
+
+// 缩小
+void MyView::zoomOut()
+{
+
+    int width,hgt;
+    (void)width;
+    (void)hgt;
+    width = sceneRect().width();
+    hgt = sceneRect().height();
+    QScrollBar * psV = verticalScrollBar();
+    QScrollBar * psH = horizontalScrollBar();
+
+    int centery = (psV->value() + psV->size().height()/2)/beishu;
+    int centerx = (psH->value() + psH->size().width()/2)/beishu;
+
+    scale(1 / 1.1, 1 / 1.1);
+    beishu /= 1.1;
+//    centerOn(450, 700 - (200 / beishu));
+
+
+    centerOn(centerx,centery);
+}
+
+void MyView::zoomone()
+{
+
+    scale(1 /beishu, 1 / beishu);
+    beishu  = 1.0;
+
+}
+
+void MyView::mouseDoubleClickEvent(QMouseEvent *event)
+{
+
+    QScrollBar * psV = verticalScrollBar();
+    QScrollBar * psH = horizontalScrollBar();
+
+    int centery = (psV->value() + psV->size().height()/2)/beishu;
+    int centerx = (psH->value() + psH->size().width()/2)/beishu;
+
+
+//    qDebug("x is %d y is %d view center x is %d  centerx is %d",event->pos().x(),
+//           event->pos().y(),
+//           viewport()->rect().center().x(),centerx);
+
+    int viewx,viewy;
+    if(beishu == 0)return;
+    viewx = centerx +(event->pos().x() - viewport()->rect().center().x())/beishu;
+    viewy = centery +(event->pos().y() - viewport()->rect().center().y())/beishu;
+
+    QPoint viewpoint;
+    viewpoint.setX(viewx);
+    viewpoint.setY(viewy);
+
+    emit dbclickxy(viewx,viewy);
+    qDebug("view x is %d view y is %d ",viewx,viewy);
+}

src/test/testsideparkcalc/myview.h

@@ -0,0 +1,41 @@
+#ifndef MYVIEW_H
+#define MYVIEW_H
+
+#include <qtimer.h>
+#include <qpainter.h>
+#include <QGraphicsView>
+#include <QWheelEvent>
+#include <QKeyEvent>
+#include <QPoint>
+#include <QPointF>
+#include <QGraphicsItem>
+#include <QKeyEvent>
+
+class MyView : public QGraphicsView
+{
+    Q_OBJECT
+
+public:
+    explicit MyView(QWidget *parent =0);
+    qreal x, y, beishu;
+
+    void zoomone();
+protected:
+    void wheelEvent(QWheelEvent *event) Q_DECL_OVERRIDE;
+    void mouseMoveEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
+    void mousePressEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
+    void mouseReleaseEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
+    void mouseDoubleClickEvent(QMouseEvent *event) Q_DECL_OVERRIDE;
+public Q_SLOTS:
+    void zoomIn();  // 放大
+    void zoomOut();  // 缩小
+
+signals:
+    void dbclickxy(double x,double y);
+
+private:
+    bool bottonstatus = false;
+    QPoint myview_lastMousePos;
+};
+
+#endif // MYVIEW_H

src/test/testsideparkcalc/sideparkcalc.cpp

@@ -95,7 +95,7 @@ void SideParkCalc::ParkAtRightCalc_Model1()
         std::cout<<" divide error."<<std::endl;
         return;
     }
-    dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
+    dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg )) + tan(fang ));
     dx1 = thetax - dx4;
     dy1 = dx1 * tan(fabs(mhdg));
     dy4 = my + dy1 +dy2 -dy3-dy5;
@@ -115,7 +115,7 @@ void SideParkCalc::ParkAtRightCalc_Model1()
         x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
         x3 = x4 + dx5;y3 = y4 + dy5;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
         hdg2 = hdg3;
-        if(dx4>0.1)
+        if(dx4>0.01)
         {
             x2 = x3 + dx4;y2 = y3 + dy4;
             flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
@@ -127,7 +127,7 @@ void SideParkCalc::ParkAtRightCalc_Model1()
 
         flen2 = mfRaidus * (fang - mhdg);
         x1 = mx - dx1;
-        y1 = my - dy1;
+        y1 = my + dy1;
         hdg1 = mhdg;
 
         flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
@@ -186,6 +186,8 @@ void SideParkCalc::ParkAtRightCalc_Model2()
     dy5 = 0;
     dx5 = mfLastDirectDis;
 
+    bool bFirstCalc = true;
+
     double dx2fix = mfRaidus * sin(mhdg);
     double dy2fix = mfRaidus * (1.0 - cos(mhdg));
 
@@ -201,6 +203,7 @@ void SideParkCalc::ParkAtRightCalc_Model2()
         fang = acos(1.0 - (my+dy2fix)/(2.0*mfRaidus)) ;
     }
 
+TwoCalc:
     dx4 = mfRaidus * sin(fang);
     dy4 = mfRaidus * (1.0 - cos(fang));
 
@@ -242,24 +245,65 @@ void SideParkCalc::ParkAtRightCalc_Model2()
 
     if((dx3<-0.1) || (dx1<-0.1))
     {
-        if(cos(mhdg)> 0.001)
+        if((dx1>0.5)&&(bFirstCalc))
+        {
+            double x_b = mx - mfLastDirectDis;
+            double y_b = my - 0;
+
+            double k = tan(mhdg);
+            double C = (2 * mfRaidus - dy2fix + k * x_b  - y_b )/(2*mfRaidus);
+
+            double a = 1 +k*k;
+            double b = 2*k*C *(-1.0);
+            double c = C*C-1;
+            double d = b*b - 4*a*c;
+            if(d >= 0)
+            {
+                double xr1,xr2;
+                xr1 = (-b + sqrt(d))/(2*a);
+                xr2 = (-b - sqrt(d))/(2*a);
+                double fa = 0;
+                if((xr1>=0)&&(xr1<1))
+                {
+                    fa = asin(xr1);
+                }
+                else
+                {
+                    if((xr2>=0)&&(xr2<1))
+                    {
+                        fa = asin(xr2);
+                    }
+                }
+
+                if((fa>0.001) && (fa<mfMaxAngle))
+                {
+                    fang = fa;
+                    bFirstCalc = false;
+                    goto TwoCalc;
+                }
+            }
+        }
+        else
         {
-            double fR = my/(1 - cos(mhdg));
-            double Rx = mx - fR*sin(mhdg);
-            if((Rx>0.3) && (fR >= mfRaidus))
+            if(cos(mhdg)> 0.001)
             {
-                mSideParkType = SideParkType::TwoStep;
-                mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
-                mvectorWheel.push_back(0.0);
-
-                double flen1,flen2;
-                flen2 = Rx;
-                flen1 = fR*fabs(mhdg);
-                mvectorlen.push_back(flen1);
-                mvectorlen.push_back(flen2);
-                mfTotalLen = flen1 + flen2;
-                mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
-                std::cout<<" Have One Step soluton."<<std::endl;
+                double fR = my/(1 - cos(mhdg));
+                double Rx = mx - fR*sin(mhdg);
+                if((Rx>0.3) && (fR >= mfRaidus))
+                {
+                    mSideParkType = SideParkType::TwoStep;
+                    mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
+                    mvectorWheel.push_back(0.0);
+
+                    double flen1,flen2;
+                    flen2 = Rx;
+                    flen1 = fR*fabs(mhdg);
+                    mvectorlen.push_back(flen1);
+                    mvectorlen.push_back(flen2);
+                    mfTotalLen = flen1 + flen2;
+                    mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
+                    std::cout<<" Have One Step soluton."<<std::endl;
+                }
             }
         }
     }
@@ -359,7 +403,7 @@ void SideParkCalc::ParkAtLeftCalc_Model1()
         std::cout<<" divide error."<<std::endl;
         return;
     }
-    dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
+    dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg )) + tan(fang ));
     dx1 = thetax - dx4;
     dy1 = dx1 * tan(fabs(mhdg));
     dy4 = y + dy1 +dy2 -dy3-dy5;
@@ -391,7 +435,7 @@ void SideParkCalc::ParkAtLeftCalc_Model1()
 
         flen2 = mfRaidus * (mhdg + fang);
         x1 = mx - dx1;
-        y1 = my + dy1;
+        y1 = my - dy1;
         hdg1 = mhdg;
 
         flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
@@ -419,6 +463,7 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
     double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
     dy5 = 0;
     dx5 = mfLastDirectDis;
+    bool bFirstCalc = true;
 
     double dx2fix = mfRaidus * sin(fabs(mhdg));
     double dy2fix = mfRaidus * (1.0 - cos(fabs(mhdg)));
@@ -435,6 +480,7 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
     {
         fang = acos(1.0 - (y+dy2fix)/(2.0*mfRaidus)) ;
     }
+TwoCalc:
 
     dx4 = mfRaidus * sin(fang);
     dy4 = mfRaidus * (1.0 - cos(fang));
@@ -444,7 +490,7 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
 
     double thetax = mx - dx2 -dx4 -dx5;
 
-    double k1 = tan(mhdg);
+    double k1 = tan(fabs(mhdg));
     double k3 = tan(fang);
 
     if(fabs(k1-k3)<0.001)
@@ -477,25 +523,66 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
 
     if((dx3<-0.1) || (dx1<-0.1))
     {
-        if(cos(fabs(mhdg))> 0.001)
+        if((dx1>0.5)&&(bFirstCalc))
+        {
+            double x_b = mx - mfLastDirectDis;
+            double y_b = y - 0;
+
+            double k = tan(fabs(mhdg));
+            double C = (2 * mfRaidus - dy2fix + k * x_b  - y_b )/(2*mfRaidus);
+
+            double a = 1 +k*k;
+            double b = 2*k*C *(-1.0);
+            double c = C*C-1;
+            double d = b*b - 4*a*c;
+            if(d >= 0)
+            {
+                double xr1,xr2;
+                xr1 = (-b + sqrt(d))/(2*a);
+                xr2 = (-b - sqrt(d))/(2*a);
+                double fa = 0;
+                if((xr1>=0)&&(xr1<1))
+                {
+                    fa = asin(xr1);
+                }
+                else
+                {
+                    if((xr2>=0)&&(xr2<1))
+                    {
+                        fa = asin(xr2);
+                    }
+                }
+
+                if((fa>0.001) && (fa<mfMaxAngle))
+                {
+                    fang = fa;
+                    bFirstCalc = false;
+                    goto TwoCalc;
+                }
+            }
+        }
+        else
         {
-            double fR = y/(1 - cos(fabs(mhdg)));
-            double Rx = mx - fR*sin(fabs(mhdg));
-            if((Rx>0.3) && (fR >= mfRaidus))
+            if(cos(fabs(mhdg))> 0.001)
             {
-                std::cout<<" Have One Step soluton."<<std::endl;
-
-                mSideParkType = SideParkType::TwoStep;
-                mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
-                mvectorWheel.push_back(0.0);
-                double flen1,flen2;
-                flen2 = Rx;
-                flen1 = fR*fabs(mhdg);
-                mvectorlen.push_back(flen1);
-                mvectorlen.push_back(flen2);
-                mfTotalLen = flen1 + flen2;
-                mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
-                std::cout<<" Have One Step soluton."<<std::endl;
+                double fR = y/(1 - cos(fabs(mhdg)));
+                double Rx = mx - fR*sin(fabs(mhdg));
+                if((Rx>0.3) && (fR >= mfRaidus))
+                {
+                    std::cout<<" Have One Step soluton."<<std::endl;
+
+                    mSideParkType = SideParkType::TwoStep;
+                    mvectorWheel.push_back((-1.0*mfMaxWheel * mfRaidus/fR));
+                    mvectorWheel.push_back(0.0);
+                    double flen1,flen2;
+                    flen2 = Rx;
+                    flen1 = fR*fabs(mhdg);
+                    mvectorlen.push_back(flen1);
+                    mvectorlen.push_back(flen2);
+                    mfTotalLen = flen1 + flen2;
+                    mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
+                    std::cout<<" Have One Step soluton."<<std::endl;
+                }
             }
         }
     }

src/test/testsideparkcalc/testsideparkcalc.pro

@@ -11,10 +11,12 @@ CONFIG += c++17
 SOURCES += \
     main.cpp \
     mainwindow.cpp \
+    myview.cpp \
     sideparkcalc.cpp
 
 HEADERS += \
     mainwindow.h \
+    myview.h \
     sideparkcalc.h
 
 FORMS += \

src/tool/map_lanetoxodr/ui/trafficlightdialog.cpp

@@ -306,3 +306,69 @@ int TrafficLightDialog::getnewsignalid()
     }while(bHaveExist == true);
     return id;
 }
+
+void TrafficLightDialog::on_pushButton_CalcST_clicked()
+{
+    Road * pRoad =mpxodr->GetRoad(ui->comboBox_Road->currentIndex());
+    if(pRoad == 0)return;
+
+    double fLon = ui->lineEdit_Lon->text().toDouble();
+    double fLat = ui->lineEdit_Lat->text().toDouble();
+
+    if(mpxodr->GetHeader() == 0)
+    {
+        return;
+    }
+
+    double lon0,lat0;
+    mpxodr->GetHeader()->GetLat0Lon0(lat0,lon0);
+
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x,y;
+    GaussProjCal(fLon,fLat,&x,&y);
+    x = x - x0;
+    y = y - y0;
+
+    double fRoadLen = pRoad->GetRoadLength();
+
+    double fs = 0;
+    double fNear = std::numeric_limits<double>::infinity();
+
+    double fnearx,fneary,fnearhdg,fnears;
+
+    while(fs<fRoadLen)
+    {
+        double geo_x,geo_y,geo_hdg;
+        pRoad->GetGeometryCoords(fs,geo_x,geo_y,geo_hdg);
+        double fdis = sqrt(pow(geo_x - x,2)+pow(geo_y - y,2));
+        if(fdis < fNear)
+        {
+            fNear = fdis;
+            fnearx = geo_x;
+            fneary = geo_y;
+            fnearhdg = geo_hdg;
+            fnears = fs;
+        }
+        fs = fs + 0.1;
+    }
+
+    if(fNear < 100)
+    {
+        double s = fnears;
+        double t = fNear;
+        double fhdg  = xodrfunc::CalcHdg(QPointF(fnearx,fneary),QPointF(x,y));
+        double fhdgdiff = fhdg  - fnearhdg;
+        while(fhdgdiff >= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+        while(fhdgdiff < 0)fhdgdiff = fhdgdiff + 2.0*M_PI;
+        if(fhdgdiff > M_PI)
+        {
+            t = t * (-1);
+        }
+
+        ui->lineEdit_s->setText(QString::number(s,'f',3));
+        ui->lineEdit_t->setText(QString::number(t,'f',3));
+    }
+
+}
+

src/tool/map_lanetoxodr/ui/trafficlightdialog.h

@@ -31,6 +31,8 @@ private slots:
 
     void on_pushButton_Update_clicked();
 
+    void on_pushButton_CalcST_clicked();
+
 private:
     Ui::TrafficLightDialog *ui;
     OpenDrive * mpxodr;

src/tool/map_lanetoxodr/ui/trafficlightdialog.ui

@@ -515,6 +515,78 @@
     <string>Edit InertialPosition</string>
    </property>
   </widget>
+  <widget class="QGroupBox" name="groupBox">
+   <property name="geometry">
+    <rect>
+     <x>520</x>
+     <y>0</y>
+     <width>441</width>
+     <height>121</height>
+    </rect>
+   </property>
+   <property name="title">
+    <string>Calc s and t</string>
+   </property>
+   <widget class="QPushButton" name="pushButton_CalcST">
+    <property name="geometry">
+     <rect>
+      <x>155</x>
+      <y>76</y>
+      <width>111</width>
+      <height>41</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Calc</string>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Lon">
+    <property name="geometry">
+     <rect>
+      <x>87</x>
+      <y>30</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_Lat">
+    <property name="geometry">
+     <rect>
+      <x>295</x>
+      <y>30</y>
+      <width>113</width>
+      <height>25</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_20">
+    <property name="geometry">
+     <rect>
+      <x>17</x>
+      <y>27</y>
+      <width>71</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Lon:</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_21">
+    <property name="geometry">
+     <rect>
+      <x>230</x>
+      <y>26</y>
+      <width>71</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Lat:</string>
+    </property>
+   </widget>
+  </widget>
  </widget>
  <resources/>
  <connections/>