diff --git a/OpenRoadEd/OpenDrive/OpenDriveXmlParser.cpp b/OpenRoadEd/OpenDrive/OpenDriveXmlParser.cpp
index b3785a28b070163cc79b58bb8b1108e59b96aa00..cf93e24ff6def359c0a73dd85102a8e1c8dc7b7e 100644
--- a/OpenRoadEd/OpenDrive/OpenDriveXmlParser.cpp
+++ b/OpenRoadEd/OpenDrive/OpenDriveXmlParser.cpp
@@ -333,6 +333,11 @@ bool OpenDriveXmlParser::ReadPlanView(Road* road, TiXmlElement *node)
{
ReadGeometryBlock(road, subNode,2); //load a polynom spline block
}
+ else if (subNodeType->ValueStr().compare("paramPoly3")==0)
+ {
+ ReadGeometryBlock(road, subNode,3); //load a polynom spline block
+ }
+
else
cout<<"Unsupported geometry type for road " << road->GetRoadName() << ": " << subNodeType->ValueStr() << endl;
@@ -363,6 +368,9 @@ bool OpenDriveXmlParser::ReadGeometryBlock (Road* road, TiXmlElement *&node, sho
case 2:
ReadGeometry(geomBlock, node, 3);
break;
+ case 3:
+ ReadGeometry(geomBlock, node, 4);
+ break;
}
return true;
@@ -420,7 +428,7 @@ bool OpenDriveXmlParser::ReadGeometry(GeometryBlock* geomBlock, TiXmlElement *no
break;
- case 3: //poly3
+ case 3: //poly3
checker=TIXML_SUCCESS;
double a,b,c,d;
checker+=subNode->QueryDoubleAttribute("a",&a);
@@ -429,12 +437,34 @@ bool OpenDriveXmlParser::ReadGeometry(GeometryBlock* geomBlock, TiXmlElement *no
checker+=subNode->QueryDoubleAttribute("d",&d);
if (checker!=TIXML_SUCCESS)
{
- cout<<"Error parsing arc attributes"<<endl;
+ cout<<"Error parsing poly3 attributes"<<endl;
return false;
}
geomBlock->AddGeometryPoly3(s,x,y,hdg,length,a,b,c,d);
break;
+
+ case 4: //parampoly3
+ checker=TIXML_SUCCESS;
+ double au,bu,cu,du;
+ double av,bv,cv,dv;
+ checker+=subNode->QueryDoubleAttribute("aU",&au);
+ checker+=subNode->QueryDoubleAttribute("bU",&bu);
+ checker+=subNode->QueryDoubleAttribute("cU",&cu);
+ checker+=subNode->QueryDoubleAttribute("dU",&du);
+ checker+=subNode->QueryDoubleAttribute("aV",&av);
+ checker+=subNode->QueryDoubleAttribute("bV",&bv);
+ checker+=subNode->QueryDoubleAttribute("cV",&cv);
+ checker+=subNode->QueryDoubleAttribute("dV",&dv);
+ if (checker!=TIXML_SUCCESS)
+ {
+ cout<<"Error parsing paramPoly3 attributes"<<endl;
+ return false;
+ }
+
+ geomBlock->AddGeometryParamPoly3(s,x,y,hdg,length,au,bu,cu,du,av,bv,cv,dv);
+ break;
+
}
return true;
diff --git a/OpenRoadEd/OpenDrive/OpenDriveXmlWriter.cpp b/OpenRoadEd/OpenDrive/OpenDriveXmlWriter.cpp
index e6fbb959920f176f30536d4c8e9e9defb381c961..34e2ebdd0d2def8a37804a2ca48ddc90b559da22 100644
--- a/OpenRoadEd/OpenDrive/OpenDriveXmlWriter.cpp
+++ b/OpenRoadEd/OpenDrive/OpenDriveXmlWriter.cpp
@@ -220,6 +220,14 @@ bool OpenDriveXmlWriter::WriteGeometryBlock (TiXmlElement *node, GeometryBlock*
{
WriteGeometry(node, geometryBlock->GetGeometryAt(0), 0);
}
+ else if(geometryBlock->CheckIfPoly3())
+ {
+ WriteGeometry(node, geometryBlock->GetGeometryAt(0), 3);
+ }
+ else if(geometryBlock->CheckIfParamPoly3())
+ {
+ WriteGeometry(node, geometryBlock->GetGeometryAt(0), 4);
+ }
else
{
WriteGeometry(node, geometryBlock->GetGeometryAt(0), 1);
@@ -315,8 +323,58 @@ bool OpenDriveXmlWriter::WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeo
case 3:
{
//poly3
+ GeometryPoly3 *lPoly3=static_cast<GeometryPoly3*>(roadGeometry);
+ TiXmlElement *nodePoly3 = new TiXmlElement("poly3");
+ nodeGeometry->LinkEndChild(nodePoly3);
+ std::stringstream svalue;
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lPoly3->GetA();
+ nodePoly3->SetAttribute("a",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lPoly3->GetB();
+ nodePoly3->SetAttribute("b",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lPoly3->GetC();
+ nodePoly3->SetAttribute("c",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lPoly3->GetD();
+ nodePoly3->SetAttribute("d",svalue.str());
+ svalue.str("");
break;
}
+ case 4:
+ {
+ //param poly3
+ GeometryParamPoly3 *lParamPoly3=static_cast<GeometryParamPoly3*>(roadGeometry);
+ TiXmlElement *nodeParamPoly3 = new TiXmlElement("paramPoly3");
+ nodeGeometry->LinkEndChild(nodeParamPoly3);
+ std::stringstream svalue;
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetAu();
+ nodeParamPoly3->SetAttribute("aU",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetBu();
+ nodeParamPoly3->SetAttribute("bU",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetCu();
+ nodeParamPoly3->SetAttribute("cU",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetDu();
+ nodeParamPoly3->SetAttribute("dU",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetAv();
+ nodeParamPoly3->SetAttribute("aV",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetBv();
+ nodeParamPoly3->SetAttribute("bV",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetCv();
+ nodeParamPoly3->SetAttribute("cV",svalue.str());
+ svalue.str("");
+ svalue << setprecision(16) << setiosflags (ios_base::scientific) << lParamPoly3->GetDv();
+ nodeParamPoly3->SetAttribute("dV",svalue.str());
+ svalue.str("");
+ break;
+ }
+
}
return true;
diff --git a/OpenRoadEd/OpenDrive/RoadGeometry.cpp b/OpenRoadEd/OpenDrive/RoadGeometry.cpp
index bfffe48d4286cc170de18329db5f961876aba79f..90e0fca82fc3a8cc8bd3da87ee8b208afe807deb 100644
--- a/OpenRoadEd/OpenDrive/RoadGeometry.cpp
+++ b/OpenRoadEd/OpenDrive/RoadGeometry.cpp
@@ -1,6 +1,7 @@
#include "RoadGeometry.h"
#define _USE_MATH_DEFINES
#include <math.h>
+#include <iostream>
//#define PI 3.14159265358979323846264338327950288
extern int fresnl( double , double *, double * );
@@ -481,6 +482,151 @@ void GeometrySpiral::GetCoords(double s_check, double &retX, double &retY, doubl
retY=mY+ tmpY*mRotCos+tmpX*mRotSin;
}
+//***********************************************************************************
+//ParamPoly3 geometry
+//***********************************************************************************
+/**
+ * Constructor that initializes the base properties of the record
+ */
+GeometryParamPoly3::GeometryParamPoly3 (double s, double x, double y, double hdg, double length,double au,double bu,double cu,double du, double av, double bv, double cv, double dv): RoadGeometry(s, x, y, hdg, length)
+{
+ SetGeomType(4);
+ mAu=au;
+ mBu=bu;
+ mCu=cu;
+ mDu=du;
+ mAv=av;
+ mBv=bv;
+ mCv=cv;
+ mDv=dv;
+ ComputeVars();
+}
+
+/**
+ * Computes the required vars
+ */
+void GeometryParamPoly3::ComputeVars()
+{
+}
+
+/**
+ * Clones and returns the new geometry record
+ */
+RoadGeometry* GeometryParamPoly3::Clone() const
+{
+ GeometryParamPoly3* ret=new GeometryParamPoly3(mS,mX,mY, mHdg, mLength,mAu,mBu,mCu,mDu,mAv,mBv,mCv,mDv);
+ return ret;
+}
+
+
+//-------------------------------------------------
+
+/**
+ * Setter for the base properties
+ */
+void GeometryParamPoly3::SetAll(double s, double x, double y, double hdg, double length,double au,double bu,double cu,double du, double av, double bv, double cv, double dv)
+{
+ SetBase(s,x,y,hdg,length,false);
+ mAu=au;
+ mBu=bu;
+ mCu=cu;
+ mDu=du;
+ mAv=av;
+ mBv=bv;
+ mCv=cv;
+ mDv=dv;
+ ComputeVars();
+}
+
+//-------------------------------------------------
+
+/**
+ * Getter for the base properties
+ */
+double GeometryParamPoly3::GetAu(void)
+{
+ return mAu;
+}
+double GeometryParamPoly3::GetBu(void)
+{
+ return mBu;
+}
+double GeometryParamPoly3::GetCu(void)
+{
+ return mCu;
+}
+double GeometryParamPoly3::GetDu(void)
+{
+ return mDu;
+}
+double GeometryParamPoly3::GetAv(void)
+{
+ return mAv;
+}
+double GeometryParamPoly3::GetBv(void)
+{
+ return mBv;
+}
+double GeometryParamPoly3::GetCv(void)
+{
+ return mCv;
+}
+double GeometryParamPoly3::GetDv(void)
+{
+ return mDv;
+}
+//-------------------------------------------------
+/**
+ * Gets the coordinates at the sample S offset
+ */
+void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY)
+{
+ double param=s_check/mLength;
+ double sX,sY;
+
+ sX=mAu+mBu*param+mCu*pow(param,2.0)+mDu*pow(param,3.0);
+ sY=mAv+mBv*param+mCv*pow(param,2.0)+mDv*pow(param,3.0);
+
+ retX=mX+sX*cos(mHdg)-sY*sin(mHdg);
+ retY=mY+sX*sin(mHdg)+sY*cos(mHdg);
+}
+
+void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, double &retHDG)
+{
+ double param=s_check/mLength;
+ double diff_sX,diff_sY;
+ double s1X,s1Y,s2X,s2Y;
+
+ GetCoords(s_check,retX,retY);
+
+ diff_sX=mBu+2.0*mCu*param+3*mDu*pow(param,2.0);
+ diff_sY=mBv+2.0*mCv*param+3*mDv*pow(param,2.0);
+
+ if(s_check==0)
+ {
+ s1X=mAu;
+ s1Y=mAv;
+ s2X=mAu+mBu*(param+0.02)+mCu*pow(param+0.02,2.0)+mDu*pow(param+0.02,3.0);
+ s2Y=mAv+mBv*(param+0.02)+mCv*pow(param+0.02,2.0)+mDv*pow(param+0.02,3.0);
+ }
+ else if(s_check==mLength)
+ {
+ s1X=mAu+mBu*(param-0.02)+mCu*pow(param-0.02,2.0)+mDu*pow(param-0.02,3.0);
+ s1Y=mAv+mBv*(param-0.02)+mCv*pow(param-0.02,2.0)+mDv*pow(param-0.02,3.0);
+ s2X=mAu+mBu*param+mCu*pow(param,2.0)+mDu*pow(param,3.0);
+ s2Y=mAv+mBv*param+mCv*pow(param,2.0)+mDv*pow(param,3.0);
+ }
+ else
+ {
+ s1X=mAu+mBu*(param-0.01)+mCu*pow(param-0.01,2.0)+mDu*pow(param-0.01,3.0);
+ s1Y=mAv+mBv*(param-0.01)+mCv*pow(param-0.01,2.0)+mDv*pow(param-0.01,3.0);
+ s2X=mAu+mBu*(param+0.01)+mCu*pow(param+0.01,2.0)+mDu*pow(param+0.01,3.0);
+ s2Y=mAv+mBv*(param+0.01)+mCv*pow(param+0.01,2.0)+mDv*pow(param+0.01,3.0);
+ }
+ retHDG=mHdg+(s2Y-s1Y)/(s2X-s1X);
+ std::cout << "param: " << param << " heading: " << retHDG << std::endl;
+}
+
@@ -519,6 +665,25 @@ void GeometryPoly3::SetAll(double s, double x, double y, double hdg, double leng
ComputeVars();
}
+/**
+ * Getter for the base properties
+ */
+double GeometryPoly3::GetA(void)
+{
+ return mA;
+}
+double GeometryPoly3::GetB(void)
+{
+ return mB;
+}
+double GeometryPoly3::GetC(void)
+{
+ return mC;
+}
+double GeometryPoly3::GetD(void)
+{
+ return mD;
+}
@@ -567,6 +732,10 @@ const GeometryBlock& GeometryBlock::operator=(const GeometryBlock& otherGeomBloc
{
delete poly;
}
+ else if(GeometryParamPoly3 *paramPoly = dynamic_cast<GeometryParamPoly3 *>(*member))
+ {
+ delete paramPoly;
+ }
}
mGeometryBlockElement.clear();
@@ -598,6 +767,10 @@ void GeometryBlock::AddGeometryPoly3(double s, double x, double y, double hdg, d
{
mGeometryBlockElement.push_back(new GeometryPoly3(s, x, y, hdg, length, a, b, c, d));
}
+void GeometryBlock::AddGeometryParamPoly3(double s, double x, double y, double hdg, double length, double au,double bu,double cu,double du,double av,double bv,double cv,double dv)
+{
+ mGeometryBlockElement.push_back(new GeometryParamPoly3(s, x, y, hdg, length, au, bu, cu, du,av,bv,cv,dv));
+}
//-------------------------------------------------
@@ -627,8 +800,47 @@ double GeometryBlock::GetBlockLength()
*/
bool GeometryBlock::CheckIfLine()
{
+ int type;
+
+ if(mGeometryBlockElement.size()>1) return false;
+ else
+ {
+ type=mGeometryBlockElement.at(0)->GetGeomType();
+ if(type==0)
+ return true;
+ else
+ return false;
+ }
+}
+
+bool GeometryBlock::CheckIfParamPoly3()
+{
+ int type;
+
+ if(mGeometryBlockElement.size()>1) return false;
+ else
+ {
+ type=mGeometryBlockElement.at(0)->GetGeomType();
+ if(type==4)
+ return true;
+ else
+ return false;
+ }
+}
+
+bool GeometryBlock::CheckIfPoly3()
+{
+ int type;
+
if(mGeometryBlockElement.size()>1) return false;
- else return true;
+ else
+ {
+ type=mGeometryBlockElement.at(0)->GetGeomType();
+ if(type==3)
+ return true;
+ else
+ return false;
+ }
}
//-------------------------------------------------
@@ -643,15 +855,38 @@ void GeometryBlock::Recalculate(double s, double x, double y, double hdg)
double lX=x;
double lY=y;
double lHdg=hdg;
+ int type;
if(mGeometryBlockElement.size()==1)
{
- GeometryLine *lGeometryLine = static_cast<GeometryLine*>(mGeometryBlockElement.at(0));
- if(lGeometryLine!=NULL)
- {
- // Updates the line to reflect the changes of the previous block
- lGeometryLine->SetBase(lS,lX,lY,lHdg,lGeometryLine->GetLength());
- }
+ type=mGeometryBlockElement.at(0)->GetGeomType();
+ if(type==0)
+ {
+ GeometryLine *lGeometryLine = static_cast<GeometryLine*>(mGeometryBlockElement.at(0));
+ if(lGeometryLine!=NULL)
+ {
+ // Updates the line to reflect the changes of the previous block
+ lGeometryLine->SetBase(lS,lX,lY,lHdg,lGeometryLine->GetLength());
+ }
+ }
+ else if(type==3)
+ {
+ GeometryPoly3 *lGeometryPoly3 = static_cast<GeometryPoly3*>(mGeometryBlockElement.at(0));
+ if(lGeometryPoly3!=NULL)
+ {
+ // Updates the line to reflect the changes of the previous block
+ lGeometryPoly3->SetBase(lS,lX,lY,lHdg,lGeometryPoly3->GetLength());
+ }
+ }
+ else if(type==4)
+ {
+ GeometryParamPoly3 *lGeometryParamPoly3 = static_cast<GeometryParamPoly3*>(mGeometryBlockElement.at(0));
+ if(lGeometryParamPoly3!=NULL)
+ {
+ // Updates the line to reflect the changes of the previous block
+ lGeometryParamPoly3->SetBase(lS,lX,lY,lHdg,lGeometryParamPoly3->GetLength());
+ }
+ }
}
else if(mGeometryBlockElement.size()==3)
{
@@ -792,6 +1027,10 @@ GeometryBlock::~GeometryBlock()
{
delete poly;
}
+ else if(GeometryParamPoly3 *paramPoly = dynamic_cast<GeometryParamPoly3 *>(*member))
+ {
+ delete paramPoly;
+ }
}
mGeometryBlockElement.clear();
}
diff --git a/OpenRoadEd/OpenDrive/RoadGeometry.h b/OpenRoadEd/OpenDrive/RoadGeometry.h
index 52983c75bb41eef3d5e30fa19532603130dd2ed5..df5db79f83e46e2f1276cebe4f794a4fd74502a9 100644
--- a/OpenRoadEd/OpenDrive/RoadGeometry.h
+++ b/OpenRoadEd/OpenDrive/RoadGeometry.h
@@ -46,7 +46,7 @@ public:
/**
* Sets the type of the geometry
- * 0: Line, 1: Arc, 2: Spiral
+ * 0: Line, 1: Arc, 2: Spiral 3: poly3 4: paramPloy3
*/
void SetGeomType(short int geomType);
@@ -261,6 +261,77 @@ protected:
virtual void ComputeVars();
};
+//----------------------------------------------------------------------------------
+/**
+ * GeometryParamPoly3 inherits the RoadGeometry class and adds parametric properties
+ *
+ */
+class GeometryParamPoly3: public RoadGeometry
+{
+private:
+ /**
+ * Base record properties
+ */
+ double mAu;
+ double mBu;
+ double mCu;
+ double mDu;
+ double mAv;
+ double mBv;
+ double mCv;
+ double mDv;
+
+ /**
+ * Computation variables
+ */
+ double mEndX;
+ double mEndY;
+
+public:
+ /**
+ * Constructor that initializes the base properties of the record
+ */
+ GeometryParamPoly3 (double s, double x, double y, double hdg, double length, double au,double bu,double cu,double du,double av,double bv,double cv,double dv);
+
+ /**
+ * Clones and returns the new geometry record
+ */
+ RoadGeometry* Clone() const;
+
+ //-------------------------------------------------
+
+ /**
+ * Setter for the base properties
+ */
+ void SetAll(double s, double x, double y, double hdg, double length,double au,double bu,double cu,double du,double av,double bv,double cv,double dv);
+
+ //-------------------------------------------------
+ /**
+ * Getter for the base properties
+ */
+ double GetAu(void);
+ double GetBu(void);
+ double GetCu(void);
+ double GetDu(void);
+ double GetAv(void);
+ double GetBv(void);
+ double GetCv(void);
+ double GetDv(void);
+
+ //-------------------------------------------------
+ /**
+ * Gets the coordinates at the sample S offset
+ */
+ void GetCoords(double s_check, double &retX, double &retY, double &retHDG);
+ void GetCoords(double s_check, double &retX, double &retY);
+protected:
+
+ /**
+ * Computes the required vars
+ */
+ virtual void ComputeVars();
+};
+
//----------------------------------------------------------------------------------
/**
@@ -294,10 +365,17 @@ public:
*/
void SetAll(double s, double x, double y, double hdg, double length, double a,double b,double c,double d);
+ //-------------------------------------------------
+ /**
+ * Getter for the base properties
+ */
+ double GetA(void);
+ double GetB(void);
+ double GetC(void);
+ double GetD(void);
};
-
//----------------------------------------------------------------------------------
/**
* GeometryBlock is a class used to combine multiple geometry records into blocks.
@@ -337,6 +415,7 @@ public:
void AddGeometryArc(double s, double x, double y, double hdg, double length, double curvature);
void AddGeometrySpiral(double s, double x, double y, double hdg, double length, double curvatureStart,double curvatureEnd);
void AddGeometryPoly3(double s, double x, double y, double hdg, double length, double a,double b,double c,double d);
+ void AddGeometryParamPoly3(double s, double x, double y, double hdg, double length, double au,double bu,double cu,double du,double av,double bv,double cv,double dv);
//-------------------------------------------------
@@ -354,6 +433,8 @@ public:
* Checks if the block is a straight line block or a turn
*/
bool CheckIfLine();
+ bool CheckIfPoly3();
+ bool CheckIfParamPoly3();
//-------------------------------------------------
diff --git a/OpenRoadEd/Osg/OSGRoad.cpp b/OpenRoadEd/Osg/OSGRoad.cpp
index dd009eb6350447a9974678a423f6551cd181ddb9..31c54ed219ddf0177fb4ccc18b020ea94447c0bc 100644
--- a/OpenRoadEd/Osg/OSGRoad.cpp
+++ b/OpenRoadEd/Osg/OSGRoad.cpp
@@ -334,7 +334,7 @@ void OSGRoad::CreateRoadGeometry (vector<osg::ref_ptr<osg::Geometry>> &geometrie
short int alphaDepthState)
{
-
+ std::cout << "road ID: " << road->GetRoadId() << std::endl;
//================================================================
//Check if everything needed for the road generation exists
//================================================================