change tool/map_lanetoxodr, add lane move mode.

src/tool/map_lanetoxodr/dialogroadmove.cpp

@@ -1,6 +1,8 @@
 #include "dialogroadmove.h"
 #include "ui_dialogroadmove.h"
 
+#include "dialogaddroadfromrtk.h"
+
 #include <math.h>
 
 #include <mainwindow.h>
@@ -21,8 +23,10 @@ DialogRoadMove::DialogRoadMove(OpenDrive * pxodr,Road * pRoad, QWidget *parent)
         setWindowTitle(QString(pRoad->GetRoadId().data()));
     }
 
+    ui->comboBox_dirmode->addItem("Lane Mode");
     ui->comboBox_dirmode->addItem("Entire Road");
     ui->comboBox_dirmode->addItem("Road Start");
+
     ui->comboBox_dirmode->setCurrentIndex(0);
 }
 
@@ -31,6 +35,232 @@ DialogRoadMove::~DialogRoadMove()
     delete ui;
 }
 
+
+void DialogRoadMove::lanemodemove(double fmovex, double fmovey, Road &newroad, Road *pRoad)
+{
+    int ngeosize = pRoad->GetGeometryBlockCount();
+    int i;
+    std::vector<iv::rtkdata> xvectorrtkdata;
+    double snow = 0;
+    for(i=0;i<ngeosize;i++)
+    {
+        if(pRoad->GetGeometryBlock(i)->GetGeometryAt(0)->GetGeomType() == 0)
+        {
+            double fstep = std::max(0.1,std::min(1.0,pRoad->GetGeometryBlock(i)->GetBlockLength()/3.0));
+            int j;
+            double x,y,fhdg;
+            int ncount = std::max(1,(int)(pRoad->GetGeometryBlock(i)->GetBlockLength()/fstep));
+            for(j=0;j<ncount;j++)
+            {
+                xodrfunc::GetRoadXYByS(pRoad,snow + fstep *j,x,y,fhdg);
+
+                double yoff = fmovex*sin(fhdg+M_PI/2.0) + fmovey *sin(fhdg);
+                double xoff = fmovex*cos(fhdg+M_PI/2.0) + fmovey *cos(fhdg);
+                x = x + xoff;
+                y = y + yoff;
+                iv::rtkdata xdata;
+                xdata.mfrels = snow + fstep * j;
+                xdata.mfrelx = x;
+                xdata.mfrely = y;
+                xvectorrtkdata.push_back(xdata);
+
+            }
+        }
+        else
+        {
+            double fstep = 0.5;
+            int j;
+            double x,y,fhdg;
+            int ncount = std::max(1,(int)(pRoad->GetGeometryBlock(i)->GetBlockLength()/fstep));
+            for(j=0;j<ncount;j++)
+            {
+                xodrfunc::GetRoadXYByS(pRoad,snow + fstep *j,x,y,fhdg);
+
+                double yoff = fmovex*sin(fhdg+M_PI/2.0) + fmovey *sin(fhdg);
+                double xoff = fmovex*cos(fhdg+M_PI/2.0) + fmovey *cos(fhdg);
+                x = x + xoff;
+                y = y + yoff;
+                iv::rtkdata xdata;
+                xdata.mfrels = snow + fstep * j;
+                xdata.mfrelx = x;
+                xdata.mfrely = y;
+                xvectorrtkdata.push_back(xdata);
+
+            }
+        }
+        snow = snow + pRoad->GetGeometryBlock(i)->GetBlockLength();
+    }
+
+    double LINE_ERROR = 0.1;
+
+
+    std::vector<geobase> xvectorgeo;
+
+    bool bComplete = false;
+    int j;
+    int ncurpos = 0;
+    int nrange = xvectorrtkdata.size();
+    while(bComplete == false)
+    {
+
+        VectorXd x_veh(nrange);
+        VectorXd y_veh(nrange);
+        for(j=0;j<nrange;j++)
+        {
+            x_veh[j] = xvectorrtkdata.at(j+ncurpos).mfrelx;
+            y_veh[j] = xvectorrtkdata.at(j+ncurpos).mfrely;
+        }
+
+        bool bArcOk = false;
+        bool bLineOk = false;
+        double dismax = 0;
+        QPointF sp,ep;
+        double fR,flen;
+        double fhdgstart,fhdgend;
+        if(nrange >= 3)
+        {
+
+            geofit::Inst().arcfitanddis(x_veh,y_veh,dismax,fR,sp,ep,fhdgstart,fhdgend,flen);
+
+            if(dismax < LINE_ERROR)
+            {
+                bArcOk = true;
+                std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+            }
+        }
+
+
+        auto coeffs = polyfit(x_veh, y_veh, 1);
+
+        dismax = 0;
+        for(j=0;j<nrange;j++)
+        {
+            double A = coeffs[1];
+            double B = -1;
+            double C = coeffs[0];
+            double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
+            if(dis>dismax)dismax = dis;
+        }
+        if(dismax < LINE_ERROR)
+        {
+            bLineOk = true;
+            std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+        }
+
+
+
+        if((nrange <= 2)||bLineOk||bArcOk)
+        {
+            if(bLineOk)
+            {
+                std::cout<<"use line"<<std::endl;
+                geobase xgeo;
+                xgeo.mnStartPoint = ncurpos;
+                xgeo.mnEndPoint = ncurpos + nrange -1 ;
+
+                double x0,y0,x1,y1;
+                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[0],
+                        y_veh[0],x0,y0);
+                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[nrange -1],
+                        y_veh[nrange - 1],x1,y1);
+                xgeo.mfA = coeffs[1];
+                xgeo.mfB = -1;
+                xgeo.mfC = coeffs[0];
+                xgeo.mfHdg = geofit::Inst().CalcHdg(x0,y0,x1,y1);
+                xgeo.mfX = x0;
+                xgeo.mfY = y0;
+                xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
+                xgeo.mnType = 0;
+                xvectorgeo.push_back(xgeo);
+            }
+            else
+            {
+                if(bArcOk)
+                {
+                    std::cout<<"use arc"<<std::endl;
+                    geobase xgeo;
+                    xgeo.mnStartPoint = ncurpos;
+                    xgeo.mnEndPoint = ncurpos + nrange -1;
+                    xgeo.mfHdg = fhdgstart;
+                    xgeo.mfHdgStart = fhdgstart;
+                    xgeo.mfHdgEnd = fhdgend;
+                    xgeo.mfX = sp.x();
+                    xgeo.mfY = sp.y();
+                    xgeo.mfLen = flen;
+                    xgeo.mnType = 1;
+                    xgeo.mfEndX = ep.x();
+                    xgeo.mfEndY = ep.y();
+                    xgeo.mR = fR;
+                    xvectorgeo.push_back(xgeo);
+                }
+            }
+            //        std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
+            ncurpos = ncurpos + nrange -1;
+            nrange = xvectorrtkdata.size() - ncurpos;
+
+        }
+        else
+        {
+            nrange = nrange/2;
+            if(nrange<2)nrange = 2;
+        }
+        if(ncurpos>=(xvectorrtkdata.size()-1))bComplete = true;
+    }
+
+
+    std::cout<<"complete "<<std::endl;
+
+
+    double xroadlen = 0;
+    for(j=0;j<xvectorgeo.size();j++)
+    {
+        xroadlen = xroadlen + xvectorgeo[j].mfLen;
+    }
+
+
+    while(newroad.GetGeometryBlockCount()>0)
+    {
+        newroad.DeleteGeometryBlock(0);
+    }
+
+  //  p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
+
+    double s = 0;
+    j= 0;
+//        for(j=0;j<4;j++)
+    for(j=0;j<xvectorgeo.size();j++)
+    {
+
+        newroad.AddGeometryBlock();
+        GeometryBlock * pgb = newroad.GetGeometryBlock(j);
+
+        geobase * pline;
+        geobase * pbez;
+        geobase * parc;
+
+        switch(xvectorgeo[j].mnType)
+        {
+        case 0:
+            pline = &xvectorgeo[j];
+            pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
+            break;
+        case 1:
+            parc = &xvectorgeo[j];
+            pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
+            break;
+        case 2:
+            pbez = &xvectorgeo[j];
+            std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
+            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]);
+            break;
+        }
+        s = s + xvectorgeo[j].mfLen;
+    }
+}
+
 void DialogRoadMove::on_pushButton_Move_clicked()
 {
     if(mpRoad == 0)
@@ -58,8 +288,13 @@ void DialogRoadMove::on_pushButton_Move_clicked()
 
     Road newroad = *mpRoad;
 
+
+    if(ui->comboBox_dirmode->currentIndex() != 0)
+    {
+
+
     double hdg = 0;
-    if(ui->comboBox_dirmode->currentIndex() == 1)
+    if(ui->comboBox_dirmode->currentIndex() == 2)
     {
     if(mpRoad->GetGeometryBlockCount()>0)
     {
@@ -85,6 +320,12 @@ void DialogRoadMove::on_pushButton_Move_clicked()
         newroad.GetGeometryBlock(i)->GetGeometryAt(0)->SetY(newroad.GetGeometryBlock(i)->GetGeometryAt(0)->GetY() + yoff);
     }
 
+    }
+    else
+    {
+        lanemodemove(fMoveX,fMoveY,newroad,mpRoad);
+    }
+
     int nnewroadid = gw->CreateRoadID();
     newroad.SetRoadId(QString::number(nnewroadid).toStdString());
 

src/tool/map_lanetoxodr/dialogroadmove.h

@@ -22,6 +22,9 @@ public:
 private slots:
     void on_pushButton_Move_clicked();
 
+
+private:
+    void lanemodemove(double fmovex,double fmovey,Road & newroad, Road * pRoad);
 private:
     Ui::DialogRoadMove *ui;