diff --git a/OpenRoadEd/OpenDrive/RoadGeometry.cpp b/OpenRoadEd/OpenDrive/RoadGeometry.cpp
index 90e0fca82fc3a8cc8bd3da87ee8b208afe807deb..21f3f8dd52b76493b9d80a4ff2cf01e723973063 100644
--- a/OpenRoadEd/OpenDrive/RoadGeometry.cpp
+++ b/OpenRoadEd/OpenDrive/RoadGeometry.cpp
@@ -1,7 +1,6 @@
 #include "RoadGeometry.h"
 #define _USE_MATH_DEFINES
 #include <math.h>
-#include <iostream>
 
 //#define PI 3.14159265358979323846264338327950288
 extern int fresnl( double , double *, double * );
@@ -595,36 +594,12 @@ void GeometryParamPoly3::GetCoords(double s_check, double &retX, double &retY, d
 {
   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;
+  retHDG=mHdg+atan2(diff_sY,diff_sX);
 }
 
 
diff --git a/OpenRoadEd/Osg/OSGRoad.cpp b/OpenRoadEd/Osg/OSGRoad.cpp
index 31c54ed219ddf0177fb4ccc18b020ea94447c0bc..b36498cdfefba19bd0af90d3119af126b6c4fea4 100644
--- a/OpenRoadEd/Osg/OSGRoad.cpp
+++ b/OpenRoadEd/Osg/OSGRoad.cpp
@@ -334,7 +334,6 @@ 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
 	//================================================================