change adcxodrviewer, fix spiral.

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

@@ -380,13 +380,51 @@ void GeometrySpiral::ComputeVars()
 {
 	mA=0;
 
+    mLengthBase = 0;
+    mX0 = 0;
+    mY0 = 0;
+    mHDG0 = 0;
+
+    mHDG0Cos = 1.0;
+    mHDG0Sin = 0.0;
+
 	//if the curvatureEnd is the non-zero curvature, then the motion is in normal direction along the spiral
-	if ((fabs(mCurvatureEnd)>1.00e-15)&&(fabs(mCurvatureStart)<=1.00e-15))
+ //   if ((fabs(mCurvatureEnd)>1.00e-15)&&(fabs(mCurvatureStart)<=1.00e-15))
+    if ((fabs(mCurvatureEnd))>(fabs(mCurvatureStart)))
 	{
 		mNormalDir=true;
-		mCurvature=mCurvatureEnd;
+        mCurvature=mCurvatureEnd;
 		//Calculate the normalization term : a = 1.0/sqrt(2*End_Radius*Total_Curve_Length) 
-		mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*mLength);			
+        mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*mLength);
+
+        double xLength = mLength;
+        if(fabs(mCurvatureStart)>0.00001)
+        {
+            mLengthBase = mLength * fabs(mCurvatureStart)/(fabs(mCurvatureEnd) - fabs(mCurvatureStart));
+            xLength = mLength  + mLengthBase;
+            mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*xLength);
+
+
+            double l_start =mLengthBase *mA/sqrtPiO2;
+
+            //Solve the Fresnel Integrals
+            fresnl(l_start,&mY0,&mX0);
+
+            if (mCurvature<0)
+                mY0=-mY0;
+
+           l_start =mLengthBase*mA;
+            double tangentAngle = l_start*l_start;
+            if (mCurvature<0)
+                tangentAngle=-tangentAngle;
+
+            mHDG0 = tangentAngle;
+
+            mHDG0Cos = cos(-mHDG0);
+            mHDG0Sin = sin(-mHDG0);
+
+        }
+
 		//Denormalization Factor
 		mDenormalizeFactor=1.0/mA;	
 
@@ -395,30 +433,74 @@ void GeometrySpiral::ComputeVars()
 		mRotSin=sin(mHdg);
 	}
 	//else the motion is in the inverse direction along the spiral
-	else
+    else
 	{
+
 		mNormalDir=false;
-		mCurvature=mCurvatureStart;
+        mCurvature=mCurvatureStart;
+
 		//Calculate the normalization term : a = 1.0/sqrt(2*End_Radius*Total_Curve_Length) 
-		mA=1.0/sqrt(2*1.0/fabs(mCurvature)*mLength);			
+        mA=1.0/sqrt(2*1.0/fabs(mCurvature)*mLength);
+
+
+        if(fabs(mCurvatureEnd)>0.00001)
+        {
+            mLengthBase = mLength * fabs(mCurvatureEnd)/(fabs(mCurvatureStart) - fabs(mCurvatureEnd));
+            double xLength = mLength  + mLengthBase;
+            mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*xLength);
+            //not complete, when have this type spiral, change this code.
+
+            double l_start =mLengthBase *mA/sqrtPiO2;
+
+            //Solve the Fresnel Integrals
+            fresnl(l_start,&mY0,&mX0);
+
+            if (mCurvature<0)
+                mY0=-mY0;
+
+           l_start =mLengthBase*mA;
+            double tangentAngle = l_start*l_start;
+            if (mCurvature<0)
+                tangentAngle=-tangentAngle;
+
+            mHDG0 = tangentAngle;
+
+            mHDG0Cos = cos(-mHDG0);
+            mHDG0Sin = sin(-mHDG0);
+
+        }
+
 
 		//Because we move in the inverse direction, we need to rotate the curve according to the heading
 		//around the last point of the normalized spiral
 		//Calculate the total length, normalize it and divide by sqrtPiO2, then, calculate the position of the final point.
-		double L=(mS2-mS)*mA/sqrtPiO2;							
+        double L=(mS2-mS + mLengthBase)*mA/sqrtPiO2;
 		fresnl(L,&mEndY,&mEndX);
 		//Invert the curve if the curvature is negative
 		if (mCurvature<0)
 			mEndY=-mEndY;
 
+        mEndX = mEndX - mX0;
+        mEndY = mEndY - mY0;
+
+        double tx,ty;
+        tx = mEndX;
+        ty = mEndY;
+
+        mEndX=tx*mHDG0Cos -ty*mHDG0Sin;
+        mEndY=ty*mHDG0Cos+tx*mHDG0Sin;
+
 		//Denormalization factor
-		mDenormalizeFactor=1.0/mA;									
+
+        mDenormalizeFactor=1.0/mA;
+
 		//Find the x,y coords of the final point of the curve in local curve coordinates
 		mEndX*=mDenormalizeFactor*sqrtPiO2;						
 		mEndY*=mDenormalizeFactor*sqrtPiO2;
 
+        double L2=(mLengthBase)*mA/sqrtPiO2;
 		//Calculate the tangent angle
-		differenceAngle=L*L*(sqrtPiO2*sqrtPiO2);
+        differenceAngle=L*L*(sqrtPiO2*sqrtPiO2) - L2*L2*(sqrtPiO2*sqrtPiO2);
 		double diffAngle;
 		//Calculate the tangent and heading angle difference that will be used to rotate the spiral
 		if (mCurvature<0)
@@ -517,35 +599,50 @@ void GeometrySpiral::GetCoords(double s_check, double &retX, double &retY, doubl
 	double l=0.0;
 	double tmpX=0.0, tmpY=0.0;
 
+
+
 	//Depending on the moving direction, calculate the length of the curve from its beginning to the current point and normalize
 	//it by multiplying with the "a" normalization term
 	//Cephes lib for solving Fresnel Integrals, uses cos/sin (PI/2 * X^2) format in its function.
 	//So, in order to use the function, transform the argument (which is just L) by dividing it by the sqrt(PI/2) factor and multiply the results by it.
 	if (mNormalDir)
 	{ 
-		l=(s_check-mS)*mA/sqrtPiO2;
+        l=(s_check-mS + mLengthBase)*mA/sqrtPiO2;
 	}
 	else
 	{
-		l=(mS2-s_check)*mA/sqrtPiO2;		
+        l=(mS2-s_check + mLengthBase)*mA/sqrtPiO2;
 	}
 
 	//Solve the Fresnel Integrals
 	fresnl(l,&tmpY,&tmpX);
+
+
 	//If the curvature is negative, invert the curve on the Y axis
 	if (mCurvature<0)
 		tmpY=-tmpY;
 
+
+    tmpX = tmpX - mX0;
+    tmpY = tmpY - mY0;
+
+    double tx,ty;
+    tx = tmpX;
+    ty = tmpY;
+
+    tmpX=tx*mHDG0Cos -ty*mHDG0Sin;
+    tmpY=ty*mHDG0Cos+tx*mHDG0Sin;
+
 	//Denormalize the results and multiply by the sqrt(PI/2) term
 	tmpX*=mDenormalizeFactor*sqrtPiO2;	
 	tmpY*=mDenormalizeFactor*sqrtPiO2;
 
 	//Calculate the heading at the found position. Kill the sqrt(PI/2) term that was added to the L
-	l=(s_check-mS)*mA;
+    l=(s_check-mS+mLengthBase)*mA;
 	double tangentAngle = l*l;
 	if (mCurvature<0)
 		tangentAngle=-tangentAngle;
-	retHDG=mHdg+tangentAngle;
+    retHDG=mHdg+tangentAngle -mHDG0;
 
 
 	if (!mNormalDir)

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

@@ -240,6 +240,14 @@ private:
 	double mRotCos;
 	double mRotSin;
 
+    double mLengthBase; //If Start Curvature not zero
+    double mX0;  //If Start Curvature not zero
+    double mY0;
+    double mHDG0; //If Start Curvature not zero
+
+    double mHDG0Cos;
+    double mHDG0Sin;
+
 public:
 	/**
 	 * Constructor that initializes the base properties of the record

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

@@ -343,6 +343,12 @@ int RoadSample::SampleRoad(Road * pRoad)
         }
     }
 
+
+    if(pRoad->GetRoadId() == "16")
+    {
+        int a = 1;
+        a++;
+    }
     std::vector<RoadPoint> * pvectorRoadPoint = &mvectorRoadPoint;
 
     std::cout<<"Road: "<<mstrroadid<<" max "<<mfRefX_max<<" "<<mfRefY_max<<"  min "<<mfRefX_min<<" "<<mfRefY_min<<std::endl;

src/tool/map_lanetoxodr/view/xodrscenfunc.cpp

@@ -181,7 +181,9 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
     unsigned int i;
 
     bool bSetBrush = false;
+    double fOldZValue = 10.0;
     std::vector<QColor> xvectorBrush;
+    std::vector<double> xvectorZValue;
     for(i=0;i<(nsize-1);i++)
     {
         unsigned int nLanePointSize1 = static_cast<unsigned int>(pvectorRP->at(i).mvectorLanePoint.size()) ;
@@ -190,6 +192,7 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
         {
             bSetBrush = false;
             xvectorBrush.clear();
+            xvectorZValue.clear();
             continue;
         }
 
@@ -215,10 +218,11 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
             pitem->setPath(xpath);
 
             QColor brushcolor = Qt::yellow;
+
             if(bSetBrush == false)
             {
 
-
+                fOldZValue = 1.0;
                 std::string strlanetype = xvectorLanePoint1[j+1].mstrLaneType;
 
                 if(xvectorLanePoint1[j].nLane > 0)
@@ -226,46 +230,62 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
                     strlanetype = xvectorLanePoint1[j].mstrLaneType;
                 }
 
-                if(strlanetype == "driving")
+                if((strlanetype == "driving")||(strlanetype == "exit")||(strlanetype == "entry")||(strlanetype == "onRamp")||(strlanetype == "offRamp")||(strlanetype == "connectingRamp"))
                 {
+                    fOldZValue = 10.0;
                     brushcolor = Qt::darkGray;
+
                 }
                 else
                 {
                     if(strlanetype == "shoulder")
                     {
+                        fOldZValue = 5.0;
                         brushcolor = QColor(0x66,0xBF,0x00);
                     }
                     else
                     {
                         if(strlanetype == "border")
                         {
+                            fOldZValue = 7.0;
                             brushcolor = Qt::darkGreen;
                         }
                         else
                         {
                             if(strlanetype == "biking")
                             {
+                                fOldZValue = 9.0;
                                 brushcolor = Qt::red;
                             }
                             else
                             {
                                 if(strlanetype == "sidewalk")
                                 {
+                                    fOldZValue = 8.0;
                                     brushcolor = QColor(0xB2,0xB2,0xD6);
                                 }
                                 else
                                 {
                                     if(strlanetype == "stop")
                                     {
+                                        fOldZValue = 7.0;
                                         brushcolor = Qt::black;
                                     }
                                     else
                                     {
-                                        if(strlanetype == "median")
+                                        if((strlanetype == "median")||(strlanetype == "restricted"))
                                         {
+                                            fOldZValue = 6.0;
                                             brushcolor = Qt::darkGreen;
                                         }
+                                        else
+                                        {
+                                            if(strlanetype == "none")
+                                            {
+                                                fOldZValue = 4.0;
+                                                brushcolor = Qt::darkGreen;
+                                            }
+                                        }
 
                                     }
                                 }
@@ -273,13 +293,17 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
                         }
                     }
                 }
+
                 xvectorBrush.push_back(brushcolor);
+                xvectorZValue.push_back(fOldZValue);
             }
             else
             {
                 brushcolor = xvectorBrush[j];
+                fOldZValue = xvectorZValue[j];
             }
 
+            pitem->setZValue(fOldZValue);
             pitem->setBrush(brushcolor);
             pitem->setPen(QPen(brushcolor,0.001));
     //          mpscene->addItem(pitem);
@@ -441,6 +465,7 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
                         brushcolor = Qt::white;
                         break;
                     }
+                    pitem->setZValue(11.0);
                     pitem->setBrush(brushcolor);
                     pitem->setPen(QPen(brushcolor,0.001));
                     //                pitem->setPos(mfViewMoveX + VIEW_WIDTH/2,-mfViewMoveY +VIEW_HEIGHT/2);

src/tool/map_lanetoxodr/xvmainwindow2.cpp

@@ -45,7 +45,7 @@ XVMainWindow::XVMainWindow(QWidget *parent) :
 
     connect(&mFileDialog,SIGNAL(accepted()),this,SLOT(onFileOpen()));
 
-    setWindowTitle("ADC OpenDrive Viewer2");
+    setWindowTitle("ADC OpenDrive Viewer2 (20230201)");
 }
 
 XVMainWindow::~XVMainWindow()
@@ -114,7 +114,7 @@ void XVMainWindow::LoadXODR(QString strpath)
 
     mvectorRoadSample.clear();
 
-    int nroadnum = pxodr->GetRoadCount();
+    int nroadnum = static_cast<int>(pxodr->GetRoadCount()) ;
     int i;
     double froadlen = 0;
     double fxmin,fxmax,fymin,fymax;
@@ -197,6 +197,7 @@ void XVMainWindow::UpdateScene()
             QGraphicsPathItem * pitem = xvectorlanepath[j];
 //            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
             pitem->setPos(mfViewMoveX,-mfViewMoveY);
+
             mpscene->addItem(pitem);
             mvectorviewitem.push_back(pitem);
         }