change mpa_lanetoxodr, for optimize, not complete.

src/common/common/xodr/OpenDrive/RoadGeometry.cpp

@@ -197,6 +197,11 @@ double RoadGeometry::GetRoadCurvature(double s_check)
     return 0.0;
 }
 
+double RoadGeometry::GetEndHdg()
+{
+    return 0.0;
+}
+
 
 
 //***********************************************************************************
@@ -251,6 +256,11 @@ double GeometryLine::GetRoadCurvature(double s_check)
     return 0.0;
 }
 
+double GeometryLine::GetEndHdg()
+{
+    return mHdg;
+}
+
 
 
 //***********************************************************************************
@@ -364,6 +374,15 @@ double GeometryArc::GetRoadCurvature(double s_check)
     return fabs(mCurvature);
 }
 
+double GeometryArc::GetEndHdg()
+{
+    double retX,retY,retHdg;
+    GetCoords(mS+ mLength,retX,retY,retHdg);
+    while(retHdg >= (2.0*M_PI))retHdg = retHdg - 2.0*M_PI;
+    while(retHdg < 0) retHdg = retHdg + 2.0*M_PI;
+    return retHdg;
+}
+
 
 
 
@@ -669,6 +688,15 @@ void GeometrySpiral::GetCoords(double s_check, double &retX, double &retY, doubl
 }
 
 
+double GeometrySpiral::GetEndHdg()
+{
+    double retX,retY,retHdg;
+    GetCoords(mS+ mLength,retX,retY,retHdg);
+    while(retHdg >= (2.0*M_PI))retHdg = retHdg - 2.0*M_PI;
+    while(retHdg < 0) retHdg = retHdg + 2.0*M_PI;
+    return retHdg;
+}
+
 
 
 
@@ -974,6 +1002,15 @@ double GeometryPoly3::GetRoadCurvature(double s_check)
     return fcurv;
 }
 
+double GeometryPoly3::GetEndHdg()
+{
+    double retX,retY,retHdg;
+    GetCoords(mS+ mLength,retX,retY,retHdg);
+    while(retHdg >= (2.0*M_PI))retHdg = retHdg - 2.0*M_PI;
+    while(retHdg < 0) retHdg = retHdg + 2.0*M_PI;
+    return retHdg;
+}
+
 //***********************************************************************************
 //Cubic Polynom geometry. Has to be implemented.  Added By Yuchuli
 //***********************************************************************************
@@ -1183,6 +1220,15 @@ double GeometryParamPoly3::GetRoadCurvature(double s_check)
     return fcurv;
 }
 
+double GeometryParamPoly3::GetEndHdg()
+{
+    double retX,retY,retHdg;
+    GetCoords(mS+ mLength,retX,retY,retHdg);
+    while(retHdg >= (2.0*M_PI))retHdg = retHdg - 2.0*M_PI;
+    while(retHdg < 0) retHdg = retHdg + 2.0*M_PI;
+    return retHdg;
+}
+
 //***********************************************************************************
 //Base class for Geometry blocks
 //***********************************************************************************

src/common/common/xodr/OpenDrive/RoadGeometry.h

@@ -102,6 +102,7 @@ public:
 	virtual void GetCoords(double s_check, double &retX, double &retY);
 	virtual void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
     virtual double GetRoadCurvature(double s_check);
+    virtual double GetEndHdg();
 protected:
 
 	/**
@@ -145,6 +146,7 @@ public:
 	void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
 
     double GetRoadCurvature(double s_check);
+    double GetEndHdg();
 
 };
 //----------------------------------------------------------------------------------
@@ -204,6 +206,9 @@ public:
 	 */
 	void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
     double GetRoadCurvature(double s_check);
+
+    double GetEndHdg();
+
 protected:
 
 	/**
@@ -284,6 +289,9 @@ public:
 	 */
 	void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
     double GetRoadCurvature(double s_check);
+
+    double GetEndHdg();
+
 protected:
 
 	/**
@@ -339,6 +347,8 @@ public:
     void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
     double GetRoadCurvature(double s_check);
 
+    double GetEndHdg();
+
 };
 
 //----------------------------------------------------------------------------------
@@ -400,6 +410,8 @@ public:
 
     void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
     double GetRoadCurvature(double s_check);
+
+    double GetEndHdg();
 };
 
 

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -12,6 +12,8 @@
 #include <QDebug>
 #include <QPointF>
 
+#include "geofit.h"
+
 xodrfunc::xodrfunc()
 {
 
@@ -1492,7 +1494,7 @@ void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
             continue;
         }
 
-        if((pgeo1->GetLength()<5)  && (pgeo1->GetLength() > 0.001) && ((pgeo1->GetGeomType() == 0) || (pgeo1->GetGeomType() == 4)))
+        if((pgeo1->GetLength()<5)  && (pgeo1->GetLength() > 0.1) && ((pgeo1->GetGeomType() == 0) || (pgeo1->GetGeomType() == 4)))
         {
             double fhdgdiff = pgeo2->GetHdg() - pgeo1->GetHdg();
             while(fhdgdiff>M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
@@ -1502,55 +1504,124 @@ void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
                 qDebug("need optimize, index: %d ",i);
                 //Optimize
 
-                if(fhdgdiff > 0)
+                //If Before geo is arc, change curvature
+                if(pgeo0->GetGeomType() == 2)
                 {
-                    if(pgeo0->GetGeomType() == 2)
+
+                    GeometryArc * pArc = (GeometryArc * )pgeo0;
+
+                    //Only Optimize Arc < M_PI/2.0
+                    if(fabs(pArc->GetLength() * pArc->GetCurvature())<=(M_PI/2.0))
                     {
-                        GeometryArc * pArc = (GeometryArc * )pgeo0;
-                        if(pArc->GetCurvature() >0)
-                        {
-                            double fobjx,fobjy;
-                            fobjx = pgeo2->GetX() + pgeo1->GetLength() * cos(pgeo2->GetHdg() + M_PI);
-                            fobjy = pgeo2->GetY() + pgeo1->GetLength() * sin(pgeo2->GetHdg() + M_PI);
 
-                            double fdis = sqrt(pow(pgeo1->GetX() -fobjx,2) + pow(pgeo1->GetY() - fobjy,2));
-                            qDebug("dis, %f",fdis);
+                        double fobjx,fobjy;
+                        fobjx = pgeo2->GetX() + pgeo1->GetLength() * cos(pgeo2->GetHdg() + M_PI);
+                        fobjy = pgeo2->GetY() + pgeo1->GetLength() * sin(pgeo2->GetHdg() + M_PI);
 
-                            double fopvalue = fdis;
-                            if(fopvalue > maxopvalue)
-                            {
-                                fopvalue = maxopvalue;
-                                double fhdg = CalcHdg(QPointF(pgeo1->GetX(),pgeo1->GetY()),QPointF(fobjx,fobjy));
-                                fobjx = pgeo1->GetX() + fopvalue * cos(fhdg);
-                                fobjy = pgeo1->GetY() + fopvalue * sin(fhdg);
-                            }
+                        //Git dis
+                        double fdis = sqrt(pow(pgeo1->GetX() -fobjx,2) + pow(pgeo1->GetY() - fobjy,2));
+                        qDebug("dis, %f",fdis);
 
-                            double farcdis = sqrt(pow(fobjx - pgeo0->GetX(),2)+ pow(fobjy - pgeo0->GetY(),2));
-                            double fhdgarctoline = CalcHdg(QPointF(pgeo0->GetX(),pgeo0->GetY()),QPointF(fobjx,fobjy));
-                            double fhdgdiff_arcline = fhdgarctoline - pgeo0->GetHdg();
-                            while(fhdgdiff_arcline>M_PI)fhdgdiff_arcline = fhdgdiff_arcline - 2.0*M_PI;
-                            while(fhdgdiff_arcline<(-M_PI))fhdgdiff_arcline = fhdgdiff_arcline + 2.0*M_PI;
+                        double fopvalue = fdis;
+
+                        //If dis > maxopvalue, set dis to maxopvalue,  and change first  geo end point.
+                        if(fopvalue > maxopvalue)
+                        {
+                            fopvalue = maxopvalue;
+                            double fhdg = CalcHdg(QPointF(pgeo1->GetX(),pgeo1->GetY()),QPointF(fobjx,fobjy));
+                            fobjx = pgeo1->GetX() + fopvalue * cos(fhdg);
+                            fobjy = pgeo1->GetY() + fopvalue * sin(fhdg);
+                        }
+
+                        //Calc dis from geo0 startpoint to obj point
+                        double farcdis = sqrt(pow(fobjx - pgeo0->GetX(),2)+ pow(fobjy - pgeo0->GetY(),2));
+                        //Calc hdg from geo0 startpoint to obj point
+                        double fhdgarctoline = CalcHdg(QPointF(pgeo0->GetX(),pgeo0->GetY()),QPointF(fobjx,fobjy));
+                        //Calc hdgdiff betwwen line hdg to geo0 start  hed
+                        double fhdgdiff_arcline = fhdgarctoline - pgeo0->GetHdg();
+                        //normalize hdg to -M_PI to M_PI;
+                        while(fhdgdiff_arcline>M_PI)fhdgdiff_arcline = fhdgdiff_arcline - 2.0*M_PI;
+                        while(fhdgdiff_arcline<(-M_PI))fhdgdiff_arcline = fhdgdiff_arcline + 2.0*M_PI;
+
+                        //geo1 hdg
+                        double flinehdg = CalcHdg(QPointF(fobjx,fobjy),QPointF(pgeo2->GetX(),pgeo2->GetY()));
+
+                        //If is arc
+                        if(fabs(fhdgdiff_arcline)> 0.0000001)
+                        {
 
                             double R = (farcdis/2.0)/sin(fabs(fhdgdiff_arcline));
                             double curvature = 1.0/R;
+                            if(fhdgdiff_arcline<0)curvature = curvature * (-1.0);
                             double farclen = R*2.0* fabs(fhdgdiff_arcline);
                             pgeo0->SetLength(farclen);
                             pArc->SetCurvature(curvature);
 
-                            double flinehdg = CalcHdg(QPointF(fobjx,fobjy),QPointF(pgeo2->GetX(),pgeo2->GetY()));
+                        }
+                        else  //Change geo0 to line
+                        {
+
+                            std::vector<GeometryBlock> * pvectorgeob =  pRoad->GetGeometryBlockVector();
+                            GeometryBlock newline;
+                            newline.AddGeometryLine(pgeo0->GetS(),pgeo0->GetX(),pgeo0->GetY(),pgeo0->GetHdg(),farcdis);
+                            pvectorgeob->erase(pvectorgeob->begin()+i-1);
+                            pvectorgeob->insert(pvectorgeob->begin()+i-1,newline);
+                        }
 
+                        //CChange to new geo.
+                        pgeo0 = pRoad->GetGeometryBlock(i-1)->GetGeometryAt(0);
+
+                        //Change geo1
+                        if(pgeo1->GetGeomType() == 0)
+                        {
                             pgeo1->SetX(fobjx);
                             pgeo1->SetY(fobjy);
                             pgeo1->SetHdg(flinehdg);
                             pgeo1->SetLength(sqrt(pow(pgeo2->GetX() - fobjx,2)+pow(pgeo2->GetY() - fobjy,2)));
-                            boptimize = true;
+                        }
+                        else
+                        {
+                            double startx,starty,starthdg,endx,endy,endhdg;
+                            endx = pgeo2->GetX();endy = pgeo2->GetY();endhdg = pgeo2->GetEndHdg();
+                            pgeo0->GetCoords(pgeo0->GetS() + pgeo0->GetLength(),startx,starty,starthdg);
+                            std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(startx,starty,starthdg,endx,endy,endhdg);
+
+                            if(xvectorgeo.size()>0)
+                            {
+                                if(xvectorgeo[0].mnType == 0)
+                                {
+                                    std::vector<GeometryBlock> * pvectorgeob =  pRoad->GetGeometryBlockVector();
+                                    GeometryBlock newline;
+                                    newline.AddGeometryLine(pgeo1->GetS(),xvectorgeo[0].mfX,xvectorgeo[0].mfY,xvectorgeo[0].mfHdg,xvectorgeo[0].mfLen);
+                                    pvectorgeob->erase(pvectorgeob->begin()+i);
+                                    pvectorgeob->insert(pvectorgeob->begin()+i,newline);
+                                }
+                                else
+                                {
+                                    std::vector<GeometryBlock> * pvectorgeob =  pRoad->GetGeometryBlockVector();
+                                    GeometryBlock newpp3;
+                                    newpp3.AddGeometryParamPoly3(pgeo1->GetS(),xvectorgeo[0].mfX,xvectorgeo[0].mfY,xvectorgeo[0].mfHdg,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]);
+                                    pvectorgeob->erase(pvectorgeob->begin()+i);
+                                    pvectorgeob->insert(pvectorgeob->begin()+i,newpp3);
+                                }
+                            }
+                            else
+                            {
+                                std::cout<<"Optimize road ,CreateBezierGeo Fail."<<std::endl;
+                            }
+                        }
 
+                        boptimize = true;
 
 
-                        }
                     }
+
+
                 }
 
+
                 i = i+2;  //Because Optimize, so move 2 step.
             }
         }