Merge branch 'master' of http://47.96.250.93:3000/adc_pilot/modularization

src/ui/ui_osgviewer/RoadManager.cpp

@@ -0,0 +1,7665 @@
+/* 
+ * esmini - Environment Simulator Minimalistic 
+ * https://github.com/esmini/esmini
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ * 
+ * Copyright (c) partners of Simulation Scenarios
+ * https://sites.google.com/view/simulationscenarios
+ */
+
+ /*
+  * This module provides an limited interface to OpenDRIVE files.
+  * It supports all geometry types (as of ODR 1.4), junctions and some properties such as lane offset
+  * but lacks many features as road markings, signals, road signs, speed and road banking
+  *
+  * It converts between world (cartesian) and road coordinates (both Track and Lane)
+  *
+  * When used standalone (outside ScenarioEngine) the road manager is initialized via the Position class like this:
+  *   roadmanager::Position::LoadOpenDrive("example.xodr");
+  *
+  * Simplest use case is to put a vehicle on the road and simply move it forward along the road, e.g:
+  *
+  *   car->pos = new roadmanager::Position(3, -2, 10, 0);
+  *   while(true)
+  *   {
+  *	    car->pos->MoveAlongS(0.1);
+  *   }
+  *
+  * The first line will create a Position object initialized at road with ID = 3, in lane = -2 and at lane offset = 0
+  * Then the position is updated along that road and lane, moving 10 cm at a time.
+  *
+  * A bit more realistic example: 
+  *
+  *   car->pos = new roadmanager::Position(odrManager->GetRoadByIdx(0)->GetId(), -2, 10, 0);
+  *   while(true)
+  *   {
+  *	    car->pos->MoveAlongS(speed * dt);
+  *   }
+  * 
+  * Here we refer to the ID of the first road in the network. And instead of static delta movement, the distance 
+  * is a function of speed and delta time since last update.
+  * 
+  */
+
+#include <iostream>
+#include <cstring>
+#include <random>
+#include <time.h>
+#include <limits>
+#include <algorithm>
+
+#include "RoadManager.hpp"
+#include "odrSpiral.h"
+#include "pugixml.hpp"
+#include "CommonMini.hpp"
+
+static std::mt19937 mt_rand;
+static unsigned int global_lane_counter;
+ 
+
+
+using namespace std;
+using namespace roadmanager;
+
+#define CURV_ZERO 0.00001
+#define MAX_TRACK_DIST 10
+#define OSI_POINT_CALC_STEPSIZE 1 // [m]
+#define OSI_TANGENT_LINE_TOLERANCE 0.01 // [m]
+
+
+int g_Lane_id;
+int g_Laneb_id;
+
+
+double Polynomial::Evaluate(double p)
+{
+	p *= p_scale_;
+
+	return (a_ + p * b_ + p * p*c_ + p * p*p*d_);
+}
+
+double Polynomial::EvaluatePrim(double p)
+{
+	p *= p_scale_;
+
+	return (b_ + 2 * p*c_ + 3 * p*p*d_);
+}
+
+double Polynomial::EvaluatePrimPrim(double p)
+{
+	p *= p_scale_;
+
+	return (2 * c_ + 6 * p*d_);
+}
+
+void Polynomial::Set(double a, double b, double c, double d, double p_scale)
+{
+	a_ = a;
+	b_ = b;
+	c_ = c;
+	d_ = d;
+	p_scale_ = p_scale;
+}
+
+OSIPoints::OSIPointStruct& OSIPoints::GetPoint(int i)
+{
+	if (point_.size() <= i || point_.size() == 0)
+	{
+		throw std::runtime_error("OSIPoints::GetPoint(int i) -> exceeds index");
+	}
+	else if (i < 0)
+	{
+		throw std::runtime_error("OSIPoints::GetXFromIdx(int i) -> index must be larger than 0");
+	}
+	else
+	{
+		return point_[i];
+	}
+}
+
+double OSIPoints::GetXfromIdx(int i) 
+{
+	if(point_.size() <= i || point_.size() == 0)
+	{
+		throw std::runtime_error("OSIPoints::GetXFromIdx(int i) -> exceeds index");
+	}
+	else if(i < 0)
+	{
+		throw std::runtime_error("OSIPoints::GetXFromIdx(int i) -> index must be larger than 0");
+	}
+	else
+	{
+		return point_[i].x;
+	}	
+}
+
+double OSIPoints::GetYfromIdx(int i) 
+{
+	if (point_.size() <= i || point_.size() == 0)
+	{
+		throw std::runtime_error("OSIPoints::GetYFromIdx(int i) -> exceeds index");
+	}
+	else if(i < 0)
+	{
+		throw std::runtime_error("OSIPoints::GetYFromIdx(int i) -> index must be larger than 0");
+	}
+	else
+	{
+		return point_[i].y;
+	}	
+}
+
+double OSIPoints::GetZfromIdx(int i) 
+{
+	if (point_.size() <= i || point_.size() == 0)
+	{
+		throw std::runtime_error("OSIPoints::GetZFromIdx(int i) -> exceeds index");
+	}
+	else if(i < 0)
+	{
+		throw std::runtime_error("OSIPoints::GetZFromIdx(int i) -> index must be larger than 0");
+	}
+	else
+	{
+		return point_[i].z;
+	}	
+}
+
+int OSIPoints::GetNumOfOSIPoints() 
+{
+	return (int)point_.size();
+}
+
+void Geometry::Print()
+{
+	LOG("Geometry virtual Print\n");
+}
+
+void Geometry::EvaluateDS(double ds, double *x, double *y, double *h)
+{
+	(void)ds; (void)x; (void)y; (void)h;
+	LOG("Geometry virtual Evaluate\n");
+}
+
+void Line::Print()
+{
+	LOG("Line x: %.2f, y: %.2f, h: %.2f length: %.2f\n", GetX(), GetY(), GetHdg(), GetLength());
+}
+
+void Line::EvaluateDS(double ds, double *x, double *y, double *h)
+{
+	*h = GetHdg();
+	*x = GetX() + ds * cos(*h);
+	*y = GetY() + ds * sin(*h);
+}
+
+void Arc::Print()
+{
+	LOG("Arc x: %.2f, y: %.2f, h: %.2f curvature: %.2f length: %.2f\n", GetX(), GetY(), GetHdg(), curvature_, GetLength());
+}
+
+void Arc::EvaluateDS(double ds, double *x, double *y, double *h)
+{
+	double x_local = 0;
+	double y_local = 0;
+
+	// arc_length = angle * radius -> angle = arc_length / radius = arc_length * curvature
+	double angle = ds * curvature_;
+
+	// Now calculate x, y in a local unit circle coordinate system
+	if (curvature_ < 0)
+	{
+		// starting from 90 degrees going clockwise
+		x_local = cos(angle + M_PI / 2.0);
+		y_local = sin(angle + M_PI / 2.0) - 1;  // -1 transform to y = 0
+	}
+	else
+	{
+		// starting from -90 degrees going counter clockwise
+		x_local = cos(angle + 3.0 * M_PI_2);
+		y_local = sin(angle + 3.0 * M_PI_2) + 1;  // +1 transform to y = 0
+	}
+
+	// Rotate according to heading and scale according to radius
+	*x = GetX() + GetRadius() * (x_local * cos(GetHdg()) - y_local * sin(GetHdg()));
+	*y = GetY() + GetRadius() * (x_local * sin(GetHdg()) + y_local * cos(GetHdg()));
+	*h = GetHdg() + angle;
+}
+
+Spiral::Spiral(double s, double x, double y, double hdg, double length, double curv_start, double curv_end) :
+	Geometry(s, x, y, hdg, length, GEOMETRY_TYPE_SPIRAL),
+	curv_start_(curv_start), curv_end_(curv_end), c_dot_(0.0), x0_(0.0), y0_(0.0), h0_(0.0), s0_(0.0), arc_(0), line_(0)
+{
+	SetCDot((curv_end_ - curv_start_) / length_);
+
+	if (fabs(GetCDot()) < SMALL_NUMBER)
+	{
+		// constant radius => clothoid is actually a line or an arc
+		if (fabs(this->GetCurvStart()) < SMALL_NUMBER)  // Line
+		{
+			line_ = new Line(s, x, y, hdg, length);
+		}
+		else  // Arc
+		{
+			arc_ = new Arc(s, x, y, hdg, length, curv_start);
+		}
+	}
+	else
+	{
+		if (fabs(curv_start_) > SMALL_NUMBER)
+		{
+			// not starting from zero curvature (straight line)
+			// How long do we need to follow the spiral to reach start curve value?
+			SetS0(curv_start_ / c_dot_);
+
+			// Find out x, y, heading of start position
+			double x0, y0, h0;
+			odrSpiral(GetS0(), c_dot_, &x0, &y0, &h0);
+
+			SetX0(x0);
+			SetY0(y0);
+			SetH0(h0);
+		}
+	}
+}
+
+void Spiral::Print()
+{
+	LOG("Spiral x: %.2f, y: %.2f, h: %.2f start curvature: %.4f end curvature: %.4f length: %.2f %s\n",
+		GetX(), GetY(), GetHdg(), GetCurvStart(), GetCurvEnd(), GetLength(),
+		arc_ != 0 ? " - actually an Arc" : line_ != 0 ? "- actually a Line" : "");
+}
+
+void Spiral::EvaluateDS(double ds, double* x, double* y, double* h)
+{
+	double xTmp, yTmp, t;
+
+	if (line_ != 0)
+	{
+		line_->EvaluateDS(ds, x, y, h);
+	}
+	else if (arc_ != 0)
+	{
+		arc_->EvaluateDS(ds, x, y, h);
+	}
+	else
+	{
+		odrSpiral(s0_ + ds, c_dot_, &xTmp, &yTmp, &t);
+
+		*h = t - GetH0() + GetHdg();
+
+		double x1, x2, y1, y2;
+
+		// transform spline segment to origo and start angle = 0
+		x1 = xTmp - GetX0();
+		y1 = yTmp - GetY0();
+		x2 = x1 * cos(-GetH0()) - y1 * sin(-GetH0());
+		y2 = x1 * sin(-GetH0()) + y1 * cos(-GetH0());
+
+		// Then transform according to segment start position and heading
+		*x = GetX() + x2 * cos(GetHdg()) - y2 * sin(GetHdg());
+		*y = GetY() + x2 * sin(GetHdg()) + y2 * cos(GetHdg());
+	}
+}
+
+double Spiral::EvaluateCurvatureDS(double ds)
+{
+	if (line_ != 0)
+	{
+		return LARGE_NUMBER;
+	}
+	else if (arc_ != 0)
+	{
+		return arc_->GetCurvature();
+	}
+	else
+	{
+		return (curv_start_ + (ds / GetLength()) * (curv_end_ - curv_start_));
+	}
+}
+
+void Spiral::SetX(double x)
+{
+	if (line_ != 0)
+	{
+		line_->SetX(x);
+	}
+	else if (arc_ != 0)
+	{
+		arc_->SetX(x);
+	}
+	else
+	{
+		x_ = x;
+	}
+}
+
+void Spiral::SetY(double y)
+{
+	if (line_ != 0)
+	{
+		line_->SetY(y);
+	}
+	else if (arc_ != 0)
+	{
+		arc_->SetY(y);
+	}
+	else
+	{
+		y_ = y;
+	}
+}
+
+void Spiral::SetHdg(double h)
+{
+	if (line_ != 0)
+	{
+		line_->SetHdg(h);
+	}
+	else if (arc_ != 0)
+	{
+		arc_->SetHdg(h);
+	}
+	else
+	{
+		hdg_ = h;
+	}
+}
+
+Poly3::Poly3(double s, double x, double y, double hdg, double length, double a, double b, double c, double d) :
+	Geometry(s, x, y, hdg, length, GEOMETRY_TYPE_POLY3), umax_(0.0)
+{
+	poly3_.Set(a, b, c, d);
+
+	double xTmp = 0;
+	double yTmp = 0;
+
+	EvaluateDSLocal(GetLength()-SMALL_NUMBER, xTmp, yTmp);
+	SetUMax(xTmp);
+}
+
+void Poly3::Print()
+{
+	LOG("Poly3 x: %.2f, y: %.2f, h: %.2f length: %.2f a: %.2f b: %.2f c: %.2f d: %.2f\n",
+		GetX(), GetY(), GetHdg(), GetLength(), poly3_.GetA(), poly3_.GetB(), poly3_.GetC(), poly3_.GetD());
+}
+
+void Poly3::EvaluateDSLocal(double ds, double &u, double &v)
+{
+	double distTmp = 0;
+	double steplen = MIN(10, ds);  // along u axis - to be tuned
+
+	u = v = 0;
+
+	if (ds > length_ - SMALL_NUMBER)
+	{
+		u = umax_;
+		v = poly3_.Evaluate(u);
+	}
+	else if (ds > SMALL_NUMBER)
+	{
+		for (double uTmp = 0; uTmp < length_; uTmp += steplen)
+		{
+			double vTmp = poly3_.Evaluate(uTmp);
+			double delta = sqrt((uTmp - u) * (uTmp - u) + (vTmp - v) * (vTmp - v));
+
+			if (distTmp + delta > ds)
+			{
+				// interpolate
+				double w = (distTmp + delta - ds) / MAX(delta, SMALL_NUMBER);
+				u = w * u + (1 - w) * uTmp;
+				v = poly3_.Evaluate(u);
+				break;
+			}
+			distTmp += delta;
+			u = uTmp;
+			v = vTmp;
+		}
+	}
+}
+
+void Poly3::EvaluateDS(double ds, double *x, double *y, double *h)
+{
+	double u_local = 0;
+	double v_local = 0;
+
+	EvaluateDSLocal(ds, u_local, v_local);
+
+	*x = GetX() + u_local * cos(GetHdg()) - v_local * sin(GetHdg());
+	*y = GetY() + u_local * sin(GetHdg()) + v_local * cos(GetHdg());
+	*h = GetHdg() + atan(poly3_.EvaluatePrim(u_local));
+}
+
+double Poly3::EvaluateCurvatureDS(double ds)
+{
+	return poly3_.EvaluatePrimPrim(ds);
+}
+
+void ParamPoly3::Print()
+{
+	LOG("ParamPoly3 x: %.2f, y: %.2f, h: %.2f length: %.2f U: %.8f, %.8f, %.8f, %.8f V: %.8f, %.8f, %.8f, %.8f\n",
+		GetX(), GetY(), GetHdg(), GetLength(), 
+		poly3U_.GetA(), poly3U_.GetB(), poly3U_.GetC(), poly3U_.GetD(),
+		poly3V_.GetA(), poly3V_.GetB(), poly3V_.GetC(), poly3V_.GetD()
+	);
+}
+
+void ParamPoly3::EvaluateDS(double ds, double *x, double *y, double *h)
+{
+	double p = S2P(ds);
+	double hdg = GetHdg();
+
+	double u_local = poly3U_.Evaluate(p);
+	double v_local = poly3V_.Evaluate(p);
+
+	*x = GetX() + u_local * cos(hdg) - v_local * sin(hdg);
+	*y = GetY() + u_local * sin(hdg) + v_local * cos(hdg);
+	*h = hdg + atan2(poly3V_.EvaluatePrim(p), poly3U_.EvaluatePrim(p));
+}
+
+double ParamPoly3::EvaluateCurvatureDS(double ds)
+{
+	return poly3V_.EvaluatePrimPrim(ds) / poly3U_.EvaluatePrim(ds);
+}
+
+void ParamPoly3::calcS2PMap(PRangeType p_range)
+{
+	double len = 0;
+	double p_step_len = 1.0 / double(PARAMPOLY3_STEPS);
+	double p = 0; 
+
+	if (p_range == PRangeType::P_RANGE_ARC_LENGTH)
+	{
+		p_step_len = length_/(PARAMPOLY3_STEPS);
+	}
+
+	// Calculate actual arc length of the curve
+	s2p_map_[0][0] = 0;
+	for (size_t i = 1; i < PARAMPOLY3_STEPS+1; i++)
+	{
+		p += p_step_len;
+
+		double pm = p - 0.5 * p_step_len; // midpoint method
+		double integrator = sqrt(
+			pow(3 * poly3U_.GetD() * pm * pm + 2 * poly3U_.GetC() * pm + poly3U_.GetB(), 2) +
+			pow(3 * poly3V_.GetD() * pm * pm + 2 * poly3V_.GetC() * pm + poly3V_.GetB(), 2));
+
+		len += p_step_len * integrator;
+		s2p_map_[i][0] = len;
+	}
+
+	// Map length (ds) to p for each sub-segment, adjust for incorrect length attribute
+	double scale_factor;
+	scale_factor = length_ / len;
+
+	for (size_t i = 0; i < PARAMPOLY3_STEPS+1; i++)
+	{
+		s2p_map_[i][0] *= scale_factor;
+		s2p_map_[i][1] = i * length_ / PARAMPOLY3_STEPS;
+	}
+}
+
+double ParamPoly3::S2P(double s)
+{
+	for (size_t i = 0; i < PARAMPOLY3_STEPS; i++)
+	{
+		if (s2p_map_[i + 1][0] > s)
+		{
+			double w = (s - s2p_map_[i][0]) / (s2p_map_[i + 1][0] - s2p_map_[i][0]);
+			return s2p_map_[i][1] + w * (s2p_map_[i + 1][1] - s2p_map_[i][1]);
+		}
+	}
+	return s2p_map_[PARAMPOLY3_STEPS][1];
+}
+
+void Elevation::Print()
+{
+	LOG("Elevation: s: %.2f A: %.4f B: %.4f C: %.4f D: %.4f\n",
+		GetS(), poly3_.GetA(), poly3_.GetB(), poly3_.GetC(), poly3_.GetD());
+}
+
+void LaneLink::Print()
+{
+	LOG("LaneLink type: %d id: %d\n", type_, id_);
+}
+
+void Lane::SetGlobalId()
+{ 
+	global_id_ = g_Lane_id; 
+	g_Lane_id++; 
+}
+
+void LaneBoundaryOSI::SetGlobalId()
+{  
+	global_id_ = g_Laneb_id; 
+	g_Laneb_id++; 
+}
+
+void LaneRoadMarkTypeLine::SetGlobalId()
+{  
+	global_id_ = g_Laneb_id; 
+	g_Laneb_id++;
+}
+
+LaneWidth *Lane::GetWidthByIndex(int index)
+{ 
+	if(lane_width_.size() <= index || lane_width_.size() == 0)
+	{
+		throw std::runtime_error("Lane::GetWidthByIndex(int index) -> exceeds index");
+	}
+	else if(lane_width_.size() < 0)
+	{
+		throw std::runtime_error("Lane::GetWidthByIndex(int index) -> index must be larger than 0");
+	}
+	else
+	{
+		return lane_width_[index];
+	}	
+}
+
+LaneWidth *Lane::GetWidthByS(double s)
+{
+	if (lane_width_.size() == 0)
+	{
+		return 0;  // No lanewidth defined
+	}
+	for (int i=0; i+1<(int)lane_width_.size(); i++)
+	{
+		if (s < lane_width_[i + 1]->GetSOffset())
+		{
+			return lane_width_[i];
+		}
+	}
+	return lane_width_.back();
+}
+
+LaneLink *Lane::GetLink(LinkType type)
+{
+	for (int i=0; i<(int)link_.size(); i++)
+	{
+		LaneLink *l = link_[i];
+		if (l->GetType() == type)
+		{
+			return l;
+		}
+	}
+	return 0; // No link of requested type exists
+}
+
+void LaneWidth::Print()
+{
+	LOG("LaneWidth: sOffset: %.2f, a: %.2f, b: %.2f, c: %.2f, d: %.2f\n",
+		s_offset_, poly3_.GetA(), poly3_.GetB(), poly3_.GetC(), poly3_.GetD());
+}
+
+LaneRoadMark* Lane::GetLaneRoadMarkByIdx(int idx)
+{
+	if(lane_roadMark_.size() <= idx || lane_roadMark_.size() == 0)
+	{
+		throw std::runtime_error("Lane::GetLaneRoadMarkByIdx(int idx) -> exceeds index");
+	}
+	else if(lane_roadMark_.size() < 0)
+	{
+		throw std::runtime_error("Lane::GetLaneRoadMarkByIdx(int idx) -> index must be larger than 0");
+	}
+	else
+	{
+		return lane_roadMark_[idx];
+	}
+}
+
+std::vector<int> Lane::GetLineGlobalIds()
+{
+	std::vector<int> line_ids; 
+	for (int i = 0; i<GetNumberOfRoadMarks(); i++)
+	{
+		LaneRoadMark* laneroadmark =  GetLaneRoadMarkByIdx(i);
+		for (int j = 0; j<laneroadmark->GetNumberOfRoadMarkTypes(); j++)
+		{
+			LaneRoadMarkType* laneroadmarktype = laneroadmark->GetLaneRoadMarkTypeByIdx(j); 
+
+			for (int h = 0; h<laneroadmarktype->GetNumberOfRoadMarkTypeLines(); h++)
+			{
+				LaneRoadMarkTypeLine* laneroadmarktypeline =  laneroadmarktype->GetLaneRoadMarkTypeLineByIdx(h);
+				line_ids.push_back(laneroadmarktypeline->GetGlobalId()); 
+			}
+		}
+	}
+
+	return line_ids; 
+}
+
+int Lane::GetLaneBoundaryGlobalId()
+{
+	if (lane_boundary_)
+	{
+		return lane_boundary_->GetGlobalId();
+	}
+	else
+	{
+		return -1;
+	}
+}
+
+LaneRoadMarkType* LaneRoadMark::GetLaneRoadMarkTypeByIdx(int idx)
+{
+	if (idx < (int)lane_roadMarkType_.size())
+	{
+		return lane_roadMarkType_[idx];
+	}
+
+	return 0;
+}
+
+LaneRoadMarkTypeLine* LaneRoadMarkType::GetLaneRoadMarkTypeLineByIdx(int idx)
+{
+	if (idx < (int)lane_roadMarkTypeLine_.size())
+	{
+		return lane_roadMarkTypeLine_[idx];
+	}
+
+	return 0;
+}
+
+void LaneRoadMarkType::AddLine(LaneRoadMarkTypeLine *lane_roadMarkTypeLine)
+{ 
+	lane_roadMarkTypeLine->SetGlobalId();
+	lane_roadMarkTypeLine_.push_back(lane_roadMarkTypeLine);  
+}
+
+void Lane::SetLaneBoundary(LaneBoundaryOSI *lane_boundary) 
+{	
+	lane_boundary->SetGlobalId();
+	lane_boundary_ = lane_boundary; 
+} 
+
+void LaneOffset::Print()
+{
+	LOG("LaneOffset s %.2f a %.4f b %.2f c %.2f d %.2f length %.2f\n",
+		s_, polynomial_.GetA(), polynomial_.GetB(), polynomial_.GetC(), polynomial_.GetD(), length_);
+}
+
+double LaneOffset::GetLaneOffset(double s)
+{
+	return (polynomial_.Evaluate(s - s_));
+}
+
+double LaneOffset::GetLaneOffsetPrim(double s)
+{
+	return (polynomial_.EvaluatePrim(s - s_));
+}
+
+void Lane::Print()
+{
+	LOG("Lane: %d, type: %d, level: %d\n", id_, type_, level_);
+	
+	for (size_t i = 0; i < link_.size(); i++)
+	{
+		link_[i]->Print();
+	}
+
+	for (size_t i=0; i<lane_width_.size(); i++)
+	{
+		lane_width_[i]->Print();
+	}
+}
+
+bool Lane::IsCenter()
+{
+	if (GetId() == 0)
+	{
+		return true;  // Ref lane no width -> no driving
+	}
+	else
+	{
+		return false;
+	}
+}
+bool Lane::IsType(Lane::LaneType type)
+{
+	if (GetId() == 0)
+	{
+		return false;  // Ref lane no width -> no driving
+	}
+
+	return bool(type_ & type);
+}
+
+bool Lane::IsDriving()
+{
+	if (GetId() == 0)
+	{
+		return false;  // Ref lane no width -> no driving
+	}
+
+	return bool(type_ & Lane::LaneType::LANE_TYPE_ANY_DRIVING);
+}
+
+LaneSection* Road::GetLaneSectionByIdx(int idx)
+{
+	if (idx >= 0 && idx < lane_section_.size())
+	{
+		return lane_section_[idx];
+	}
+	else
+	{
+		return 0;
+	}
+}
+
+int Road::GetLaneSectionIdxByS(double s, int start_at)
+{
+	if (start_at < 0 || start_at > lane_section_.size() - 1)
+	{
+		return -1;
+	}
+
+	LaneSection *lane_section = lane_section_[start_at];
+	size_t i = start_at;
+
+	if (s < lane_section->GetS() && start_at > 0)  
+	{
+		// Look backwards
+		for (i = start_at - 1; i > 0; i--)  // No need to check the first one
+		{
+			lane_section = GetLaneSectionByIdx((int)i);
+			if (s > lane_section->GetS())
+			{
+				break;
+			}
+		}
+	}
+	else  
+	{
+		// look forward
+		for (i = start_at; i < GetNumberOfLaneSections()-1; i++) // No need to check the last one 
+		{
+			lane_section = GetLaneSectionByIdx((int)i);
+			if (s < lane_section->GetS() + lane_section->GetLength())
+			{
+				break;
+			}
+		}
+	}
+
+	return (int)i;
+}
+
+LaneInfo Road::GetLaneInfoByS(double s, int start_lane_section_idx, int start_lane_id, int laneTypeMask)
+{
+	LaneInfo lane_info;
+	
+	lane_info.lane_section_idx_ = start_lane_section_idx;
+	lane_info.lane_id_ = start_lane_id;
+
+	if (lane_info.lane_section_idx_ >= (int)lane_section_.size())
+	{
+		LOG("Error idx %d > n_lane_sections %d\n", lane_info.lane_section_idx_, (int)lane_section_.size());
+	}
+	else
+	{
+		LaneSection *lane_section = lane_section_[lane_info.lane_section_idx_];
+
+		// check if we passed current section
+		if (s > lane_section->GetS() + lane_section->GetLength() || s < lane_section->GetS())
+		{
+			if (s > lane_section->GetS() + lane_section->GetLength())
+			{
+				while (s > lane_section->GetS() + lane_section->GetLength() && lane_info.lane_section_idx_ + 1 < GetNumberOfLaneSections())
+				{
+					// Find out connecting lane, then move to next lane section
+					lane_info.lane_id_ = lane_section->GetConnectingLaneId(lane_info.lane_id_, SUCCESSOR);
+					lane_section = GetLaneSectionByIdx(++lane_info.lane_section_idx_);
+				}
+			}
+			else if (s < lane_section->GetS())
+			{
+				while (s < lane_section->GetS() && lane_info.lane_section_idx_ > 0)
+				{
+					// Move to previous lane section
+					lane_info.lane_id_ = lane_section->GetConnectingLaneId(lane_info.lane_id_, PREDECESSOR);
+					lane_section = GetLaneSectionByIdx(--lane_info.lane_section_idx_);
+				}
+			}
+
+			// If new lane is not of snapping type, try to move into a close valid lane
+			Lane* lane = lane_section->GetLaneById(lane_info.lane_id_);
+			if (lane == 0 || !(laneTypeMask & lane_section->GetLaneById(lane_info.lane_id_)->GetLaneType()))
+			{
+				double offset = 0;
+				double t = 0;
+
+				if (lane == 0)
+				{
+					LOG("No valid connecting lane (s: %.2f lane_id %d) - looking for a valid lane from center outwards", s, lane_info.lane_id_);
+				}
+				else
+				{
+					t = lane->GetOffsetFromRef() + GetLaneWidthByS(s, lane->GetId());
+				}
+				lane_info.lane_id_ = lane_section->GetLaneByIdx(lane_section->GetClosestLaneIdx(s, t, offset, true, laneTypeMask))->GetId();
+				if (lane_info.lane_id_ == 0)
+				{
+					LOG("Failed to find a closest snapping lane");
+				}
+			}
+		}
+	}
+
+	return lane_info;
+}
+
+double Road::GetLaneWidthByS(double s, int lane_id)
+{
+	LaneSection *lsec;
+
+	if (GetNumberOfLaneSections() < 1)
+	{
+		return 0.0;
+	}
+
+	for (size_t i = 0; i < GetNumberOfLaneSections(); i++)
+	{
+		lsec = GetLaneSectionByIdx((int)i);
+		if (s < lsec->GetS() + lsec->GetLength())
+		{
+			return lsec->GetWidth(s, lane_id);
+		}
+	}
+
+	return 0.0;
+}
+
+double Road::GetSpeedByS(double s)
+{
+	if (type_.size() > 0)
+	{
+		size_t i;
+		for (i = 0; i < type_.size() - 1 && s > type_[i + 1]->s_; i++);
+
+		return type_[i]->speed_;
+	}
+
+	// No type entries, fall back to a speed based on nr of lanes
+	return 0;
+}
+
+Geometry* Road::GetGeometry(int idx)
+{
+	if (idx < 0 || idx + 1 > (int)geometry_.size())
+	{
+		LOG("Road::GetGeometry index %d out of range [0:%d]\n", idx, (int)geometry_.size());
+		return 0;
+	}
+	return geometry_[idx]; 
+}
+
+void LaneSection::Print()
+{
+	LOG("LaneSection: %.2f, %d lanes:\n", s_, (int)lane_.size());
+	
+	for (size_t i=0; i<lane_.size(); i++)
+	{
+		lane_[i]->Print();
+	}
+}
+
+Lane* LaneSection::GetLaneByIdx(int idx)
+{
+	if (idx < (int)lane_.size())
+	{
+		return lane_[idx];
+	}
+
+	return 0;
+}
+
+bool LaneSection::IsOSILaneById(int id)
+{
+	Lane* lane = GetLaneById(id);
+	if (lane == 0)
+	{
+		return false;
+	}
+	else
+	{
+		return !lane->IsCenter();
+	}
+		
+	
+}
+
+Lane* LaneSection::GetLaneById(int id)
+{
+	for (size_t i=0; i<lane_.size(); i++)
+	{
+		if (lane_[i]->GetId() == id)
+		{
+			return lane_[i];
+		}
+	}
+	return 0;
+}
+
+int LaneSection::GetLaneIdByIdx(int idx)
+{
+	if (idx > (int)lane_.size() - 1)
+	{
+		LOG("LaneSection::GetLaneIdByIdx Error: index %d, only %d lanes\n", idx, (int)lane_.size());
+		return 0;
+	}
+	else
+	{
+		return (lane_[idx]->GetId());
+	}
+}
+
+int LaneSection::GetLaneIdxById(int id)
+{
+	for (int i = 0; i<(int)lane_.size(); i++)
+	{
+		if (lane_[i]->GetId() == id)
+		{
+			return i;
+		}
+	}
+	return -1;
+}
+
+int LaneSection::GetLaneGlobalIdByIdx(int idx)
+{
+	if (idx > (int)lane_.size() - 1)
+	{
+		LOG("LaneSection::GetLaneIdByIdx Error: index %d, only %d lanes\n", idx, (int)lane_.size());
+		return 0;
+	}
+	else
+	{
+		return (lane_[idx]->GetGlobalId());
+	}
+}
+int LaneSection::GetLaneGlobalIdById(int id)
+{
+	for (size_t i=0; i<(int)lane_.size(); i++)
+	{
+		if (lane_[i]->GetId() == id)
+		{
+			return lane_[i]->GetGlobalId();
+		}
+	}
+	return -1;
+}
+
+int LaneSection::GetNumberOfDrivingLanes()
+{
+	int counter = 0;
+
+	for (size_t i = 0; i < lane_.size(); i++)
+	{
+		if (lane_[i]->IsDriving())
+		{
+			counter++;
+		}
+	}
+	return counter;
+}
+
+int LaneSection::GetNumberOfDrivingLanesSide(int side)
+{
+	int counter = 0;
+
+	for (size_t i = 0; i < lane_.size(); i++)
+	{
+		if (SIGN(lane_[i]->GetId()) == SIGN(side) && lane_[i]->IsDriving())
+		{
+			counter++;
+		}
+	}
+	return counter;
+}
+
+int LaneSection::GetNUmberOfLanesRight()
+{
+	int counter = 0;
+
+	for (size_t i=0; i<lane_.size(); i++)
+	{
+		if (lane_[i]->GetId() < 0)
+		{
+			counter++;
+		}
+	}
+	return counter;
+}
+
+int LaneSection::GetNUmberOfLanesLeft()
+{
+	int counter = 0;
+
+	for (size_t i = 0; i < lane_.size(); i++)
+	{
+		if (lane_[i]->GetId() > 0)
+		{
+			counter++;
+		}
+	}
+	return counter;
+}
+
+double LaneSection::GetWidth(double s, int lane_id)
+{
+	if (lane_id == 0)
+	{
+		return 0.0;  // reference lane has no width
+	}
+
+	Lane *lane = GetLaneById(lane_id);
+	if (lane == 0)
+	{
+		LOG("Error (lane id %d)\n", lane_id);
+		return 0.0;
+	}
+
+	LaneWidth *lane_width = lane->GetWidthByS(s - s_);
+	if (lane_width == 0) // No lane width registered
+	{
+		return 0.0;
+	}
+
+	// Calculate local s-parameter in width segment
+	double ds = s - (s_ + lane_width->GetSOffset());
+
+	// Calculate width at local s
+	return lane_width->poly3_.Evaluate(ds);
+}
+
+double LaneSection::GetOuterOffset(double s, int lane_id)
+{
+	if (lane_id == 0)
+	{
+		return 0;
+	}
+
+	double width = GetWidth(s, lane_id);
+
+	if (abs(lane_id) == 1)
+	{
+		// this is the last lane, next to reference lane of width = 0. Stop here.
+		return width;
+	}
+	else
+	{
+		int step = lane_id < 0 ? +1 : -1;
+		return (width + GetOuterOffset(s, lane_id + step));
+	}
+}
+
+double LaneSection::GetCenterOffset(double s, int lane_id)
+{
+	if (lane_id == 0)
+	{
+		// Reference lane (0) has no width
+		return 0.0;
+	}
+	double inner_offset = GetOuterOffset(s, lane_id);
+	double width = GetWidth(s, lane_id);
+
+	// Center is simply mean value of inner and outer lane boundries
+	return inner_offset - width / 2;
+}
+
+double LaneSection::GetOuterOffsetHeading(double s, int lane_id)
+{
+	if (lane_id == 0)
+	{
+		return 0.0;  // reference lane has no width
+	}
+
+	Lane *lane = GetLaneById(lane_id);
+	if (lane == 0)
+	{
+		LOG("LaneSection::GetOuterOffsetHeading Error (lane id %d)\n", lane_id);
+		return 0.0;
+	}
+
+	LaneWidth *lane_width = lane->GetWidthByS(s - s_);
+	if (lane_width == 0) // No lane width registered
+	{
+		return 0.0;
+	}
+
+	// Calculate local s-parameter in width segment
+	double ds = s - (s_ + lane_width->GetSOffset());
+
+	// Calculate heading at local s
+	double heading = lane_width->poly3_.EvaluatePrim(ds);
+
+	if (abs(lane_id) == 1)
+	{
+		// this is the last lane, next to reference lane of width = 0. Stop here.
+		return heading;
+	}
+	else
+	{
+		int step = lane_id < 0 ? +1 : -1;
+		return (heading + GetOuterOffsetHeading(s, lane_id + step));
+	}
+}
+
+double LaneSection::GetCenterOffsetHeading(double s, int lane_id)
+{
+	int step = lane_id < 0 ? +1 : -1;
+
+	if (lane_id == 0)
+	{
+		// Reference lane (0) has no width
+		return 0.0;
+	}
+	double inner_offset_heading = GetOuterOffsetHeading(s, lane_id + step);
+	double outer_offset_heading = GetOuterOffsetHeading(s, lane_id);
+
+	// Center is simply mean value of inner and outer lane boundries
+	return (inner_offset_heading + outer_offset_heading) / 2;
+}
+
+void LaneSection::AddLane(Lane *lane)
+{
+	lane->SetGlobalId();
+	global_lane_counter++;
+
+	// Keep list sorted on lane ID, from + to -
+	if (lane_.size() > 0 && lane->GetId() > lane_.back()->GetId())
+	{
+		for (size_t i = 0; i < lane_.size(); i++)
+		{
+			if (lane->GetId() > lane_[i]->GetId())
+			{
+				lane_.insert(lane_.begin() + i, lane);
+				break;
+			}
+		}
+	}
+	else
+	{
+		lane_.push_back(lane);
+	}
+}
+
+int LaneSection::GetConnectingLaneId(int incoming_lane_id, LinkType link_type)
+{
+	int id = incoming_lane_id;
+
+	if (GetLaneById(id) == 0)
+	{
+		LOG("Lane id %d not available in lane section!", id);
+		return 0;
+	}
+
+	if (GetLaneById(id)->GetLink(link_type))
+	{
+		id = GetLaneById(id)->GetLink(link_type)->GetId();
+	}
+	else
+	{
+		// if no driving lane found - stay on same index
+		id = incoming_lane_id;
+	}
+	
+	return id;
+}
+
+double LaneSection::GetWidthBetweenLanes(int lane_id1, int lane_id2, double s)
+{
+	double lanewidth = (std::fabs(GetCenterOffset(s, lane_id1)) - std::fabs(GetCenterOffset(s, lane_id2)));
+
+	return lanewidth;
+}
+
+// Offset from lane1 to lane2 in direction of reference line
+double LaneSection::GetOffsetBetweenLanes(int lane_id1, int lane_id2, double s)
+{
+	double laneCenter1 = GetCenterOffset(s, lane_id1) * SIGN(lane_id1);
+	double laneCenter2 = GetCenterOffset(s, lane_id2) * SIGN(lane_id2);
+	return (laneCenter2 - laneCenter1);
+}
+
+// Offset from closest left road mark to current position
+RoadMarkInfo Lane::GetRoadMarkInfoByS(int track_id, int lane_id, double s)
+{
+	Position* pos = new roadmanager::Position();
+	Road *road = pos->GetRoadById(track_id);
+	LaneSection *lsec;
+	Lane *lane;
+	LaneRoadMark *lane_roadMark;
+	LaneRoadMarkType *lane_roadMarkType;
+	LaneRoadMarkTypeLine *lane_roadMarkTypeLine;
+	RoadMarkInfo rm_info = {-1, -1};
+	int lsec_idx, number_of_lsec, number_of_roadmarks, number_of_roadmarktypes, number_of_roadmarklines;
+	double s_roadmark, s_roadmarkline, s_end_roadmark, s_end_roadmarkline = 0, lsec_end = 0;
+	if (road == 0)
+	{
+		LOG("Position::Set Error: track %d not available\n", track_id);
+		lsec_idx = -1;
+	}
+	else
+	{
+		lsec_idx = road->GetLaneSectionIdxByS(s);
+	}
+
+	lsec = road->GetLaneSectionByIdx(lsec_idx);
+
+	if (lsec == 0)
+	{
+		LOG("Position::Set Error: lane section %d not available\n", lsec_idx);
+	}
+	else
+	{
+		number_of_lsec = road->GetNumberOfLaneSections();
+		if (lsec_idx == number_of_lsec-1)
+		{
+			lsec_end = road->GetLength();
+		}
+		else
+		{
+			lsec_end = road->GetLaneSectionByIdx(lsec_idx+1)->GetS();
+		}
+	}
+
+	lane = lsec->GetLaneById(lane_id);
+	if (lane == 0)
+	{
+		LOG("Position::Set Error: lane section %d not available\n", lane_id);
+	}
+
+	number_of_roadmarks = lane->GetNumberOfRoadMarks();
+
+	if (number_of_roadmarks > 0)
+	{
+		for (int m=0; m<number_of_roadmarks; m++)
+		{
+			lane_roadMark = lane->GetLaneRoadMarkByIdx(m);
+			s_roadmark = lsec->GetS() + lane_roadMark->GetSOffset();
+			if (m == number_of_roadmarks-1)
+			{
+				s_end_roadmark = lsec_end;
+			}
+			else
+			{
+				s_end_roadmark = lane->GetLaneRoadMarkByIdx(m+1)->GetSOffset();
+			}
+
+			// Check the existence of "type" keyword under roadmark
+			number_of_roadmarktypes = lane_roadMark->GetNumberOfRoadMarkTypes();
+			if (number_of_roadmarktypes != 0)
+			{
+				lane_roadMarkType = lane_roadMark->GetLaneRoadMarkTypeByIdx(0);
+				number_of_roadmarklines = lane_roadMarkType->GetNumberOfRoadMarkTypeLines();
+
+				// Looping through each roadmarkline under roadmark
+				for (int n=0; n<number_of_roadmarklines; n++)
+				{
+					lane_roadMarkTypeLine = lane_roadMarkType->GetLaneRoadMarkTypeLineByIdx(n);
+					s_roadmarkline = s_roadmark + lane_roadMarkTypeLine->GetSOffset();
+					if (lane_roadMarkTypeLine != 0)
+					{
+						if (n == number_of_roadmarklines-1)
+						{
+							s_end_roadmarkline = s_end_roadmark;
+						}
+						else
+						{
+							s_end_roadmarkline = lane_roadMarkType->GetLaneRoadMarkTypeLineByIdx(n+1)->GetSOffset();
+						}
+					}
+
+					if (s >= s_roadmarkline && s < s_end_roadmarkline)
+					{
+						rm_info.roadmark_idx_ = m;
+						rm_info.roadmarkline_idx_ = n;
+					}
+					else
+					{
+						continue;
+					}
+				}
+			}
+			else
+			{
+				rm_info.roadmarkline_idx_ = 0;
+				if (s >= s_roadmark && s < s_end_roadmark)
+				{
+					rm_info.roadmark_idx_ = m;
+				}
+				else
+				{
+					continue;
+				}
+			}
+		}	
+	}
+	delete pos;
+	return rm_info;
+}
+
+RoadLink::RoadLink(LinkType type, pugi::xml_node node)
+{
+	string element_type = node.attribute("elementType").value();
+	string contact_point_type = "";
+	type_ = type;
+	element_id_ = atoi(node.attribute("elementId").value());
+	
+	if (node.attribute("contactPoint") != NULL)
+	{
+		contact_point_type = node.attribute("contactPoint").value();
+	}
+
+	if (element_type == "road")
+	{
+		element_type_ = ELEMENT_TYPE_ROAD;
+		if (contact_point_type == "start")
+		{
+			contact_point_type_ = CONTACT_POINT_START;
+		}
+		else if (contact_point_type == "end")
+		{
+			contact_point_type_ = CONTACT_POINT_END;
+		}
+		else
+		{
+			LOG("Unsupported element type: %s\n", contact_point_type.c_str());
+			contact_point_type_ = CONTACT_POINT_UNKNOWN;
+		}
+	}
+	else if (element_type == "junction")
+	{
+		element_type_ = ELEMENT_TYPE_JUNCTION;
+		contact_point_type_ = CONTACT_POINT_NONE;
+	}
+	else
+	{
+		LOG("Unsupported element type: %s\n", element_type.c_str());
+		element_type_ = ELEMENT_TYPE_UNKNOWN;
+	}
+}
+
+void RoadLink::Print()
+{
+	cout << "RoadLink type: " << type_ << " id: " << element_id_ << " element type: " << element_type_ << " contact point type: " << contact_point_type_ << endl;
+}
+
+Road::~Road()
+{
+	for (size_t i=0; i<geometry_.size(); i++)
+	{
+		delete(geometry_[i]);
+	}
+	for (size_t i=0; i<elevation_profile_.size(); i++)
+	{
+		delete(elevation_profile_[i]);
+	}
+	for (size_t i = 0; i < super_elevation_profile_.size(); i++)
+	{
+		delete(super_elevation_profile_[i]);
+	}
+	for (size_t i=0; i<link_.size(); i++)
+	{
+		delete(link_[i]);
+	}
+}
+
+void Road::Print()
+{
+	LOG("Road id: %d length: %.2f\n", id_, GetLength());
+	cout << "Geometries:" << endl;
+
+	for (size_t i = 0; i < geometry_.size(); i++)
+	{
+		cout << "Geometry type: " << geometry_[i]->GetType() << endl;
+	}
+
+	for (size_t i=0; i<link_.size(); i++)
+	{
+		link_[i]->Print();
+	}
+
+	for (size_t i=0; i<lane_section_.size(); i++)
+	{
+		lane_section_[i]->Print();
+	}
+
+	for (size_t i=0; i<lane_offset_.size(); i++)
+	{
+		lane_offset_[i]->Print();
+	}
+}
+
+void Road::AddLine(Line *line)
+{
+	geometry_.push_back((Geometry*)line);
+}
+
+void Road::AddArc(Arc *arc)
+{
+	geometry_.push_back((Geometry*)arc);
+}
+
+void Road::AddSpiral(Spiral *spiral)
+{
+	geometry_.push_back((Geometry*)spiral);
+}
+
+void Road::AddPoly3(Poly3 *poly3)
+{
+	geometry_.push_back((Geometry*)poly3);
+}
+
+void Road::AddParamPoly3(ParamPoly3 *param_poly3)
+{
+	geometry_.push_back((Geometry*)param_poly3);
+}
+
+void Road::AddElevation(Elevation *elevation)
+{
+	// Adjust last elevation length
+	if (elevation_profile_.size() > 0)
+	{
+		Elevation *e_previous = elevation_profile_.back();
+		e_previous->SetLength(elevation->GetS() - e_previous->GetS());
+	}
+	elevation->SetLength(length_ - elevation->GetS());
+
+	elevation_profile_.push_back((Elevation*)elevation);
+}
+
+void Road::AddSuperElevation(Elevation *super_elevation)
+{
+	// Adjust last super elevation length
+	if (super_elevation_profile_.size() > 0)
+	{
+		Elevation *e_previous = super_elevation_profile_.back();
+		e_previous->SetLength(super_elevation->GetS() - e_previous->GetS());
+	}
+	super_elevation->SetLength(length_ - super_elevation->GetS());
+
+	super_elevation_profile_.push_back((Elevation*)super_elevation);
+}
+
+Elevation* Road::GetElevation(int idx)
+{ 
+	if (idx < 0 || idx >= elevation_profile_.size())
+	{
+		return 0;
+	}
+	
+	return elevation_profile_[idx];
+}
+
+Elevation* Road::GetSuperElevation(int idx)
+{
+	if (idx < 0 || idx >= super_elevation_profile_.size())
+	{
+		return 0;
+	}
+
+	return super_elevation_profile_[idx];
+}
+
+void Road::AddSignal(Signal *signal)
+{
+	// Adjust signal length
+	if (signal_.size() > 0)
+	{
+		Signal *sig_previous = signal_.back();
+		sig_previous->SetLength(signal->GetS() - sig_previous->GetS());
+	}
+	signal->SetLength(length_ - signal->GetS());
+
+	LOG("Add signal[%d]: %s", (int)signal_.size(), signal->GetName().c_str());
+	signal_.push_back((Signal*)signal);
+}
+
+int Road::GetNumberOfSignals()
+{
+	return (int)signal_.size();
+}
+
+Signal* Road::GetSignal(int idx)
+{ 
+	if (idx < 0 || idx >= signal_.size())
+	{
+		return 0;
+	}
+	
+	return signal_[idx];
+}
+
+void Road::AddObject(Object* object)
+{
+	LOG("Add object[%d]: %s", (int)object_.size(), object->GetName().c_str());
+	object_.push_back(object);
+}
+
+Object* Road::GetObject(int idx)
+{
+	if (idx < 0 || idx >= object_.size())
+	{
+		return 0;
+	}
+
+	return object_[idx];
+}
+
+OutlineCornerRoad::OutlineCornerRoad(int roadId, double s, double t, double dz, double height):
+	roadId_(roadId), s_(s), t_(t), dz_(dz), height_(height)
+{
+
+}
+
+void OutlineCornerRoad::GetPos(double& x, double& y, double& z)
+{
+	roadmanager::Position pos;
+	pos.SetTrackPos(roadId_, s_, t_);
+	x = pos.GetX();
+	y = pos.GetY();
+	z = pos.GetZ() + dz_;
+}
+
+OutlineCornerLocal::OutlineCornerLocal(int roadId, double s, double t, double u, double v, double zLocal, double height, double heading) :
+	roadId_(roadId), s_(s), t_(t), u_(u), v_(v), zLocal_(zLocal), height_(height), heading_(heading)
+{
+
+}
+
+void OutlineCornerLocal::GetPos(double& x, double& y, double& z)
+{
+	roadmanager::Position pref;
+	pref.SetTrackPos(roadId_, s_, t_);
+	double total_heading = GetAngleSum(pref.GetH(), heading_);
+	double u2, v2;
+	RotateVec2D(u_, v_, total_heading, u2, v2);
+
+	x = pref.GetX() + u2;
+	y = pref.GetY() + v2;
+	z = pref.GetZ() + zLocal_;
+}
+
+double Road::GetLaneOffset(double s)
+{
+	int i = 0;
+
+	if (lane_offset_.size() == 0)
+	{
+		return 0;
+	}
+
+	for (; i + 1 < (int)lane_offset_.size(); i++)
+	{
+		if (s < lane_offset_[i + 1]->GetS())
+		{
+			break;
+		}
+	}
+	return (lane_offset_[i]->GetLaneOffset(s));
+}
+
+double Road::GetLaneOffsetPrim(double s)
+{
+	int i = 0;
+
+	if (lane_offset_.size() == 0)
+	{
+		return 0;
+	}
+
+	for (; i + 1 < (int)lane_offset_.size(); i++)
+	{
+		if (s < lane_offset_[i + 1]->GetS())
+		{
+			break;
+		}
+	}
+	return (lane_offset_[i]->GetLaneOffsetPrim(s));
+}
+
+int Road::GetNumberOfLanes(double s)
+{
+	LaneSection *lsec = GetLaneSectionByS(s);
+
+	if (lsec)
+	{
+		return (lsec->GetNumberOfLanes());
+	}
+
+	return 0;
+}
+
+int Road::GetNumberOfDrivingLanes(double s)
+{
+	LaneSection *lsec = GetLaneSectionByS(s);
+
+	if (lsec)
+	{
+		return (lsec->GetNumberOfDrivingLanes());
+	}
+	
+	return 0;
+}
+
+Lane* Road::GetDrivingLaneByIdx(double s, int idx)
+{
+	int count = 0;
+
+	LaneSection *ls = GetLaneSectionByS(s);
+
+	for (int i = 0; i < ls->GetNumberOfLanes(); i++)
+	{
+		if (ls->GetLaneByIdx(i)->IsDriving())
+		{
+			if (count++ == idx)
+			{
+				return ls->GetLaneByIdx(i);
+			}
+		}
+	}
+
+	return 0;
+}
+
+int Road::GetNumberOfDrivingLanesSide(double s, int side)
+{
+	int i;
+
+	for (i = 0; i < GetNumberOfLaneSections() - 1; i++)
+	{
+		if (s < lane_section_[i+1]->GetS())
+		{
+			break;
+		}
+	}
+
+	return (lane_section_[i]->GetNumberOfDrivingLanesSide(side));
+}
+
+double Road::GetWidth(double s, int side, int laneTypeMask)
+{
+	double offset = 0;
+	size_t i;
+	int index;
+
+	for (i = 0; i < GetNumberOfLaneSections() - 1; i++)
+	{
+		if (s < lane_section_[i+1]->GetS())
+		{
+			break;
+		}
+	}
+
+	if (i < GetNumberOfLaneSections())
+	{
+		LaneSection* lsec = lane_section_[i];
+		// Since the lanes are sorted from left to right, 
+		// side == +1 means first lane and side == -1 means last lane
+
+		if (side > 0)
+		{
+			index = 0;
+		}
+		else
+		{
+			index = lsec->GetNumberOfLanes() - 1;
+		}
+
+		int lane_id = lsec->GetLaneIdByIdx(index);
+		int step = side > 0 ? +1 : -1;
+		
+		// Find outmost lane matching requested lane type
+		while (lane_id != 0 && !(lsec->GetLaneByIdx(index)->GetLaneType() & laneTypeMask))
+		{
+			lane_id = lsec->GetLaneIdByIdx(index += step);
+		}
+
+		offset = fabs(lsec->GetOuterOffset(s, lane_id));
+	}
+
+	return offset;
+}
+
+void Road::AddLaneOffset(LaneOffset *lane_offset)
+{
+	// Adjust lane offset length
+	if (lane_offset_.size() > 0)
+	{
+		LaneOffset *lo_previous = lane_offset_.back();
+		lo_previous->SetLength(lane_offset->GetS() - lo_previous->GetS());
+	}
+	lane_offset->SetLength(length_ - lane_offset->GetS());
+	
+	lane_offset_.push_back((LaneOffset*)lane_offset);
+}
+
+double Road::GetCenterOffset(double s, int lane_id)
+{
+	// First find out what lane section 
+	LaneSection *lane_section = GetLaneSectionByS(s);
+	if (lane_section)
+	{
+		return lane_section->GetCenterOffset(s, lane_id);
+	}
+
+	return 0.0;
+}
+
+RoadLink* Road::GetLink(LinkType type)
+{
+	for (size_t i=0; i<link_.size(); i++)
+	{
+		if (link_[i]->GetType() == type)
+		{
+			return link_[i];
+		}
+	}
+	return 0;  // Link of requested type is missing
+}
+
+void Road::AddLaneSection(LaneSection *lane_section)
+{
+	// Adjust last elevation section length
+	if (lane_section_.size() > 0)
+	{
+		LaneSection *ls_previous = lane_section_.back();
+		ls_previous->SetLength(lane_section->GetS() - ls_previous->GetS());
+	}
+	lane_section->SetLength(length_ - lane_section->GetS());
+
+	lane_section_.push_back((LaneSection*)lane_section);
+}
+
+bool Road::GetZAndPitchByS(double s, double *z, double *pitch, int *index)
+{
+	if (GetNumberOfElevations() > 0)
+	{
+		if (*index < 0 || *index >= GetNumberOfElevations())
+		{
+			*index = 0;
+		}
+		Elevation *elevation = GetElevation(*index);
+		if (elevation == NULL)
+		{
+			LOG("Elevation error NULL, nelev: %d elev_idx: %d\n", GetNumberOfElevations(), *index);
+			return false;
+		}
+
+		if (elevation && s > elevation->GetS() + elevation->GetLength())
+		{
+			while (s > elevation->GetS() + elevation->GetLength() && *index < GetNumberOfElevations() - 1)
+			{
+				// Move to next elevation section
+				elevation = GetElevation(++*index);
+			}
+		}
+		else if (elevation && s < elevation->GetS())
+		{
+			while (s < elevation->GetS() && *index > 0)
+			{
+				// Move to previous elevation section
+				elevation = GetElevation(--*index);
+			}
+		}
+
+		if (elevation)
+		{
+			double p = s - elevation->GetS();
+			*z = elevation->poly3_.Evaluate(p);
+			*pitch = -elevation->poly3_.EvaluatePrim(p);
+
+			return true;
+		}
+	}
+	*z = 0;
+	return false;
+}
+
+bool Road::UpdateZAndRollBySAndT(double s, double t, double *z, double *roll, int *index)
+{
+	if (GetNumberOfSuperElevations() > 0)
+	{
+		if (*index < 0 || *index >= GetNumberOfSuperElevations())
+		{
+			*index = 0;
+		}
+		Elevation *super_elevation = GetSuperElevation(*index);
+		if (super_elevation == NULL)
+		{
+			LOG("Superelevation error NULL, nelev: %d elev_idx: %d\n", GetNumberOfSuperElevations(), *index);
+			return false;
+		}
+
+		if (super_elevation && s > super_elevation->GetS() + super_elevation->GetLength())
+		{
+			while (s > super_elevation->GetS() + super_elevation->GetLength() && *index < GetNumberOfSuperElevations() - 1)
+			{
+				// Move to next elevation section
+				super_elevation = GetSuperElevation(++ *index);
+			}
+		}
+		else if (super_elevation && s < super_elevation->GetS())
+		{
+			while (s < super_elevation->GetS() && *index > 0)
+			{
+				// Move to previous elevation section
+				super_elevation = GetSuperElevation(-- *index);
+			}
+		}
+
+		if (super_elevation)
+		{
+			double ds = s - super_elevation->GetS();
+			*roll = super_elevation->poly3_.Evaluate(ds);
+			*z += sin(*roll) * (t + GetLaneOffset(s));
+
+			return true;
+		}
+	}
+	return false;
+}
+
+Road* OpenDrive::GetRoadById(int id)
+{
+	for (size_t i=0; i<road_.size(); i++)
+	{
+		if (road_[i]->GetId() == id)
+		{
+			return road_[i];
+		}
+	}
+	return 0;
+}
+
+Road *OpenDrive::GetRoadByIdx(int idx)
+{
+	if (idx >= 0 && idx < (int)road_.size())
+	{
+		return road_[idx];
+	}
+	else
+	{
+		return 0;
+	}
+}
+
+Geometry *OpenDrive::GetGeometryByIdx(int road_idx, int geom_idx)
+{
+	if (road_idx >= 0 && road_idx < (int)road_.size())
+	{
+		return road_[road_idx]->GetGeometry(geom_idx);
+	}
+	else
+	{
+		return 0;
+	}
+}
+
+Junction* OpenDrive::GetJunctionById(int id)
+{
+	for (size_t i=0; i<junction_.size(); i++)
+	{
+		if (junction_[i]->GetId() == id)
+		{
+			return junction_[i];
+		}
+	}
+	return 0;
+}
+
+Junction *OpenDrive::GetJunctionByIdx(int idx)
+{	
+	if (idx >= 0 && idx < (int)junction_.size())
+	{
+		return junction_[idx];
+	}
+	else
+	{
+		LOG("GetJunctionByIdx error (idx %d, njunctions %d)\n", idx, (int)junction_.size());
+		return 0;
+	}
+}
+
+OpenDrive::OpenDrive(const char *filename)
+{
+	if (!LoadOpenDriveFile(filename))
+	{
+		LOG("Error loading OpenDrive %s\n", filename);
+		throw std::invalid_argument("Failed to load OpenDrive file");
+	}
+}
+
+void OpenDrive::InitGlobalLaneIds()
+{
+	g_Lane_id = 0; 
+	g_Laneb_id = 0;
+}
+
+extern double glat0;// = 39.0677643;//39.1201777;
+extern double glon0;// = 117.3548368;//117.02802270;
+
+bool OpenDrive::LoadOpenDriveFile(const char *filename, bool replace)
+{
+	mt_rand.seed((unsigned int)time(0));
+
+	if (replace)
+	{
+		InitGlobalLaneIds();
+
+		for (size_t i=0; i<road_.size(); i++)
+		{
+			delete road_[i];
+		}
+		road_.clear();
+
+		for (size_t i=0; i<junction_.size(); i++)
+		{
+			delete junction_[i];
+		}
+		junction_.clear();
+	}
+
+	odr_filename_ = filename;
+
+	if (odr_filename_ == "")
+	{
+		return false;
+	}
+
+	pugi::xml_document doc;
+
+	// First assume absolute path
+	pugi::xml_parse_result result = doc.load_file(filename);
+	if (!result)
+	{
+		return false;
+	}
+
+	pugi::xml_node node = doc.child("OpenDRIVE");
+	if (node == NULL)
+	{
+		cout << "Root null" << endl;
+		throw std::invalid_argument("The file does not seem to be an OpenDRIVE");
+	}
+
+    pugi::xml_node nodeheader = node.child("header");
+    if(nodeheader != NULL)
+    {
+        glat0 = atof(nodeheader.attribute("lat0").value());
+        glon0 = atof(nodeheader.attribute("lon0").value());
+    }
+
+	for (pugi::xml_node road_node = node.child("road"); road_node; road_node = road_node.next_sibling("road"))
+	{
+		Road *r = new Road(atoi(road_node.attribute("id").value()), road_node.attribute("name").value());
+		r->SetLength(atof(road_node.attribute("length").value()));
+		r->SetJunction(atoi(road_node.attribute("junction").value()));
+
+		for (pugi::xml_node type_node = road_node.child("type"); type_node; type_node = type_node.next_sibling("type"))
+		{
+			RoadTypeEntry *r_type = new RoadTypeEntry();
+			
+			std::string type = type_node.attribute("type").value();
+			if (type == "unknown")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_UNKNOWN;
+			}
+			else if (type == "rural")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_RURAL;
+			}
+			else if (type == "motorway")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_MOTORWAY;
+			}
+			else if (type == "town")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_TOWN;
+			}
+			else if (type == "lowSpeed")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_LOWSPEED;
+			}
+			else if (type == "pedestrian")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_PEDESTRIAN;
+			}
+			else if (type == "bicycle")
+			{
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_BICYCLE;
+			}
+			else if (type == "")
+			{
+				LOG("Missing road type - setting default (rural)");
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_RURAL;
+			}
+			else
+			{
+				LOG("Unsupported road type: %s - assuming rural", type.c_str());
+				r_type->road_type_ = roadmanager::RoadType::ROADTYPE_RURAL;
+			}
+
+			r_type->s_ = atof(type_node.attribute("s").value());
+
+			// Check for optional speed record
+			pugi::xml_node speed = type_node.child("speed");
+			if (speed != NULL)
+			{
+				r_type->speed_ = atof(speed.attribute("max").value());
+				std::string unit = speed.attribute("unit").value();
+				if (unit == "km/h")
+				{
+					r_type->speed_ /= 3.6;  // Convert to m/s
+				}
+				else if (unit == "mph")
+				{
+					r_type->speed_ *= 0.44704; // Convert to m/s
+				}
+				else if (unit == "m/s")
+				{
+					// SE unit - do nothing
+				}
+				else 
+				{
+					LOG("Unsupported speed unit: %s - assuming SE unit m/s", unit.c_str());
+				}
+			}
+
+			r->AddRoadType(r_type);
+		}
+
+		pugi::xml_node link = road_node.child("link");
+		if (link != NULL)
+		{
+			pugi::xml_node successor = link.child("successor");
+			if (successor != NULL)
+			{
+				r->AddLink(new RoadLink(SUCCESSOR, successor));
+			}
+
+			pugi::xml_node predecessor = link.child("predecessor");
+			if (predecessor != NULL)
+			{
+				r->AddLink(new RoadLink(PREDECESSOR, predecessor));
+			}
+
+			if (r->GetJunction() != -1)
+			{
+				// As connecting road it is expected to have connections in both ends
+				if (successor == NULL)
+				{
+					LOG("Warning: connecting road %d in junction %d lacks successor", r->GetId(), r->GetJunction());
+				}
+				if (predecessor == NULL)
+				{
+					LOG("Warning: connecting road %d in junction %d lacks predesessor", r->GetId(), r->GetJunction());
+				}
+			}
+		}
+
+		pugi::xml_node plan_view = road_node.child("planView");
+		if (plan_view != NULL)
+		{
+			for (pugi::xml_node geometry = plan_view.child("geometry"); geometry; geometry = geometry.next_sibling())
+			{
+				double s = atof(geometry.attribute("s").value());
+				double x = atof(geometry.attribute("x").value());
+				double y = atof(geometry.attribute("y").value());
+				double hdg = atof(geometry.attribute("hdg").value());
+				double length = atof(geometry.attribute("length").value());
+
+				pugi::xml_node type = geometry.last_child();
+				if (type != NULL)
+				{
+					// Find out the type of geometry
+					if (!strcmp(type.name(), "line"))
+					{
+						r->AddLine(new Line(s, x, y, hdg, length));
+					}
+					else if (!strcmp(type.name(), "arc"))
+					{
+						double curvature = atof(type.attribute("curvature").value());
+						r->AddArc(new Arc(s, x, y, hdg, length, curvature));
+					}
+					else if (!strcmp(type.name(), "spiral"))
+					{
+						double curv_start = atof(type.attribute("curvStart").value());
+						double curv_end = atof(type.attribute("curvEnd").value());
+						r->AddSpiral(new Spiral(s, x, y, hdg, length, curv_start, curv_end));
+					}
+					else if (!strcmp(type.name(), "poly3"))
+					{
+						double a = atof(type.attribute("a").value());
+						double b = atof(type.attribute("b").value());
+						double c = atof(type.attribute("c").value());
+						double d = atof(type.attribute("d").value());
+						r->AddPoly3(new Poly3(s, x, y, hdg, length, a, b, c, d));
+					}
+					else if (!strcmp(type.name(), "paramPoly3"))
+					{
+						double aU = atof(type.attribute("aU").value());
+						double bU = atof(type.attribute("bU").value());
+						double cU = atof(type.attribute("cU").value());
+						double dU = atof(type.attribute("dU").value());
+						double aV = atof(type.attribute("aV").value());
+						double bV = atof(type.attribute("bV").value());
+						double cV = atof(type.attribute("cV").value());
+						double dV = atof(type.attribute("dV").value());
+						ParamPoly3::PRangeType p_range = ParamPoly3::P_RANGE_NORMALIZED;
+						
+						pugi::xml_attribute attr = type.attribute("pRange");
+						if (attr && !strcmp(attr.value(), "arcLength"))
+						{
+							p_range = ParamPoly3::P_RANGE_ARC_LENGTH;
+						}
+
+						ParamPoly3 *pp3 = new ParamPoly3(s, x, y, hdg, length, aU, bU, cU, dU, aV, bV, cV, dV, p_range);
+						if (pp3 != NULL)
+						{
+							r->AddParamPoly3(pp3);
+						}
+						else
+						{
+							LOG("ParamPoly3: Major error\n");
+						}
+					}
+					else
+					{
+						cout << "Unknown geometry type: " << type.name() << endl;
+					}
+				}
+				else
+				{
+					cout << "Type == NULL" << endl;
+				}
+			}
+		}
+		
+		pugi::xml_node elevation_profile = road_node.child("elevationProfile");
+		if (elevation_profile != NULL)
+		{
+			for (pugi::xml_node elevation = elevation_profile.child("elevation"); elevation; elevation = elevation.next_sibling())
+			{
+				double s = atof(elevation.attribute("s").value());
+				double a = atof(elevation.attribute("a").value());
+				double b = atof(elevation.attribute("b").value());
+				double c = atof(elevation.attribute("c").value());
+				double d = atof(elevation.attribute("d").value());
+
+				Elevation *ep = new Elevation(s, a, b, c, d);
+				if (ep != NULL)
+				{
+					r->AddElevation(ep);
+				}
+				else
+				{
+					LOG("Elevation: Major error\n");
+				}
+			}
+		}
+
+		pugi::xml_node super_elevation_profile = road_node.child("lateralProfile");
+		if (super_elevation_profile != NULL)
+		{
+			for (pugi::xml_node super_elevation = super_elevation_profile.child("superelevation"); super_elevation; super_elevation = super_elevation.next_sibling())
+			{
+				double s = atof(super_elevation.attribute("s").value());
+				double a = atof(super_elevation.attribute("a").value());
+				double b = atof(super_elevation.attribute("b").value());
+				double c = atof(super_elevation.attribute("c").value());
+				double d = atof(super_elevation.attribute("d").value());
+
+				Elevation *ep = new Elevation(s, a, b, c, d);
+				if (ep != NULL)
+				{
+					r->AddSuperElevation(ep);
+				}
+				else
+				{
+					LOG("SuperElevation: Major error\n");
+				}
+			}
+		}
+		
+		pugi::xml_node lanes = road_node.child("lanes");
+		if (lanes != NULL)
+		{
+			for (pugi::xml_node_iterator child = lanes.children().begin(); child != lanes.children().end(); child++)
+			{
+				if (!strcmp(child->name(), "laneOffset"))
+				{
+					double s = atof(child->attribute("s").value());
+					double a = atof(child->attribute("a").value());
+					double b = atof(child->attribute("b").value());
+					double c = atof(child->attribute("c").value());
+					double d = atof(child->attribute("d").value());
+					r->AddLaneOffset(new LaneOffset(s, a, b, c, d));
+				}
+				else if (!strcmp(child->name(), "laneSection"))
+				{
+					double s = atof(child->attribute("s").value());
+					LaneSection *lane_section = new LaneSection(s);
+					r->AddLaneSection(lane_section);
+
+					for (pugi::xml_node_iterator child2 = child->children().begin(); child2 != child->children().end(); child2++)
+					{
+						if (!strcmp(child2->name(), "left"))
+						{
+							//LOG("Lane left\n");
+						}
+						else if (!strcmp(child2->name(), "right"))
+						{
+							//LOG("Lane right\n");
+						}
+						else if (!strcmp(child2->name(), "center"))
+						{
+							//LOG("Lane center\n");
+						}
+						else if (!strcmp(child2->name(), "userData"))
+						{
+							// Not supported
+							continue;
+						}
+						else
+						{
+							LOG("Unsupported lane side: %s\n", child2->name());
+							continue;
+						}
+						for (pugi::xml_node_iterator lane_node = child2->children().begin(); lane_node != child2->children().end(); lane_node++)
+						{
+							if (strcmp(lane_node->name(), "lane"))
+							{
+								LOG("Unexpected element: %s, expected \"lane\"\n", lane_node->name());
+								continue;
+							}
+
+							Lane::LaneType lane_type = Lane::LANE_TYPE_NONE;
+							if (lane_node->attribute("type") == 0 || !strcmp(lane_node->attribute("type").value(), ""))
+							{
+								LOG("Lane type error");
+							}
+							if (!strcmp(lane_node->attribute("type").value(), "none"))
+							{
+								lane_type = Lane::LANE_TYPE_NONE;
+							}
+							else  if (!strcmp(lane_node->attribute("type").value(), "driving"))
+							{
+								lane_type = Lane::LANE_TYPE_DRIVING;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "stop"))
+							{
+								lane_type = Lane::LANE_TYPE_STOP;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "shoulder"))
+							{
+								lane_type = Lane::LANE_TYPE_SHOULDER;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "biking"))
+							{
+								lane_type = Lane::LANE_TYPE_BIKING;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "sidewalk"))
+							{
+								lane_type = Lane::LANE_TYPE_SIDEWALK;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "border"))
+							{
+								lane_type = Lane::LANE_TYPE_BORDER;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "restricted"))
+							{
+								lane_type = Lane::LANE_TYPE_RESTRICTED;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "parking"))
+							{
+								lane_type = Lane::LANE_TYPE_PARKING;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "bidirectional"))
+							{
+								lane_type = Lane::LANE_TYPE_BIDIRECTIONAL;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "median"))
+							{
+								lane_type = Lane::LANE_TYPE_MEDIAN;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "special1"))
+							{
+								lane_type = Lane::LANE_TYPE_SPECIAL1;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "special2"))
+							{
+								lane_type = Lane::LANE_TYPE_SPECIAL2;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "special3"))
+							{
+								lane_type = Lane::LANE_TYPE_SPECIAL3;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "roadmarks"))
+							{
+								lane_type = Lane::LANE_TYPE_ROADMARKS;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "tram"))
+							{
+								lane_type = Lane::LANE_TYPE_TRAM;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "rail"))
+							{
+								lane_type = Lane::LANE_TYPE_RAIL;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "entry") ||
+								!strcmp(lane_node->attribute("type").value(), "mwyEntry"))
+							{
+								lane_type = Lane::LANE_TYPE_ENTRY;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "exit") ||
+								!strcmp(lane_node->attribute("type").value(), "mwyExit"))
+							{
+								lane_type = Lane::LANE_TYPE_EXIT;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "offRamp"))
+							{
+								lane_type = Lane::LANE_TYPE_OFF_RAMP;
+							}
+							else if (!strcmp(lane_node->attribute("type").value(), "onRamp"))
+							{
+								lane_type = Lane::LANE_TYPE_ON_RAMP;
+							}
+							else
+							{
+								LOG("unknown lane type: %s (road id=%d)\n", lane_node->attribute("type").value(), r->GetId());
+							}
+
+							int lane_id = atoi(lane_node->attribute("id").value());
+
+							// If lane ID == 0, make sure it's not a driving lane
+							if (lane_id == 0 && lane_type == Lane::LANE_TYPE_DRIVING)
+							{
+								lane_type = Lane::LANE_TYPE_NONE;
+							}
+							
+							Lane *lane = new Lane(lane_id, lane_type);
+							if (lane == NULL)
+							{
+								LOG("Error: creating lane\n");
+								return false;
+							}
+							lane_section->AddLane(lane);
+
+							// Link
+							pugi::xml_node lane_link = lane_node->child("link");
+							if (lane_link != NULL)
+							{
+								pugi::xml_node successor = lane_link.child("successor");
+								if (successor != NULL)
+								{
+									lane->AddLink(new LaneLink(SUCCESSOR, atoi(successor.attribute("id").value())));
+								}
+								pugi::xml_node predecessor = lane_link.child("predecessor");
+								if (predecessor != NULL)
+								{
+									lane->AddLink(new LaneLink(PREDECESSOR, atoi(predecessor.attribute("id").value())));
+								}
+							}
+
+							// Width
+							for (pugi::xml_node width = lane_node->child("width"); width; width = width.next_sibling("width"))
+							{
+								double s_offset = atof(width.attribute("sOffset").value());
+								double a = atof(width.attribute("a").value());
+								double b = atof(width.attribute("b").value());
+								double c = atof(width.attribute("c").value());
+								double d = atof(width.attribute("d").value());
+								lane->AddLaneWidth(new LaneWidth(s_offset, a, b, c, d));
+							}
+							
+							// roadMark
+							for (pugi::xml_node roadMark = lane_node->child("roadMark"); roadMark; roadMark = roadMark.next_sibling("roadMark"))
+							{
+								// s_offset
+								double s_offset = atof(roadMark.attribute("sOffset").value());
+
+								// type
+								LaneRoadMark::RoadMarkType roadMark_type = LaneRoadMark::NONE_TYPE;
+								if (roadMark.attribute("type") == 0 || !strcmp(roadMark.attribute("type").value(), ""))
+								{
+									LOG("Lane road mark type error");
+								}
+								if (!strcmp(roadMark.attribute("type").value(), "none"))
+								{
+									roadMark_type = LaneRoadMark::NONE_TYPE;
+									// None type indicates no roadmark, skip 
+									continue;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "solid"))
+								{
+									roadMark_type = LaneRoadMark::SOLID;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "broken"))
+								{
+									roadMark_type = LaneRoadMark::BROKEN;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "solid solid"))
+								{
+									roadMark_type = LaneRoadMark::SOLID_SOLID;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "solid broken"))
+								{
+									roadMark_type = LaneRoadMark::SOLID_BROKEN;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "broken solid"))
+								{
+									roadMark_type = LaneRoadMark::BROKEN_SOLID;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "broken broken"))
+								{
+									roadMark_type = LaneRoadMark::BROKEN_BROKEN;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "botts dots"))
+								{
+									roadMark_type = LaneRoadMark::BOTTS_DOTS;
+								}
+								else  if (!strcmp(roadMark.attribute("type").value(), "grass"))
+								{
+									roadMark_type = LaneRoadMark::GRASS;
+								}	
+								else  if (!strcmp(roadMark.attribute("type").value(), "curb"))
+								{
+									roadMark_type = LaneRoadMark::CURB;
+								}
+								else
+								{
+									LOG("unknown lane road mark type: %s (road id=%d)\n", roadMark.attribute("type").value(), r->GetId());
+								}
+
+								// weight - consider it optional with default value = STANDARD
+								LaneRoadMark::RoadMarkWeight roadMark_weight = LaneRoadMark::STANDARD;
+								if (roadMark.attribute("weight") != 0 && strcmp(roadMark.attribute("weight").value(), ""))
+								{
+									if (!strcmp(roadMark.attribute("weight").value(), "standard"))
+									{
+										roadMark_weight = LaneRoadMark::STANDARD;
+									}
+									else  if (!strcmp(roadMark.attribute("weight").value(), "bold"))
+									{
+										roadMark_weight = LaneRoadMark::BOLD;
+									}
+									else
+									{
+										LOG("unknown lane road mark weight: %s (road id=%d)\n", roadMark.attribute("type").value(), r->GetId());
+									}
+								}
+
+								// color - consider it optional with default value = STANDARD_COLOR
+								LaneRoadMark::RoadMarkColor roadMark_color = LaneRoadMark::STANDARD_COLOR;
+								if (roadMark.attribute("color") != 0 && strcmp(roadMark.attribute("color").value(), ""))
+								{
+									if (!strcmp(roadMark.attribute("color").value(), "standard"))
+									{
+										roadMark_color = LaneRoadMark::STANDARD_COLOR;
+									}
+									else  if (!strcmp(roadMark.attribute("color").value(), "blue"))
+									{
+										roadMark_color = LaneRoadMark::BLUE;
+									}
+									else  if (!strcmp(roadMark.attribute("color").value(), "green"))
+									{
+										roadMark_color = LaneRoadMark::GREEN;
+									}
+									else  if (!strcmp(roadMark.attribute("color").value(), "red"))
+									{
+										roadMark_color = LaneRoadMark::RED;
+									}
+									else  if (!strcmp(roadMark.attribute("color").value(), "white"))
+									{
+										roadMark_color = LaneRoadMark::WHITE;
+									}
+									else  if (!strcmp(roadMark.attribute("color").value(), "yellow"))
+									{
+										roadMark_color = LaneRoadMark::YELLOW;
+									}
+									else
+									{
+										LOG("unknown lane road mark color: %s (road id=%d)\n", roadMark.attribute("color").value(), r->GetId());
+									}
+								}
+
+								// material
+								LaneRoadMark::RoadMarkMaterial roadMark_material = LaneRoadMark::STANDARD_MATERIAL;
+
+								// laneChange
+								LaneRoadMark::RoadMarkLaneChange roadMark_laneChange = LaneRoadMark::NONE_LANECHANGE;
+								if (roadMark.attribute("laneChange") == 0 || !strcmp(roadMark.attribute("laneChange").value(), ""))
+								{
+									LOG("Lane road mark lane change error");
+								}
+								if (!strcmp(roadMark.attribute("laneChange").value(), "none"))
+								{
+									roadMark_laneChange = LaneRoadMark::NONE_LANECHANGE;
+								}
+								else  if (!strcmp(roadMark.attribute("laneChange").value(), "increase"))
+								{
+									roadMark_laneChange = LaneRoadMark::INCREASE;
+								}
+								else  if (!strcmp(roadMark.attribute("laneChange").value(), "decrease"))
+								{
+									roadMark_laneChange = LaneRoadMark::DECREASE;
+								}	
+								else  if (!strcmp(roadMark.attribute("laneChange").value(), "both"))
+								{
+									roadMark_laneChange = LaneRoadMark::BOTH;
+								}
+								else
+								{
+									LOG("unknown lane road mark lane change: %s (road id=%d)\n", roadMark.attribute("laneChange").value(), r->GetId());
+								}
+
+								double roadMark_width = atof(roadMark.attribute("width").value());
+								double roadMark_height = atof(roadMark.attribute("height").value());
+								LaneRoadMark *lane_roadMark = new LaneRoadMark(s_offset, roadMark_type, roadMark_weight, roadMark_color, 
+								roadMark_material, roadMark_laneChange, roadMark_width, roadMark_height);
+								lane->AddLaneRoadMark(lane_roadMark);
+
+								// sub_type
+								LaneRoadMarkType* lane_roadMarkType = 0;
+								for (pugi::xml_node sub_type = roadMark.child("type"); sub_type; sub_type = sub_type.next_sibling("type"))
+								{
+									if (sub_type != NULL)
+									{
+										std::string sub_type_name = sub_type.attribute("name").value();
+										double sub_type_width = atof(sub_type.attribute("width").value());
+										lane_roadMarkType = new LaneRoadMarkType(sub_type_name, sub_type_width);
+										lane_roadMark->AddType(lane_roadMarkType);
+
+										for (pugi::xml_node line = sub_type.child("line"); line; line = line.next_sibling("line"))
+										{
+											double length = atof(line.attribute("length").value());
+											double space = atof(line.attribute("space").value());
+											double t_offset = atof(line.attribute("t_offset").value());
+											double s_offset_l = atof(line.attribute("s_offset").value());
+
+											// rule
+											LaneRoadMarkTypeLine::RoadMarkTypeLineRule rule = LaneRoadMarkTypeLine::NONE;
+											if (line.attribute("rule") == 0 || !strcmp(line.attribute("rule").value(), ""))
+											{
+												LOG("Lane road mark type line rule error");
+											}
+											if (!strcmp(line.attribute("rule").value(), "none"))
+											{
+												rule = LaneRoadMarkTypeLine::NONE;
+											}
+											else  if (!strcmp(line.attribute("rule").value(), "caution"))
+											{
+												rule = LaneRoadMarkTypeLine::CAUTION;
+											}
+											else  if (!strcmp(line.attribute("rule").value(), "no passing"))
+											{
+												rule = LaneRoadMarkTypeLine::NO_PASSING;
+											}
+											else
+											{
+												LOG("unknown lane road mark type line rule: %s (road id=%d)\n", line.attribute("rule").value(), r->GetId());
+											}
+
+											double width = atof(line.attribute("width").value());
+
+											LaneRoadMarkTypeLine *lane_roadMarkTypeLine = new LaneRoadMarkTypeLine(length, space, t_offset, s_offset_l, rule, width);
+											lane_roadMarkType->AddLine(lane_roadMarkTypeLine);
+										}
+									}
+								}
+								if (roadMark_type != LaneRoadMark::NONE_TYPE && lane_roadMarkType == 0)
+								{
+									if (roadMark_type == LaneRoadMark::SOLID ||
+										roadMark_type == LaneRoadMark::CURB)
+									{
+										lane_roadMarkType = new LaneRoadMarkType("stand-in", 0.15);
+										lane_roadMark->AddType(lane_roadMarkType);
+										LaneRoadMarkTypeLine::RoadMarkTypeLineRule rule = LaneRoadMarkTypeLine::NONE;
+										LaneRoadMarkTypeLine* lane_roadMarkTypeLine = new LaneRoadMarkTypeLine(0, 0, 0, 0, rule, 0.15);
+										lane_roadMarkType->AddLine(lane_roadMarkTypeLine);
+									}
+									else if (roadMark_type == LaneRoadMark::BROKEN ||
+											 roadMark_type == LaneRoadMark::BROKEN_BROKEN)
+									{
+										lane_roadMarkType = new LaneRoadMarkType("stand-in", 0.15);
+										lane_roadMark->AddType(lane_roadMarkType);
+										LaneRoadMarkTypeLine::RoadMarkTypeLineRule rule = LaneRoadMarkTypeLine::NONE;
+										LaneRoadMarkTypeLine* lane_roadMarkTypeLine = new LaneRoadMarkTypeLine(4, 8, 0, 0, rule, 0.15);
+										lane_roadMarkType->AddLine(lane_roadMarkTypeLine);
+									}
+									else
+									{
+										LOG("No road mark created for road %d lane %d. Type %d not supported. Either swich type or add a roadMark <type> element.", r->GetId(), lane_id, roadMark_type);
+									}
+								}
+							}
+						}
+					}
+				}
+				else
+				{
+					LOG("Unsupported lane type: %s\n", child->name());
+				}
+			}
+		}
+
+		pugi::xml_node signals = road_node.child("signals");
+		if (signals != NULL)
+		{
+			for (pugi::xml_node signal = signals.child("signal"); signal; signal = signal.next_sibling())
+			{
+				double s = atof(signal.attribute("s").value());
+				double t = atof(signal.attribute("t").value());
+				int ids = atoi(signal.attribute("id").value());
+				std::string name = signal.attribute("name").value();
+				
+				// dynamic
+				bool dynamic = false;
+				if (!strcmp(signal.attribute("dynamic").value(), ""))
+				{
+					LOG("Signal dynamic check error");
+				}
+				if (!strcmp(signal.attribute("dynamic").value(), "no"))
+				{
+					dynamic = false;
+				}
+				else  if (!strcmp(signal.attribute("rule").value(), "yes"))
+				{
+					dynamic = true;
+				}
+				else
+				{
+					LOG("unknown dynamic signal identification: %s (road ids=%d)\n", signal.attribute("dynamic").value(), r->GetId());
+				}
+
+				// orientation
+				Signal::Orientation orientation = Signal::NONE;
+				if (signal.attribute("orientation") == 0 || !strcmp(signal.attribute("orientation").value(), ""))
+				{
+					LOG("Road signal orientation error");
+				}
+				if (!strcmp(signal.attribute("orientation").value(), "none"))
+				{
+					orientation = Signal::NONE;
+				}
+				else  if (!strcmp(signal.attribute("orientation").value(), "+"))
+				{
+					orientation = Signal::POSITIVE;
+				}
+				else  if (!strcmp(signal.attribute("orientation").value(), "-"))
+				{
+					orientation = Signal::NEGATIVE;
+				}
+				else
+				{
+					LOG("unknown road signal orientation: %s (road ids=%d)\n", signal.attribute("orientation").value(), r->GetId());
+				}
+
+				double  z_offset = atof(signal.attribute("zOffset").value());
+				std::string country = signal.attribute("country").value();
+
+				// type
+				Signal::Type type = Signal::NONETYPE;
+				if (signal.attribute("type") == 0 || !strcmp(signal.attribute("type").value(), ""))
+				{
+					LOG("Road signal type error");
+				}
+				if (!strcmp(signal.attribute("type").value(), "none") || !strcmp(signal.attribute("type").value(), "-1"))
+				{
+					type = Signal::NONETYPE;
+				}				
+				else  if (!strcmp(signal.attribute("type").value(), "1000001"))
+				{
+					type = Signal::T1000001;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000002"))
+				{
+					type = Signal::T1000002;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000007"))
+				{
+					type = Signal::T1000007;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000008"))
+				{
+					type = Signal::T1000008;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000009"))
+				{
+					type = Signal::T1000009;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000010"))
+				{
+					type = Signal::T1000010;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000011"))
+				{
+					type = Signal::T1000011;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000012"))
+				{
+					type = Signal::T1000012;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000013"))
+				{
+					type = Signal::T1000013;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000014"))
+				{
+					type = Signal::T1000014;
+				}
+				else  if (!strcmp(signal.attribute("type").value(), "1000015"))
+				{
+					type = Signal::T1000015;
+				}																
+				else
+				{
+					LOG("unknown road signal type: %s (road ids=%d)\n", signal.attribute("type").value(), r->GetId());
+				}
+
+				// sub_type
+				Signal::SubType sub_type = Signal::NONESUBTYPE;
+				if (signal.attribute("subtype") == 0 || !strcmp(signal.attribute("subtype").value(), ""))
+				{
+					LOG("Road signal sub-type error");
+				}
+				if (!strcmp(signal.attribute("subtype").value(), "none") || !strcmp(signal.attribute("subtype").value(), "-1"))
+				{
+					sub_type = Signal::NONESUBTYPE;
+				}
+				else  if (!strcmp(signal.attribute("subtype").value(), "10"))
+				{
+					sub_type = Signal::SUBT10;
+				}
+				else  if (!strcmp(signal.attribute("subtype").value(), "20"))
+				{
+					sub_type = Signal::SUBT20;
+				}
+				else  if (!strcmp(signal.attribute("subtype").value(), "30"))
+				{
+					sub_type = Signal::SUBT30;
+				}
+				else  if (!strcmp(signal.attribute("subtype").value(), "40"))
+				{
+					sub_type = Signal::SUBT40;
+				}
+				else  if (!strcmp(signal.attribute("subtype").value(), "50"))
+				{
+					sub_type = Signal::SUBT50;
+				}
+				else
+				{
+					LOG("unknown road signal sub-type: %s (road ids=%d)\n", signal.attribute("subtype").value(), r->GetId());
+				}
+
+				double value = atof(signal.attribute("value").value());
+				std::string unit = signal.attribute("unit").value();
+				double height = atof(signal.attribute("height").value());
+				double width = atof(signal.attribute("width").value());
+				std::string text = signal.attribute("text").value();
+				double h_offset = atof(signal.attribute("hOffset").value());
+				double pitch = atof(signal.attribute("pitch").value());
+				double roll = atof(signal.attribute("roll").value());
+
+				Signal *sig = new Signal(s, t, ids, name, dynamic, orientation, z_offset, country, type, sub_type, value, unit, height,
+				width, text, h_offset, pitch, roll);
+				if (sig != NULL)
+				{
+					r->AddSignal(sig);
+				}
+				else
+				{
+					LOG("Signal: Major error\n");
+				}
+			}
+		}
+
+		pugi::xml_node objects = road_node.child("objects");
+		if (objects != NULL)
+		{
+			for (pugi::xml_node object = objects.child("object"); object; object = object.next_sibling())
+			{
+				double s = atof(object.attribute("s").value());
+				double t = atof(object.attribute("t").value());
+				int ids = atoi(object.attribute("id").value());
+				std::string name = object.attribute("name").value();
+
+				// orientation
+				Object::Orientation orientation = Object::Orientation::NONE;
+				if (object.attribute("orientation") == 0 || !strcmp(object.attribute("orientation").value(), ""))
+				{
+					LOG("Road object orientation error");
+				}
+				if (!strcmp(object.attribute("orientation").value(), "none"))
+				{
+					orientation = Object::Orientation::NONE;
+				}
+				else  if (!strcmp(object.attribute("orientation").value(), "+"))
+				{
+					orientation = Object::Orientation::POSITIVE;
+				}
+				else  if (!strcmp(object.attribute("orientation").value(), "-"))
+				{
+					orientation = Object::Orientation::NEGATIVE;
+				}
+				else
+				{
+					LOG("unknown road object orientation: %s (road ids=%d)\n", object.attribute("orientation").value(), r->GetId());
+				}
+				
+				std::string type = object.attribute("type").value();
+				double  z_offset = atof(object.attribute("zOffset").value());
+				double length = atof(object.attribute("length").value());
+				double height = atof(object.attribute("height").value());
+				double width = atof(object.attribute("width").value());
+				double heading = atof(object.attribute("hdg").value());
+				double pitch = atof(object.attribute("pitch").value());
+				double roll = atof(object.attribute("roll").value());
+
+				Object* obj = new Object(s, t, ids, name, orientation, z_offset, type, length, height,
+					width, heading, pitch, roll);
+				if (obj != NULL)
+				{
+					r->AddObject(obj);
+
+				}
+				else
+				{
+					LOG("Object: Major error\n");
+				}
+
+				pugi::xml_node outlines_node = object.child("outlines");
+				if (outlines_node != NULL)
+				{
+					for (pugi::xml_node outline_node = outlines_node.child("outline"); outline_node; outline_node = outline_node.next_sibling())
+					{
+						int id = atoi(outline_node.attribute("id").value());
+						bool closed = !strcmp(outline_node.attribute("closed").value(), "true") ? true : false;
+						Outline* outline = new Outline(id, Outline::FillType::FILL_TYPE_UNDEFINED, closed);
+
+						for (pugi::xml_node corner_node = outline_node.first_child(); corner_node; corner_node = corner_node.next_sibling())
+						{
+							OutlineCorner* corner = 0;
+							
+							if (!strcmp(corner_node.name(), "cornerRoad"))
+							{
+								double sc = atof(corner_node.attribute("s").value());
+								double tc = atof(corner_node.attribute("t").value());
+								double dz = atof(corner_node.attribute("dz").value());
+								double heightc = atof(corner_node.attribute("height").value());
+
+								corner = (OutlineCorner*)(new OutlineCornerRoad(r->GetId(), sc, tc, dz, heightc));
+							}
+							else if (!strcmp(corner_node.name(), "cornerLocal"))
+							{
+								double u = atof(corner_node.attribute("u").value());
+								double v = atof(corner_node.attribute("v").value());
+								double zLocal = atof(corner_node.attribute("z").value());
+								double heightc = atof(corner_node.attribute("height").value());
+
+								corner = (OutlineCorner*)(new OutlineCornerLocal(r->GetId(), obj->GetS(), obj->GetT(), u, v, zLocal, heightc, heading));
+							}
+							outline->AddCorner(corner);
+						}
+						obj->AddOutline(outline);
+					}
+				}
+			}
+		}
+
+		if (r->GetNumberOfLaneSections() == 0)
+		{
+			// Add empty center reference lane
+			LaneSection *lane_section = new LaneSection(0.0);
+			lane_section->AddLane(new Lane(0, Lane::LANE_TYPE_NONE));
+			r->AddLaneSection(lane_section);
+		}
+
+		road_.push_back(r);
+	}
+
+	for (pugi::xml_node junction_node = node.child("junction"); junction_node; junction_node = junction_node.next_sibling("junction"))
+	{
+		int idj = atoi(junction_node.attribute("id").value());
+		std::string name = junction_node.attribute("name").value();
+
+		Junction *j = new Junction(idj, name);
+
+		for (pugi::xml_node connection_node = junction_node.child("connection"); connection_node; connection_node = connection_node.next_sibling("connection"))
+		{
+			if (connection_node != NULL)
+			{
+				int idc = atoi(connection_node.attribute("id").value());
+				(void)idc;
+				int incoming_road_id = atoi(connection_node.attribute("incomingRoad").value());
+				int connecting_road_id = atoi(connection_node.attribute("connectingRoad").value());
+				Road *incoming_road = GetRoadById(incoming_road_id);
+				Road *connecting_road = GetRoadById(connecting_road_id);
+
+				// Check that the connecting road is referring back to this junction
+				if (connecting_road->GetJunction() != j->GetId())
+				{
+					LOG("Warning: Connecting road (id %d) junction attribute (%d) is not referring back to junction %d which is making use of it", 
+						connecting_road->GetId(), connecting_road->GetJunction(), j->GetId());
+				}
+
+				ContactPointType contact_point = CONTACT_POINT_UNKNOWN;
+				std::string contact_point_str = connection_node.attribute("contactPoint").value();
+				if (contact_point_str == "start")
+				{
+					contact_point = CONTACT_POINT_START;
+				}
+				else if (contact_point_str == "end")
+				{
+					contact_point = CONTACT_POINT_END;
+				}
+				else
+				{
+					LOG("Unsupported contact point: %s\n", contact_point_str.c_str());
+				}
+
+				Connection *connection = new Connection(incoming_road, connecting_road, contact_point);
+
+				for (pugi::xml_node lane_link_node = connection_node.child("laneLink"); lane_link_node; lane_link_node = lane_link_node.next_sibling("laneLink"))
+				{
+					int from_id = atoi(lane_link_node.attribute("from").value());
+					int to_id = atoi(lane_link_node.attribute("to").value());
+					connection->AddJunctionLaneLink(from_id, to_id);
+				}
+				j->AddConnection(connection);
+			}
+		}
+		junction_.push_back(j);
+	}
+
+	// CheckConnections();
+
+	if (!SetRoadOSI())
+	{
+		LOG("Failed to create OSI points for OpenDrive road!");
+	}
+
+	return true;
+}
+
+Connection::Connection(Road* incoming_road, Road *connecting_road, ContactPointType contact_point)
+{
+	// Find corresponding road objects
+	incoming_road_ = incoming_road;
+	connecting_road_ = connecting_road;
+	contact_point_ = contact_point;
+}
+
+Connection::~Connection()
+{ 
+	for (size_t i=0; i<lane_link_.size(); i++) 
+	{
+		delete lane_link_[i];
+	}
+}
+
+void Connection::AddJunctionLaneLink(int from, int to)
+{
+	lane_link_.push_back(new JunctionLaneLink(from, to));
+}
+
+int Connection::GetConnectingLaneId(int incoming_lane_id)
+{
+	for (size_t i = 0; i < lane_link_.size(); i++)
+	{
+		if (lane_link_[i]->from_ == incoming_lane_id)
+		{
+			return lane_link_[i]->to_;
+		}
+	}
+	return 0;
+}
+
+void Connection::Print()
+{
+	LOG("Connection: incoming %d connecting %d\n", incoming_road_->GetId(), connecting_road_->GetId());
+	for (size_t i = 0; i < lane_link_.size(); i++)
+	{
+		lane_link_[i]->Print();
+	}
+}
+
+Junction::~Junction()
+{ 
+	for (size_t i=0; i<connection_.size(); i++)
+	{
+		delete connection_[i];
+	}
+}
+
+int Junction::GetNumberOfRoadConnections(int roadId, int laneId)
+{
+	int counter = 0;
+
+	for (int i = 0; i < GetNumberOfConnections(); i++)
+	{
+		Connection * connection = GetConnectionByIdx(i);
+		if (connection && connection->GetIncomingRoad() && roadId == connection->GetIncomingRoad()->GetId())
+		{
+			for (int j = 0; j < connection->GetNumberOfLaneLinks(); j++)
+			{
+				JunctionLaneLink *lane_link = connection->GetLaneLink(j);
+				if (laneId == lane_link->from_)
+				{
+					counter++;
+				}
+			}
+		}
+	}
+	return counter;
+}
+
+LaneRoadLaneConnection Junction::GetRoadConnectionByIdx(int roadId, int laneId, int idx, int laneTypeMask)
+{
+	int counter = 0;
+	LaneRoadLaneConnection lane_road_lane_connection;
+
+	for (int i = 0; i < GetNumberOfConnections(); i++)
+	{
+		Connection *connection = GetConnectionByIdx(i);
+
+		if (connection && connection->GetIncomingRoad() && roadId == connection->GetIncomingRoad()->GetId())
+		{
+			for (int j = 0; j < connection->GetNumberOfLaneLinks(); j++)
+			{
+				JunctionLaneLink *lane_link = connection->GetLaneLink(j);
+				if (laneId == lane_link->from_)
+				{
+					if (counter == idx)
+					{
+						lane_road_lane_connection.SetLane(laneId);
+						lane_road_lane_connection.contact_point_ = connection->GetContactPoint();
+						lane_road_lane_connection.SetConnectingRoad(connection->GetConnectingRoad()->GetId());
+						lane_road_lane_connection.SetConnectingLane(lane_link->to_);
+						// find out driving direction
+						int laneSectionId;
+						if (lane_link->to_ < 0)
+						{
+							laneSectionId = 0;
+						}
+						else
+						{
+							laneSectionId = connection->GetConnectingRoad()->GetNumberOfLaneSections() - 1;
+						}
+						if (!(connection->GetConnectingRoad()->GetLaneSectionByIdx(laneSectionId)->GetLaneById(lane_link->to_)->GetLaneType() & laneTypeMask))
+						{
+							LOG("OpenDrive::GetJunctionConnection target lane not driving! from %d, %d to %d, %d\n",
+								roadId, laneId, connection->GetConnectingRoad()->GetId(), lane_link->to_);
+						}
+
+						return lane_road_lane_connection;
+					}
+					counter++;
+				}
+			}
+		}
+	}
+
+//	LOG("RoadConnection not found!");
+	return lane_road_lane_connection;
+}
+
+int Junction::GetNoConnectionsFromRoadId(int incomingRoadId)
+{
+	int counter = 0;
+
+	for (int i = 0; i < GetNumberOfConnections(); i++)
+	{
+		Connection * connection = GetConnectionByIdx(i);
+		if (connection && connection->GetIncomingRoad()->GetId() == incomingRoadId)
+		{
+			counter++;
+		}
+	}
+	
+	return counter;
+}
+
+int Junction::GetConnectingRoadIdFromIncomingRoadId(int incomingRoadId, int index)
+{
+	int counter = 0;
+
+	for (int i = 0; i < GetNumberOfConnections(); i++)
+	{
+		Connection * connection = GetConnectionByIdx(i);
+		if (connection && connection->GetIncomingRoad()->GetId() == incomingRoadId)
+		{
+			if (counter == index)
+			{
+				return GetConnectionByIdx(i)->GetConnectingRoad()->GetId();
+			}
+			else
+			{
+				counter++;
+			}
+		}
+	}
+	return -1;
+}
+
+void Junction::Print()
+{
+	LOG("Junction %d %s: \n", id_, name_.c_str());
+
+	for (size_t i=0; i<connection_.size(); i++)
+	{
+		connection_[i]->Print();
+	}
+}
+
+bool RoadPath::CheckRoad(Road *checkRoad, RoadPath::PathNode *srcNode, Road *fromRoad)
+{
+	RoadLink* nextLink = 0;
+
+	if (srcNode->link->GetElementType() == RoadLink::RoadLink::ELEMENT_TYPE_ROAD)
+	{
+		if (srcNode->link->GetContactPointType() == ContactPointType::CONTACT_POINT_END)
+		{
+			nextLink = checkRoad->GetLink(LinkType::PREDECESSOR);
+		}
+		else
+		{
+			nextLink = checkRoad->GetLink(LinkType::SUCCESSOR);
+		}
+	}
+	else if (srcNode->link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_JUNCTION)
+	{
+		if (checkRoad->GetLink(LinkType::SUCCESSOR) && 
+			checkRoad->GetLink(LinkType::SUCCESSOR)->GetElementId() == fromRoad->GetId())
+		{
+			nextLink = checkRoad->GetLink(LinkType::PREDECESSOR);
+		}
+		else if (checkRoad->GetLink(LinkType::PREDECESSOR) &&
+			checkRoad->GetLink(LinkType::PREDECESSOR)->GetElementId() == fromRoad->GetId())
+		{
+			nextLink = checkRoad->GetLink(LinkType::SUCCESSOR);
+		}
+	}
+
+	if (nextLink == 0)
+	{
+		// end of road
+		return false;
+	}
+
+	// Check if next node is already visited
+	for (size_t i = 0; i < visited_.size(); i++)
+	{
+		if (visited_[i]->link == nextLink)
+		{
+			// Already visited, ignore and return
+			return false;
+		}
+	}
+
+	// Check if next node is already among unvisited
+	size_t i;
+	for (i = 0; i < unvisited_.size(); i++)
+	{
+		if (unvisited_[i]->link == nextLink)
+		{
+			// Consider it, i.e. calc distance and potentially store it (if less than old) 
+			if (srcNode->dist + checkRoad->GetLength() < unvisited_[i]->dist)
+			{
+				unvisited_[i]->dist = srcNode->dist + checkRoad->GetLength();
+			}
+		}
+	}
+
+	if (i == unvisited_.size())
+	{
+		// link not visited before, add it
+		PathNode *pNode = new PathNode;
+		pNode->dist = srcNode->dist + checkRoad->GetLength();
+		pNode->link = nextLink;
+		pNode->fromRoad = checkRoad;
+		pNode->previous = srcNode;
+		unvisited_.push_back(pNode);
+	}
+
+	return true;
+}
+
+int RoadPath::Calculate(double &dist)
+{
+	OpenDrive* odr = startPos_->GetOpenDrive();
+	RoadLink *link = 0;
+	Junction *junction = 0;
+	Road* startRoad = odr->GetRoadById(startPos_->GetTrackId());
+	Road* targetRoad = odr->GetRoadById(targetPos_->GetTrackId());
+	Road* pivotRoad = startRoad;
+	Road* nextRoad = startRoad;
+	bool found = false;
+	double tmpDist = 0;
+	size_t i;
+	
+	// This method will find and measure the length of the shortest path 
+	// between a start position and a target position
+	// The implementation is based on Dijkstra's algorithm
+	// https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
+
+	if (pivotRoad == 0)
+	{
+		LOG("Invalid startpos road ID: %d", startPos_->GetTrackId());
+		return -1;
+	}
+
+	// Look both forward and backwards from start position
+	for (i = 0; i < 2; i++)
+	{
+		if (i == 0)
+		{
+			tmpDist = startPos_->GetS();  // distance to first road link is distance to start of road
+			link = pivotRoad->GetLink(LinkType::PREDECESSOR);  // Find link to previous road or junction
+		}
+		else
+		{
+			tmpDist = pivotRoad->GetLength() - startPos_->GetS();  // distance to end of road
+			link = pivotRoad->GetLink(LinkType::SUCCESSOR);  // Find link to previous road or junction
+		}
+
+		// If only forward direction would be of interest, add something like:
+		// if (abs(startPos_->GetHRelative()) > M_PI_2 && abs(startPos_->GetHRelative()) < 3 * M_PI / 2)
+		// then choose predecessor, else successor 
+
+		if (link)
+		{
+			PathNode* pNode = new PathNode;
+			pNode->dist = tmpDist;
+			pNode->link = link;
+			pNode->fromRoad = pivotRoad;
+			pNode->previous = 0;
+			unvisited_.push_back(pNode);
+		}
+	}
+
+	if (startRoad == targetRoad)
+	{
+		dist = targetPos_->GetS() - startPos_->GetS();
+
+		// Special case: On same road, distance is equal to delta s
+		if (startPos_->GetLaneId() < 0)
+		{
+			if (startPos_->GetHRelative() > M_PI_2 && startPos_->GetHRelative() < 3 * M_PI_2)
+			{
+				// facing opposite road direction
+				dist *= -1;
+			}
+		}
+		else
+		{
+			// decreasing in lanes with positive IDs
+			dist *= -1;
+			
+			if (startPos_->GetHRelative() < M_PI_2 || startPos_->GetHRelative() > 3 * M_PI_2)
+			{
+				// facing along road direction
+				dist *= -1;
+			}
+		}
+
+		return 0;
+	}
+
+	if (unvisited_.size() == 0)
+	{
+		// No links
+		dist = 0;
+		return -1;
+	}
+
+	for (i = 0; i < 100 && !found && unvisited_.size() > 0; i++)
+	{
+		found = false;
+		
+		// Find unvisited PathNode with shortest distance
+		double minDist = LARGE_NUMBER;
+		int minIndex = 0;
+		for (size_t j = 0; j < unvisited_.size(); j++)
+		{
+			if (unvisited_[j]->dist < minDist)
+			{
+				minIndex = (int)j;
+				minDist = unvisited_[j]->dist;
+			}
+		}
+
+		link = unvisited_[minIndex]->link;
+		tmpDist = unvisited_[minIndex]->dist;
+		pivotRoad = unvisited_[minIndex]->fromRoad;
+
+		// - Inspect all unvisited neighbor nodes (links), measure edge (road) distance to that link
+		// - Note the total distance 
+		// - If not already in invisited list, put it there. 
+		// - Update distance to this link if shorter than previously registered value
+		if (link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_ROAD)
+		{
+			// only one edge (road)
+			nextRoad = odr->GetRoadById(link->GetElementId());
+
+			if (nextRoad == targetRoad)
+			{
+				// Special case: On same road, distance is equal to delta s, direction considered
+				if (link->GetContactPointType() == ContactPointType::CONTACT_POINT_START)
+				{
+					tmpDist += targetPos_->GetS();
+				}
+				else
+				{
+					tmpDist += nextRoad->GetLength() - targetPos_->GetS();
+				}
+				
+				found = true;
+			}
+			else
+			{
+				CheckRoad(nextRoad, unvisited_[minIndex], pivotRoad);
+			}
+		}
+		else if (link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_JUNCTION)
+		{
+			// check all outgoing edges (connecting roads) from the link (junction)
+			junction = odr->GetJunctionById(link->GetElementId());
+			for (size_t j = 0; j < junction->GetNoConnectionsFromRoadId(pivotRoad->GetId()); j++)
+			{
+				nextRoad = odr->GetRoadById(junction->GetConnectingRoadIdFromIncomingRoadId(pivotRoad->GetId(), (int)j));
+				if (nextRoad == 0)
+				{
+					return 0;
+				}
+
+				if (nextRoad == targetRoad)
+				{
+					// Special case: On same road, distance is equal to delta s, direction considered
+					if (nextRoad->GetLink(LinkType::PREDECESSOR)->GetElementId() == pivotRoad->GetId())
+					{
+						tmpDist += targetPos_->GetS();
+					}
+					else
+					{
+						tmpDist += nextRoad->GetLength() - targetPos_->GetS();
+					}
+					found = true;
+				}
+				else
+				{ 
+					CheckRoad(nextRoad, unvisited_[minIndex], pivotRoad);
+				}
+			}
+		}
+		
+		// Mark pivot link as visited (move it from unvisited to visited)
+		visited_.push_back(unvisited_[minIndex]);
+		unvisited_.erase(unvisited_.begin() + minIndex);
+	}
+
+	if (found)
+	{
+		// Find out whether the path goes forward or backwards from starting position
+		if (visited_.size() > 0)
+		{
+			RoadPath::PathNode* node = visited_.back();
+
+			while (node)
+			{
+				if (node->previous == 0)
+				{
+					// This is the first node - inspect whether it is in front or behind start position
+					if ((node->link == startRoad->GetLink(LinkType::PREDECESSOR) && 
+						abs(startPos_->GetHRelative()) > M_PI_2 && abs(startPos_->GetHRelative()) < 3 * M_PI / 2) ||
+						((node->link == startRoad->GetLink(LinkType::SUCCESSOR) &&
+						abs(startPos_->GetHRelative()) < M_PI_2 || abs(startPos_->GetHRelative()) > 3 * M_PI / 2)))
+					{
+						direction_ = 1;
+					}
+					else
+					{
+						direction_ = -1;
+					}
+				}
+				node = node->previous;
+			}
+		}
+	}
+
+	dist = direction_ * tmpDist;
+
+	return found ? 0 : -1;
+}
+
+
+RoadPath::~RoadPath()
+{
+	for (size_t i = 0; i < visited_.size(); i++)
+	{
+		delete(visited_[i]);
+	}
+	visited_.clear();
+
+	for (size_t i = 0; i < unvisited_.size(); i++)
+	{
+		delete(unvisited_[i]);
+	}
+	unvisited_.clear();
+}
+
+OpenDrive::~OpenDrive()
+{
+	for (size_t i = 0; i < road_.size(); i++)
+	{
+		delete(road_[i]);
+	}
+	for (size_t i = 0; i < junction_.size(); i++)
+	{
+		delete(junction_[i]);
+	}
+}
+
+int OpenDrive::GetTrackIdxById(int id)
+{
+	for (int i = 0; i<(int)road_.size(); i++)
+	{
+		if (road_[i]->GetId() == id)
+		{
+			return i;
+		}
+	}
+	LOG("OpenDrive::GetTrackIdxById Error: Road id %d not found\n", id);
+	return -1;
+}
+
+int OpenDrive::GetTrackIdByIdx(int idx)
+{
+	if (idx >= 0 && idx < (int)road_.size())
+	{
+		return (road_[idx]->GetId());
+	}
+	LOG("OpenDrive::GetTrackIdByIdx: idx %d out of range [0:%d]\n", idx, (int)road_.size());
+	return 0;
+}
+
+int OpenDrive::IsDirectlyConnected(int road1_id, int road2_id, double &angle)
+{
+	Road *road1 = GetRoadById(road1_id);
+	Road *road2 = GetRoadById(road2_id);
+	RoadLink *link;
+
+	// Look from road 1, both ends, for road 2 
+		
+	for (int i = 0; i < 2; i++)
+	{
+		link = road1->GetLink(i == 0 ? LinkType::SUCCESSOR : LinkType::PREDECESSOR);
+		if (link)
+		{
+			if (link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_ROAD)
+			{
+				if (link->GetElementId() == road2->GetId())
+				{
+					angle = 0;
+					return i == 0 ? 1 : -1;
+				}
+			}
+			else if (link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_JUNCTION)
+			{
+				Junction *junction = GetJunctionById(link->GetElementId());
+
+				for (int j = 0; junction != NULL && j < junction->GetNumberOfConnections(); j++)
+				{
+					Connection *connection = junction->GetConnectionByIdx(j);
+					Position test_pos1;
+					Position test_pos2;
+
+					double heading1, heading2;
+
+					// check case where road1 is incoming road
+					if (connection->GetIncomingRoad() == NULL)
+					{
+						LOG("Junction %d connection %d missing incoming road", junction->GetId(), j);
+						return -1;
+					}
+					if (connection->GetConnectingRoad() == NULL)
+					{
+						LOG("Junction %d connection %d missing connecting road", junction->GetId(), j);
+						return -1;
+					}
+					if (connection->GetIncomingRoad()->GetId() == road1_id && connection->GetConnectingRoad()->GetId() == road2_id)
+					{
+						test_pos1.SetLanePos(road2_id, 0, 0, 0);
+						test_pos2.SetLanePos(road2_id, 0, road2->GetLength(), 0);
+
+						if (connection->GetContactPoint() == CONTACT_POINT_END)
+						{
+							heading1 = test_pos2.GetH() + M_PI;
+							heading2 = test_pos1.GetH() + M_PI;
+						}
+						else if(connection->GetContactPoint() == CONTACT_POINT_START)
+						{
+							heading1 = test_pos1.GetH();
+							heading2 = test_pos2.GetH();
+						}
+						else
+						{
+							LOG("Unexpected contact point %d", connection->GetContactPoint());
+							return 0;
+						}
+
+						angle = GetAbsAngleDifference(heading1, heading2);
+
+						return i == 0 ? 1 : -1;
+					}
+					// then check other case where road1 is outgoing from connecting road (connecting road is a road within junction)
+					else if (connection->GetConnectingRoad()->GetId() == road2_id && 
+						((connection->GetContactPoint() == ContactPointType::CONTACT_POINT_START && 
+							connection->GetConnectingRoad()->GetLink(LinkType::SUCCESSOR) && connection->GetConnectingRoad()->GetLink(LinkType::SUCCESSOR)->GetElementId() == road1_id) ||
+						 (connection->GetContactPoint() == ContactPointType::CONTACT_POINT_END && 
+							connection->GetConnectingRoad()->GetLink(LinkType::PREDECESSOR) && connection->GetConnectingRoad()->GetLink(LinkType::PREDECESSOR)->GetElementId() == road1_id))
+						)
+					{
+						if (connection->GetContactPoint() == CONTACT_POINT_START) // connecting road ends up connecting to road_1
+						{
+							test_pos1.SetLanePos(road2_id, 0, road2->GetLength(), 0);
+							test_pos2.SetLanePos(road2_id, 0, 0, 0);
+
+							if (connection->GetConnectingRoad()->GetLink(LinkType::SUCCESSOR)->GetContactPointType() == CONTACT_POINT_START)  // connecting to start of road_1
+							{
+								heading1 = test_pos2.GetH();
+								heading2 = test_pos1.GetH();
+							}
+							else if (connection->GetConnectingRoad()->GetLink(LinkType::SUCCESSOR)->GetContactPointType() == CONTACT_POINT_END)  // connecting to end of road_1
+							{
+								heading1 = test_pos2.GetH() + M_PI;
+								heading2 = test_pos1.GetH() + M_PI;
+							}
+							else
+							{
+								LOG("Unexpected contact point %d", connection->GetConnectingRoad()->GetLink(LinkType::PREDECESSOR)->GetContactPointType());
+								return 0;
+							}
+						}
+						else if (connection->GetContactPoint() == CONTACT_POINT_END) // connecting road start point connecting to road_1 
+						{
+							test_pos1.SetLanePos(road2_id, 0, 0, 0);
+							test_pos2.SetLanePos(road2_id, 0, road2->GetLength(), 0);
+
+							if (connection->GetConnectingRoad()->GetLink(LinkType::PREDECESSOR)->GetContactPointType() == CONTACT_POINT_START)  // connecting to start of road_1
+							{
+								heading1 = test_pos2.GetH() + M_PI;
+								heading2 = test_pos1.GetH() + M_PI;
+							}
+							else if (connection->GetConnectingRoad()->GetLink(LinkType::PREDECESSOR)->GetContactPointType() == CONTACT_POINT_END)  // connecting to end of road_1
+							{
+								heading1 = test_pos2.GetH();
+								heading2 = test_pos1.GetH();
+							}
+							else
+							{
+								LOG("Unexpected contact point %d", connection->GetConnectingRoad()->GetLink(LinkType::PREDECESSOR)->GetContactPointType());
+								return 0;
+							}
+						}
+						else
+						{
+							LOG("Unexpected contact point %d", connection->GetContactPoint());
+							return 0;
+						}
+
+						angle = GetAbsAngleDifference(heading1, heading2);
+
+						return i == 0 ? 1 : -1;
+					}
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+bool OpenDrive::IsIndirectlyConnected(int road1_id, int road2_id, int* &connecting_road_id, int* &connecting_lane_id, int lane1_id, int lane2_id)
+{
+	Road *road1 = GetRoadById(road1_id);
+	Road *road2 = GetRoadById(road2_id);
+	RoadLink *link = 0;
+
+	LinkType link_type[2] = { SUCCESSOR , PREDECESSOR };
+
+	// Try both ends
+	for (int k = 0; k < 2; k++)
+	{
+		link = road1->GetLink(link_type[k]);
+		if (link == 0)
+		{
+			continue;
+		}
+
+		LaneSection *lane_section = 0;
+
+		if (link->GetElementType() == RoadLink::ELEMENT_TYPE_ROAD)
+		{
+			if (link->GetElementId() == road2->GetId())
+			{
+				if (lane1_id != 0 && lane2_id != 0)
+				{
+					// Check lane connected
+					if (link_type[k] == SUCCESSOR)
+					{
+						lane_section = road1->GetLaneSectionByIdx(road1->GetNumberOfLaneSections() - 1);
+					}
+					else if (link_type[k] == PREDECESSOR)
+					{
+						lane_section = road1->GetLaneSectionByIdx(0);
+					}
+					else
+					{
+						LOG("Error LinkType %d not suppoered\n", link_type[k]);
+						return false;
+					}
+					if (lane_section == 0)
+					{
+						LOG("Error lane section == 0\n");
+						return false;
+					}
+					Lane *lane = lane_section->GetLaneById(lane1_id);
+					(void)lane;
+					if (!(lane_section->GetConnectingLaneId(lane1_id, link_type[k]) == lane2_id))
+					{
+						return false;
+					}
+					// Now, check other end lane connectivitiy
+				}
+				return true;
+			}
+		}
+		// check whether the roads are connected via a junction connecting road and specified lane
+		else if (link->GetElementType() == RoadLink::ELEMENT_TYPE_JUNCTION)
+		{
+			Junction *junction = GetJunctionById(link->GetElementId());
+
+			for (int i = 0; i < junction->GetNumberOfConnections(); i++)
+			{
+				Connection *connection = junction->GetConnectionByIdx(i);
+
+				if (connection->GetIncomingRoad()->GetId() == road1_id)
+				{
+					Road* connecting_road = connection->GetConnectingRoad();
+					RoadLink* exit_link = 0;
+
+					// Found a connecting road - first check if this is the second road
+					if (connecting_road->GetId() == road2_id)
+					{
+						// Check lanes
+						for (int j = 0; j < connection->GetNumberOfLaneLinks(); j++)
+						{
+							if (connection->GetLaneLink(j)->from_ == lane1_id &&
+								connection->GetLaneLink(j)->to_ == lane2_id)
+							{
+								return true;
+							}
+						}
+					}
+					
+					// Then check if it connects to second road
+					if (connection->GetContactPoint() == ContactPointType::CONTACT_POINT_START)
+					{
+						exit_link = connecting_road->GetLink(SUCCESSOR);
+					}
+					else
+					{
+						exit_link = connecting_road->GetLink(PREDECESSOR);
+					}
+
+					if (exit_link->GetElementId() == road2_id)
+					{
+						// Finally check that lanes are connected through the junction
+						// Look at lane section and locate lane connecting both roads
+						// Assume connecting road has only one lane section 
+						lane_section = connecting_road->GetLaneSectionByIdx(0);
+						if (lane_section == 0)
+						{
+							LOG("Error lane section == 0\n");
+							return false;
+						}
+						for (int j = 0; j < lane_section->GetNumberOfLanes(); j++)
+						{
+							Lane *lane = lane_section->GetLaneByIdx(j);
+							LaneLink *lane_link_predecessor = lane->GetLink(PREDECESSOR);
+							LaneLink *lane_link_successor = lane->GetLink(SUCCESSOR);
+							if (lane_link_predecessor == 0 || lane_link_successor == 0)
+							{
+								continue;
+							}
+							if ((connection->GetContactPoint() == ContactPointType::CONTACT_POINT_START &&
+								 lane_link_predecessor->GetId() == lane1_id && 
+								 lane_link_successor->GetId() == lane2_id) ||
+								(connection->GetContactPoint() == ContactPointType::CONTACT_POINT_END &&
+								 lane_link_predecessor->GetId() == lane2_id &&
+								 lane_link_successor->GetId() == lane1_id))
+							{
+								// Found link
+								if (connecting_road_id != 0)
+								{
+									*connecting_road_id = connection->GetConnectingRoad()->GetId();
+								}
+								if (connecting_lane_id != 0)
+								{
+									*connecting_lane_id = lane->GetId();
+								}
+								return true;
+							}
+						}
+					}
+				}
+			}
+		}
+		else
+		{
+			LOG("Error: LinkElementType %d unsupported\n", link->GetElementType());
+		}
+	}
+	
+	LOG("Link not found");
+	
+	return false;
+}
+
+int OpenDrive::CheckConnectedRoad(Road *road, RoadLink *link, ContactPointType expected_contact_point_type, RoadLink *link2)
+{
+	if (link2 == 0)
+	{
+		return -1;
+	}
+
+	if (link2->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_ROAD)
+	{
+		if (link->GetElementId() == road->GetId())
+		{
+			if (link->GetContactPointType() != expected_contact_point_type)
+			{
+				LOG("Found connecting road from other end, but contact point is wrong (expected START, got %s)",
+					ContactPointType2Str(link->GetContactPointType()).c_str());
+				return -1;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int OpenDrive::CheckJunctionConnection(Junction *junction, Connection *connection)
+{
+	if (junction == 0)
+	{
+		return -1;
+	}
+	
+	// Check if junction is referred to from the connected road
+	Road *road = connection->GetConnectingRoad();
+	if (road == 0)
+	{
+		LOG("Error no connecting road");
+		return -1;
+	}
+
+	RoadLink *link[2];
+	link[0] = road->GetLink(LinkType::PREDECESSOR);
+	link[1] = road->GetLink(LinkType::SUCCESSOR);
+	for (int i = 0; i < 2; i++)
+	{
+		if (link[i] != 0)
+		{
+			if (link[i]->GetElementType() != RoadLink::ElementType::ELEMENT_TYPE_ROAD)
+			{
+				LOG("Expected element type ROAD, found %s", link[i]->GetElementType());
+				return -1;
+			}
+			
+			if (link[i]->GetElementId() != connection->GetIncomingRoad()->GetId())
+			{
+				// Check connection from this outgoing road
+				Road *roadc = GetRoadById(link[i]->GetElementId());
+				RoadLink *link2[2];
+				link2[0] = roadc->GetLink(LinkType::PREDECESSOR);
+				link2[1] = roadc->GetLink(LinkType::SUCCESSOR);
+				for (int j = 0; j < 2; j++)
+				{
+					if (link2[j] != 0)
+					{
+						if (link2[j]->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_JUNCTION &&
+							link2[j]->GetElementId() == junction->GetId())
+						{
+							// Now finally find the reverse link 
+							for (int k = 0; k < junction->GetNumberOfConnections(); k++)
+							{
+								if (junction->GetConnectionByIdx(k)->GetIncomingRoad() == roadc)
+								{
+									// Sharing same connecting road?
+									if (junction->GetConnectionByIdx(k)->GetConnectingRoad() == connection->GetConnectingRoad())
+									{
+										return 0;
+									}
+								}
+							}
+							LOG("Warning: Missing reverse connection from road %d to %d via junction %d connecting road %d. Potential issue in the OpenDRIVE file.", 
+								connection->GetIncomingRoad()->GetId(), roadc->GetId(), junction->GetId(), connection->GetConnectingRoad()->GetId());
+						}
+					}
+				}
+			}
+		}
+	}
+
+	return -1;
+}
+
+int OpenDrive::CheckLink(Road *road, RoadLink *link, ContactPointType expected_contact_point_type)
+{
+	// does this connection exist in the other direction?
+	if (link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_ROAD)
+	{
+		Road *connecting_road = GetRoadById(link->GetElementId());
+		if (connecting_road != 0)
+		{
+			if (CheckConnectedRoad(road, link, expected_contact_point_type, connecting_road->GetLink(LinkType::PREDECESSOR)) == 0)
+			{
+				return 0;
+			}
+			else if (CheckConnectedRoad(road, link, expected_contact_point_type, connecting_road->GetLink(LinkType::SUCCESSOR)) == 0)
+			{
+				return 0;
+			}
+			else
+			{
+				LOG("Warning: Reversed road link %d->%d not found. Might be a flaw in the OpenDRIVE description.", road->GetId(), connecting_road->GetId());
+			}
+		}
+	}
+	else if (link->GetElementType() == RoadLink::ElementType::ELEMENT_TYPE_JUNCTION)
+	{
+		Junction *junction = GetJunctionById(link->GetElementId());
+
+		// Check all outgoing connections
+		for (size_t i = 0; i < junction->GetNumberOfConnections(); i++)
+		{
+			Connection *connection = junction->GetConnectionByIdx((int)i);
+
+			if (connection->GetIncomingRoad() == road)
+			{
+				CheckJunctionConnection(junction, connection);
+			}
+		}
+	}
+
+	return 0;
+}
+
+int OpenDrive::CheckConnections()
+{
+	int counter = 0;
+	RoadLink *link;
+
+	for (size_t i = 0; i < road_.size(); i++)
+	{
+		// Check for connections
+		if ((link = road_[i]->GetLink(LinkType::PREDECESSOR)) != 0)
+		{
+			CheckLink(road_[i], link, ContactPointType::CONTACT_POINT_START);
+		}
+		if ((link = road_[i]->GetLink(LinkType::SUCCESSOR)) != 0)
+		{
+			CheckLink(road_[i], link, ContactPointType::CONTACT_POINT_END);
+		}
+	}
+	
+	return counter;
+}
+
+void OpenDrive::Print()
+{
+	LOG("Roads:\n");
+	for (size_t i=0; i<road_.size(); i++)
+	{
+		road_[i]->Print();
+	}
+
+	LOG("junctions\n");
+	for (size_t i=0; i<junction_.size(); i++)
+	{
+		junction_[i]->Print();
+	}
+}
+
+void Position::Init()
+{
+	track_id_ = -1;
+	lane_id_ = 0;
+	s_ = 0.0;
+	s_route_ = 0.0;
+	s_trajectory_ = 0.0;
+	t_ = 0.0;
+	offset_ = 0.0;
+	x_ = 0.0;
+	y_ = 0.0;
+	z_ = 0.0;
+	h_ = 0.0;
+	p_ = 0.0;
+	r_ = 0.0;
+	velX_ = 0.0;
+	velY_ = 0.0;
+	velZ_ = 0.0;
+	accX_ = 0.0;
+	accY_ = 0.0;
+	accZ_ = 0.0;
+	h_rate_ = 0.0;
+	p_rate_ = 0.0;
+	r_rate_ = 0.0;
+	h_acc_ = 0.0;
+	p_acc_ = 0.0;
+	r_acc_ = 0.0;
+	h_offset_ = 0.0;
+	h_road_ = 0.0;
+	h_relative_ = 0.0;
+	curvature_ = 0.0;
+	p_road_ = 0.0;
+	p_relative_ = 0.0;
+	r_road_ = 0.0;
+	r_relative_ = 0.0;
+	rel_pos_ = 0;
+	type_ = PositionType::NORMAL;
+	orientation_type_ = OrientationType::ORIENTATION_ABSOLUTE;
+	snapToLaneTypes_ = Lane::LaneType::LANE_TYPE_ANY_DRIVING;
+	status_ = 0;
+	lockOnLane_ = false;
+
+	z_road_ = 0.0;
+	track_idx_ = -1;
+	geometry_idx_ = -1;
+	lane_section_idx_ = -1;
+	lane_idx_ = -1;
+	elevation_idx_ = -1;
+	super_elevation_idx_ = -1;
+	route_ = 0;
+	trajectory_ = 0;
+}
+
+Position::Position()
+{
+	Init();
+}
+
+Position::Position(int track_id, double s, double t)
+{
+	Init();
+	SetTrackPos(track_id, s, t);
+}
+
+Position::Position(int track_id, int lane_id, double s, double offset)
+{
+	Init();
+	SetLanePos(track_id, lane_id, s, offset);
+}
+
+Position::Position(double x, double y, double z, double h, double p, double r)
+{
+	Init();
+	SetInertiaPos(x, y, z, h, p, r);
+}
+
+Position::Position(double x, double y, double z, double h, double p, double r, bool calculateTrackPosition)
+{
+	Init();
+	SetInertiaPos(x, y, z, h, p, r, calculateTrackPosition);
+}
+
+Position::~Position()
+{
+	
+}
+
+bool Position::LoadOpenDrive(const char *filename)
+{
+	return(GetOpenDrive()->LoadOpenDriveFile(filename));
+}
+
+OpenDrive* Position::GetOpenDrive()
+{
+	static OpenDrive od;
+	return &od; 
+}
+
+bool OpenDrive::CheckLaneOSIRequirement(std::vector<double> x0, std::vector<double> y0, std::vector<double> x1, std::vector<double> y1)
+{
+	double x0_tan_diff, y0_tan_diff, x1_tan_diff, y1_tan_diff;
+	x0_tan_diff = x0[2]-x0[0];
+	y0_tan_diff = y0[2]-y0[0];
+	x1_tan_diff = x1[2]-x1[0];
+	y1_tan_diff = y1[2]-y1[0];
+
+	// Avoiding Zero Denominator in OSI point calculations
+	if(x0_tan_diff == 0) {x0_tan_diff+=0.001; }
+
+	if(y0_tan_diff == 0) {y0_tan_diff+=0.001; }
+
+	if(x1_tan_diff == 0) {x1_tan_diff+=0.001; }
+
+	if(y1_tan_diff == 0) {y1_tan_diff+=0.001; }
+
+	// Creating tangent line around the point (First Point) with given tolerance
+	double k_0 = y0_tan_diff/x0_tan_diff;
+	double m_0 = y0[1] - k_0*x0[1];
+
+	// Creating tangent line around the point (Second Point) with given tolerance 
+	double k_1 = y1_tan_diff/x1_tan_diff;
+	double m_1 = y1[1] - k_1*x1[1];
+
+	// Intersection point of the tangent lines
+	double intersect_tangent_x = (m_0 - m_1) / (k_1 - k_0);
+	double intersect_tangent_y = k_0*intersect_tangent_x + m_0;
+
+	// Creating real line between the First Point and Second Point
+	double k = (y1[1] - y0[1]) / (x1[1] - x0[1]);
+	double m = y0[1] - k*x0[1];
+
+	// The maximum distance can be found between the real line and a tangent line: passing through [u_intersect, y_intersect] with slope "k"
+	// The perpendicular line to the tangent line can be formulated as f(Q) = intersect_tangent_y + (intersect_tangent_x / k) - Q/k
+	// Then the point on the real line which gives maximum distance -> f(Q) = k*Q + m
+	double intersect_x = (intersect_tangent_y + (intersect_tangent_x/k) - m) / (k + 1/k);
+	double intersect_y = k*intersect_x + m;
+	double max_distance = sqrt(pow(intersect_y-intersect_tangent_y,2) + pow(intersect_x-intersect_tangent_x,2));
+	
+	// Max distance can be "nan" when the lane is perfectly straigt and hence k = 0.
+	// In this case, it satisfies OSI_LANE_CALC_REQUIREMENT since it is a perfect line
+	if (max_distance < SE_Env::Inst().GetOSIMaxLateralDeviation() || isnan(max_distance))
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+void OpenDrive::SetLaneOSIPoints()
+{
+	// Initialization
+	Position* pos = new roadmanager::Position();
+	Road *road;
+	LaneSection *lsec;
+	Lane *lane;
+	int number_of_lane_sections, number_of_lanes, counter;
+	double lsec_end;
+	std::vector<OSIPoints::OSIPointStruct> osi_point;
+	std::vector<double> x0, y0, x1, y1;
+	double s0, s1, s1_prev;
+	bool osi_requirement;
+	double max_segment_length = SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+
+	// Looping through each road 
+	for (int i=0; i<road_.size(); i++)
+	{
+		road = road_[i];
+		
+		if (road->GetNumberOfSuperElevations() > 0)
+		{
+			// If road has lateral profile, then increase sampling resolution
+			max_segment_length = 0.1 * SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+		}
+		else
+		{
+			max_segment_length = SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+		}
+
+		// Looping through each lane section
+		number_of_lane_sections = road_[i]->GetNumberOfLaneSections();
+		for (int j=0; j<number_of_lane_sections; j++)
+		{
+			// Get the ending position of the current lane section
+			lsec = road->GetLaneSectionByIdx(j);
+			if (j == number_of_lane_sections-1)
+			{
+				lsec_end = road->GetLength();	
+			}
+			else
+			{
+				lsec_end = road->GetLaneSectionByIdx(j+1)->GetS();
+			}
+			
+			// Starting points of the each lane section for OSI calculations
+			s0 = lsec->GetS();
+			s1 = s0 + OSI_POINT_CALC_STEPSIZE;
+			s1_prev = s0;
+
+			// Looping through each lane
+			number_of_lanes = lsec->GetNumberOfLanes();
+			for (int k=0; k<number_of_lanes; k++)
+			{
+				lane = lsec->GetLaneByIdx(k);
+				counter = 0;
+
+				// Looping through sequential points along the track determined by "OSI_POINT_CALC_STEPSIZE"
+				while(true)
+				{
+					counter++;
+
+					// Make sure we stay within lane section length
+					s1 = MIN(s1, lsec_end - OSI_TANGENT_LINE_TOLERANCE);
+
+					// [XO, YO] = closest position with given (-) tolerance
+					pos->SetLanePos(road->GetId(), lane->GetId(), MAX(0, s0-OSI_TANGENT_LINE_TOLERANCE), 0, j);
+					x0.push_back(pos->GetX());
+					y0.push_back(pos->GetY());
+
+					// [XO, YO] = Real position with no tolerance
+					pos->SetLanePos(road->GetId(), lane->GetId(), s0, 0, j);
+					x0.push_back(pos->GetX());
+					y0.push_back(pos->GetY());
+
+					// Add the starting point of each lane as osi point
+					if (counter == 1)
+					{
+						OSIPoints::OSIPointStruct p = { s0, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+						osi_point.push_back(p);
+					}
+
+					// [XO, YO] = closest position with given (+) tolerance
+					pos->SetLanePos(road->GetId(), lane->GetId(), s0+OSI_TANGENT_LINE_TOLERANCE, 0, j);
+					x0.push_back(pos->GetX());
+					y0.push_back(pos->GetY());
+
+					// [X1, Y1] = closest position with given (-) tolerance																																																																																																												
+					pos->SetLanePos(road->GetId(), lane->GetId(), s1-OSI_TANGENT_LINE_TOLERANCE, 0, j);
+					x1.push_back(pos->GetX());																																	
+					y1.push_back(pos->GetY());
+
+					// [X1, Y1] = Real position with no tolerance																																																								
+					pos->SetLanePos(road->GetId(), lane->GetId(), s1, 0, j);
+					x1.push_back(pos->GetX());
+					y1.push_back(pos->GetY());
+
+					// [X1, Y1] = closest position with given (+) tolerance
+					pos->SetLanePos(road->GetId(), lane->GetId(), s1+OSI_TANGENT_LINE_TOLERANCE, 0, j);
+					x1.push_back(pos->GetX());
+					y1.push_back(pos->GetY());
+
+					// Check OSI Requirement between current given points
+					if (x1[1]-x0[1] != 0 && y1[1]-y0[1] != 0)
+					{
+						osi_requirement = CheckLaneOSIRequirement(x0, y0, x1, y1);
+					}
+					else
+					{
+						osi_requirement = true;
+					}
+					
+					// If requirement is satisfied -> look further points
+					// If requirement is not satisfied:
+						// Assign last unique satisfied point as OSI point
+						// Continue searching from the last satisfied point
+					if ((osi_requirement && s1 - s0 < max_segment_length) || s1 - s0 < 0.1)
+					{
+						s1_prev = s1;
+						s1 = s1 + OSI_POINT_CALC_STEPSIZE;
+
+					}
+					else 
+					{
+						if (s1 - s0 < OSI_POINT_CALC_STEPSIZE + SMALL_NUMBER)
+						{
+							// Back to last point and try smaller step forward
+							s1_prev = s1;
+							s1 = MIN(s0 + (s1 - s0) * 0.5, lsec_end - OSI_TANGENT_LINE_TOLERANCE);
+						}
+						else
+						{
+							s0 = s1_prev;
+							s1_prev = s1;
+							s1 = MIN(s0 + OSI_POINT_CALC_STEPSIZE, lsec_end - OSI_TANGENT_LINE_TOLERANCE);
+
+							if (counter != 1)
+							{
+								pos->SetLanePos(road->GetId(), lane->GetId(), s0, 0, j);
+								OSIPoints::OSIPointStruct p = { s0, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+								osi_point.push_back(p);
+							}
+						}
+					}
+
+					// If the end of the lane reached, assign end of the lane as final OSI point for current lane
+					if (s1 + OSI_TANGENT_LINE_TOLERANCE >= lsec_end)
+					{
+						pos->SetLanePos(road->GetId(), lane->GetId(), MAX(0, lsec_end-SMALL_NUMBER), 0, j);
+						OSIPoints::OSIPointStruct p = { lsec_end, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+						osi_point.push_back(p);
+						break;
+					}
+
+					// Clear x-y collectors for next iteration
+					x0.clear();
+					y0.clear();
+					x1.clear();
+					y1.clear();
+				}
+
+				// Set all collected osi points for the current lane
+				lane->osi_points_.Set(osi_point);
+
+				// Clear osi collectors for next iteration
+				osi_point.clear();
+
+				// Re-assign the starting point of the next lane as the start point of the current lane section for OSI calculations
+				s0 = lsec->GetS();
+				s1 = s0+OSI_POINT_CALC_STEPSIZE;
+				s1_prev = s0;
+			}
+		}
+	}
+}
+
+void OpenDrive::SetLaneBoundaryPoints()
+{
+	// Initialization
+	Position* pos = new roadmanager::Position();
+	Road *road;
+	LaneSection *lsec;
+	Lane *lane;
+	int number_of_lane_sections, number_of_lanes, counter;
+	double lsec_end;
+	std::vector<double> x0, y0, x1, y1;
+	std::vector<OSIPoints::OSIPointStruct> osi_point;
+	double s0, s1, s1_prev;
+	bool osi_requirement; 
+	double max_segment_length = SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+
+	// Looping through each road 
+	for (int i=0; i<road_.size(); i++)
+	{
+		road = road_[i];
+
+		if (road->GetNumberOfSuperElevations() > 0)
+		{
+			// If road has lateral profile, then increase sampling resolution
+			max_segment_length = 0.1 * SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+		}
+		else
+		{
+			max_segment_length = SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+		}
+
+		// Looping through each lane section
+		number_of_lane_sections = road_[i]->GetNumberOfLaneSections();
+		for (int j=0; j<number_of_lane_sections; j++)
+		{
+			// Get the ending position of the current lane section
+			lsec = road->GetLaneSectionByIdx(j);
+			if (j == number_of_lane_sections-1)
+			{
+				lsec_end = road->GetLength();	
+			}
+			else
+			{
+				lsec_end = road->GetLaneSectionByIdx(j+1)->GetS();
+			}
+			
+			// Starting points of the each lane section for OSI calculations
+			s0 = lsec->GetS();
+			s1 = s0 + OSI_POINT_CALC_STEPSIZE;
+			s1_prev = s0;
+
+			// Looping through each lane
+			number_of_lanes = lsec->GetNumberOfLanes();
+			for (int k=0; k<number_of_lanes; k++)
+			{
+				lane = lsec->GetLaneByIdx(k);
+				counter = 0;
+
+				int n_roadmarks = lane->GetNumberOfRoadMarks(); 
+				if (n_roadmarks == 0)
+				{
+					// Looping through sequential points along the track determined by "OSI_POINT_CALC_STEPSIZE"
+					while(true)
+					{
+						counter++;
+
+						// Make sure we stay within lane section length
+						s1 = MIN(s1, lsec_end - OSI_TANGENT_LINE_TOLERANCE);
+
+						// [XO, YO] = closest position with given (-) tolerance
+						pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), MAX(0, s0-OSI_TANGENT_LINE_TOLERANCE), 0, j);
+						x0.push_back(pos->GetX());
+						y0.push_back(pos->GetY());
+
+						// [XO, YO] = Real position with no tolerance
+						pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), s0, 0, j);
+						x0.push_back(pos->GetX());
+						y0.push_back(pos->GetY());
+
+						// Add the starting point of each lane as osi point
+						if (counter == 1)
+						{
+							OSIPoints::OSIPointStruct p = { s0, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+							osi_point.push_back(p);
+						}
+
+						// [XO, YO] = closest position with given (+) tolerance
+						pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), s0+OSI_TANGENT_LINE_TOLERANCE, 0, j);
+						x0.push_back(pos->GetX());
+						y0.push_back(pos->GetY());
+
+						// [X1, Y1] = closest position with given (-) tolerance																																																																																																												
+						pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), s1-OSI_TANGENT_LINE_TOLERANCE, 0, j);
+						x1.push_back(pos->GetX());																																	
+						y1.push_back(pos->GetY());
+
+						// [X1, Y1] = Real position with no tolerance																																																								
+						pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), s1, 0, j);
+						x1.push_back(pos->GetX());
+						y1.push_back(pos->GetY());
+
+						// [X1, Y1] = closest position with given (+) tolerance
+						pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), s1+OSI_TANGENT_LINE_TOLERANCE, 0, j);
+						x1.push_back(pos->GetX());
+						y1.push_back(pos->GetY());
+
+						// Check OSI Requirement between current given points
+						if (x1[1]-x0[1] != 0 && y1[1]-y0[1] != 0)
+						{
+							osi_requirement = CheckLaneOSIRequirement(x0, y0, x1, y1);
+						}
+						else
+						{
+							osi_requirement = true;
+						}
+						
+						// If requirement is satisfied -> look further points
+						// If requirement is not satisfied:
+						// Assign last satisfied point as OSI point
+						// Continue searching from the last satisfied point
+						if (osi_requirement && s1 - s0 < max_segment_length)
+						{
+							s1_prev = s1;
+							s1 = s1 + OSI_POINT_CALC_STEPSIZE;
+
+						}
+						else
+						{
+							s0 = s1_prev;
+							s1_prev = s1;
+							s1 = s0 + OSI_POINT_CALC_STEPSIZE;
+
+							if (counter != 1)
+							{
+								pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), s0, 0, j);
+								OSIPoints::OSIPointStruct p = { s0, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+								osi_point.push_back(p);
+							}
+						}
+
+						// If the end of the lane reached, assign end of the lane as final OSI point for current lane
+						if (s1 + OSI_TANGENT_LINE_TOLERANCE >= lsec_end)
+						{
+							pos->SetLaneBoundaryPos(road->GetId(), lane->GetId(), MAX(0, lsec_end - SMALL_NUMBER), 0, j);
+							OSIPoints::OSIPointStruct p = { lsec_end, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+							osi_point.push_back(p);
+							break;
+						}
+
+						// Clear x-y collectors for next iteration
+						x0.clear();
+						y0.clear();
+						x1.clear();
+						y1.clear();
+					}
+					// Initialization of LaneBoundary class
+					LaneBoundaryOSI * lb = new LaneBoundaryOSI((int)0); 
+					// add the lane boundary class to the lane class and generating the global id 
+					lane->SetLaneBoundary(lb); 
+					//Fills up the osi points in the lane boundary class 
+					lb->osi_points_.Set(osi_point);
+					// Clear osi collectors for next iteration
+					osi_point.clear();
+
+					// Re-assign the starting point of the next lane as the start point of the current lane section for OSI calculations
+					s0 = lsec->GetS();
+					s1 = s0+OSI_POINT_CALC_STEPSIZE;
+					s1_prev = s0;
+				}
+			}
+		}
+	} 
+}
+
+void OpenDrive::SetRoadMarkOSIPoints()
+{
+	// Initialization
+	Position* pos = new roadmanager::Position();
+	Road *road;
+	LaneSection *lsec;
+	Lane *lane;
+	LaneRoadMark *lane_roadMark;
+	LaneRoadMarkType *lane_roadMarkType;
+	LaneRoadMarkTypeLine *lane_roadMarkTypeLine;
+	int number_of_lane_sections, number_of_lanes, number_of_roadmarks, number_of_roadmarktypes, number_of_roadmarklines, counter;
+	double s0, s1, s1_prev, lsec_end, s_roadmark, s_end_roadmark, s_roadmarkline, s_end_roadmarkline;
+	std::vector<double> x0, y0, x1, y1;
+	std::vector<OSIPoints::OSIPointStruct> osi_point;
+	bool osi_requirement;
+	double max_segment_length = SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+
+	// Looping through each road 
+	for (int i=0; i<road_.size(); i++)
+	{
+		road = road_[i];
+
+		if (road->GetNumberOfSuperElevations() > 0)
+		{
+			// If road has lateral profile, then increase sampling resolution
+			max_segment_length = 0.1 * SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+		}
+		else
+		{
+			max_segment_length = SE_Env::Inst().GetOSIMaxLongitudinalDistance();
+		}
+
+		// Looping through each lane section
+		number_of_lane_sections = road_[i]->GetNumberOfLaneSections();
+		for (int j=0; j<number_of_lane_sections; j++)
+		{
+			// Get the ending position of the current lane section
+			lsec = road->GetLaneSectionByIdx(j);
+			if (j == number_of_lane_sections-1)
+			{
+				lsec_end = road->GetLength();
+			}
+			else
+			{
+				lsec_end = road->GetLaneSectionByIdx(j+1)->GetS();
+			}
+
+			// Looping through each lane
+			number_of_lanes = lsec->GetNumberOfLanes();
+			for (int k=0; k<number_of_lanes; k++)
+			{
+				lane = lsec->GetLaneByIdx(k);
+
+				// Looping through each roadMark within the lane
+				number_of_roadmarks = lane->GetNumberOfRoadMarks();
+				if (number_of_roadmarks != 0)
+				{
+					
+					for (int m=0; m<number_of_roadmarks; m++)
+					{
+						lane_roadMark = lane->GetLaneRoadMarkByIdx(m);
+						s_roadmark = lsec->GetS() + lane_roadMark->GetSOffset();
+						if (m == number_of_roadmarks-1)
+						{
+							s_end_roadmark = MAX(0, lsec_end - SMALL_NUMBER);
+						}
+						else
+						{
+							s_end_roadmark = MAX(0, lsec->GetS() + lane->GetLaneRoadMarkByIdx(m+1)->GetSOffset() - SMALL_NUMBER);
+						}
+						
+						// Check the existence of "type" keyword under roadmark
+						number_of_roadmarktypes = lane_roadMark->GetNumberOfRoadMarkTypes();
+						if (number_of_roadmarktypes != 0)
+						{
+							lane_roadMarkType = lane_roadMark->GetLaneRoadMarkTypeByIdx(0);
+							number_of_roadmarklines = lane_roadMarkType->GetNumberOfRoadMarkTypeLines();
+
+							// Looping through each roadmarkline under roadmark
+							for (int n=0; n<number_of_roadmarklines; n++)
+							{
+								lane_roadMarkTypeLine = lane_roadMarkType->GetLaneRoadMarkTypeLineByIdx(n);
+								s_roadmarkline = s_roadmark + lane_roadMarkTypeLine->GetSOffset();
+								if (lane_roadMarkTypeLine != 0)
+								{
+									if (n == number_of_roadmarklines-1)
+									{
+										s_end_roadmarkline = s_end_roadmark;
+									}
+									else
+									{
+										s_end_roadmarkline = lsec->GetS() + lane_roadMarkType->GetLaneRoadMarkTypeLineByIdx(n + 1)->GetSOffset();
+									}
+
+									if (lane_roadMark->GetType() == LaneRoadMark::RoadMarkType::BROKEN)
+									{
+
+										// Setting OSI points for each roadmarkline
+										while(true)
+										{
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s_roadmarkline, 0, j);
+											OSIPoints::OSIPointStruct p = { s_roadmarkline, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+											osi_point.push_back(p);
+
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s_roadmarkline+lane_roadMarkTypeLine->GetLength(), 0, j);
+											p = { s_roadmarkline + lane_roadMarkTypeLine->GetLength(), pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+											osi_point.push_back(p);
+
+											s_roadmarkline += lane_roadMarkTypeLine->GetLength() + lane_roadMarkTypeLine->GetSpace();
+											if (s_roadmarkline < SMALL_NUMBER || s_roadmarkline > s_end_roadmarkline - SMALL_NUMBER)
+											{
+												if (s_roadmarkline < SMALL_NUMBER)
+												{
+													LOG("Roadmark length + space = 0 - ignoring");
+												}
+												break;
+											}
+										}
+									}
+									else if (lane_roadMark->GetType() == LaneRoadMark::RoadMarkType::SOLID)
+									{
+										s0 = s_roadmarkline;
+										s1 = s0+OSI_POINT_CALC_STEPSIZE;
+										s1_prev = s0;
+										counter = 0;
+										
+										while(true)
+										{
+											counter++;
+
+											// Make sure we stay within road length
+											s1 = MIN(s1, road->GetLength() - OSI_TANGENT_LINE_TOLERANCE);
+
+											// [XO, YO] = closest position with given (-) tolerance
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s0-OSI_TANGENT_LINE_TOLERANCE, 0, j);
+											x0.push_back(pos->GetX());
+											y0.push_back(pos->GetY());
+
+											// [XO, YO] = Real position with no tolerance
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s0, 0, j);
+											x0.push_back(pos->GetX());
+											y0.push_back(pos->GetY());
+
+											// Add the starting point of each lane as osi point
+											if (counter == 1)
+											{
+												OSIPoints::OSIPointStruct p = { s0, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+												osi_point.push_back(p);
+											}
+
+											// [XO, YO] = closest position with given (+) tolerance
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s0+OSI_TANGENT_LINE_TOLERANCE, 0, j);
+											x0.push_back(pos->GetX());
+											y0.push_back(pos->GetY());
+
+											// [X1, Y1] = closest position with given (-) tolerance																																																																																																												
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s1-OSI_TANGENT_LINE_TOLERANCE, 0, j);
+											x1.push_back(pos->GetX());																																	
+											y1.push_back(pos->GetY());
+
+											// [X1, Y1] = Real position with no tolerance																																																								
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s1, 0, j);
+											x1.push_back(pos->GetX());
+											y1.push_back(pos->GetY());
+
+											// [X1, Y1] = closest position with given (+) tolerance
+											pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s1+OSI_TANGENT_LINE_TOLERANCE, 0, j);
+											x1.push_back(pos->GetX());
+											y1.push_back(pos->GetY());
+
+											// Check OSI Requirement between current given points
+											osi_requirement = CheckLaneOSIRequirement(x0, y0, x1, y1);
+
+											// If requirement is satisfied -> look further points
+											// If requirement is not satisfied:
+												// Assign last satisfied point as OSI point
+												// Continue searching from the last satisfied point
+											if (osi_requirement && s1 - s0 < max_segment_length)
+											{
+												s1_prev = s1;
+												s1 = s1 + OSI_POINT_CALC_STEPSIZE;
+
+											}
+											else
+											{
+												s0 = s1_prev;
+												s1_prev = s1;
+												s1 = s0 + OSI_POINT_CALC_STEPSIZE;
+
+												if (counter != 1)
+												{
+													pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s0, 0, j);
+													OSIPoints::OSIPointStruct p = { s0, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+													osi_point.push_back(p);
+												}
+											}
+
+											// If the end of the road mark line reached, assign end of the road mark line as final OSI point for current road mark line
+											if (s1 > s_end_roadmarkline - SMALL_NUMBER)
+											{
+												pos->SetRoadMarkPos(road->GetId(), lane->GetId(), m, 0, n, s_end_roadmarkline, 0, j);
+												OSIPoints::OSIPointStruct p = { s_end_roadmarkline, pos->GetX(), pos->GetY(), pos->GetZ(), pos->GetHRoad() };
+												osi_point.push_back(p);
+												break;
+											}
+
+											// Clear x-y collectors for next iteration
+											x0.clear();
+											y0.clear();
+											x1.clear();
+											y1.clear();
+
+										}
+									}
+
+
+									// Set all collected osi points for the current lane rpadmarkline
+									lane_roadMarkTypeLine->osi_points_.Set(osi_point);
+
+									// Clear osi collectors for roadmarks for next iteration
+									osi_point.clear();
+								}
+								else
+								{
+									LOG("LaneRoadMarkTypeLine %d for LaneRoadMarkType for LaneRoadMark %d for lane %d is not defined", n, m, lane->GetId());
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+bool OpenDrive::SetRoadOSI()
+{
+	LOG("Generating OSI lanes");
+	SetLaneOSIPoints();
+	LOG("Generating OSI road marks");
+	SetRoadMarkOSIPoints();
+	LOG("Generating OSI lane boundaries");
+	SetLaneBoundaryPoints();
+	LOG("OSI road features done");
+	return true;
+}
+
+int LaneSection::GetClosestLaneIdx(double s, double t, double &offset, bool noZeroWidth, int laneTypeMask)
+{
+	double min_offset = t;  // Initial offset relates to reference line
+	int candidate_lane_idx = -1;
+
+	for (int i = 0; i < GetNumberOfLanes(); i++)  // Search through all lanes
+	{
+		int lane_id = GetLaneIdByIdx(i);
+		double laneCenterOffset = SIGN(lane_id) * GetCenterOffset(s, lane_id);
+
+		// Only consider lanes with matching lane type
+		if (laneTypeMask & GetLaneById(lane_id)->GetLaneType() && (!noZeroWidth || GetWidth(s, lane_id) > SMALL_NUMBER))
+		{
+			if (candidate_lane_idx == -1 || fabs(t - laneCenterOffset) < fabs(min_offset))
+			{
+				min_offset = t - laneCenterOffset;
+				candidate_lane_idx = i;
+			}
+		}
+	}
+
+	offset = min_offset;
+
+	if (candidate_lane_idx == -1)
+	{
+		// Fall back to reference lane
+		candidate_lane_idx = GetLaneIdxById(0);
+	}
+
+	return candidate_lane_idx;
+}
+
+int Position::GotoClosestDrivingLaneAtCurrentPosition()
+{
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	if (road == 0)
+	{
+		LOG("No road %d", track_idx_);
+		return -1;
+	}
+
+	LaneSection *lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+	if (lane_section == 0)
+	{
+		LOG("No lane section for idx %d - keeping current lane setting\n", lane_section_idx_);
+		return -1;
+	}
+
+	double offset;
+	int lane_idx = lane_section->GetClosestLaneIdx(s_, t_, offset, true);
+
+	if (lane_idx == -1)
+	{
+		LOG("Failed to find a valid drivable lane");
+		return -1;
+	}
+
+	lane_id_ = lane_section->GetLaneIdByIdx(lane_idx);
+
+	offset_ = offset;
+
+	return 0;
+}
+
+void Position::Track2Lane()
+{
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	if (road == 0)
+	{
+		LOG("Position::Track2Lane Error: No road %d\n", track_idx_);
+		return;
+	}
+
+	Geometry *geometry = road->GetGeometry(geometry_idx_);
+	if (geometry == 0)
+	{
+		LOG("Position::Track2Lane Error: No geometry %d\n", geometry_idx_);
+		return;
+	}
+
+	// Find LaneSection according to s, starting from current
+	int lane_section_idx = road->GetLaneSectionIdxByS(s_, lane_section_idx_);
+	LaneSection *lane_section = road->GetLaneSectionByIdx(lane_section_idx);
+	if (lane_section == 0)
+	{
+		LOG("No lane section for idx %d - keeping current lane setting", lane_section_idx_);
+		return;
+	}
+
+	// Find the closest driving lane within the lane section
+	double offset;
+	int lane_idx = lane_section->GetClosestLaneIdx(s_, t_, offset, true, snapToLaneTypes_);
+
+	if (lane_idx == -1)
+	{
+		LOG("Failed find closest lane");
+		return;
+	}
+
+	offset_ = offset;
+	// Update cache indices
+	lane_idx_ = lane_idx;
+	lane_id_ = lane_section->GetLaneIdByIdx(lane_idx_);
+	lane_section_idx_ = lane_section_idx;
+}
+
+int Position::XYZH2TrackPos(double x3, double y3, double z3, double h3, bool alignZPitchRoll)
+{
+	// Overall method:
+	//   1. Iterate over all roads, looking at OSI points of each lane sections center line (lane 0)
+	//   2. Identify line segment (between two OSI points) closest to xyz point
+	//   3. Identify which vertex of the line is closest
+	//   4. Given the normals of lines on each side of the vertex, identify which line the points projects onto
+	//   5. The s value for projected xyz point on the line segment corresponds to the rate 
+	//      between angle from xyz point to projected point and the difference of angle normals
+
+	Road *road, *current_road = 0;
+	Road *roadMin = 0;
+	bool directlyConnected = false;
+	double weight = 0; // Add some resistance to switch from current road, applying a stronger bound to current road
+	double angle = 0;
+	bool search_done = false;
+	double closestS = 0;
+	int j2, k2, jMin=-1, kMin=-1, jMinLocal, kMinLocal;
+	double closestPointDist = INFINITY;
+	bool closestPointInside = false;
+	bool insideCurrentRoad = false;  // current postion projects on current road
+	double headingDiffMin = INFINITY;
+	bool closestPointDirectlyConnected = false;
+
+
+	if (GetOpenDrive()->GetNumOfRoads() == 0)
+	{
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	current_road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+
+	// First step is to identify closest road and OSI line segment
+
+	size_t nrOfRoads;
+	if (route_)
+	{
+		// Route assigned. Iterate over all roads in the route. I.e. check all waypoints road ID.
+		nrOfRoads = route_->waypoint_.size();
+	}
+	else
+	{
+		// Iterate over all roads in the road network
+		nrOfRoads = GetOpenDrive()->GetNumOfRoads();
+	}
+	
+	for (int i = -1; !search_done && i < (int)nrOfRoads; i++)
+	{
+		if (i == -1)
+		{
+			// First check current road (from last known position). 
+			if (current_road)
+			{
+				road = current_road;
+			}
+			else
+			{
+				continue;  // Skip, no current road
+			}
+		}
+		else
+		{
+			if (current_road && i == track_idx_)
+			{
+				continue; // Skip, already checked this one
+			}
+			else
+			{
+				if (route_)
+				{
+					road = GetOpenDrive()->GetRoadById(route_->waypoint_[i]->GetTrackId());
+				}
+				else
+				{
+					road = GetOpenDrive()->GetRoadByIdx(i);
+				}
+			}
+		}
+
+		// Check whether complete road is too far away - then skip to next
+		const double potentialWidthOfRoad = 25;
+		if (PointDistance2D(x3, y3, road->GetGeometry(0)->GetX(), road->GetGeometry(0)->GetY()) -
+			(road->GetLength() + potentialWidthOfRoad) > closestPointDist)  // add potential width of the road
+		{
+			continue;
+		}
+
+		weight = 0;
+		angle = 0;
+
+		// Add resistance to leave current road or directly connected ones 
+		// actual weights are totally unscientific... up to tuning
+		if (road != current_road)
+		{
+			if (current_road && GetOpenDrive()->IsDirectlyConnected(current_road->GetId(), road->GetId(), angle))
+			{
+				directlyConnected = true;
+			}
+			else
+			{
+				weight += 3;  // For non connected roads add additional "penalty" threshold  
+				directlyConnected = false;
+			}
+		}
+
+		for (int j = 0; j < road->GetNumberOfLaneSections(); j++)
+		{
+			OSIPoints* osiPoints = road->GetLaneSectionByIdx(j)->GetLaneById(0)->GetOSIPoints();
+			double sLocal = -1;
+
+			// skip last point on last lane section
+			int numPoints = osiPoints->GetNumOfOSIPoints();
+			if (j == road->GetNumberOfLaneSections() - 1)
+			{
+				numPoints--;
+			}
+
+			// Find closest line or point
+			for (int k = 0; k < numPoints; k++)
+			{
+				double distTmp = 0;
+				OSIPoints::OSIPointStruct& osi_point = osiPoints->GetPoint(k);
+				double z = osi_point.z;
+				bool inside = false;
+
+				// in case of multiple roads with the same reference line, also look at width of the road of relevant side
+				// side of road is determined by cross product of position (relative OSI point) and road heading
+				double cp = GetCrossProduct2D(cos(osi_point.h), sin(osi_point.h), x3 - osi_point.x, y3 - osi_point.y);
+				double width = road->GetWidth(osi_point.s, SIGN(cp), ~Lane::LaneType::LANE_TYPE_NONE);
+
+				double x2, y2, z2, sLocalTmp;
+
+				jMinLocal = j;
+				kMinLocal = k;
+
+				double px, py;
+
+				if (k == osiPoints->GetNumOfOSIPoints() - 1)
+				{
+					// End of lane section, look into next one
+					j2 = MIN(j + 1, road->GetNumberOfLaneSections() - 1);
+					k2 = MIN(1, road->GetLaneSectionByIdx(j2)->GetLaneById(0)->GetOSIPoints()->GetNumOfOSIPoints() - 1);
+				}
+				else
+				{
+					k2 = k + 1;
+					j2 = j;
+				}
+				x2 = road->GetLaneSectionByIdx(j2)->GetLaneById(0)->GetOSIPoints()->GetPoint(k2).x;
+				y2 = road->GetLaneSectionByIdx(j2)->GetLaneById(0)->GetOSIPoints()->GetPoint(k2).y;
+				z2 = road->GetLaneSectionByIdx(j2)->GetLaneById(0)->GetOSIPoints()->GetPoint(k2).z;
+
+				// OSI points is an approximation of actual geometry
+				// Check potential additional area formed by actual normal and OSI points normal
+				// at start and end
+				if ((j == 0 && k == 0) || ((j > 1 || k > 1) && (j == road->GetNumberOfLaneSections() - 1 && k == osiPoints->GetNumOfOSIPoints() - 2)))
+				{
+					double x, y, h;
+					Position pos;
+
+					if (j == 0 && k == 0)
+					{
+						// road startpoint
+						pos.SetLanePos(road->GetId(), 0, 0, 0);
+					}
+					else 
+					{
+						// road endpoint
+						pos.SetLanePos(road->GetId(), 0, road->GetLength(), 0);
+					}
+					x = pos.GetX();
+					y = pos.GetY();
+					h = pos.GetH();
+
+					// Calculate actual normal
+					double n_actual_angle = GetAngleSum(h, M_PI_2);
+					double n_actual_x, n_actual_y;
+					RotateVec2D(1, 0, n_actual_angle, n_actual_x, n_actual_y);
+
+					// Calculate normal of OSI line
+					double h_osi = GetAngleOfVector(x2 - osi_point.x, y2 - osi_point.y);
+					double n_osi_angle = GetAngleSum(h_osi, M_PI_2);
+					double n_osi_x, n_osi_y;
+					RotateVec2D(1, 0, n_osi_angle, n_osi_x, n_osi_y);
+
+					// Calculate vector from road endpoint (first or last) to obj pos
+					double vx = x3 - x;
+					double vy = y3 - y;
+					
+					// make sure normals points same side as point
+					double forward_x = 1.0;
+					double forward_y = 0.0;
+					RotateVec2D(1.0, 0.0, h, forward_x, forward_y);
+					if (GetCrossProduct2D(forward_x, forward_y, vx, vy) < 0)
+					{
+						n_actual_x = -n_actual_x;
+						n_actual_y = -n_actual_y;
+						n_osi_x = -n_osi_x;
+						n_osi_y = -n_osi_y;
+					}
+
+					double cp_actual = GetCrossProduct2D(vx, vy, n_actual_x, n_actual_y);
+					double cp_osi = GetCrossProduct2D(vx, vy, n_osi_x, n_osi_y);
+					
+					if (SIGN(cp_actual) != SIGN(cp_osi))
+					{
+						inside = true;
+						distTmp = GetLengthOfLine2D(vx, vy, 0, 0);
+						jMinLocal = j;
+						kMinLocal = k;
+					}
+
+					//printf("rid %d inside %d j %d k %d cp_actual %.2f cp_osi %.2f\n", road->GetId(), inside, j, k, cp_actual, cp_osi);
+				}
+
+				if (!inside)
+				{
+					// Ok, now look along the OSI lines, between the OSI points along the road centerline
+
+					ProjectPointOnVector2D(x3, y3, osi_point.x, osi_point.y, x2, y2, px, py);
+					distTmp = PointDistance2D(x3, y3, px, py);
+
+					inside = PointInBetweenVectorEndpoints(px, py, osi_point.x, osi_point.y, x2, y2, sLocalTmp);
+					if (!inside && k > 0 && (SIGN(sLocalTmp) != SIGN(sLocal)))
+					{
+						// In between two line segments, or more precisely in the triangle area outside a 
+						// convex vertex corner between two line segments. Consider beeing inside road segment.
+						inside = true;
+					}
+					sLocal = sLocalTmp;
+
+					// Find closest point of the two
+					if (PointSquareDistance2D(x3, y3, osi_point.x, osi_point.y) <
+						PointSquareDistance2D(x3, y3, x2, y2))
+					{
+						jMinLocal = j;
+						kMinLocal = k;
+					}
+					else
+					{
+						jMinLocal = j2;
+						kMinLocal = k2;
+					}
+
+				}
+
+				// subtract width of the road
+				distTmp = distTmp - width;
+				if (distTmp < 0)
+				{
+					// On road - distance is zero, but continue search because
+					// we could be in a junction where roads are overlapping
+					distTmp = 0;
+				}
+
+				if (inside)
+				{
+					z = (1 - sLocal) * osi_point.z + sLocal * z2;
+				}
+				else
+				{
+					// Find combined longitudinal and lateral distance to line endpoint 
+					// sLocal represent now (outside line segment) distance to closest line segment end point
+					distTmp = sqrt(distTmp * distTmp + sLocal * sLocal);
+				}
+				
+				if (fabs(z3 - z) > 2.0)
+				{
+					// Add threshold for considering z - to avoid noise in co-planar distance calculations
+					distTmp += fabs(z3 - z);
+				}
+
+				if (!insideCurrentRoad && road == current_road)
+				{
+					// Register whether current position is on current road
+					// Allow for 2 meter lateral slack outside road edges
+					insideCurrentRoad = inside && distTmp < 2;
+				}
+				else if (insideCurrentRoad)
+				{
+					// Only add weight if position inside current road
+					// longitudinal (end points) and lateral (road width)
+					distTmp += weight;
+				}
+				
+				if (distTmp < closestPointDist + SMALL_NUMBER)
+				{
+					bool directlyConnectedCandidate = false;
+
+					if (directlyConnected && closestPointDirectlyConnected)
+					{
+						// For directly connected roads (junction), we might have options 
+						// among equally close ones, find the one which goes the most straight forward
+						if (fabs(distTmp - closestPointDist) < SMALL_NUMBER)
+						{
+							if (angle < headingDiffMin)
+							{ 
+								directlyConnectedCandidate = true;
+							}
+						}
+					}
+
+					if (directlyConnectedCandidate || distTmp < closestPointDist)
+					{
+						closestPointDist = distTmp;
+						roadMin = road;
+						jMin = jMinLocal;
+						kMin = kMinLocal;
+						closestPointInside = inside;
+						closestPointDirectlyConnected = directlyConnected;
+
+						if (directlyConnected)
+						{
+							headingDiffMin = angle;
+						}
+					}
+				}
+			}
+		}
+	}
+	if (closestPointInside)
+	{
+		status_ &= ~Position::POSITION_STATUS_MODES::POS_STATUS_END_OF_ROAD;
+	}
+	else
+	{
+		status_ |= Position::POSITION_STATUS_MODES::POS_STATUS_END_OF_ROAD;
+	}
+
+	// The closest OSI vertex has been identified
+	// Now, find out exact road s-value based on interpolation of normal angles
+	// for the two lines having the vertex in common 
+
+	if (roadMin == 0)
+	{
+		LOG("Error finding minimum distance\n");
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	if (jMin != -1 && kMin != -1)
+	{
+		// Find out what line the points projects to, starting or ending with closest point?
+		// Do this by comparing the angle to the position with the road normal at found point
+
+		OSIPoints::OSIPointStruct osip_closest, osip_first, osip_second;
+		osip_closest = roadMin->GetLaneSectionByIdx(jMin)->GetLaneById(0)->GetOSIPoints()->GetPoint(kMin);
+
+		double xTangent = cos(osip_closest.h);
+		double yTangent = sin(osip_closest.h);
+		double dotP = GetDotProduct2D(xTangent, yTangent, x3 - osip_closest.x, y3 - osip_closest.y);
+		
+		int jFirst, jSecond, kFirst, kSecond;
+
+		if (dotP > 0)
+		{
+			// Positive dot product means closest OSI point is behind
+			osip_first = osip_closest;
+			jFirst = jMin;
+			kFirst = kMin;
+
+			if (kMin < roadMin->GetLaneSectionByIdx(jMin)->GetLaneById(0)->GetOSIPoints()->GetNumOfOSIPoints() - 1)
+			{
+				jSecond = jMin;
+				kSecond = kMin + 1;
+			}
+			else
+			{
+				if (jMin < roadMin->GetNumberOfLaneSections() - 1)
+				{
+					jSecond = jMin + 1;
+					if (roadMin->GetLaneSectionByIdx(jSecond)->GetLaneById(0)->GetOSIPoints()->GetNumOfOSIPoints() > 1)
+					{
+						kSecond = 1; // Skip first point, it's the same as last in last lane section
+					}
+					else
+					{
+						kSecond = 0; // Only one point available in lane section - don't go further
+					}
+				}
+				else
+				{
+					// Last point
+					jSecond = jMin;
+					kSecond = kMin;
+				}
+			}
+			osip_second = roadMin->GetLaneSectionByIdx(jSecond)->GetLaneById(0)->GetOSIPoints()->GetPoint(kSecond);
+		}
+		else
+		{
+			// Negative dot product means closest OSI point is ahead
+			osip_second = osip_closest;
+			jSecond = jMin;
+			kSecond = kMin;
+
+			if (kMin > 0)
+			{
+				jFirst = jMin;
+				kFirst = kMin - 1;
+			}
+			else
+			{
+				if (jMin > 0)
+				{
+					jFirst = jMin - 1;
+					if (roadMin->GetLaneSectionByIdx(jFirst)->GetLaneById(0)->GetOSIPoints()->GetNumOfOSIPoints() > 1)
+					{
+						// Skip last point, it's the same as first in successor lane section
+						kFirst = roadMin->GetLaneSectionByIdx(jFirst)->GetLaneById(0)->GetOSIPoints()->GetNumOfOSIPoints() - 2; 
+					}
+					else
+					{
+						// Only one point available in lane section - don't go further
+						kFirst = roadMin->GetLaneSectionByIdx(jFirst)->GetLaneById(0)->GetOSIPoints()->GetNumOfOSIPoints() - 1;
+					}
+				}
+				else
+				{
+					// First point
+					jFirst = jMin;
+					kFirst = kMin;
+				}
+			}
+			osip_first = roadMin->GetLaneSectionByIdx(jFirst)->GetLaneById(0)->GetOSIPoints()->GetPoint(kFirst);
+		}
+
+		if (jFirst == jSecond && kFirst == kSecond)  
+		{
+			// Same point
+			closestS = osip_first.s;
+		}
+		else
+		{
+			// Different points
+			double angleBetweenNormals, angleToPosition;
+			double normalIntersectionX, normalIntersectionY;
+			double sNorm = 0;
+
+			// Check for straight line
+			if (fabs(osip_first.h - osip_second.h) < SMALL_NUMBER)
+			{
+				double px, py;
+				ProjectPointOnVector2D(x3, y3, osip_first.x, osip_first.y, osip_second.x, osip_second.y, px, py);
+
+				// Find relative position of projected point on line segment
+				double l1 = GetLengthOfLine2D(osip_first.x, osip_first.y, px, py);
+				double l2 = GetLengthOfLine2D(osip_first.x, osip_first.y, osip_second.x, osip_second.y);
+				sNorm = l1 / l2;
+			}
+			else
+			{
+				// Find normals at end points of line segment
+				double xn0, yn0, xn1, yn1;
+				RotateVec2D(cos(osip_first.h), sin(osip_first.h), M_PI_2, xn0, yn0);
+				RotateVec2D(cos(osip_second.h), sin(osip_second.h), M_PI_2, xn1, yn1);
+
+				// Find intersection of extended normals 
+				GetIntersectionOfTwoLineSegments(
+					osip_first.x, osip_first.y,
+					osip_first.x + xn0, osip_first.y + yn0,
+					osip_second.x, osip_second.y,
+					osip_second.x + xn1, osip_second.y + yn1,
+					normalIntersectionX, normalIntersectionY);
+
+				// Align normal vectors to direction from intersection towards line segment
+				NormalizeVec2D(osip_first.x - normalIntersectionX, osip_first.y - normalIntersectionY, xn0, yn0);
+				NormalizeVec2D(osip_second.x - normalIntersectionX, osip_second.y - normalIntersectionY, xn1, yn1);
+
+				// Find angle between normals
+				angleBetweenNormals = acos(GetDotProduct2D(-xn0, -yn0, -xn1, -yn1));
+
+				// Find angle between the two vectors:
+				// 1. line between normals intersection and the point of query
+				// 2. Normal in the first point of closest line segment (turned around to match direction of first line)
+				double lx = normalIntersectionX - x3;
+				double ly = normalIntersectionY - y3;
+				double lLength = sqrt(lx * lx + ly * ly);
+				angleToPosition = acos(GetDotProduct2D(-xn0, -yn0, lx / lLength, ly / lLength));
+
+				// Finally calculate interpolation factor 
+				sNorm = angleToPosition / angleBetweenNormals;
+
+				//printf("road_id %d jMin %d kMin %d lx %.2f ly %.2f angle0 %.2f angle1 %.2f normalIntersectionX %.2f normalIntersectionY %.2f sNorm %.2f\n",
+				//	roadMin->GetId(), jMin, kMin, lx, ly, angleToPosition, angleBetweenNormals, normalIntersectionX, normalIntersectionY, sNorm);
+			}
+
+			closestS = (1 - sNorm) * osip_first.s + sNorm * osip_second.s;
+			closestS = CLAMP(closestS, 0, roadMin->GetLength());
+		}
+	}
+	else
+	{
+		LOG("Unexpected: No closest OSI point found!");
+	}
+
+	double fixedLaneOffset = 0;
+	int fixedLaneId = 0;
+	if (lockOnLane_)
+	{
+		// Register lateral position of previous lane
+		LaneSection* lsec = current_road->GetLaneSectionByIdx(lane_section_idx_);
+		if (lsec)
+		{
+			fixedLaneOffset = SIGN(lane_id_) * lsec->GetCenterOffset(s_, lane_id_);
+
+			// Now find cloest lane at that lateral position, at updated s value
+			double laneOffset;
+			int lane_idx = lsec->GetClosestLaneIdx(closestS, fixedLaneOffset, laneOffset, true, Lane::LaneType::LANE_TYPE_ANY_DRIVING);
+			fixedLaneId = lsec->GetLaneIdByIdx(lane_idx);
+		}
+	}
+
+	// Set position exact on center line
+	int retvalue = SetTrackPos(roadMin->GetId(), closestS, 0, UpdateTrackPosMode::UPDATE_XYZ);
+	double xCenterLine = x_;
+	double yCenterLine = y_;
+
+	// Find out actual lateral position
+	double latOffset = PointToLineDistance2DSigned(
+		x3, y3, xCenterLine, yCenterLine,
+		xCenterLine + cos(GetHRoad()), yCenterLine + sin(GetHRoad()));
+
+	// Update lateral offsets
+	if (lockOnLane_)
+	{
+		SetLanePos(roadMin->GetId(), fixedLaneId, closestS, latOffset - fixedLaneOffset, UpdateTrackPosMode::UPDATE_NOT_XYZH);
+	}
+	else
+	{
+		SetTrackPos(roadMin->GetId(), closestS, latOffset, UpdateTrackPosMode::UPDATE_NOT_XYZH);
+	}
+
+	static int rid = 0;
+	if (roadMin->GetId() != rid)
+	{
+		rid = roadMin->GetId();
+	}
+
+	// Set specified position and heading
+	SetX(x3);
+	SetY(y3);
+	SetHeading(h3);
+
+	EvaluateRoadZPitchRoll(alignZPitchRoll);
+
+	// If on a route, calculate corresponding route position
+	if (route_)
+	{
+		CalcRoutePosition();
+	}
+
+	return retvalue;
+}
+	
+
+bool Position::EvaluateRoadZPitchRoll(bool alignZPitchRoll)
+{
+	if (track_id_ < 0)
+	{
+		return false;
+	}
+	bool ret_value = GetRoadById(track_id_)->GetZAndPitchByS(s_, &z_road_, &p_road_, &elevation_idx_);
+	ret_value &= GetRoadById(track_id_)->UpdateZAndRollBySAndT(s_, t_, &z_road_, &r_road_, &super_elevation_idx_);
+
+	if (alignZPitchRoll)
+	{
+		SetZ(z_road_);
+
+		// Adjust pitch and roll of road in driving direction
+		if (GetHRelative() > M_PI_2 && GetHRelative() < 3 * M_PI_2)
+		{
+			p_road_ *= -1;
+			r_road_ *= -1;
+		}
+
+		SetPitch(p_relative_ + p_road_);
+		SetRoll(r_relative_ + r_road_);
+	}
+
+	return ret_value;
+}
+
+int Position::Track2XYZ(bool alignH)
+{
+	if (GetOpenDrive()->GetNumOfRoads() == 0)
+	{
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	if (road == 0)
+	{
+		LOG("Position::Track2XYZ Error: No road %d\n", track_idx_);
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	Geometry *geometry = road->GetGeometry(geometry_idx_);
+	if (geometry == 0)
+	{
+		LOG("Position::Track2XYZ Error: No geometry %d\n", geometry_idx_);
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	geometry->EvaluateDS(s_ - geometry->GetS(), &x_, &y_, &h_road_);
+	
+	// Consider lateral t position, perpendicular to track heading
+	double x_local = (t_ + road->GetLaneOffset(s_)) * cos(h_road_ + M_PI_2);
+	double y_local = (t_ + road->GetLaneOffset(s_)) * sin(h_road_ + M_PI_2);
+
+	h_road_ += atan(road->GetLaneOffsetPrim(s_)) + h_offset_;
+	h_road_ = GetAngleInInterval2PI(h_road_);
+
+	if (alignH)
+	{
+		// Update heading, taking relative heading into account
+		h_ = GetAngleInInterval2PI(h_road_ + h_relative_);
+	}
+	else
+	{
+		// road heading might have changed - update h_relative
+		h_relative_ = GetAngleInInterval2PI(h_ - h_road_);
+	}
+
+	x_ += x_local;
+	y_ += y_local;
+
+	// z = Elevation 
+	EvaluateRoadZPitchRoll(true);
+
+	return ErrorCode::ERROR_NO_ERROR;
+}
+
+void Position::LaneBoundary2Track()
+{
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	t_ = 0;
+
+	if (road != 0 && road->GetNumberOfLaneSections() > 0)
+	{
+		LaneSection *lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+		if (lane_section != 0 && lane_id_ !=0)
+		{
+			t_ = offset_ + lane_section->GetOuterOffset(s_, lane_id_) * (lane_id_ < 0 ? -1 : 1);
+			h_offset_ = lane_section->GetOuterOffsetHeading(s_, lane_id_) * (lane_id_ < 0 ? -1 : 1);
+		}
+	}
+}
+
+void Position::Lane2Track()
+{
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	t_ = 0;
+
+	if (road != 0 && road->GetNumberOfLaneSections() > 0)
+	{
+		LaneSection *lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+		if (lane_section != 0)
+		{
+			t_ = offset_ + lane_section->GetCenterOffset(s_, lane_id_) * (lane_id_ < 0 ? -1 : 1);
+			h_offset_ = lane_section->GetCenterOffsetHeading(s_, lane_id_) * (lane_id_ < 0 ? -1 : 1);
+		}
+	}
+}
+
+void Position::RoadMark2Track()
+{
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	t_ = 0;
+
+	if (road != 0 && road->GetNumberOfLaneSections() > 0)
+	{
+		LaneSection *lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+		if (lane_section != 0 && lane_id_ !=0)
+		{
+			t_ = offset_ + lane_section->GetOuterOffset(s_, lane_id_) * (lane_id_ < 0 ? -1 : 1);
+			h_offset_ = lane_section->GetOuterOffsetHeading(s_, lane_id_) * (lane_id_ < 0 ? -1 : 1);
+		}
+
+		Lane *lane = lane_section->GetLaneByIdx(lane_idx_);
+		LaneRoadMark *lane_roadmark = lane->GetLaneRoadMarkByIdx(roadmark_idx_);
+		LaneRoadMarkType *lane_roadmarktype = lane_roadmark->GetLaneRoadMarkTypeByIdx(roadmarktype_idx_);
+		LaneRoadMarkTypeLine *lane_roadmarktypeline = lane_roadmarktype->GetLaneRoadMarkTypeLineByIdx(roadmarkline_idx_);
+		
+		if (lane_roadmarktypeline != 0)
+		{
+			t_ = t_ + lane_roadmarktypeline->GetTOffset();
+		}
+	}
+}
+
+void Position::XYZ2Track(bool alignZAndPitch)
+{
+	XYZH2TrackPos(x_, y_, z_, h_, alignZAndPitch);
+}
+
+int Position::SetLongitudinalTrackPos(int track_id, double s)
+{
+	Road *road;
+
+	if (GetOpenDrive()->GetNumOfRoads() == 0)
+	{
+		return ErrorCode::ERROR_GENERIC;
+	}
+	
+	if ((road = GetOpenDrive()->GetRoadById(track_id)) == 0)
+	{
+		LOG("Position::Set Error: track %d not found\n", track_id);
+		
+		// Just hard code values and return
+		track_id_ = track_id;
+		s_ = s;
+
+		return ErrorCode::ERROR_GENERIC;
+	}
+	if (track_id != track_id_)
+	{
+		// update internal track and geometry indices
+		track_id_ = track_id;
+		track_idx_ = GetOpenDrive()->GetTrackIdxById(track_id);
+		geometry_idx_ = 0;
+		elevation_idx_ = 0;
+		super_elevation_idx_ = 0;
+		lane_section_idx_ = 0;
+		lane_id_ = 0;
+		lane_idx_ = 0;
+	}
+
+
+	Geometry *geometry = road->GetGeometry(geometry_idx_);
+	// check if still on same geometry
+	if (s > geometry->GetS() + geometry->GetLength())
+	{
+		while (s > geometry->GetS() + geometry->GetLength() && geometry_idx_ < road->GetNumberOfGeometries() - 1)
+		{
+			// Move to next geometry
+			geometry = road->GetGeometry(++geometry_idx_);
+		}
+	}
+	else if (s < geometry->GetS())
+	{
+		while (s < geometry->GetS() && geometry_idx_ > 0)
+		{
+			// Move to previous geometry
+			geometry = road->GetGeometry(--geometry_idx_);
+		}
+	}
+
+
+	if (s > road->GetLength())
+	{
+		if (s > road->GetLength() + SMALL_NUMBER)
+		{
+			LOG("Position::Set Warning: s (%.2f) too large, track %d only %.2f m long\n", s, track_id_, road->GetLength());
+		}
+		s_ = road->GetLength();
+		status_ |= POS_STATUS_END_OF_ROAD;
+		return ErrorCode::ERROR_END_OF_ROAD;
+	}
+	else
+	{
+		s_ = s;
+	}
+
+	if (s < SMALL_NUMBER || s > road->GetLength() - SMALL_NUMBER)
+	{
+		status_ |= POS_STATUS_END_OF_ROAD;
+	}
+	else
+	{
+		status_ &= ~POS_STATUS_END_OF_ROAD;
+	}
+
+	return 0;
+}
+
+int Position::SetTrackPos(int track_id, double s, double t, UpdateTrackPosMode updateMode)
+{
+	int retvalue = SetLongitudinalTrackPos(track_id, s);
+
+	t_ = t;
+	Track2Lane();
+	if (updateMode == UpdateTrackPosMode::UPDATE_XYZ)
+	{
+		Track2XYZ(false);
+	}
+	else if (updateMode == UpdateTrackPosMode::UPDATE_XYZH)
+	{
+		Track2XYZ(true);
+	}
+
+	return retvalue;
+}
+
+void Position::ForceLaneId(int lane_id)
+{
+	if (lane_idx_ < 0 || lane_section_idx_ < 0)
+	{
+		return;
+	}
+	// find out lateral distance between current and target lane
+	Road *road = GetRoadById(GetTrackId());
+
+	double lat_dist = road->GetLaneSectionByIdx(lane_section_idx_)->GetOffsetBetweenLanes(lane_id_, lane_id, GetS());
+
+	lane_id_ = lane_id;
+	offset_ -= lat_dist;
+}
+
+static std::string LinkType2Str(LinkType type)
+{
+	if (type == LinkType::PREDECESSOR)
+	{
+		return "PREDECESSOR";
+	}
+	else if (type == LinkType::SUCCESSOR)
+	{
+		return "SUCCESSOR";
+	}
+	else if (type == LinkType::NONE)
+	{
+		return "NONE";
+	}
+	else if (type == LinkType::UNKNOWN)
+	{
+		return "UNKNOWN";
+	}
+	else
+	{
+		return "UNDEFINED";
+	}
+}
+
+std::string OpenDrive::ContactPointType2Str(ContactPointType type)
+{
+	if (type == ContactPointType::CONTACT_POINT_START)
+	{
+		return "PREDECESSOR";
+	}
+	else if (type == ContactPointType::CONTACT_POINT_END)
+	{
+		return "SUCCESSOR";
+	}
+	else if (type == ContactPointType::CONTACT_POINT_NONE)
+	{
+		return "NONE";
+	}
+	else if (type == ContactPointType::CONTACT_POINT_UNKNOWN)
+	{
+		return "UNKNOWN";
+	}
+	else
+	{
+		return "UNDEFINED";
+	}
+}
+
+std::string OpenDrive::ElementType2Str(RoadLink::ElementType type)
+{
+	if (type == RoadLink::ElementType::ELEMENT_TYPE_JUNCTION)
+	{
+		return "JUNCTION";
+	}
+	else if (type == RoadLink::ElementType::ELEMENT_TYPE_ROAD)
+	{
+		return "ROAD";
+	}
+	else if (type == RoadLink::ElementType::ELEMENT_TYPE_UNKNOWN)
+	{
+		return "UNKNOWN";
+	}
+	else
+	{
+		return "UNDEFINED";
+	}
+}
+
+int Position::MoveToConnectingRoad(RoadLink *road_link, ContactPointType &contact_point_type, Junction::JunctionStrategyType strategy)
+{
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	Road *next_road = 0;
+	LaneSection *lane_section;
+	Lane *lane;
+	int new_lane_id = 0;
+
+	if (road == 0)
+	{
+		LOG("Invalid road id %d\n", road->GetId());
+		return -1;
+	}
+
+	if (road_link->GetElementId() == -1)
+	{
+		LOG("No connecting road or junction at rid %d link_type %s", road->GetId(), LinkType2Str(road_link->GetType()).c_str());
+		return -1;
+	}
+	
+	// Get lane info from current road
+	lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+	if (lane_section == 0)
+	{
+		LOG("No lane section rid %d ls_idx %d link_type  %s", road->GetId(), lane_section_idx_, LinkType2Str(road_link->GetType()).c_str());
+		return -1;
+	}
+
+	lane = lane_section->GetLaneByIdx(lane_idx_);
+	if (lane == 0)
+	{
+		LOG("No lane rid %d lidx %d nlanes %d link_type %s lsecidx %d\n", 
+			road->GetId(), lane_idx_, lane_section->GetNumberOfLanes(), LinkType2Str(road_link->GetType()).c_str(), lane_section_idx_);
+		return -1;
+	}
+	
+	if (road_link->GetElementType() == RoadLink::ELEMENT_TYPE_ROAD)
+	{
+		LaneLink *lane_link = lane->GetLink(road_link->GetType());
+		if (lane_link != 0)
+		{
+			new_lane_id = lane->GetLink(road_link->GetType())->GetId();
+			if (new_lane_id == 0)
+			{
+				LOG("Road+ new lane id %d\n", new_lane_id);
+			}
+		}
+		else
+		{
+			//LOG("No lane link from rid %d lid %d to rid %d", GetTrackId(), GetLaneId(), road_link->GetElementId());
+		}
+		contact_point_type = road_link->GetContactPointType();
+		next_road = GetOpenDrive()->GetRoadById(road_link->GetElementId());
+	}
+	else if (road_link->GetElementType() == RoadLink::ELEMENT_TYPE_JUNCTION)
+	{
+		Junction *junction = GetOpenDrive()->GetJunctionById(road_link->GetElementId());
+
+		if (junction == 0)
+		{
+			LOG("Error: junction %d not existing\n", road_link->GetElementType());
+			return -1;
+		}
+	
+		int connection_idx = 0;
+		int n_connections = junction->GetNumberOfRoadConnections(road->GetId(), lane->GetId());
+
+		if (n_connections == 0)
+		{
+//			LOG("No connections from road id %d lane id %d in junction %d", road->GetId(), lane->GetId(), junction->GetId());
+			return -1;
+		}
+		else if (n_connections == 1)
+		{
+			connection_idx = 0;
+		}
+		else
+		{
+			// find valid connecting road, if multiple choices choose either most straight one OR by random
+			if (strategy == Junction::JunctionStrategyType::STRAIGHT)
+			{
+				// Find the straighest link
+				int best_road_index = 0;
+				double min_heading_diff = 1E10; // set huge number
+				for (int i = 0; i < n_connections; i++)
+				{
+					LaneRoadLaneConnection lane_road_lane_connection = 
+						junction->GetRoadConnectionByIdx(road->GetId(), lane->GetId(), i, snapToLaneTypes_);
+					next_road = GetOpenDrive()->GetRoadById(lane_road_lane_connection.GetConnectingRoadId()); 
+
+					Position test_pos;
+					if (lane_road_lane_connection.contact_point_ == CONTACT_POINT_START)
+					{
+						// Inspect heading at the connecting road
+						test_pos.SetLanePos(next_road->GetId(), new_lane_id, next_road->GetLength(), 0);
+					}
+					else if (lane_road_lane_connection.contact_point_ == CONTACT_POINT_END)
+					{
+						test_pos.SetLanePos(next_road->GetId(), new_lane_id, 0, 0);
+					}
+					else
+					{
+						LOG("Unexpected contact point type: %d", road_link->GetContactPointType());
+					}
+
+					// Transform angle into a comparable format
+
+					double heading_diff = GetAbsAngleDifference(test_pos.GetHRoad(), GetHRoad());
+					if (heading_diff > M_PI / 2)
+					{
+						heading_diff = fabs(heading_diff - M_PI);  // don't care of driving direction here
+					}
+
+					if (heading_diff < min_heading_diff)
+					{
+						min_heading_diff = heading_diff;
+						best_road_index = i;
+					}
+				}
+				connection_idx = best_road_index;
+			}
+			else if (strategy == Junction::JunctionStrategyType::RANDOM)
+			{
+				connection_idx = (int)(n_connections * (double)mt_rand() / mt_rand.max());
+			}
+		}
+
+		LaneRoadLaneConnection lane_road_lane_connection = junction->GetRoadConnectionByIdx(road->GetId(), lane->GetId(), connection_idx, snapToLaneTypes_);
+		contact_point_type = lane_road_lane_connection.contact_point_;
+
+		new_lane_id = lane_road_lane_connection.GetConnectinglaneId();
+		next_road = GetOpenDrive()->GetRoadById(lane_road_lane_connection.GetConnectingRoadId());
+	}
+
+	if (next_road == 0)
+	{
+		LOG("No next road\n");
+		return -1;
+	}
+
+	if (new_lane_id == 0)
+	{
+		LOG("No connection from rid %d lid %d -> rid %d eltype %d - try moving to closest lane\n", 
+			road->GetId(), lane->GetId(), road_link->GetElementId(), road_link->GetElementType());
+
+		// Find closest lane on new road - by convert to track pos and then set lane offset = 0
+		if (road_link->GetContactPointType() == CONTACT_POINT_START)
+		{
+			SetTrackPos(next_road->GetId(), 0, GetT(), UpdateTrackPosMode::UPDATE_NOT_XYZH);
+		}
+		else if (road_link->GetContactPointType() == CONTACT_POINT_END)
+		{
+			SetTrackPos(next_road->GetId(), next_road->GetLength(), GetT(), UpdateTrackPosMode::UPDATE_NOT_XYZH);
+		}
+		offset_ = 0;
+
+		return 0;
+	}
+
+	double new_offset = offset_;
+	if ((road_link->GetType() == LinkType::PREDECESSOR && contact_point_type == ContactPointType::CONTACT_POINT_START) ||
+		(road_link->GetType() == LinkType::SUCCESSOR && contact_point_type == ContactPointType::CONTACT_POINT_END))
+	{
+		h_relative_ = GetAngleSum(h_relative_, M_PI);
+		new_offset = -offset_;
+	}
+
+	// Find out if connecting to start or end of new road
+	if (road_link->GetContactPointType() == CONTACT_POINT_START)
+	{
+		// Specify first (0) lane section
+		SetLanePos(next_road->GetId(), new_lane_id, 0, new_offset, 0);
+	}
+	else if (road_link->GetContactPointType() == CONTACT_POINT_END)
+	{
+		// Find out and specify last lane section
+		SetLanePos(next_road->GetId(), new_lane_id, next_road->GetLength(), new_offset, GetRoadById(road_link->GetElementId())->GetNumberOfLaneSections()-1);
+	}
+	else if (road_link->GetContactPointType() == CONTACT_POINT_NONE && road_link->GetElementType() == RoadLink::ELEMENT_TYPE_JUNCTION)
+	{
+		if (contact_point_type == CONTACT_POINT_START)
+		{
+			SetLanePos(next_road->GetId(), new_lane_id, 0, new_offset);
+		}
+		else if (contact_point_type == CONTACT_POINT_END)
+		{
+			SetLanePos(next_road->GetId(), new_lane_id, next_road->GetLength(), new_offset);
+		}
+		else
+		{
+			LOG("Unexpected contact point: %d\n", contact_point_type);
+		}
+	}
+	else
+	{
+		LOG("Unsupported contact point type %d\n", road_link->GetContactPointType());
+		return -1;
+	}
+
+	return 0;
+}
+
+int Position::MoveAlongS(double ds, double dLaneOffset, Junction::JunctionStrategyType strategy)
+{
+	RoadLink *link;
+	double ds_signed = ds;
+	int max_links = 8;  // limit lookahead through junctions/links 
+	ContactPointType contact_point_type;
+
+	if (type_ == PositionType::RELATIVE_LANE)
+	{
+		// Create a temporary position to evaluate in relative lane coordinates
+		Position pos = *this->rel_pos_;
+
+		// First move position along s
+		pos.MoveAlongS(this->s_);
+		
+		// Then move laterally
+		pos.SetLanePos(pos.track_id_, pos.lane_id_ + this->lane_id_, pos.s_, pos.offset_ + this->offset_);
+
+		this->x_ = pos.x_;
+		this->y_ = pos.y_;
+		this->z_ = pos.z_;
+		this->h_ = pos.h_;
+		this->p_ = pos.p_;
+		this->r_ = pos.r_;
+
+		return 0;
+	}
+
+	if (GetOpenDrive()->GetNumOfRoads() == 0 || track_idx_ < 0)
+	{
+		// No roads available or current track undefined
+		return 0;
+	}
+
+	double s_stop = 0;
+	ds_signed = (GetLaneId() > 0 ? -1 : 1) * ds; // adjust sign of ds according to lane direction - right lane is < 0 in road dir
+	double signed_dLaneOffset = dLaneOffset;
+	
+	// move from road to road until ds-value is within road length or maximum of connections has been crossed
+	for (int i = 0; i < max_links; i++)
+	{
+		if (s_ + ds_signed > GetOpenDrive()->GetRoadByIdx(track_idx_)->GetLength())
+		{
+			// Calculate remaining s-value once we moved to the connected road
+			ds_signed = s_ + ds_signed - GetOpenDrive()->GetRoadByIdx(track_idx_)->GetLength();
+			link = GetOpenDrive()->GetRoadByIdx(track_idx_)->GetLink(SUCCESSOR);
+
+			// register s-value at end of the road, to be used in case of bad connection
+			s_stop = GetOpenDrive()->GetRoadByIdx(track_idx_)->GetLength();
+		}
+		else if (s_ + ds_signed < 0)
+		{
+			// Calculate remaining s-value once we moved to the connected road
+			ds_signed = s_ + ds_signed;
+			link = GetOpenDrive()->GetRoadByIdx(track_idx_)->GetLink(PREDECESSOR);
+
+			// register s-value at end of the road, to be used in case of bad connection
+			s_stop = 0;
+		}
+		else  // New position is within current track (road)
+		{
+			break;
+		}
+
+		// If link is OK then move to the start- or endpoint of the connected road, depending on contact point
+		if (!link || link->GetElementId() == -1 || MoveToConnectingRoad(link, contact_point_type, strategy) != 0)
+		{
+			// Failed to find a connection, stay at end of current road
+			SetLanePos(track_id_, lane_id_, s_stop, offset_);
+
+			status_ |= POS_STATUS_END_OF_ROAD;
+			return ErrorCode::ERROR_END_OF_ROAD;
+		}
+
+		// Adjust sign of ds based on connection point
+		if (contact_point_type == ContactPointType::CONTACT_POINT_END)
+		{
+			ds_signed = -fabs(ds_signed);
+			signed_dLaneOffset = -dLaneOffset;
+		}
+		else 
+		{
+			ds_signed = fabs(ds_signed);
+			signed_dLaneOffset = dLaneOffset;
+		}
+	}
+
+	// Finally, update the position with the adjusted s and offset values
+	SetLanePos(track_id_, lane_id_, s_ + ds_signed, offset_ + signed_dLaneOffset);
+
+	// Check if lane has narrowed down to zero width
+	Road* road = GetOpenDrive()->GetRoadById(track_id_);
+	LaneInfo li = road->GetLaneInfoByS(GetS(), lane_section_idx_, lane_id_, snapToLaneTypes_);
+	if (road->GetLaneWidthByS(GetS(), li.lane_id_) < SMALL_NUMBER)
+	{
+		double offset = 0;
+		int old_lane_id = lane_id_;
+		int new_lane_idx = road->GetLaneSectionByIdx(li.lane_section_idx_)->GetClosestLaneIdx(GetS(), GetT(), offset, true, snapToLaneTypes_);
+		int new_lane_id = road->GetLaneSectionByIdx(li.lane_section_idx_)->GetLaneByIdx(new_lane_idx)->GetId();
+		SetLanePos(track_id_, new_lane_id, GetS(), 0);
+		LOG("Lane %d on road %d is or became zero width, moved to closest available lane: %d", road->GetId(), old_lane_id, GetLaneId());
+	}
+
+	if (s_ < SMALL_NUMBER || s_ > road->GetLength() - SMALL_NUMBER)
+	{
+		status_ |= POS_STATUS_END_OF_ROAD;
+	}
+	else
+	{
+		status_ &= ~POS_STATUS_END_OF_ROAD;
+	}
+
+	return 0;
+}
+
+int Position::SetLanePos(int track_id, int lane_id, double s, double offset, int lane_section_idx)
+{
+	offset_ = offset;
+	int retvalue;
+	
+	if ((retvalue = SetLongitudinalTrackPos(track_id, s)) != ErrorCode::ERROR_NO_ERROR)
+	{
+		lane_id_ = lane_id;
+		offset_ = offset;
+		return retvalue;
+	}
+
+	Road *road = GetOpenDrive()->GetRoadById(track_id);
+	if (road == 0)
+	{
+		LOG("Position::Set Error: track %d not available\n", track_id);
+		lane_id_ = lane_id;
+		offset_ = offset;
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	if (lane_id != lane_id_ && lane_section_idx == -1)
+	{
+		// New lane ID might indicate a discreet jump to a new, distant position, reset lane section, if not specified in func parameter)
+		lane_section_idx = road->GetLaneSectionIdxByS(s);
+	}
+
+	LaneSection *lane_section = 0;
+	if (lane_section_idx > -1)  // If lane section was specified or reset
+	{
+		lane_section_idx_ = lane_section_idx;
+		lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+		lane_id_ = lane_id;
+	}
+	else  // Find LaneSection and info according to s
+	{
+		LaneInfo lane_info = road->GetLaneInfoByS(s_, lane_section_idx_, lane_id_, snapToLaneTypes_);
+		lane_section_idx_ = lane_info.lane_section_idx_;
+		lane_id_ = lane_info.lane_id_;
+
+		lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+	}
+
+	if (lane_section != 0)
+	{
+		lane_idx_ = lane_section->GetLaneIdxById(lane_id_);
+		if (lane_idx_ == -1)
+		{
+			LOG("lane_idx %d fail for lane id %d\n", lane_idx_, lane_id_);
+			lane_idx_ = 0;
+		}
+	}
+	else
+	{
+		LOG("Position::Set (lanepos) Error - lanesection NULL lsidx %d rid %d lid %d\n",
+			lane_section_idx_, road->GetId(), lane_id_);
+	}
+
+	Lane2Track();
+	Track2XYZ();
+
+	return 0;
+}
+
+void Position::SetLaneBoundaryPos(int track_id, int lane_id, double s, double offset, int lane_section_idx)
+{
+	offset_ = offset;
+	int old_lane_id = lane_id_;
+	int old_track_id = track_id_;
+	int retval;
+
+	if ((retval = SetLongitudinalTrackPos(track_id, s)) != 0)
+	{
+		lane_id_ = lane_id;
+		offset_ = offset;
+		return;
+	}
+
+	Road *road = GetOpenDrive()->GetRoadById(track_id);
+	if (road == 0)
+	{
+		LOG("Position::Set Error: track %d not available\n", track_id);
+		lane_id_ = lane_id;
+		offset_ = offset;
+		return;
+	}
+
+	if (lane_id != lane_id_ && lane_section_idx == -1)
+	{
+		// New lane ID might indicate a discreet jump to a new, distant position, reset lane section, if not specified in func parameter)
+		lane_section_idx = road->GetLaneSectionIdxByS(s);
+	}
+
+	LaneSection *lane_section = 0;
+	if (lane_section_idx > -1)  // If lane section was specified or reset
+	{
+		lane_section_idx_ = lane_section_idx;
+		lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+		lane_id_ = lane_id;
+	}
+	else  // Find LaneSection and info according to s
+	{
+		LaneInfo lane_info = road->GetLaneInfoByS(s_, lane_section_idx_, lane_id_, snapToLaneTypes_);
+		lane_section_idx_ = lane_info.lane_section_idx_;
+		lane_id_ = lane_info.lane_id_;
+		
+		lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+	}
+
+	if (lane_section != 0)
+	{
+		lane_idx_ = lane_section->GetLaneIdxById(lane_id_);
+		if (lane_idx_ == -1)
+		{
+			LOG("lane_idx %d fail for lane id %d\n", lane_idx_, lane_id_);
+			lane_idx_ = 0;
+		}
+	}
+	else
+	{
+		LOG("Position::Set (lanepos) Error - lanesection NULL lsidx %d rid %d lid %d\n",
+			lane_section_idx_, road->GetId(), lane_id_);
+	}
+
+	// Check road direction when on new track 
+	if (old_lane_id != 0 && lane_id_ != 0 && track_id_ != old_track_id && SIGN(lane_id_) != SIGN(old_lane_id))
+	{
+		h_relative_ = GetAngleSum(h_relative_, M_PI);
+	}
+
+	// If moved over to opposite driving direction, then turn relative heading 180 degrees
+	//if (old_lane_id != 0 && lane_id_ != 0 && SIGN(lane_id_) != SIGN(old_lane_id))
+	//{
+	//	h_relative_ = GetAngleSum(h_relative_, M_PI);
+	//}
+
+	//Lane2Track();
+	LaneBoundary2Track(); 
+	Track2XYZ();
+
+	return;
+}
+
+void Position::SetRoadMarkPos(int track_id, int lane_id, int roadmark_idx, int roadmarktype_idx, int roadmarkline_idx, double s, double offset, int lane_section_idx)
+{
+	offset_ = offset;
+	int old_lane_id = lane_id_;
+	int old_track_id = track_id_;
+
+	Road* road = GetOpenDrive()->GetRoadById(track_id);
+	if (road == 0)
+	{
+		LOG("Position::Set Error: track %d not available\n", track_id);
+		lane_id_ = lane_id;
+		offset_ = offset;
+		return;
+	}
+
+	if (s > road->GetLength())
+	{
+		// Truncate road mark point to road length
+		s = road->GetLength();
+	}
+
+	if (SetLongitudinalTrackPos(track_id, s) != 0)
+	{
+		lane_id_ = lane_id;
+		offset_ = offset;
+		roadmark_idx_ = roadmark_idx;
+		roadmarktype_idx_ = roadmarktype_idx;
+		roadmarkline_idx_ = roadmarkline_idx;
+		return;
+	}
+
+
+	if (lane_id != lane_id_ && lane_section_idx == -1)
+	{
+		// New lane ID might indicate a discreet jump to a new, distant position, reset lane section, if not specified in func parameter)
+		lane_section_idx = road->GetLaneSectionIdxByS(s);
+	}
+
+	LaneSection *lane_section = 0;
+	if (lane_section_idx > -1)  // If lane section was specified or reset
+	{
+		lane_section_idx_ = lane_section_idx;
+		lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+		lane_id_ = lane_id;
+	}
+	else  // Find LaneSection and info according to s
+	{
+		LaneInfo lane_info = road->GetLaneInfoByS(s_, lane_section_idx_, lane_id_, snapToLaneTypes_);
+		lane_section_idx_ = lane_info.lane_section_idx_;
+		lane_id_ = lane_info.lane_id_;
+		
+		lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+	}
+
+	if (lane_section != 0)
+	{
+		lane_idx_ = lane_section->GetLaneIdxById(lane_id_);
+		if (lane_idx_ == -1)
+		{
+			LOG("lane_idx %d fail for lane id %d\n", lane_idx_, lane_id_);
+			lane_idx_ = 0;
+		}
+	}
+	else
+	{
+		LOG("Position::Set (lanepos) Error - lanesection NULL lsidx %d rid %d lid %d\n",
+			lane_section_idx_, road->GetId(), lane_id_);
+	}
+
+	// Check road direction when on new track 
+	if (old_lane_id != 0 && lane_id_ != 0 && track_id_ != old_track_id && SIGN(lane_id_) != SIGN(old_lane_id))
+	{
+		h_relative_ = GetAngleSum(h_relative_, M_PI);
+	}
+
+	Lane *lane = lane_section->GetLaneByIdx(lane_idx_);
+	if (lane != 0)
+	{
+		roadmark_idx_ = roadmark_idx;
+	}
+	
+	LaneRoadMark *lane_roadmark = lane->GetLaneRoadMarkByIdx(roadmark_idx_);
+	if (lane_roadmark != 0)
+	{
+		s_ = MIN(s_, road->GetLength());
+	}
+	else
+	{
+		LOG("roadmark_idx_ %d fail for lane id %d\n", roadmark_idx_, lane_idx_);
+		roadmark_idx_ = 0;
+	}
+
+	if (lane_roadmark->GetNumberOfRoadMarkTypes() != 0)
+	{
+		roadmarktype_idx_ = roadmarktype_idx;
+	}
+	else
+	{
+		roadmarktype_idx_ = 0;
+	}
+	
+	LaneRoadMarkType *lane_roadmarktype = lane_roadmark->GetLaneRoadMarkTypeByIdx(roadmarktype_idx_);
+	if (lane_roadmarktype != 0)
+	{
+		roadmarkline_idx_ = roadmarkline_idx;
+		LaneRoadMarkTypeLine *lane_roadmarktypeline = lane_roadmarktype->GetLaneRoadMarkTypeLineByIdx(roadmarkline_idx_);
+		if (lane_roadmarktypeline != 0)
+		{
+			s_ = MIN(s_ + lane_roadmarktypeline->GetSOffset(), road->GetLength());
+		}
+		else
+		{
+			LOG("roadmarktypeline_idx_ %d fail for roadmarktype_idx %d\n", roadmarkline_idx_, roadmarktype_idx_);
+			roadmarkline_idx_ = 0;
+		}
+	}
+	else
+	{
+		LOG("roadmarktype_idx_ %d fail for roadmark_idx %d\n", roadmarktype_idx_, roadmark_idx_);
+		roadmarkline_idx_ = 0;
+	}
+
+
+	RoadMark2Track();
+	Track2XYZ();
+}
+
+int Position::SetInertiaPos(double x, double y, double z, double h, double p, double r, bool updateTrackPos)
+{
+	x_ = x;
+	y_ = y;
+	z_ = z;
+
+	if (updateTrackPos)
+	{
+		XYZ2Track(false);
+	}
+
+	// Now when road orientation is known, call functions for 
+	// updating angles both absolute and relative the road 
+	SetHeading(h);
+	SetPitch(p);
+	SetRoll(r);
+
+	return 0;
+}
+
+int Position::SetInertiaPos(double x, double y, double h, bool updateTrackPos)
+{
+	x_ = x;
+	y_ = y;
+
+	if (updateTrackPos)
+	{
+		XYZ2Track(true);
+	}
+
+	// Now when road orientation is known, call functions for 
+	// updating angles both absolute and relative the road 
+	SetHeading(h);
+
+	return 0;
+}
+
+void Position::SetHeading(double heading)
+{
+	h_ = heading;
+	h_relative_ = GetAngleInInterval2PI(GetAngleDifference(h_, h_road_));
+}
+
+void Position::SetHeadingRelative(double heading)
+{
+	h_relative_ = GetAngleInInterval2PI(heading);
+	h_ = GetAngleSum(h_road_, h_relative_);
+}
+
+void Position::SetHeadingRelativeRoadDirection(double heading)
+{
+	if (h_relative_ > M_PI_2 && h_relative_ < 3 * M_PI_2)
+	{
+		// Driving towards road direction
+		h_relative_ = GetAngleInInterval2PI(-heading + M_PI);
+	}
+	else
+	{
+		h_relative_ = GetAngleInInterval2PI(heading);
+	}
+	h_ = GetAngleSum(h_road_, h_relative_);
+}
+
+void Position::SetRoll(double roll)
+{
+	r_ = roll;
+	r_relative_ = GetAngleInInterval2PI(GetAngleDifference(r_, r_road_));
+}
+
+void Position::SetRollRelative(double roll)
+{
+	r_relative_ = GetAngleInInterval2PI(roll);
+	r_ = GetAngleSum(r_road_, r_relative_);
+}
+
+void Position::SetPitch(double pitch)
+{
+	p_ = pitch;
+	p_relative_ = GetAngleInInterval2PI(GetAngleDifference(p_, p_road_));
+}
+
+void Position::SetPitchRelative(double pitch)
+{
+	p_relative_ = GetAngleInInterval2PI(pitch);
+	p_ = GetAngleSum(p_road_, p_relative_);
+}
+
+double Position::GetCurvature()
+{
+	Geometry *geom = GetOpenDrive()->GetGeometryByIdx(track_idx_, geometry_idx_);
+	
+	if (geom)
+	{
+		return geom->EvaluateCurvatureDS(GetS() - geom->GetS());
+	}
+	else
+	{
+		return 0;
+	}
+}
+
+double Position::GetHRoadInDrivingDirection()
+{
+	return h_road_ + (lane_id_ > 0 ? M_PI : 0);
+}
+
+double Position::GetPRoadInDrivingDirection()
+{
+	return p_road_ * (lane_id_ > 0 ? -1 : 1);
+}
+
+double Position::GetHRelativeDrivingDirection()
+{
+	return GetAngleDifference(h_, GetDrivingDirection());
+}
+
+double Position::GetSpeedLimit()
+{
+	double speed_limit = 70 / 3.6;  // some default speed
+	Road *road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	
+	if (road)
+	{
+		speed_limit = road->GetSpeedByS(s_);
+
+		if (speed_limit < SMALL_NUMBER)
+		{
+			// No speed limit defined, set a value depending on number of lanes
+			speed_limit = GetOpenDrive()->GetRoadByIdx(track_idx_)->GetNumberOfDrivingLanesSide(GetS(), SIGN(GetLaneId())) > 1 ? 120 / 3.6 : 60 / 3.6;
+		}
+	}
+
+	return speed_limit;
+}
+
+double Position::GetDrivingDirection()
+{
+	double x, y, h;
+	Geometry *geom = GetOpenDrive()->GetGeometryByIdx(track_idx_, geometry_idx_);
+
+	if (!geom)
+	{
+		return h_;
+	}
+
+	geom->EvaluateDS(GetS() - geom->GetS(), &x, &y, &h);
+
+	// adjust 180 degree according to side of road
+	if (GetLaneId() > 0)  // Left side of road reference line
+	{
+		h = GetAngleSum(h, M_PI);
+	}
+
+	return(h);
+}
+
+void Position::CopyRMPos(Position *from)
+{
+	// Preserve route field
+	Route *tmp = route_;
+	
+	*this = *from;
+	route_ = tmp;
+}
+
+
+void Position::PrintTrackPos()
+{
+	LOG("	Track pos: (road_id %d, s %.2f, t %.2f, h %.2f)", track_id_, s_, t_, h_);
+}
+
+void Position::PrintLanePos()
+{
+	LOG("	Lane pos: (road_id %d, lane_id %d, s %.2f, offset %.2f, h %.2f)", track_id_, lane_id_, s_, offset_, h_);
+}
+
+void Position::PrintInertialPos()
+{
+	LOG("	Inertial pos: (x %.2f, y %.2f, z %.2f, h %.2f, p %.2f, r %.2f)", x_, y_, z_, h_, p_, r_);
+}
+
+void Position::Print()
+{
+	LOG("Position:");
+	PrintTrackPos();
+	PrintLanePos();
+	PrintInertialPos();
+}
+
+void Position::PrintXY()
+{
+	LOG("%.2f, %.2f\n", x_, y_);
+}
+
+bool Position::IsOffRoad()
+{
+	Road* road = GetOpenDrive()->GetRoadByIdx(track_idx_);
+	if (road)
+	{
+		// Check whether outside road width
+		if (fabs(t_) > road->GetWidth(GetS(), SIGN(t_), ~Lane::LaneType::LANE_TYPE_NONE))
+		{
+			return true;
+		}
+	}
+	
+	return false;
+}
+
+double Position::getRelativeDistance(Position target_position, double &x, double &y)
+{
+	// Calculate diff vector from current to target
+	double diff_x, diff_y;
+
+	diff_x = target_position.GetX() - GetX();
+	diff_y = target_position.GetY() - GetY();
+
+	// Compensate for current heading (rotate so that current heading = 0)
+	x = diff_x * cos(-GetH()) - diff_y * sin(-GetH());
+	y = diff_x * sin(-GetH()) + diff_y * cos(-GetH());
+
+	// Now just check whether diff vector X-component is less than 0 (behind current)
+	int sign = x > 0 ? 1 : -1;
+
+	// Return length of dist vector
+	return sign * sqrt((x * x) + (y * y));
+}
+
+void Position::CalcRoutePosition()
+{
+	if (route_ == 0)
+	{
+		return;
+	}
+
+	// Loop over waypoints - look for current track ID and sum the distance (route s) up to current position
+	double dist = 0;
+	for (size_t i = 0; i < route_->waypoint_.size(); i++)
+	{
+		int direction = route_->GetWayPointDirection((int)i);
+		if (direction == 0)
+		{
+			LOG("Unexpected lack of connection in route at waypoint %d", i);
+			return;
+		}
+
+		if (i == 0)
+		{
+			// Subtract initial s-value for the first waypoint
+			
+			if (direction > 0)  // route in waypoint road direction
+			{
+				dist = GetRoadById(route_->waypoint_[i]->GetTrackId())->GetLength() - route_->waypoint_[i]->s_;
+			}
+			else
+			{
+				// going towards road direction - remaining distance equals route s-value
+				dist = route_->waypoint_[i]->s_;
+			}
+		}
+		else
+		{
+			// Add length of intermediate waypoint road
+			dist += GetRoadById(route_->waypoint_[i]->GetTrackId())->GetLength();
+		}
+
+		if (GetTrackId() == route_->waypoint_[i]->GetTrackId())
+		{
+			// current position is at the road of this waypoint - i.e. along the route
+			// remove remaming s from road
+			if (direction > 0)
+			{
+				dist -= (GetRoadById(route_->waypoint_[i]->GetTrackId())->GetLength() - GetS());
+			}
+			else
+			{
+				dist -= GetS();
+			}
+			s_route_ = dist;
+			break;
+		}
+	}
+}
+
+void Position::SetRoute(Route *route)
+{
+	route_ = route; 
+
+	// Also find out current position in terms of route position
+	CalcRoutePosition();
+}
+
+void Position::SetTrajectory(Trajectory* trajectory)
+{
+	trajectory_ = trajectory;
+
+	// Reset trajectory S value
+	s_trajectory_ = 0;
+}
+
+bool Position::Delta(Position pos_b, PositionDiff &diff)
+{
+	double dist = 0;
+	bool found;
+
+	RoadPath *path = new RoadPath(this, &pos_b);
+
+	found = (path->Calculate(dist) == 0);
+	if (found)
+	{
+		diff.dLaneId = pos_b.GetLaneId() - GetLaneId();
+		diff.ds = dist;
+		diff.dt = pos_b.GetT() - GetT();
+
+#if 0   // Change to 1 to print some info on stdout - e.g. for debugging
+		printf("Dist %.2f Path (reversed): %d", dist, pos_b.GetTrackId());
+		if (path->visited_.size() > 0)
+		{
+			RoadPath::PathNode* node = path->visited_.back();
+
+			while (node)
+			{
+				if (node->fromRoad != 0)
+				{
+					printf(" <- %d", node->fromRoad->GetId());
+				}
+				node = node->previous;
+			}
+		}
+		printf("\n");
+#endif
+	}
+	else
+	{
+		diff.dLaneId = 0;
+		diff.ds = 0;
+		diff.dt = 0;
+	}
+
+	delete path;
+
+	return found;
+}
+
+bool Position::IsAheadOf(Position target_position)
+{
+	// Calculate diff vector from current to target
+	double diff_x, diff_y;
+	double diff_x0;
+
+	diff_x = target_position.GetX() - GetX();
+	diff_y = target_position.GetY() - GetY();
+
+	// Compensate for current heading (rotate so that current heading = 0) 
+	// Only x component needed
+	diff_x0 = diff_x * cos(-GetH()) - diff_y * sin(-GetH());
+
+	// Now just check whether diff vector X-component is less than 0 (behind current)
+	return(diff_x0 < 0);
+}
+
+int Position::GetRoadLaneInfo(RoadLaneInfo *data)
+{
+	if (fabs(GetCurvature()) > SMALL_NUMBER)
+	{
+		double radius = 1.0 / GetCurvature();
+		radius -= GetT(); // curvature positive in left curves, lat_offset positive left of reference lane
+		data->curvature = (1.0 / radius);
+	}
+	else
+	{
+		// curvature close to zero (straight segment), radius infitite - curvature the same in all lanes
+		data->curvature = GetCurvature();
+	}
+
+	data->pos[0] = GetX();
+	data->pos[1] = GetY();
+	data->pos[2] = GetZRoad();
+	data->heading = GetHRoad();
+	data->pitch = GetPRoad();
+	data->roll = GetRRoad();
+
+	// Then find out the width of the lane at current s-value
+	Road *road = GetRoadById(GetTrackId());
+	if (road)
+	{
+		data->width = road->GetLaneWidthByS(GetS(), GetLaneId());
+		data->speed_limit = road->GetSpeedByS(GetS());
+	}
+
+	return 0;
+}
+
+int Position::GetRoadLaneInfo(double lookahead_distance, RoadLaneInfo *data, LookAheadMode lookAheadMode)
+{
+	Position target(*this);  // Make a copy of current position
+
+	if (lookAheadMode == LOOKAHEADMODE_AT_ROAD_CENTER)
+	{
+		// Look along reference lane requested, move pivot position to t=0 plus a small number in order to 
+		// fall into the right direction
+		target.SetTrackPos(target.GetTrackId(), target.GetS(), SMALL_NUMBER * SIGN(GetLaneId()));
+	}
+	else if (lookAheadMode == LOOKAHEADMODE_AT_LANE_CENTER)
+	{
+		// Look along current lane center requested, move pivot position accordingly 
+		target.SetLanePos(target.GetTrackId(), target.GetLaneId(), target.GetS(), 0);
+	}
+
+	if (fabs(lookahead_distance) > SMALL_NUMBER)
+	{
+		int retval = target.MoveAlongS(lookahead_distance, 0, Junction::STRAIGHT);
+		
+		if (retval != 0)
+		{
+			return retval;
+		}
+	}
+	
+	target.GetRoadLaneInfo(data);
+
+	return 0;
+}
+
+void Position::CalcProbeTarget(Position *target, RoadProbeInfo *data)
+{
+	target->GetRoadLaneInfo(&data->road_lane_info);
+
+	// find out local x, y, z
+	double diff_x = target->GetX() - GetX();
+	double diff_y = target->GetY() - GetY();
+	double diff_z = target->GetZRoad() - GetZRoad();
+
+	data->relative_pos[0] = diff_x * cos(-GetH()) - diff_y * sin(-GetH());
+	data->relative_pos[1] = diff_x * sin(-GetH()) + diff_y * cos(-GetH());
+	data->relative_pos[2] = diff_z;
+
+#if 0
+	// for validation
+	data->global_pos[0] = GetX() + data->local_pos[0] * cos(GetH()) - data->local_pos[1] * sin(GetH());
+	data->global_pos[1] = GetY() + data->local_pos[0] * sin(GetH()) + data->local_pos[1] * cos(GetH());
+	data->global_pos[2] = GetZ() + data->local_pos[2];
+#endif
+
+	// Calculate angle - by dot product
+	if (fabs(data->relative_pos[0]) < SMALL_NUMBER && fabs(data->relative_pos[1]) < SMALL_NUMBER && fabs(data->relative_pos[2]) < SMALL_NUMBER)
+	{
+		data->relative_h = GetH();
+	}
+	else
+	{
+		double dot_prod =
+			(data->relative_pos[0] * 1.0 + data->relative_pos[1] * 0.0) /
+			sqrt(data->relative_pos[0] * data->relative_pos[0] + data->relative_pos[1] * data->relative_pos[1]);
+		data->relative_h = SIGN(data->relative_pos[1]) * acos(dot_prod);
+	}
+
+}
+
+int Position::GetProbeInfo(double lookahead_distance, RoadProbeInfo *data, LookAheadMode lookAheadMode)
+{
+	if (GetOpenDrive()->GetNumOfRoads() == 0)
+	{
+		return -1;
+	}
+	Position target(*this);  // Make a copy of current position
+
+	if (lookAheadMode == LOOKAHEADMODE_AT_ROAD_CENTER)
+	{
+		// Look along reference lane requested, move pivot position to t=0 plus a small number in order to 
+		// fall into the right direction
+		target.SetTrackPos(target.GetTrackId(), target.GetS(), SMALL_NUMBER * SIGN(GetLaneId()));
+	}
+	else if (lookAheadMode == LOOKAHEADMODE_AT_LANE_CENTER)
+	{
+		// Look along current lane center requested, move pivot position accordingly 
+		target.SetLanePos(target.GetTrackId(), target.GetLaneId(), target.GetS(), 0);
+	}
+
+	if (fabs(lookahead_distance) > SMALL_NUMBER)
+	{
+		int retval;
+		if (target.route_)
+		{
+			retval = target.MoveRouteDS(lookahead_distance);
+		}
+		else
+		{
+			retval = target.MoveAlongS(lookahead_distance, 0, Junction::STRAIGHT);
+		}
+
+		if (retval != 0)
+		{
+			return retval;
+		}
+	}
+
+	CalcProbeTarget(&target, data);
+
+	return 0;
+}
+
+int Position::GetProbeInfo(Position *target_pos, RoadProbeInfo *data)
+{
+	CalcProbeTarget(target_pos, data);
+
+	return 0;
+}
+
+int Position::GetTrackId() 
+{ 
+	if (rel_pos_ && rel_pos_ != this && 
+		(type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD))
+	{
+		return rel_pos_->GetTrackId();
+	}
+
+	return track_id_; 
+}
+
+int Position::GetLaneId()
+{
+	if (rel_pos_ && rel_pos_ != this && type_ == PositionType::RELATIVE_LANE)
+	{
+		return rel_pos_->GetLaneId() + lane_id_;
+	}
+
+	return lane_id_;
+}
+
+int Position::GetLaneGlobalId()
+{
+	Road *road = GetRoadById(GetTrackId());
+	if (road == 0)
+	{
+		LOG("No road %d", track_idx_);
+		return -1;
+	}
+
+	LaneSection *lane_section = road->GetLaneSectionByIdx(lane_section_idx_);
+
+	if (lane_section == 0)
+	{
+		LOG("No lane section for idx %d - keeping current lane setting\n", lane_section_idx_);
+		return -1;
+	}
+
+	double offset;
+	int lane_idx = lane_section->GetClosestLaneIdx(s_, t_, offset, false, Lane::LaneType::LANE_TYPE_ANY);
+
+	if (lane_idx == -1)
+	{
+		LOG("Failed to find a valid drivable lane");
+		return -1;
+	}
+
+	// Check if it is not a center lane
+	int lane_id = lane_section->GetLaneIdByIdx(lane_idx);
+	if(!lane_section->IsOSILaneById(lane_id))
+	{
+		if(offset>=0)
+		{
+			lane_id = 1;
+		} else {
+			lane_id = -1;
+		}
+		lane_idx = lane_section->GetLaneIdxById(lane_id);
+	}
+
+	return lane_section->GetLaneGlobalIdByIdx(lane_idx);
+}
+
+double Position::GetS()
+{
+	if (rel_pos_ && rel_pos_ != this && 
+		(type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD))
+	{
+		return rel_pos_->GetS() + s_;
+	}
+
+	return s_;
+}
+
+double Position::GetT()
+{
+	if (rel_pos_ && rel_pos_ != this && 
+		(type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD))
+	{
+		return rel_pos_->GetT() + t_;
+	}
+
+	return t_;
+}
+
+double Position::GetOffset()
+{
+	if (rel_pos_ && rel_pos_ != this && 
+		(type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD))
+	{
+		return rel_pos_->GetOffset() + offset_;
+	}
+
+	return offset_;
+}
+
+double Position::GetX()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return x_;
+	}
+	else if (type_ == PositionType::RELATIVE_OBJECT)
+	{
+		return rel_pos_->GetX() + x_ * cos(rel_pos_->GetH()) - y_ * sin(rel_pos_->GetH());
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD)
+	{
+		return x_ + rel_pos_->GetX();
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		return x_;
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return x_;
+}
+
+double Position::GetY()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return y_;
+	}
+	else if (type_ == PositionType::RELATIVE_OBJECT)
+	{
+		return rel_pos_->GetY() + y_ * cos(rel_pos_->GetH()) + x_ * sin(rel_pos_->GetH());
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD)
+	{
+		return y_ + rel_pos_->GetY();
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		return y_;
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return y_;
+}
+
+double Position::GetZ()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return z_;
+	}
+	else if (type_ == PositionType::RELATIVE_OBJECT || type_ == PositionType::RELATIVE_WORLD)
+	{
+		return z_ + rel_pos_->GetZ();
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		return z_;
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return z_;
+}
+
+double Position::GetH()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return h_;
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD || type_ == PositionType::RELATIVE_OBJECT)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return h_;
+		}
+		else
+		{
+			return h_ + rel_pos_->GetH();
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return h_;
+		}
+		else
+		{
+			return h_ + GetHRoadInDrivingDirection();
+		}
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+	
+	return h_;
+}
+
+double Position::GetHRelative()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return h_relative_;
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD || type_ == PositionType::RELATIVE_OBJECT)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return h_relative_;
+		}
+		else
+		{
+			return GetAngleInInterval2PI(h_relative_ + rel_pos_->GetHRelative());
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return h_relative_;
+		}
+		else
+		{
+			return GetAngleInInterval2PI(h_relative_ + GetHRoadInDrivingDirection());
+		}
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return h_relative_;
+}
+
+double Position::GetP()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return p_;
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD || type_ == PositionType::RELATIVE_OBJECT)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return p_;
+		}
+		else
+		{
+			return p_ + rel_pos_->GetP();
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return p_;
+		}
+		else
+		{
+			return p_ + GetPRoadInDrivingDirection();
+		}
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return p_;
+}
+
+double Position::GetPRelative()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return p_relative_;
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD || type_ == PositionType::RELATIVE_OBJECT)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return p_relative_;
+		}
+		else
+		{
+			return GetAngleInInterval2PI(p_relative_ + rel_pos_->GetPRelative());
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return p_relative_;
+		}
+		else
+		{
+			return GetAngleInInterval2PI(p_relative_ + GetPRoadInDrivingDirection());
+		}
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return p_relative_;
+}
+
+double Position::GetR()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return r_;
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD || type_ == PositionType::RELATIVE_OBJECT)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return r_;
+		}
+		else
+		{
+			return r_ + rel_pos_->GetR();
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return r_;
+		}
+		else
+		{
+			return r_;  // road R not implemented yet
+		}
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return r_;
+}
+
+double Position::GetRRelative()
+{
+	if (!rel_pos_ || rel_pos_ == this)
+	{
+		return r_relative_;
+	}
+	else if (type_ == PositionType::RELATIVE_WORLD || type_ == PositionType::RELATIVE_OBJECT)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return r_relative_;
+		}
+		else
+		{
+			return GetAngleInInterval2PI(r_relative_ + rel_pos_->GetRRelative());
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_LANE || type_ == PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_ABSOLUTE)
+		{
+			return r_relative_;
+		}
+		else
+		{
+			return GetAngleInInterval2PI(r_relative_ + r_road_);
+		}
+	}
+	else
+	{
+		LOG("Unexpected PositionType: %d", type_);
+	}
+
+	return r_relative_;
+}
+
+int Position::SetRoutePosition(Position *position)
+{
+	if(!route_)
+	{
+		return -1;
+	}
+
+	// Is it a valid position, i.e. is it along the route
+	for (size_t i=0; i<route_->waypoint_.size(); i++)
+	{
+		if (route_->waypoint_[i]->GetTrackId() == position->GetTrackId()) // Same road
+		{
+			// Update current position 
+			Route *tmp = route_;  // save route pointer, copy the 
+			*this = *position;
+			route_ = tmp;
+			return 0;
+		}
+	}
+
+	return -1;
+}
+
+int Position::MoveRouteDS(double ds)
+{
+	if (!route_)
+	{
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	if (route_->waypoint_.size() == 0)
+	{
+		return ErrorCode::ERROR_GENERIC;
+	}
+	
+	return SetRouteS(route_, s_route_ + ds);
+}
+
+int Position::SetRouteLanePosition(Route *route, double route_s, int laneId, double  laneOffset)
+{
+	SetRouteS(route, route_s);
+
+	// Override lane data
+	SetLanePos(track_id_, laneId, s_, laneOffset);
+
+	return 0;
+}
+
+void PolyLine::AddVertex(Position pos, double time, bool calculateHeading)
+{
+	Vertex* v = new Vertex();
+	v->pos_ = pos;
+	v->time_ = time;
+	v->calc_heading_ = calculateHeading;
+	vertex_.push_back(v);
+	if (vertex_.size() > 1)
+	{
+		length_ += PointDistance2D(
+			vertex_[vertex_.size() - 2]->pos_.GetX(),
+			vertex_[vertex_.size() - 2]->pos_.GetY(),
+			vertex_[vertex_.size() - 1]->pos_.GetX(),
+			vertex_[vertex_.size() - 1]->pos_.GetY());
+	}
+}
+
+int Position::MoveTrajectoryDS(double ds)
+{
+	if (!trajectory_)
+	{
+		return -1;
+	}
+
+	if (trajectory_->shape_->type_ == Shape::POLYLINE)
+	{
+		PolyLine* pline = (PolyLine*)trajectory_->shape_;
+		if (pline->vertex_.size() == 0)
+		{
+			return -1;
+		}
+		SetTrajectoryS(trajectory_, s_trajectory_ + ds);
+	}
+	else if (trajectory_->shape_->type_ == Shape::CLOTHOID)
+	{
+		SetTrajectoryS(trajectory_, s_trajectory_ + ds);
+	}
+	else
+	{
+		LOG("Trajectory types Clothoid and Nurbs not supported yet");
+	}
+
+	return 0;
+}
+
+int Position::SetTrajectoryPosByTime(Trajectory* trajectory, double time)
+{
+	double s = 0;
+
+	// Find out corresponding S-value
+	size_t i = 0;
+	if (trajectory->shape_->type_ == Shape::POLYLINE)
+	{
+		PolyLine* pline = (PolyLine*)trajectory->shape_;
+
+		for (i = 0; i < pline->vertex_.size()-1; i++)
+		{
+			double t0 = pline->vertex_[i]->time_;
+			double t1 = pline->vertex_[i+1]->time_;
+			Position* vp0 = &pline->vertex_[i]->pos_;
+			Position* vp1 = &pline->vertex_[i+1]->pos_;
+
+			double dist = PointDistance2D(vp0->GetX(), vp0->GetY(), vp1->GetX(), vp1->GetY());
+
+			if (time < t1)
+			{
+				double a = (time - t0) / (t1 - t0);
+				s += a * dist;
+				break;
+			}
+			s += dist;
+		}
+	}
+	else if (trajectory->shape_->type_ == Shape::CLOTHOID)
+	{
+		Clothoid* clothoid = (Clothoid*)trajectory->shape_;
+
+		if (time > clothoid->t_start_ && time < clothoid->t_end_)
+		{
+			double t = time - clothoid->t_start_;
+			s = clothoid->length_ * (t - clothoid->t_start_) / (clothoid->t_end_ - clothoid->t_start_);
+		}
+		else
+		{
+			LOG("Requested time %.2f outside range [%.2f, %.2f]", time, clothoid->t_start_, clothoid->t_end_);
+		}
+	}
+	else
+	{
+		LOG("Trajectory types Clothoid and Nurbs not supported yet");
+	}
+	
+//	LOG("i %d t %.2f s %.2f", i, time, s);
+
+	SetTrajectoryS(trajectory_, s);
+
+	return 0;
+}
+
+int Position::SetTrajectoryS(Trajectory* trajectory, double traj_s)
+{
+	if (!trajectory)
+	{
+		return -1;
+	}
+
+	s_trajectory_ = traj_s;
+	
+	if (trajectory->shape_->type_ == Shape::POLYLINE)
+	{
+		PolyLine* pline = (PolyLine*)trajectory->shape_;
+		if (pline->vertex_.size() == 0)
+		{
+			return -1;
+		}
+		else if (pline->vertex_.size() == 1)
+		{
+			// Only one vertex 
+			Position* vpos = &pline->vertex_[0]->pos_;
+			SetInertiaPos(vpos->GetX(), vpos->GetY(), vpos->GetZ(), vpos->GetH(), vpos->GetP(), vpos->GetR(), false);
+			
+			return 0;
+		}
+
+		double tot_dist = 0;
+		size_t i;
+		for (i = 0; i < pline->vertex_.size()-1; i++)
+		{
+			Position* vp0 = &pline->vertex_[i]->pos_;
+			Position* vp1 = &pline->vertex_[i+1]->pos_;
+			
+			double dist = PointDistance2D(vp0->GetX(), vp0->GetY(), vp1->GetX(), vp1->GetY());
+			if (traj_s < tot_dist + dist)
+			{
+				// At segment, make a linear interpolation
+				double a = (traj_s - tot_dist) / dist; // a = interpolation factor
+
+				SetInertiaPos(
+					(1 - a) * vp0->GetX() + a * vp1->GetX(),
+					(1 - a) * vp0->GetY() + a * vp1->GetY(),
+					(1 - a) * vp0->GetZ() + a * vp1->GetZ(),
+					GetAngleInInterval2PI(vp0->GetH() + a * GetAngleDifference(vp1->GetH(), vp0->GetH())),
+					GetAngleInInterval2PI(vp0->GetP() + a * GetAngleDifference(vp1->GetP(), vp0->GetP())),
+					GetAngleInInterval2PI(vp0->GetR() + a * GetAngleDifference(vp1->GetR(), vp0->GetR())),
+					true);
+				
+				return 0;
+			}
+			tot_dist += dist;
+		}
+
+		if (i == pline->vertex_.size() - 1)
+		{
+			// passed length of trajectory, use final vertex
+			Position* vpos = &pline->vertex_.back()->pos_;
+			SetInertiaPos(vpos->GetX(), vpos->GetY(), vpos->GetZ(), vpos->GetH(), vpos->GetP(), vpos->GetR(), false);
+
+			return 0;
+		}
+	}
+	else if (trajectory->shape_->type_ == Shape::CLOTHOID)
+	{
+		double x, y, h;
+		Clothoid* clothoid = (Clothoid*)trajectory_->shape_;
+
+		clothoid->spiral_->EvaluateDS(traj_s, &x, &y, &h);
+		SetInertiaPos(x, y, 0.0, h, 0.0, 0.0, false);
+		return 0;
+	}
+	else
+	{
+		LOG("Trajectory type Nurbs not supported yet");
+	}
+
+	return -1;
+}
+
+int Position::SetRouteS(Route *route, double route_s)
+{
+	if (route->waypoint_.size() == 0)
+	{
+		LOG("SetOffset No waypoints!");
+		return ErrorCode::ERROR_GENERIC;
+	}
+
+	OpenDrive *od = route->waypoint_[0]->GetOpenDrive();
+
+	double initial_s_offset = 0;
+
+	if (route->GetWayPointDirection(0) > 0)
+	{
+		initial_s_offset = route->waypoint_[0]->GetS();
+	}
+	else
+	{
+		initial_s_offset = od->GetRoadById(route->waypoint_[0]->GetTrackId())->GetLength() - route->waypoint_[0]->GetS();
+	}
+
+	double route_length = 0;
+	s_route_ = route_s;
+	double new_offset = offset_;
+
+	// Find out what road and local s value
+	for (size_t i = 0; i < route->waypoint_.size(); i++)
+	{
+		int track_id = route->waypoint_[i]->GetTrackId();
+		double road_length = route->waypoint_[i]->GetOpenDrive()->GetRoadById(track_id)->GetLength();
+		if (s_route_ < route_length + road_length - initial_s_offset)
+		{
+			// Found road segment
+			double local_s = 0;
+
+			int route_direction = route->GetWayPointDirection((int)i);
+
+			if (route_direction == 0)
+			{
+				LOG("Unexpected lack of connection within route at waypoint %d", i);
+				return ErrorCode::ERROR_GENERIC;
+			}
+
+			local_s = s_route_ - route_length + initial_s_offset;
+			if (route_direction < 0)  // along waypoint road direction
+			{
+				local_s = road_length - local_s;
+				new_offset = -offset_;
+				if (GetHRelative() < M_PI_2 || GetHRelative() > 3 * M_PI_2)
+				{
+					SetHeadingRelative(GetAngleSum(GetHRelative(), M_PI));
+				}
+			}
+			else
+			{
+				new_offset = offset_;
+				if (GetHRelative() > M_PI_2 && GetHRelative() < 3 * M_PI_2)
+				{
+					SetHeadingRelative(GetAngleSum(GetHRelative(), M_PI));
+				}
+
+			}
+
+			return (SetLanePos(route->waypoint_[i]->GetTrackId(), route->waypoint_[i]->GetLaneId(), local_s, new_offset));
+		}
+		route_length += road_length - initial_s_offset;
+		initial_s_offset = 0;  // For all following road segments, calculate length from beginning
+	}
+
+	status_ |= POS_STATUS_END_OF_ROUTE;
+	return ErrorCode::ERROR_END_OF_ROUTE;
+}
+
+void Position::ReleaseRelation()
+{
+	// Freeze position and then disconnect 
+	if (type_ == Position::PositionType::RELATIVE_LANE)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_RELATIVE)
+		{
+			// Save requested heading angle, since SetLanePos will modify it
+			double h = h_relative_;
+
+			// Resolve requested position
+			SetLanePos(GetTrackId(), GetLaneId(), GetS(), GetOffset());
+
+			// Finally set requested heading
+			if (lane_id_ > 0)
+			{
+				// In lanes going opposite road direction, add 180 degrees
+				h = GetAngleSum(h, M_PI);
+			}
+			SetHeadingRelative(h);
+			SetPitch(GetP());
+			SetRoll(GetR());
+		}
+		else
+		{
+			double h = h_;
+			SetLanePos(GetTrackId(), GetLaneId(), GetS(), GetOffset());
+			SetHeading(h);
+		}
+	}
+	if (type_ == Position::PositionType::RELATIVE_ROAD)
+	{
+		if (orientation_type_ == OrientationType::ORIENTATION_RELATIVE)
+		{
+			// Save requested heading angle, since SetLanePos will modify it
+			double h = h_relative_;
+
+			// Resolve requested position
+			SetTrackPos(GetTrackId(), GetS(), GetT());
+
+			// Finally set requested heading
+			if (lane_id_ > 0)
+			{
+				// In lanes going opposite road direction, add 180 degrees
+				h = GetAngleSum(h, M_PI);
+			}
+			SetHeadingRelative(h);
+			SetPitch(GetP());
+			SetRoll(GetR());
+		}
+		else
+		{
+			double h = h_;
+			SetTrackPos(GetTrackId(), GetS(), GetT());
+			SetHeading(h);
+		}
+	}
+	else if (type_ == PositionType::RELATIVE_OBJECT || type_ == PositionType::RELATIVE_WORLD)
+	{
+		SetInertiaPos(GetX(), GetY(), GetZ(), GetH(), GetP(), GetR(), true);
+	}
+	SetRelativePosition(0, PositionType::NORMAL);
+}
+
+int Route::AddWaypoint(Position *position)
+{
+	// Validate waypoint: Is it on the previous road or a connecting one?
+	if (waypoint_.size() > 0)
+	{
+		bool connected = false;
+		Position *prev_pos = waypoint_.back();
+
+		int connecting_road_id = 0;
+		int *connecting_road_id_ptr = &connecting_road_id;
+		int connecting_lane_id = 0;
+		int *connecting_lane_id_ptr = &connecting_lane_id;
+
+		if (prev_pos->GetTrackId() != position->GetTrackId())
+		{
+			if (position->GetOpenDrive()->IsIndirectlyConnected(prev_pos->GetTrackId(), position->GetTrackId(), 
+				connecting_road_id_ptr, connecting_lane_id_ptr, prev_pos->GetLaneId(), position->GetLaneId()))
+			{
+				connected = true;
+				if (connecting_road_id != 0)
+				{
+					// Adding waypoint for junction connecting road
+					Position *connected_pos = new Position(connecting_road_id, connecting_lane_id, 0, 0);
+					waypoint_.push_back(connected_pos);
+					LOG("Route::AddWaypoint Added connecting waypoint %d: %d, %d, %.2f\n",
+						(int)waypoint_.size() - 1, connecting_road_id, connecting_lane_id, 0.0);
+				}
+			}
+		}
+
+		if (!connected)
+		{
+			LOG("Error: waypoint (%d, %d) is not connected to the previous one (%d, %d)\n",
+				position->GetTrackId(), position->GetLaneId(), prev_pos->GetTrackId(), prev_pos->GetLaneId());
+
+			return -1;
+		}
+	}
+
+	waypoint_.push_back(position);
+	LOG("Route::AddWaypoint Added waypoint %d: %d, %d, %.2f\n", (int)waypoint_.size()-1, position->GetTrackId(), position->GetLaneId(), position->GetS());
+
+	return 0;
+}
+
+int Route::GetWayPointDirection(int index)
+{
+	if (waypoint_.size() == 0 || index < 0 || index >= waypoint_.size())
+	{
+		LOG("Waypoint index %d out of range (%d)", index, waypoint_.size());
+		return 0;
+	}
+
+	if (waypoint_.size() == 1)
+	{
+		LOG("Only one waypoint, no direction");
+		return 0;
+	}
+
+	OpenDrive *od = waypoint_[index]->GetOpenDrive();
+	Road *road = od->GetRoadById(waypoint_[index]->GetTrackId());
+	int connected = 0;
+	double angle;
+
+	if (index == waypoint_.size() - 1)
+	{
+		// Find connection point to previous waypoint road
+		Road *prev_road = od->GetRoadById(waypoint_[index-1]->GetTrackId());
+
+		connected = -od->IsDirectlyConnected(road->GetId(), prev_road->GetId(), angle);
+	} 
+	else
+	{
+		// Find connection point to next waypoint road
+		Road *next_road = od->GetRoadById(waypoint_[index+1]->GetTrackId());
+		
+		connected = od->IsDirectlyConnected(road->GetId(), next_road->GetId(), angle);
+	}
+	
+	return connected;
+}
+
+double Route::GetLength()
+{
+
+	return 0.0;
+}
+
+void Route::setName(std::string name)
+{
+	this->name_ = name;
+}
+
+std::string Route::getName()
+{
+	return name_;
+}
+
+void Trajectory::Freeze()
+{
+	if (shape_->type_ == Shape::ShapeType::POLYLINE)
+	{
+		PolyLine* pline = (PolyLine*)shape_;
+		
+		pline->length_ = 0;
+
+		for (size_t i = 0; i < pline->vertex_.size(); i++)
+		{
+			pline->vertex_[i]->pos_.ReleaseRelation();
+			if (i > 0)
+			{
+				pline->length_ += PointDistance2D(
+					pline->vertex_[i-1]->pos_.GetX(),
+					pline->vertex_[i-1]->pos_.GetY(),
+					pline->vertex_[i]->pos_.GetX(),
+					pline->vertex_[i]->pos_.GetY());
+
+				// Calculate heading from last point
+				if (pline->vertex_[i-1]->calc_heading_)
+				{
+					double dx = pline->vertex_[i]->pos_.GetX() - pline->vertex_[i - 1]->pos_.GetX();
+					double dy = pline->vertex_[i]->pos_.GetY() - pline->vertex_[i - 1]->pos_.GetY();
+					pline->vertex_[i-1]->pos_.SetHeading(atan2(dy, dx));
+				}
+
+			}
+		}
+	}
+	else if (shape_->type_ == Shape::ShapeType::CLOTHOID)
+	{
+		Clothoid* clothoid = (Clothoid*)shape_;
+		
+		clothoid->pos_.ReleaseRelation();
+
+		clothoid->spiral_->SetX(clothoid->pos_.GetX());
+		clothoid->spiral_->SetY(clothoid->pos_.GetY());
+		clothoid->spiral_->SetHdg(clothoid->pos_.GetH());
+
+	}
+	else
+	{
+		LOG("Nurbs trajectory type not supported yet");
+	}
+}

src/ui/ui_osgviewer/RoadManager.hpp

@@ -0,0 +1,1881 @@
+/* 
+ * esmini - Environment Simulator Minimalistic 
+ * https://github.com/esmini/esmini
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ * 
+ * Copyright (c) partners of Simulation Scenarios
+ * https://sites.google.com/view/simulationscenarios
+ */
+
+#ifndef OPENDRIVE_HH_
+#define OPENDRIVE_HH_
+
+#include <cmath>
+#include <string>
+#include <vector>
+#include <list>
+#include "pugixml.hpp"
+#include "CommonMini.hpp"
+
+#define PARAMPOLY3_STEPS 100
+
+namespace roadmanager
+{
+	class Polynomial
+	{
+	public:
+		Polynomial() : a_(0), b_(0), c_(0), d_(0), p_scale_(1.0) {}
+		Polynomial(double a, double b, double c, double d, double p_scale = 1) : a_(a), b_(b), c_(c), d_(d), p_scale_(p_scale) {}
+		void Set(double a, double b, double c, double d, double p_scale = 1);
+		void SetA(double a) { a_ = a; }
+		void SetB(double b) { b_ = b; }
+		void SetC(double c) { c_ = c; }
+		void SetD(double d) { d_ = d; }
+		double GetA() { return a_; }
+		double GetB() { return b_; }
+		double GetC() { return c_; }
+		double GetD() { return d_; }
+		double GetPscale() { return p_scale_; }
+		double Evaluate(double s);
+		double EvaluatePrim(double s);
+		double EvaluatePrimPrim(double s);
+
+	private:
+		double a_;
+		double b_;
+		double c_;
+		double d_;
+		double p_scale_;
+	};
+
+	class OSIPoints
+	{
+		public:
+			typedef struct
+			{
+				double s;
+				double x;
+				double y;
+				double z;
+				double h;
+			} OSIPointStruct;
+
+			OSIPoints() {}
+			OSIPoints(std::vector<OSIPointStruct> points) : point_(points) {}
+			void Set(std::vector<OSIPointStruct> points) { point_ = points; }
+			std::vector<OSIPointStruct>& GetPoints() {return point_;}
+			OSIPointStruct& GetPoint(int i);
+			double GetXfromIdx(int i);
+			double GetYfromIdx(int i);
+			double GetZfromIdx(int i);
+			int GetNumOfOSIPoints();
+
+		private:
+			std::vector<OSIPointStruct> point_;
+	};
+
+	class Geometry
+	{
+	public:
+		enum GeometryType
+		{
+			GEOMETRY_TYPE_UNKNOWN,
+			GEOMETRY_TYPE_LINE,
+			GEOMETRY_TYPE_ARC,
+			GEOMETRY_TYPE_SPIRAL,
+			GEOMETRY_TYPE_POLY3,
+			GEOMETRY_TYPE_PARAM_POLY3,
+		};
+
+		Geometry() : s_(0.0), x_(0.0), y_(0), hdg_(0), length_(0), type_(GeometryType::GEOMETRY_TYPE_UNKNOWN) {}
+		Geometry(double s, double x, double y, double hdg, double length, GeometryType type) :
+			s_(s), x_(x), y_(y), hdg_(hdg), length_(length), type_(type) {}
+		virtual ~Geometry() {}
+
+		GeometryType GetType() { return type_; }
+		double GetLength() { return length_; }
+		virtual double GetX() { return x_; }
+		void SetX(double x) { x_ = x; }
+		virtual double GetY() { return y_; }
+		void SetY(double y) { y_ = y; }
+		virtual double GetHdg() { return GetAngleInInterval2PI(hdg_); }
+		void SetHdg(double hdg) { hdg_ = hdg; }
+		double GetS() { return s_; }
+		virtual double EvaluateCurvatureDS(double ds) = 0;
+		virtual void Print();
+		virtual void EvaluateDS(double ds, double *x, double *y, double *h);
+
+	protected:
+		double s_;
+		double x_;
+		double y_;
+		double hdg_;
+		double length_;
+		GeometryType type_;
+	};
+
+
+	class Line : public Geometry
+	{
+	public:
+		Line() {}
+		Line(double s, double x, double y, double hdg, double length) : Geometry(s, x, y, hdg, length, GEOMETRY_TYPE_LINE) {}
+		~Line() {};
+
+		void Print();
+		void EvaluateDS(double ds, double *x, double *y, double *h);
+		double EvaluateCurvatureDS(double ds) { (void)ds; return 0; }
+
+	};
+
+
+	class Arc : public Geometry
+	{
+	public:
+		Arc(): curvature_(0.0) {}
+		Arc(double s, double x, double y, double hdg, double length, double curvature) :
+			Geometry(s, x, y, hdg, length, GEOMETRY_TYPE_ARC), curvature_(curvature) {}
+		~Arc() {}
+
+		double EvaluateCurvatureDS(double ds) { (void)ds; return curvature_; }
+		double GetRadius() { return std::fabs(1.0 / curvature_); }
+		double GetCurvature() { return curvature_; }
+		void Print();
+		void EvaluateDS(double ds, double *x, double *y, double *h);
+
+	private:
+		double curvature_;
+	};
+
+
+	class Spiral : public Geometry
+	{
+	public:
+		Spiral(): curv_start_(0.0), curv_end_(0.0), c_dot_(0.0), x0_(0.0), y0_(0.0), h0_(0.0), s0_(0.0), arc_(0), line_(0) {}
+		Spiral(double s, double x, double y, double hdg, double length, double curv_start, double curv_end);
+			
+		~Spiral() {};
+
+		double GetCurvStart() { return curv_start_; }
+		double GetCurvEnd() { return curv_end_; }
+		double GetX0() { return x0_; }
+		double GetY0() { return y0_; }
+		double GetH0() { return h0_; }
+		double GetS0() { return s0_; }
+		double GetCDot() { return c_dot_; }
+		void SetX0(double x0) { x0_ = x0; }
+		void SetY0(double y0) { y0_ = y0; }
+		void SetH0(double h0) { h0_ = h0; }
+		void SetS0(double s0) { s0_ = s0; }
+		void SetCDot(double c_dot) { c_dot_ = c_dot; }
+		void Print();
+		void EvaluateDS(double ds, double *x, double *y, double *h);
+		double EvaluateCurvatureDS(double ds);
+		void SetX(double x);
+		void SetY(double y);
+		void SetHdg(double h);
+
+		Arc* arc_;
+		Line* line_;
+
+	private:
+		double curv_start_;
+		double curv_end_;
+		double c_dot_;
+		double x0_; // 0 if spiral starts with curvature = 0
+		double y0_; // 0 if spiral starts with curvature = 0
+		double h0_; // 0 if spiral starts with curvature = 0
+		double s0_; // 0 if spiral starts with curvature = 0
+	};
+
+
+	class Poly3 : public Geometry
+	{
+	public:
+		Poly3(): umax_(0.0) {}
+		Poly3(double s, double x, double y, double hdg, double length, double a, double b, double c, double d);
+		~Poly3() {};
+		
+		void SetUMax(double umax) { umax_ = umax; }
+		double GetUMax() { return umax_; }
+		void Print();
+		Polynomial GetPoly3() {return poly3_;}
+		void EvaluateDS(double ds, double *x, double *y, double *h);
+		double EvaluateCurvatureDS(double ds);
+
+		Polynomial poly3_;
+
+	private:
+		double umax_;
+		void EvaluateDSLocal(double ds, double& u, double& v);
+	};
+
+
+	class ParamPoly3 : public Geometry
+	{
+	public:
+		enum PRangeType
+		{
+			P_RANGE_UNKNOWN,
+			P_RANGE_NORMALIZED,
+			P_RANGE_ARC_LENGTH
+		};
+
+		ParamPoly3() {}
+		ParamPoly3(
+			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, PRangeType p_range) :
+			Geometry(s, x, y, hdg, length, GeometryType::GEOMETRY_TYPE_PARAM_POLY3)
+		{
+			poly3U_.Set(aU, bU, cU, dU, p_range == PRangeType::P_RANGE_NORMALIZED ? 1.0/length : 1.0);
+			poly3V_.Set(aV, bV, cV, dV, p_range == PRangeType::P_RANGE_NORMALIZED ? 1.0/length : 1.0);
+			calcS2PMap(p_range);
+		}
+		~ParamPoly3() {};
+
+		void Print();
+		Polynomial GetPoly3U() {return poly3U_;}
+		Polynomial GetPoly3V() {return poly3V_;}
+		void EvaluateDS(double ds, double *x, double *y, double *h);
+		double EvaluateCurvatureDS(double ds);
+		void calcS2PMap(PRangeType p_range);
+		double s2p_map_[PARAMPOLY3_STEPS+1][2];
+		double S2P(double s);
+
+		Polynomial poly3U_;
+		Polynomial poly3V_;
+	};
+
+
+	class Elevation
+	{
+	public:
+		Elevation(): s_(0.0), length_(0.0) {}
+		Elevation(double s, double a, double b, double c, double d) : s_(s), length_(0)
+		{
+			poly3_.Set(a, b, c, d);
+		}
+		~Elevation() {};
+
+		double GetS() { return s_; }
+		void SetLength(double length) { length_ = length; }
+		double GetLength() { return length_; }
+		void Print();
+
+		Polynomial poly3_;
+
+	private:
+		double s_;
+		double length_;
+	};
+
+	typedef enum 
+	{
+		UNKNOWN,
+		SUCCESSOR,
+		PREDECESSOR,
+		NONE
+	} LinkType;
+
+
+	class LaneLink
+	{
+	public:
+		LaneLink(LinkType type, int id) : type_(type), id_(id) {}
+
+		LinkType GetType() { return type_; }
+		int GetId() { return id_; }
+		void Print();
+
+	private:
+		LinkType type_;
+		int id_;
+	};
+
+	class LaneWidth
+	{
+	public:
+		LaneWidth(double s_offset, double a, double b, double c, double d) : s_offset_(s_offset)
+		{
+			poly3_.Set(a, b, c, d);
+		}
+
+		double GetSOffset() { return s_offset_; }
+		void Print();
+
+		Polynomial poly3_;
+
+	private:
+		double s_offset_;
+	};
+
+	class LaneBoundaryOSI
+	{
+	public:
+		LaneBoundaryOSI(int gbid): global_id_(gbid) {}
+		~LaneBoundaryOSI() {};
+		void SetGlobalId();
+		int GetGlobalId() { return global_id_; }
+		OSIPoints* GetOSIPoints() {return &osi_points_;}
+		OSIPoints osi_points_;
+	private:
+		int global_id_;  // Unique ID for OSI 
+	};
+
+	struct RoadMarkInfo
+	{
+		int roadmark_idx_;
+		int roadmarkline_idx_;
+	};
+
+	class LaneRoadMarkTypeLine
+	{
+	public:
+		enum RoadMarkTypeLineRule
+		{
+			NO_PASSING,
+			CAUTION,
+			NONE
+		};
+
+		LaneRoadMarkTypeLine(double length, double space, double t_offset, double s_offset, RoadMarkTypeLineRule rule, double width): 
+		length_(length), space_(space), t_offset_(t_offset), s_offset_(s_offset), rule_(rule), width_(width) {}
+		~LaneRoadMarkTypeLine() {}; 
+		double GetSOffset() { return s_offset_; }
+		double GetTOffset() { return t_offset_; }
+		double GetLength() {return length_;}
+		double GetSpace() {return space_;}
+		double GetWidth() {return width_;}
+		OSIPoints GetOSIPoints() {return osi_points_;}
+		OSIPoints osi_points_;
+		void SetGlobalId();
+		int GetGlobalId() { return global_id_; }
+
+	private:
+		double length_;
+		double space_;
+		double t_offset_;
+		double s_offset_;
+		RoadMarkTypeLineRule rule_;
+		double width_;
+		int global_id_;  // Unique ID for OSI
+	};
+
+	class LaneRoadMarkType
+	{
+	public:
+		LaneRoadMarkType(std::string name, double width) : name_(name), width_(width) {}
+		
+		void AddLine(LaneRoadMarkTypeLine *lane_roadMarkTypeLine); 
+		std::string GetName() { return name_; }
+		double GetWidth() { return width_; }
+		LaneRoadMarkTypeLine* GetLaneRoadMarkTypeLineByIdx(int idx);
+		int GetNumberOfRoadMarkTypeLines() { return (int)lane_roadMarkTypeLine_.size(); }
+		void Print();
+
+	private:
+		std::string name_;
+		double width_;
+		std::vector<LaneRoadMarkTypeLine*> lane_roadMarkTypeLine_;
+	};
+
+	class LaneRoadMark
+	{
+	public:
+		enum RoadMarkType
+		{
+			NONE_TYPE = 1,
+			SOLID = 2,
+			BROKEN = 3,
+			SOLID_SOLID = 4,
+			SOLID_BROKEN = 5,
+			BROKEN_SOLID = 6,
+			BROKEN_BROKEN = 7,
+			BOTTS_DOTS = 8,
+			GRASS = 9,
+			CURB = 10
+		};
+
+		enum RoadMarkWeight
+		{
+			STANDARD,
+			BOLD
+		};
+
+		enum RoadMarkColor
+		{
+			STANDARD_COLOR, // equivalent to white
+			BLUE,
+			GREEN,
+			RED,
+			WHITE,
+			YELLOW
+		};
+
+		enum RoadMarkMaterial
+		{
+			STANDARD_MATERIAL // only "standard" is available for now
+		};
+
+		enum RoadMarkLaneChange
+		{
+			INCREASE,
+			DECREASE,
+			BOTH,
+			NONE_LANECHANGE
+		};
+
+		LaneRoadMark(double s_offset, RoadMarkType type, RoadMarkWeight weight, RoadMarkColor color, 
+		RoadMarkMaterial material, RoadMarkLaneChange lane_change, double width, double height): 
+		s_offset_(s_offset), type_(type), weight_(weight), color_(color), material_(material), lane_change_(lane_change), 
+		width_(width), height_(height) {}
+
+		void AddType(LaneRoadMarkType *lane_roadMarkType) { lane_roadMarkType_.push_back(lane_roadMarkType); }
+
+		double GetSOffset() { return s_offset_; }
+		double GetWidth() { return width_; }
+		double GetHeight() { return height_; }
+		RoadMarkType GetType() { return type_; }
+		RoadMarkWeight GetWeight() { return weight_; }
+		RoadMarkColor GetColor() { return color_; }
+		RoadMarkMaterial GetMaterial() { return material_; }
+		RoadMarkLaneChange GetLaneChange() { return lane_change_; }
+
+		int GetNumberOfRoadMarkTypes() { return (int)lane_roadMarkType_.size(); }
+		LaneRoadMarkType* GetLaneRoadMarkTypeByIdx(int idx);
+
+	private:
+		double s_offset_;
+		RoadMarkType type_;
+		RoadMarkWeight weight_;
+		RoadMarkColor color_;
+		RoadMarkMaterial material_;
+		RoadMarkLaneChange lane_change_;
+		double width_;
+		double height_;
+		std::vector<LaneRoadMarkType*> lane_roadMarkType_;
+	};
+
+	class LaneOffset
+	{
+	public:
+		LaneOffset(): s_(0.0), length_(0.0) {}
+		LaneOffset(double s, double a, double b, double c, double d) : s_(s), length_(0.0) 
+		{ 
+			polynomial_.Set(a, b, c, d); 
+		}
+		~LaneOffset() {}
+
+		void Set(double s, double a, double b, double c, double d) 
+		{
+			s_ = s;
+			polynomial_.Set(a, b, c, d); 
+		}
+		void SetLength(double length) { length_ = length; }
+		double GetS() { return s_; }
+		Polynomial GetPolynomial() { return polynomial_; }
+		double GetLength() { return length_; }
+		double GetLaneOffset(double s);
+		double GetLaneOffsetPrim(double s);
+		void Print();
+
+	private:
+		Polynomial polynomial_;
+		double s_;
+		double length_;
+	};
+
+	class Lane
+	{
+	public:
+		enum LanePosition
+		{
+			LANE_POS_CENTER,
+			LANE_POS_LEFT,
+			LANE_POS_RIGHT
+		};
+
+		typedef enum 
+		{
+			LANE_TYPE_NONE =          (1 << 0),
+			LANE_TYPE_DRIVING =       (1 << 1),
+			LANE_TYPE_STOP =          (1 << 2),
+			LANE_TYPE_SHOULDER =      (1 << 3),
+			LANE_TYPE_BIKING =        (1 << 4),
+			LANE_TYPE_SIDEWALK =      (1 << 5),
+			LANE_TYPE_BORDER =        (1 << 6),
+			LANE_TYPE_RESTRICTED =    (1 << 7),
+			LANE_TYPE_PARKING =       (1 << 8),
+			LANE_TYPE_BIDIRECTIONAL = (1 << 9),
+			LANE_TYPE_MEDIAN =        (1 << 10),
+			LANE_TYPE_SPECIAL1 =      (1 << 11),
+			LANE_TYPE_SPECIAL2 =      (1 << 12),
+			LANE_TYPE_SPECIAL3 =      (1 << 13),
+			LANE_TYPE_ROADMARKS =     (1 << 14),
+			LANE_TYPE_TRAM =          (1 << 15),
+			LANE_TYPE_RAIL =          (1 << 16),
+			LANE_TYPE_ENTRY =         (1 << 17),
+			LANE_TYPE_EXIT =          (1 << 18),
+			LANE_TYPE_OFF_RAMP =      (1 << 19),
+			LANE_TYPE_ON_RAMP =       (1 << 20),
+			LANE_TYPE_ANY_DRIVING =   LANE_TYPE_DRIVING |
+			                          LANE_TYPE_ENTRY | 
+			                          LANE_TYPE_EXIT | 
+			                          LANE_TYPE_OFF_RAMP | 
+			                          LANE_TYPE_ON_RAMP | 
+			                          LANE_TYPE_PARKING,
+			LANE_TYPE_ANY_ROAD =      LANE_TYPE_ANY_DRIVING |
+			                          LANE_TYPE_RESTRICTED |
+			                          LANE_TYPE_STOP,
+			LANE_TYPE_ANY =           (0xFFFFFFFF)
+		} LaneType;
+
+		// Construct & Destruct
+		Lane() : id_(0), type_(LaneType::LANE_TYPE_NONE), level_(0), offset_from_ref_(0.0), global_id_(0), lane_boundary_(0) {}
+		Lane(int id, Lane::LaneType type) : id_(id), type_(type), level_(1), offset_from_ref_(0), global_id_(0), lane_boundary_(0) {}
+		~Lane() {}
+
+		// Base Get Functions
+		int GetId() { return id_; }
+		double GetOffsetFromRef() { return offset_from_ref_; }
+		LaneType GetLaneType() { return type_; }
+		int GetGlobalId() { return global_id_; }
+
+		// Add Functions
+		void AddLink(LaneLink *lane_link) { link_.push_back(lane_link); }
+		void AddLaneWidth(LaneWidth *lane_width) { lane_width_.push_back(lane_width); }
+		void AddLaneRoadMark(LaneRoadMark *lane_roadMark) { lane_roadMark_.push_back(lane_roadMark); }
+		
+		// Get Functions
+		int GetNumberOfRoadMarks() { return (int)lane_roadMark_.size(); }
+		int GetNumberOfLinks() { return (int)link_.size(); }
+		int GetNumberOfLaneWidths() { return (int)lane_width_.size(); }
+
+		LaneLink *GetLink(LinkType type);
+		LaneWidth *GetWidthByIndex(int index);
+		LaneWidth *GetWidthByS(double s);
+		LaneRoadMark* GetLaneRoadMarkByIdx(int idx);
+
+		RoadMarkInfo GetRoadMarkInfoByS(int track_id, int lane_id, double s);
+		OSIPoints* GetOSIPoints() { return &osi_points_;}
+		std::vector<int> GetLineGlobalIds(); 
+		LaneBoundaryOSI* GetLaneBoundary() {return lane_boundary_; }
+		int GetLaneBoundaryGlobalId();
+
+		// Set Functions
+		void SetGlobalId();
+		void SetLaneBoundary(LaneBoundaryOSI *lane_boundary);
+		void SetOffsetFromRef(double offset) { offset_from_ref_ = offset; }
+
+		// Others
+		bool IsType(Lane::LaneType type);
+		bool IsCenter();
+		bool IsDriving();
+		void Print();
+		OSIPoints osi_points_;
+
+	private:
+		int id_;		// center = 0, left > 0, right < 0
+		int global_id_;  // Unique ID for OSI 
+		LaneType type_;
+		int level_;	// boolean, true = keep lane on level
+		double offset_from_ref_;
+		std::vector<LaneLink*> link_;
+		std::vector<LaneWidth*> lane_width_;
+		std::vector<LaneRoadMark*> lane_roadMark_;
+		LaneBoundaryOSI* lane_boundary_;
+	};
+
+	class LaneSection
+	{
+	public:
+		LaneSection(double s) : s_(s), length_(0) {}
+		void AddLane(Lane *lane);
+		double GetS() { return s_; }
+		Lane* GetLaneByIdx(int idx);
+		Lane* GetLaneById(int id);
+		int GetLaneIdByIdx(int idx);
+		int GetLaneIdxById(int id);
+		bool IsOSILaneById(int id);
+		int GetLaneGlobalIdByIdx(int idx);
+		int GetLaneGlobalIdById(int id);
+		double GetOuterOffset(double s, int lane_id);
+		double GetWidth(double s, int lane_id);
+		int GetClosestLaneIdx(double s, double t, double &offset, bool noZeroWidth, int laneTypeMask = Lane::LaneType::LANE_TYPE_ANY_DRIVING);
+		
+		/**
+		Get lateral position of lane center, from road reference lane (lane id=0)
+		Example: If lane id 1 is 5 m wide and lane id 2 is 4 m wide, then 
+				lane 1 center offset is 5/2 = 2.5 and lane 2 center offset is 5 + 4/2 = 7
+		@param s distance along the road segment
+		@param lane_id lane specifier, starting from center -1, -2, ... is on the right side, 1, 2... on the left 
+		*/
+		double GetCenterOffset(double s, int lane_id);
+		double GetOuterOffsetHeading(double s, int lane_id);
+		double GetCenterOffsetHeading(double s, int lane_id);
+		double GetLength() { return length_; }
+		int GetNumberOfLanes() { return (int)lane_.size(); }
+		int GetNumberOfDrivingLanes();
+		int GetNumberOfDrivingLanesSide(int side);
+		int GetNUmberOfLanesRight();
+		int GetNUmberOfLanesLeft();
+		void SetLength(double length) { length_ = length; }
+		int GetConnectingLaneId(int incoming_lane_id, LinkType link_type);
+		double GetWidthBetweenLanes(int lane_id1, int lane_id2, double s);
+		double GetOffsetBetweenLanes(int lane_id1, int lane_id2, double s);
+		void Print();
+
+	private:
+		double s_;
+		double length_;
+		std::vector<Lane*> lane_;
+	};
+
+	enum ContactPointType
+	{
+		CONTACT_POINT_UNKNOWN,
+		CONTACT_POINT_START,
+		CONTACT_POINT_END,
+		CONTACT_POINT_NONE,  // No contact point for element type junction
+	};
+
+	class RoadLink
+	{
+	public:
+		typedef enum 
+		{
+			ELEMENT_TYPE_UNKNOWN,
+			ELEMENT_TYPE_ROAD,
+			ELEMENT_TYPE_JUNCTION,
+		} ElementType;
+
+		RoadLink() : type_(NONE), element_id_(-1), element_type_(ELEMENT_TYPE_UNKNOWN), contact_point_type_(CONTACT_POINT_UNKNOWN) {}
+		RoadLink(LinkType type, ElementType element_type, int element_id, ContactPointType contact_point_type) :
+			type_(type), element_id_(element_id), element_type_(element_type),  contact_point_type_(contact_point_type) {}
+		RoadLink(LinkType type, pugi::xml_node node);
+
+		int GetElementId() { return element_id_; }
+		LinkType GetType() { return type_; }
+		RoadLink::ElementType GetElementType() { return element_type_; }
+		ContactPointType GetContactPointType() { return contact_point_type_; }
+
+		void Print();
+
+	private:
+		LinkType type_;
+		int element_id_;
+		ElementType element_type_;
+		ContactPointType contact_point_type_;
+	};
+
+	struct LaneInfo
+	{
+		int lane_section_idx_;
+		int lane_id_;
+	};
+
+	enum RoadType
+	{
+		ROADTYPE_UNKNOWN,
+		ROADTYPE_RURAL,
+		ROADTYPE_MOTORWAY,
+		ROADTYPE_TOWN,
+		ROADTYPE_LOWSPEED,
+		ROADTYPE_PEDESTRIAN,
+		ROADTYPE_BICYCLE
+	};
+
+	typedef struct
+	{
+		double s_;
+		RoadType road_type_;
+		double speed_;  // m/s
+	} RoadTypeEntry;
+
+	class Signal
+	{
+	public:
+		enum Orientation
+		{
+			POSITIVE,
+			NEGATIVE,
+			NONE,
+		};
+
+		enum Type
+		{
+			NONETYPE,
+			T1000001, // traditional red-yellow-green light
+			T1000002, // 2 subtypes: pedestrian light (red or green & only red)
+			T1000007, // 4 subtypes: pedestrian + cyclists light (red or green & only red & only yellow & only green)
+			T1000008, // 3 subtypes: yellow light (no arrow & left arrow & right arrow)
+			T1000009, // 2 subtypes: red-yellow light & solid yellow or green light
+			T1000010, // 2 subtypes: yellow-green light (only left arrows & only right arrows)
+			T1000011, // 5 subtypes: red-yellow-green light (only left arrows & only right arrows & only straight arrows & straight+left arrows & straight+right arrows)
+			T1000012, // 2 subtypes: green light (left arrow & right arrow)
+			T1000013, // red-green cyclist light
+			T1000014, // yellow tram light
+			T1000015  // yellow pedestrian light
+		};
+
+		enum SubType
+		{
+			NONESUBTYPE,
+			SUBT10,
+			SUBT20,
+			SUBT30,
+			SUBT40,
+			SUBT50
+		};
+
+		Signal(double s, double t, int id, std::string name, bool dynamic, Orientation orientation, double z_offset, std::string country,
+		Type type, SubType sub_type, double value, std::string unit, double height, double width, std::string text, double h_offset,
+		double pitch, double roll) : s_(s), t_(t), id_(id), name_(name), dynamic_(dynamic), orientation_(orientation), z_offset_(z_offset), 
+		country_(country), type_(type), sub_type_(sub_type), value_(value), unit_(unit), height_(height), width_(width), text_(text),
+		h_offset_(h_offset), pitch_(pitch), roll_(roll), length_(0) {}
+				
+		std::string GetName() { return name_; }
+		int GetId() { return id_; }
+		double GetS() { return s_; }
+		double GetT() { return t_; }
+		void SetLength(double length) { length_ = length; }
+		double GetLength() { return length_; }
+		double GetHOffset() { return h_offset_; }
+		double GetZOffset() { return z_offset_; }
+		Orientation GetOrientation() { return orientation_; }
+
+	private:
+		double s_;
+		double t_;
+		int id_;
+		std::string name_;
+		bool dynamic_;
+		Orientation orientation_;
+		double z_offset_;
+		std::string country_;
+		Type type_;
+		SubType sub_type_;
+		double value_;
+		std::string unit_;
+		double height_;
+		double width_;
+		std::string text_;
+		double h_offset_;
+		double pitch_;
+		double roll_;
+		double length_;
+	};
+
+	class OutlineCorner
+	{
+	public:
+		virtual void GetPos(double& x, double& y, double& z) = 0;
+		virtual double GetHeight() = 0;
+	};
+
+	class OutlineCornerRoad : public OutlineCorner
+	{
+	public:
+		OutlineCornerRoad(int roadId, double s, double t, double dz, double height);
+		void GetPos(double& x, double& y, double& z);
+		double GetHeight() { return height_; }
+
+	private:
+		int roadId_;
+		double s_, t_, dz_, height_;
+	};
+
+	class OutlineCornerLocal : public OutlineCorner
+	{
+	public:
+		OutlineCornerLocal(int roadId, double s, double t, double u, double v, double zLocal, double height, double heading);
+		void GetPos(double& x, double& y, double& z);
+		double GetHeight() { return height_; }
+
+	private:
+		int roadId_;
+		double s_, t_, u_, v_, zLocal_, height_, heading_;
+	};
+	
+	class Outline
+	{
+	public:
+		typedef enum
+		{
+			FILL_TYPE_GRASS,
+			FILL_TYPE_CONCRETE,
+			FILL_TYPE_COBBLE,
+			FILL_TYPE_ASPHALT,
+			FILL_TYPE_PAVEMENT,
+			FILL_TYPE_GRAVEL,
+			FILL_TYPE_SOIL,
+			FILL_TYPE_UNDEFINED
+		} FillType;
+		
+		int id_;
+		FillType fillType_;
+		bool closed_;
+		std::vector<OutlineCorner*> corner_;
+
+		Outline(int id, FillType fillType, bool closed) : 
+			id_(id), fillType_(fillType), closed_(closed) {}
+		
+		~Outline()
+		{
+			for (size_t i = 0; i < corner_.size(); i++) delete(corner_[i]);
+			corner_.clear();
+		}
+
+		void AddCorner(OutlineCorner* outlineCorner) { corner_.push_back(outlineCorner); }
+	};
+
+	class Object
+	{
+	public:
+		enum Orientation
+		{
+			POSITIVE,
+			NEGATIVE,
+			NONE,
+		};
+
+		Object(double s, double t, int id, std::string name, Orientation orientation, double z_offset, std::string type,
+			double length, double height, double width, double heading, double pitch, double roll) :
+			s_(s), t_(t), id_(id), name_(name), orientation_(orientation), z_offset_(z_offset), type_(type),
+			length_(length), height_(height), width_(width), heading_(heading), pitch_(pitch), roll_(roll) {}
+
+		~Object()
+		{
+			for (size_t i = 0; i < outlines_.size(); i++) delete(outlines_[i]);
+			outlines_.clear();
+		}
+
+		std::string GetName() { return name_; }
+		double GetS() { return s_; }
+		double GetT() { return t_; }
+		double GetHOffset() { return heading_; }
+		double GetZOffset() { return z_offset_; }
+		double GetHeight() { return height_; }
+		Orientation GetOrientation() { return orientation_; }
+		void AddOutline(Outline* outline) { outlines_.push_back(outline); }
+		int GetNumberOfOutlines() { return (int)outlines_.size(); }
+		Outline* GetOutline(int i) { return (0 <= i && i < outlines_.size()) ? outlines_[i] : 0; }
+
+	private:
+		std::string type_;
+		std::string name_;
+		int id_;
+		double s_;
+		double t_;
+		double z_offset_;
+		Orientation orientation_;
+		double length_;
+		double height_;
+		double width_;
+		double heading_;
+		double pitch_;
+		double roll_;
+		std::vector<Outline*> outlines_;
+	};
+
+	class Road
+	{
+	public:
+
+		Road(int id, std::string name) : id_(id), name_(name), length_(0), junction_(0) {}
+		~Road();
+
+		void Print();
+		void SetId(int id) { id_ = id; }
+		int GetId() { return id_; }
+		void SetName(std::string name) { name_ = name; }
+		Geometry *GetGeometry(int idx);
+		int GetNumberOfGeometries() { return (int)geometry_.size(); }
+
+		/** 
+		Retrieve the lanesection specified by vector element index (idx)
+		useful for iterating over all available lane sections, e.g:
+		for (int i = 0; i < road->GetNumberOfLaneSections(); i++)
+		{
+			int n_lanes = road->GetLaneSectionByIdx(i)->GetNumberOfLanes();
+		...
+		@param idx index into the vector of lane sections
+		*/
+		LaneSection *GetLaneSectionByIdx(int idx);
+		
+		/**
+		Retrieve the lanesection index at specified s-value
+		@param s distance along the road segment
+		*/
+		int GetLaneSectionIdxByS(double s, int start_at = 0);
+
+		/**
+		Retrieve the lanesection at specified s-value
+		@param s distance along the road segment
+		*/
+		LaneSection* GetLaneSectionByS(double s, int start_at = 0) { return GetLaneSectionByIdx(GetLaneSectionIdxByS(s, start_at)); }
+
+		/**
+		Get lateral position of lane center, from road reference lane (lane id=0)
+		Example: If lane id 1 is 5 m wide and lane id 2 is 4 m wide, then
+		lane 1 center offset is 5/2 = 2.5 and lane 2 center offset is 5 + 4/2 = 7
+		@param s distance along the road segment
+		@param lane_id lane specifier, starting from center -1, -2, ... is on the right side, 1, 2... on the left
+		*/
+		double GetCenterOffset(double s, int lane_id);
+
+		LaneInfo GetLaneInfoByS(double s, int start_lane_link_idx, int start_lane_id, int laneTypeMask = Lane::LaneType::LANE_TYPE_ANY_DRIVING);
+		double GetLaneWidthByS(double s, int lane_id);
+		double GetSpeedByS(double s);
+		bool GetZAndPitchByS(double s, double *z, double *pitch, int *index);
+		bool UpdateZAndRollBySAndT(double s, double t, double *z, double *roll, int *index);
+		int GetNumberOfLaneSections() { return (int)lane_section_.size(); }
+		std::string GetName() { return name_; }
+		void SetLength(double length) { length_ = length; }
+		double GetLength() { return length_; }
+		void SetJunction(int junction) { junction_ = junction; }
+		int GetJunction() { return junction_; }
+		void AddLink(RoadLink *link) { link_.push_back(link); }
+		void AddRoadType(RoadTypeEntry *type) { type_.push_back(type); }
+		RoadLink *GetLink(LinkType type);
+		void AddLine(Line *line);
+		void AddArc(Arc *arc);
+		void AddSpiral(Spiral *spiral);
+		void AddPoly3(Poly3 *poly3);
+		void AddParamPoly3(ParamPoly3 *param_poly3);
+		void AddElevation(Elevation *elevation);
+		void AddSuperElevation(Elevation *super_elevation);
+		void AddLaneSection(LaneSection *lane_section);
+		void AddLaneOffset(LaneOffset *lane_offset);
+		void AddSignal(Signal *signal);
+		void AddObject(Object* object);
+		Elevation *GetElevation(int idx);
+		Elevation *GetSuperElevation(int idx);
+		int GetNumberOfSignals();
+		Signal* GetSignal(int idx);
+		int GetNumberOfObjects() { return (int)object_.size(); }
+		Object* GetObject(int idx);
+		int GetNumberOfElevations() { return (int)elevation_profile_.size(); }
+		int GetNumberOfSuperElevations() { return (int)super_elevation_profile_.size(); }
+		double GetLaneOffset(double s);
+		double GetLaneOffsetPrim(double s);
+		int GetNumberOfLanes(double s);
+		int GetNumberOfDrivingLanes(double s);
+		Lane* GetDrivingLaneByIdx(double s, int idx);
+		int GetNumberOfDrivingLanesSide(double s, int side);  // side = -1 right, 1 left
+
+		/// <summary>Get width of road</summary>
+		/// <param name="s">Longitudinal position/distance along the road</param>
+		/// <param name="side">Side of the road: -1=right, 1=left, 0=both</param>
+		/// <param name="laneTypeMask">Bitmask specifying what lane types to consider - see Lane::LaneType</param>
+		/// <returns>Width (m)</returns>
+		double GetWidth(double s, int side, int laneTypeMask = Lane::LaneType::LANE_TYPE_ANY);   // side: -1=right, 1=left, 0=both
+
+	protected:
+		int id_;
+		std::string name_;
+		double length_;
+		int junction_;
+		std::vector<RoadTypeEntry*> type_;
+		std::vector<RoadLink*> link_;
+		std::vector<Geometry*> geometry_;
+		std::vector<Elevation*> elevation_profile_;
+		std::vector<Elevation*> super_elevation_profile_;
+		std::vector<LaneSection*> lane_section_;
+		std::vector<LaneOffset*> lane_offset_;
+		std::vector<Signal*> signal_;
+		std::vector<Object*> object_;
+	};
+
+	class LaneRoadLaneConnection
+	{
+	public:
+		LaneRoadLaneConnection() :
+			lane_id_(0), connecting_road_id_(-1), connecting_lane_id_(0), contact_point_(ContactPointType::CONTACT_POINT_NONE) {}
+		LaneRoadLaneConnection(int lane_id, int connecting_road_id, int connecting_lane_id) :
+			lane_id_(lane_id), connecting_road_id_(connecting_road_id), connecting_lane_id_(connecting_lane_id), contact_point_(ContactPointType::CONTACT_POINT_NONE) {}
+		void SetLane(int id) { lane_id_ = id; }
+		void SetConnectingRoad(int id) { connecting_road_id_ = id; }
+		void SetConnectingLane(int id) { connecting_lane_id_ = id; }
+		int GetLaneId() { return lane_id_; }
+		int GetConnectingRoadId() { return connecting_road_id_; }
+		int GetConnectinglaneId() { return connecting_lane_id_; }
+
+		ContactPointType contact_point_;
+	private:
+		int lane_id_;
+		int connecting_road_id_;
+		int connecting_lane_id_;
+	};
+
+	class JunctionLaneLink
+	{
+	public:
+		JunctionLaneLink(int from, int to) : from_(from), to_(to) {}
+		int from_;
+		int to_;
+		void Print() { printf("JunctionLaneLink: from %d to %d\n", from_, to_); }
+	};
+
+	class Connection
+	{
+	public:
+		Connection(Road *incoming_road, Road *connecting_road, ContactPointType contact_point);
+		~Connection();
+		int GetNumberOfLaneLinks() { return (int)lane_link_.size(); }
+		JunctionLaneLink *GetLaneLink(int idx) { return lane_link_[idx]; }
+		int GetConnectingLaneId(int incoming_lane_id);
+		Road *GetIncomingRoad() { return incoming_road_; }
+		Road *GetConnectingRoad() { return connecting_road_; }
+		ContactPointType GetContactPoint() { return contact_point_; }
+		void AddJunctionLaneLink(int from, int to);
+		void Print();
+
+	private:
+		Road *incoming_road_;
+		Road *connecting_road_;
+		ContactPointType contact_point_;
+		std::vector<JunctionLaneLink*> lane_link_;
+	};
+
+	class Junction
+	{
+	public:
+		typedef enum
+		{
+			RANDOM,
+			STRAIGHT,
+		} JunctionStrategyType;
+
+		Junction(int id, std::string name) : id_(id), name_(name) {}
+		~Junction();
+		int GetId() { return id_; }
+		std::string GetName() { return name_; }
+		int GetNumberOfConnections() { return (int)connection_.size(); }
+		int GetNumberOfRoadConnections(int roadId, int laneId);
+		LaneRoadLaneConnection GetRoadConnectionByIdx(int roadId, int laneId, int idx, 
+			int laneTypeMask = Lane::LaneType::LANE_TYPE_ANY_DRIVING);
+		void AddConnection(Connection *connection) { connection_.push_back(connection); }
+		int GetNoConnectionsFromRoadId(int incomingRoadId);
+		Connection *GetConnectionByIdx(int idx) { return connection_[idx]; }
+		int GetConnectingRoadIdFromIncomingRoadId(int incomingRoadId, int index);
+		void Print();
+
+	private:
+		std::vector<Connection*> connection_;
+		int id_;
+		std::string name_;
+	};
+
+	class OpenDrive
+	{
+	public:
+		OpenDrive() {}; 
+		OpenDrive(const char *filename);
+		~OpenDrive();
+
+		/**
+			Load a road network, specified in the OpenDRIVE file format
+			@param filename OpenDRIVE file
+			@param replace If true any old road data will be erased, else new will be added to the old
+		*/
+		bool LoadOpenDriveFile(const char *filename, bool replace = true);
+		
+		/**
+			Initialize the global ids for lanes
+		*/ 
+		void InitGlobalLaneIds();
+
+		/**
+			Get the filename of currently loaded OpenDRIVE file
+		*/
+		std::string GetOpenDriveFilename() { return odr_filename_; }
+
+		/**
+			Setting information based on the OSI standards for OpenDrive elements
+		*/
+		bool SetRoadOSI();
+		bool CheckLaneOSIRequirement(std::vector<double> x0, std::vector<double> y0, std::vector<double> x1, std::vector<double> y1);
+		void SetLaneOSIPoints();
+		void SetRoadMarkOSIPoints();
+
+		/**
+			Checks all lanes - if a lane has RoadMarks it does nothing. If a lane does not have roadmarks 
+			then it creates a LaneBoundary following the lane border (left border for left lanes, right border for right lanes)
+		*/
+		void SetLaneBoundaryPoints();
+		
+		/**
+			Retrieve a road segment specified by road ID 
+			@param id road ID as specified in the OpenDRIVE file
+		*/
+		Road* GetRoadById(int id);
+
+		/**
+			Retrieve a road segment specified by road vector element index
+			useful for iterating over all available road segments, e.g:
+			for (int i = 0; i < GetNumOfRoads(); i++)
+			{
+				int n_lanes = GetRoadyIdx(i)->GetNumberOfLanes();
+			...
+			@param idx index into the vector of roads
+		*/
+		Road* GetRoadByIdx(int idx);
+		Geometry* GetGeometryByIdx(int road_idx, int geom_idx);
+		int GetTrackIdxById(int id);
+		int GetTrackIdByIdx(int idx);
+		int GetNumOfRoads() { return (int)road_.size(); }
+		Junction* GetJunctionById(int id);
+		Junction* GetJunctionByIdx(int idx);
+
+		int GetNumOfJunctions() { return (int)junction_.size(); }
+		/**
+			Check if two roads are connected directly
+			@param road1_id Id of the first road
+			@param road2_id Id of the second road
+			@param angle if connected, the angle between road 2 and road 1 is returned here
+			@return 0 if not connected, -1 if road 2 is the predecessor of road 1, +1 if road 2 is the successor of road 1
+		*/
+
+		int IsDirectlyConnected(int road1_id, int road2_id, double &angle);
+		bool IsIndirectlyConnected(int road1_id, int road2_id, int* &connecting_road_id, int* &connecting_lane_id, int lane1_id = 0, int lane2_id = 0);
+
+		/**
+			Add any missing connections so that road connectivity is two-ways
+			Look at all road connections, and make sure they are defined both ways
+			@param idx index into the vector of roads
+			@return number of added connections
+		*/
+		int CheckConnections();
+		int CheckLink(Road *road, RoadLink *link, ContactPointType expected_contact_point_type);
+		int CheckConnectedRoad(Road *road, RoadLink *link, ContactPointType expected_contact_point_type, RoadLink *link2);
+		int CheckJunctionConnection(Junction *junction, Connection *connection);
+		std::string ContactPointType2Str(ContactPointType type);
+		std::string ElementType2Str(RoadLink::ElementType type);
+
+		void Print();
+	
+	private:
+		pugi::xml_node root_node_;
+		std::vector<Road*> road_;
+		std::vector<Junction*> junction_;
+		std::string odr_filename_;
+	};
+
+	typedef struct
+	{
+		double pos[3];		// position, in global coordinate system
+		double heading;		// road heading at steering target point
+		double pitch;		// road pitch (inclination) at steering target point
+		double roll;		// road roll (camber) at steering target point
+		double width;		// lane width
+		double curvature;	// road curvature at steering target point
+		double speed_limit; // speed limit given by OpenDRIVE type entry
+		int roadId;         // road ID 
+		int laneId;         // lane ID
+		double laneOffset;  // lane offset (lateral distance from lane center) 
+		double s;           // s (longitudinal distance along reference line)
+		double t;           // t (lateral distance from reference line)
+	} RoadLaneInfo;
+	
+	typedef struct
+	{
+		RoadLaneInfo road_lane_info;  // Road info at probe target position
+		double relative_pos[3];       // probe target position relative vehicle (pivot position object) coordinate system
+		double relative_h;			  // heading angle to probe target from and relatove to vehicle (pivot position)
+	} RoadProbeInfo;
+
+	typedef struct
+	{
+		double ds;				// delta s (longitudinal distance)
+		double dt;				// delta t (lateral distance)
+		int dLaneId;			// delta laneId (increasing left and decreasing to the right)
+	} PositionDiff;
+
+	// Forward declarations
+	class Route;
+	class Trajectory;
+
+	class Position
+	{
+	public:
+
+		enum PositionType
+		{
+			NORMAL,
+			ROUTE,
+			RELATIVE_OBJECT,
+			RELATIVE_WORLD,
+			RELATIVE_LANE,
+			RELATIVE_ROAD
+		};
+
+		enum OrientationType
+		{
+			ORIENTATION_RELATIVE,
+			ORIENTATION_ABSOLUTE
+		};
+
+		enum LookAheadMode
+		{
+			LOOKAHEADMODE_AT_LANE_CENTER,
+			LOOKAHEADMODE_AT_ROAD_CENTER,
+			LOOKAHEADMODE_AT_CURRENT_LATERAL_OFFSET,
+		};
+
+		enum ErrorCode
+		{
+			ERROR_NO_ERROR = 0,
+			ERROR_GENERIC = -1,
+			ERROR_END_OF_ROAD = -2,
+			ERROR_END_OF_ROUTE = -3,
+			ERROR_OFF_ROAD = -4,
+		};
+
+		enum UpdateTrackPosMode
+		{
+			UPDATE_NOT_XYZH,
+			UPDATE_XYZ,
+			UPDATE_XYZH
+		};
+
+		typedef enum
+		{
+			POS_STATUS_END_OF_ROAD = (1 << 0),
+			POS_STATUS_END_OF_ROUTE = (1 << 1)
+		} POSITION_STATUS_MODES;
+
+		explicit Position();
+		explicit Position(int track_id, double s, double t);
+		explicit Position(int track_id, int lane_id, double s, double offset);
+		explicit Position(double x, double y, double z, double h, double p, double r);
+		explicit Position(double x, double y, double z, double h, double p, double r, bool calculateTrackPosition);
+		~Position();
+		
+		void Init();
+		static bool LoadOpenDrive(const char *filename);
+		static OpenDrive* GetOpenDrive();
+		int GotoClosestDrivingLaneAtCurrentPosition();
+
+		/**
+		Specify position by track coordinate (road_id, s, t)
+		@param track_id Id of the road (track)
+		@param s Distance to the position along and from the start of the road (track)
+		@param updateMode UPDATE_NOT_XYZH: no update of x, y, z, h UPDATE_XYZ: recalculate x, y, z UPDATE_XYZH relaculate x, y, z and align h
+		@return Non zero return value indicates error of some kind
+		*/
+		int SetTrackPos(int track_id, double s, double t, UpdateTrackPosMode updateMode = UpdateTrackPosMode::UPDATE_XYZH);
+		void ForceLaneId(int lane_id);
+		int SetLanePos(int track_id, int lane_id, double s, double offset, int lane_section_idx = -1);
+		void SetLaneBoundaryPos(int track_id, int lane_id, double s, double offset, int lane_section_idx = -1);
+		void SetRoadMarkPos(int track_id, int lane_id, int roadmark_idx, int roadmarktype_idx, int roadmarkline_idx, double s, double offset, int lane_section_idx = -1);
+
+		/**
+		Specify position by cartesian x, y, z and heading, pitch, roll
+		@param x x
+		@param y y
+		@param z z
+		@param h heading
+		@param p pitch
+		@param r roll
+		@param updateTrackPos True: road position will be calculated False: don't update road position
+		@return Non zero return value indicates error of some kind
+		*/
+		int SetInertiaPos(double x, double y, double z, double h, double p, double r, bool updateTrackPos = true);
+
+		/**
+		Specify position by cartesian x, y and heading. z, pitch and roll will be aligned to road.
+		@param x x
+		@param y y
+		@param h heading
+		@param updateTrackPos True: road position will be calculated False: don't update road position
+		@return Non zero return value indicates error of some kind
+		*/
+		int SetInertiaPos(double x, double y, double h, bool updateTrackPos = true);
+		void SetHeading(double heading);
+		void SetHeadingRelative(double heading);
+		void SetHeadingRelativeRoadDirection(double heading);
+		void SetRoll(double roll);
+		void SetRollRelative(double roll);
+		void SetPitch(double roll);
+		void SetPitchRelative(double pitch);
+
+		/**
+		Specify position by cartesian coordinate (x, y, z, h)
+		@param x X-coordinate
+		@param y Y-coordinate
+		@param z Z-coordinate
+		@param alignZAndPitch Align Z-coordinate to road elevation and Heading to tangent of the road 
+		@return Non zero return value indicates error of some kind
+		*/
+		int XYZH2TrackPos(double x, double y, double z, double h, bool alignZAndPitch = true);
+		
+		int MoveToConnectingRoad(RoadLink *road_link, ContactPointType &contact_point_type, Junction::JunctionStrategyType strategy = Junction::RANDOM);
+		
+		void SetRelativePosition(Position* rel_pos, PositionType type)
+		{
+			rel_pos_ = rel_pos;
+			type_ = type;
+		}
+
+		Position* GetRelativePosition() { return rel_pos_; }
+
+		void ReleaseRelation();
+
+		void SetRoute(Route *route);
+		void CalcRoutePosition();
+		const roadmanager::Route* GetRoute() const { return route_; }
+		Route* GetRoute() { return route_; }
+		Trajectory* GetTrajectory() { return trajectory_; }
+
+		void SetTrajectory(Trajectory* trajectory);
+
+		/**
+		Set the current position along the route.
+		@param position A regular position created with road, lane or world coordinates
+		@return Non zero return value indicates error of some kind
+		*/
+		int SetRoutePosition(Position *position);
+
+		/**
+		Retrieve the S-value of the current route position. Note: This is the S along the
+		complete route, not the actual individual roads.
+		*/
+		double GetRouteS() { return s_; }
+
+		/**
+		Move current position forward, or backwards, ds meters along the route
+		@param ds Distance to move, negative will move backwards
+		@return Non zero return value indicates error of some kind, most likely End Of Route
+		*/
+		int MoveRouteDS(double ds);
+
+		/**
+		Move current position to specified S-value along the route
+		@param route_s Distance to move, negative will move backwards
+		@param laneId Explicit (not delta/offset) lane ID
+		@param laneOffset Explicit (not delta/offset) lane offset value
+		@return Non zero return value indicates error of some kind
+		*/
+		int SetRouteLanePosition(Route* route, double route_s, int laneId, double  laneOffset);
+
+		/**
+		Move current position to specified S-value along the route
+		@param route_s Distance to move, negative will move backwards
+		@return Non zero return value indicates error of some kind, most likely End Of Route
+		*/
+		int SetRouteS(Route* route, double route_s);
+
+		/**
+		Move current position forward, or backwards, ds meters along the trajectory
+		@param ds Distance to move, negative will move backwards
+		@return Non zero return value indicates error of some kind
+		*/
+		int MoveTrajectoryDS(double ds);
+
+		/**
+		Move current position to specified S-value along the trajectory
+		@param trajectory_s Distance to move, negative will move backwards
+		@return Non zero return value indicates error of some kind
+		*/
+		int SetTrajectoryS(Trajectory* trajectory, double trajectory_s);
+
+		int SetTrajectoryPosByTime(Trajectory* trajectory, double time);
+
+		/**
+		Retrieve the S-value of the current route position. Note: This is the S along the
+		complete route, not the actual individual roads.
+		*/
+		double GetTrajectoryS() { return s_trajectory_; }
+
+		/**
+		Straight (not route) distance between the current position and the one specified in argument
+		@param target_position The position to measure distance from current position. 
+		@param x (meter). X component of the relative distance.
+		@param y (meter). Y component of the relative distance.
+		@return distance (meter). Negative if the specified position is behind the current one.
+		*/
+		double getRelativeDistance(Position target_position, double &x, double &y);
+
+		/**
+		Find out the difference between two position objects, in effect subtracting the values 
+		It can be used to calculate the distance from current position to another one (pos_b)
+		@param pos_b The position from which to subtract the current position (this position object)
+		@return true if position found and parameter values are valid, else false
+		*/
+		bool Delta(Position pos_b, PositionDiff &diff);
+
+		/**
+		Is the current position ahead of the one specified in argument
+		This method is more efficient than getRelativeDistance
+		@param target_position The position to compare the current to.
+		@return true of false
+		*/
+		bool IsAheadOf(Position target_position);
+
+		/**
+		Get information suitable for driver modeling of a point at a specified distance from object along the road ahead
+		@param lookahead_distance The distance, along the road, to the point
+		@param data Struct to fill in calculated values, see typdef for details
+		@param lookAheadMode Measurement strategy: Along reference lane, lane center or current lane offset. See roadmanager::Position::LookAheadMode enum
+		@return 0 if successful, -1 if not
+		*/
+		int GetProbeInfo(double lookahead_distance, RoadProbeInfo *data, LookAheadMode lookAheadMode);
+
+		/**
+		Get information suitable for driver modeling of a point at a specified distance from object along the road ahead
+		@param target_pos The target position
+		@param data Struct to fill in calculated values, see typdef for details
+		@return 0 if successful, -1 if not
+		*/
+		int GetProbeInfo(Position *target_pos, RoadProbeInfo *data);
+
+		/**
+		Get information of current lane at a specified distance from object along the road ahead
+		@param lookahead_distance The distance, along the road, to the point
+		@param data Struct to fill in calculated values, see typdef for details
+		@param lookAheadMode Measurement strategy: Along reference lane, lane center or current lane offset. See roadmanager::Position::LookAheadMode enum
+		@return 0 if successful, -1 if not
+		*/
+		int GetRoadLaneInfo(double lookahead_distance, RoadLaneInfo *data, LookAheadMode lookAheadMode);
+		int GetRoadLaneInfo(RoadLaneInfo *data);
+
+		/**
+		Get information of current lane at a specified distance from object along the road ahead
+		@param lookahead_distance The distance, along the road, to the point
+		@param data Struct to fill in calculated values, see typdef for details
+		@return 0 if successful, -1 if not
+		*/
+//		int GetTrailInfo(double lookahead_distance, RoadLaneInfo *data);
+
+		void CalcProbeTarget(Position *target, RoadProbeInfo *data);
+
+		/**
+		Move position along the road network, forward or backward, from the current position
+		It will automatically follow connecting lanes between connected roads 
+		If multiple options (only possible in junctions) it will choose randomly 
+		@param ds distance to move from current position
+		*/
+		int MoveAlongS(double ds, double dLaneOffset = 0, Junction::JunctionStrategyType strategy = Junction::JunctionStrategyType::RANDOM);
+
+		/**
+		Retrieve the track/road ID from the position object
+		@return track/road ID
+		*/
+		int GetTrackId();
+
+		/**
+		Retrieve the lane ID from the position object
+		@return lane ID
+		*/
+		int GetLaneId();
+		/**
+		Retrieve the global lane ID from the position object
+		@return lane ID
+		*/
+		int GetLaneGlobalId();
+
+		/**
+		Retrieve a road segment specified by road ID
+		@param id road ID as specified in the OpenDRIVE file
+		*/
+		Road *GetRoadById(int id) { return GetOpenDrive()->GetRoadById(id);	}
+
+		/**
+		Retrieve the s value (distance along the road segment)
+		*/
+		double GetS();
+
+		/**
+		Retrieve the t value (lateral distance from reference lanem (id=0))
+		*/
+		double GetT();
+
+		/**
+		Retrieve the offset from current lane
+		*/
+		double GetOffset();
+
+		/**
+		Retrieve the world coordinate X-value
+		*/
+		double GetX();
+
+		/**
+		Retrieve the world coordinate Y-value
+		*/
+		double GetY();
+
+		/**
+		Retrieve the world coordinate Z-value
+		*/
+		double GetZ();
+
+		/**
+		Retrieve the road Z-value 
+		*/
+		double GetZRoad() const { return z_road_; }
+
+		/**
+		Retrieve the world coordinate heading angle (radians)
+		*/
+		double GetH();
+
+		/**
+		Retrieve the road heading angle (radians)
+		*/
+		double GetHRoad() const { return h_road_; }
+
+		/**
+		Retrieve the road heading angle (radians) relative driving direction (lane sign considered)
+		*/
+		double GetHRoadInDrivingDirection();
+
+		/**
+		Retrieve the heading angle (radians) relative driving direction (lane sign considered)
+		*/
+		double GetHRelativeDrivingDirection();
+
+		/**
+		Retrieve the relative heading angle (radians)
+		*/
+		double GetHRelative();
+
+		/**
+		Retrieve the world coordinate pitch angle (radians)
+		*/
+		double GetP();
+
+		/**
+		Retrieve the road pitch value
+		*/
+		double GetPRoad() const { return p_road_; }
+
+		/**
+		Retrieve the relative pitch angle (radians)
+		*/
+		double GetPRelative();
+
+		/**
+		Retrieve the world coordinate roll angle (radians)
+		*/
+		double GetR();
+
+		/**
+		Retrieve the road roll value
+		*/
+		double GetRRoad() const { return r_road_; }
+
+		/**
+		Retrieve the relative roll angle (radians)
+		*/
+		double GetRRelative();
+
+		/**
+		Retrieve the road pitch value, driving direction considered
+		*/
+		double GetPRoadInDrivingDirection();
+
+
+		/**
+		Retrieve the road curvature at current position
+		*/
+		double GetCurvature();
+
+		/**
+		Retrieve the speed limit at current position
+		*/
+		double GetSpeedLimit();
+
+		/**
+		Retrieve the road heading/direction at current position, and in the direction given by current lane
+		*/
+		double GetDrivingDirection();
+
+		PositionType GetType() { return type_; }
+
+		void SetTrackId(int trackId) { track_id_ = trackId; }
+		void SetLaneId(int laneId) { lane_id_ = laneId; }
+		void SetS(double s) { s_ = s; }
+		void SetOffset(double offset) { offset_ = offset; }
+		void SetT(double t) { t_ = t; }
+		void SetX(double x) { x_ = x; }
+		void SetY(double y) { y_ = y; }
+		void SetZ(double z) { z_ = z; }
+		void SetH(double h) { h_ = h; }
+		void SetP(double p) { p_ = p; }
+		void SetR(double r) { r_ = r; }
+		void SetVel(double x_vel, double y_vel, double z_vel) { velX_ = x_vel, velY_ = y_vel, velZ_ = z_vel; }
+		void SetAcc(double x_acc, double y_acc, double z_acc) { accX_ = x_acc, accY_ = y_acc, accZ_ = z_acc; }
+		void SetAngularVel(double h_vel, double p_vel, double r_vel) { h_rate_ = h_vel, p_rate_ = p_vel, r_rate_ = r_vel; }
+		void SetAngularAcc(double h_acc, double p_acc, double r_acc) { h_acc_ = h_acc, p_acc_ = p_acc, r_acc_ = r_acc; }
+		double GetVelX() { return velX_; }
+		double GetVelY() { return velY_; }
+		double GetVelZ() { return velZ_; }
+		double GetAccX() { return accX_; }
+		double GetAccY() { return accY_; }
+		double GetAccZ() { return accZ_; }
+		double GetHRate() { return h_rate_; }
+		double GetPRate() { return p_rate_; }
+		double GetRRate() { return r_rate_; }
+		double GetHAcc() { return h_acc_; }
+		double GetPAcc() { return p_acc_; }
+		double GetRAcc() { return r_acc_; }
+
+		int GetStatusBitMask() { return status_; }
+
+		void SetOrientationType(OrientationType type) { orientation_type_ = type; }
+
+		/**
+		Specify which lane types the position object snaps to (is aware of)
+		@param laneTypes A combination (bitmask) of lane types
+		@return -
+		*/
+		void SetSnapLaneTypes(int laneTypeMask) { snapToLaneTypes_ = laneTypeMask; }
+
+		void CopyRMPos(Position *from);
+
+		void PrintTrackPos();
+		void PrintLanePos();
+		void PrintInertialPos();
+
+		void Print();
+		void PrintXY();
+
+		bool IsOffRoad();
+
+		void ReplaceObjectRefs(Position* pos1, Position* pos2)
+		{
+			if (rel_pos_ == pos1)
+			{
+				rel_pos_ = pos2;
+			}
+		}
+
+		/**
+			Controls whether to keep lane ID regardless of lateral position or snap to closest lane (default)
+			@parameter mode True=keep lane False=Snap to closest (default)
+		*/
+		void SetLockOnLane(bool mode) { lockOnLane_ = mode; }
+
+	protected:
+		void Track2Lane();
+		int Track2XYZ(bool alignH = true);
+		void Lane2Track();
+		void RoadMark2Track();
+		/**
+		Set position to the border of lane (right border for right lanes, left border for left lanes)
+		*/
+		void LaneBoundary2Track();
+		void XYZ2Track(bool alignZAndPitch = false);
+		int SetLongitudinalTrackPos(int track_id, double s);
+		bool EvaluateRoadZPitchRoll(bool alignZPitchRoll);
+
+		// Control lane belonging
+		bool lockOnLane_;  // if true then keep logical lane regardless of lateral position, default false
+
+		// route reference
+		Route  *route_;			// if pointer set, the position corresponds to a point along (s) the route
+
+		// route reference
+		Trajectory* trajectory_; // if pointer set, the position corresponds to a point along (s) the trajectory
+
+		// track reference
+		int     track_id_;
+		double  s_;					// longitudinal point/distance along the track
+		double  t_;					// lateral position relative reference line (geometry)
+		int     lane_id_;			// lane reference
+		double  offset_;			// lateral position relative lane given by lane_id
+		double  h_road_;			// heading of the road
+		double  h_offset_;			// local heading offset given by lane width and offset 
+		double  h_relative_;		// heading relative to the road (h_ = h_road_ + h_relative_)
+		double  s_route_;			// longitudinal point/distance along the route
+		double  s_trajectory_;		// longitudinal point/distance along the trajectory
+		double  curvature_;
+		double  p_relative_;		// pitch relative to the road (h_ = h_road_ + h_relative_)
+		double  r_relative_;		// roll relative to the road (h_ = h_road_ + h_relative_)
+		Position* rel_pos_;
+		PositionType type_;
+		OrientationType orientation_type_;  // Applicable for relative positions
+		int snapToLaneTypes_;  // Bitmask of lane types that the position will snap to
+		int status_;           // Bitmask of various states, e.g. off_road, end_of_road
+
+		// inertial reference
+		double	x_;
+		double	y_;
+		double	z_;
+		double	h_;
+		double	p_;
+		double	r_;
+		double  h_rate_;
+		double  p_rate_;
+		double  r_rate_;
+		double  h_acc_;
+		double  p_acc_;
+		double  r_acc_;
+		double	velX_;
+		double	velY_;
+		double	velZ_;
+		double	accX_;
+		double	accY_;
+		double	accZ_;
+		double	z_road_;
+		double	p_road_;
+		double	r_road_;
+
+		// keep track for fast incremental updates of the position
+		int		track_idx_;				// road index 
+		int		lane_idx_;				// lane index 
+		int 	roadmark_idx_;  		// laneroadmark index
+		int 	roadmarktype_idx_;  		// laneroadmark index
+		int 	roadmarkline_idx_;  	// laneroadmarkline index
+		int		lane_section_idx_;		// lane section
+		int		geometry_idx_;			// index of the segment within the track given by track_idx
+		int		elevation_idx_;			// index of the current elevation entry 
+		int		super_elevation_idx_;   // index of the current super elevation entry 
+	};
+
+
+	// A route is a sequence of positions, at least one per road along the route
+	class Route
+	{
+	public:
+		explicit Route() {}
+
+		/**
+		Adds a waypoint to the route. One waypoint per road. At most one junction between waypoints.
+		@param position A regular position created with road, lane or world coordinates
+		@return Non zero return value indicates error of some kind
+		*/
+		int AddWaypoint(Position *position);
+		int GetWayPointDirection(int index);
+
+		void setName(std::string name);
+		std::string getName();
+		double GetLength();
+
+		std::vector<Position*> waypoint_;
+		std::string name_;
+	};
+
+	// A Road Path is a linked list of road links (road connections or junctions)
+	// between a starting position and a target position
+	// The path can be calculated automatically 
+	class RoadPath
+	{
+	public:
+
+		typedef struct PathNode
+		{
+			RoadLink *link;
+			double dist;
+			Road* fromRoad;
+			PathNode* previous;
+		} PathNode;
+
+		std::vector<PathNode*> visited_;
+		std::vector<PathNode*> unvisited_;
+		Position *startPos_;
+		Position *targetPos_;
+		int direction_;  // direction of path from starting pos. 0==not set, 1==forward, 2==backward
+
+		RoadPath(Position* startPos, Position* targetPos) : startPos_(startPos), targetPos_(targetPos) {};
+		~RoadPath();
+
+		/**
+		Calculate shortest path between starting position and target position, 
+		using Dijkstra's algorithm https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
+		it also calculates the length of the path, or distance between the positions
+		positive distance means that the shortest path was found in forward direction
+		negative distance means that the shortest path goes in opposite direction from the heading of the starting position
+		@param dist A reference parameter into which the calculated path distance is stored
+		@return 0 on success, -1 on failure e.g. path not found
+		*/
+		int Calculate(double &dist);
+	
+	private:
+		bool CheckRoad(Road* checkRoad, RoadPath::PathNode* srcNode, Road* fromRoad);
+	};
+
+
+	// Trajectory stuff
+	class Shape
+	{
+	public:
+		typedef enum
+		{
+			POLYLINE,
+			CLOTHOID,
+			NURBS,
+			SHAPE_TYPE_UNDEFINED
+		} ShapeType;
+
+		Shape(ShapeType type) : type_(type), length_(0) {}
+
+		ShapeType type_;
+		double length_;
+	};
+
+	class PolyLine : public Shape
+	{
+	public:
+
+		class Vertex
+		{
+		public:
+			Position pos_;
+			double time_;
+			bool calc_heading_;
+		};
+
+		PolyLine() : Shape(ShapeType::POLYLINE) {}
+		void AddVertex(Position pos, double time, bool calculateHeading);
+
+		std::vector<Vertex*> vertex_;
+	};
+
+	class Clothoid : public Shape
+	{
+	public:
+
+		Clothoid(roadmanager::Position pos, double curv, double curvDot, double len, double tStart, double tEnd) : Shape(ShapeType::CLOTHOID)
+		{
+			pos_ = pos;
+			spiral_ = new roadmanager::Spiral(0, pos_.GetX(), pos_.GetY(), pos_.GetH(), len, curv, curv + curvDot * len);
+			length_ = len;
+			t_start_ = tStart;
+			t_end_ = tEnd;
+		}
+
+		Position pos_;
+		roadmanager::Spiral* spiral_;  // make use of the OpenDRIVE clothoid definition
+		double t_start_;
+		double t_end_;
+	};
+
+	class Nurbs : public Shape
+	{
+	public:
+		Nurbs() : Shape(ShapeType::NURBS) {}
+	};
+
+	class Trajectory
+	{
+	public:
+		Shape* shape_;
+
+		Trajectory(Shape* shape, std::string name, bool closed) : shape_(shape), name_(name), closed_(closed) {}
+		Trajectory() : shape_(0), closed_(false) {}
+		void Freeze();
+
+		std::string name_;
+		bool closed_;
+	};
+
+
+} // namespace
+
+#endif // OPENDRIVE_HH_

src/ui/ui_osgviewer/main.cpp

@@ -30,6 +30,13 @@
 #include "RoadManager.hpp"
 #include "CommonMini.hpp"
 #include "alog.h"
+#include "xmlparam.h"
+
+#include <QMutex>
+
+#include <QCoreApplication>
+
+#include <objectarray.pb.h>
 
 
 #define DEFAULT_SPEED   70  // km/h
@@ -70,6 +77,18 @@ typedef struct
     viewer::EntityModel * model;
 } RadarObj;
 
+typedef struct
+{
+    double m_x;
+    double m_y;
+    double m_z;
+    double mlength;
+    double mwidth;
+    double mheight;
+    double mfYaw;
+    viewer::EntityModel * model;
+} LidarObj;
+
 std::vector<Car*> cars;
 
 std::vector<RadarObj *> gradarobj;
@@ -79,6 +98,12 @@ Car * gADCIV_car;
 
 viewer::EntityModel *  gtestRadar;
 
+const int LIDARMAX = 300;
+iv::lidar::objectarray glidar;
+bool gbLidarupdate;
+QMutex gMutexLidar;
+std::vector<LidarObj *> glidarobj;
+
 // Car models used for populating the road network
 // path should be relative the OpenDRIVE file
 static const char* carModelsFiles_[] =
@@ -101,15 +126,15 @@ std::vector<osg::ref_ptr<osg::LOD>> carModels_;
 #include "radarobjectarray.pb.h"
 #include "gnss_coordinate_convert.h"
 
-double glat0 = 39.1201777;
-double glon0 = 117.02802270;
+double glat0 = 39.0677643;//39.1201777;
+double glon0 = 117.3548368;//117.02802270;
 
 double gfrel_x = 0;
 double gfrel_y = 0;
 double gfrel_z = 0;
 double gvehicle_hdg = 0;
 
-double gvehicleheight = 7.6;//1.6
+double gvehicleheight = 1.6;//7.6;//1.6
 double gfspeed = 0;
 
 void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -189,6 +214,29 @@ void ListenRADAR(const char * strdata,const unsigned int nSize,const unsigned in
     }
 }
 
+void Listenlidarcnndectect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    std::vector<iv::lidar::object>  lidarobjvec;
+//    strtolidarobj(lidarobjvec,strdata,nSize);
+
+    iv::lidar::objectarray lidarobjvec;
+    std::string in;
+    in.append(strdata,nSize);
+    lidarobjvec.ParseFromString(in);
+    gMutexLidar.lock();
+    glidar.CopyFrom(lidarobjvec);
+    gbLidarupdate = true;
+    gMutexLidar.unlock();
+//    qDebug("obj size is %d ",lidarobjvec.obj_size());
+//    if(lidarobjvec.obj_size() > 1)
+//    {
+//        qDebug("type is %s",lidarobjvec.obj(0).type_name().data());
+//    }
+//    gw->UpdateLidarObj(lidarobjvec);
+}
+
+
 
 void log_callback(const char *str)
 {
@@ -293,6 +341,21 @@ int SetupRadar(viewer::Viewer *viewer)
     return 0;
 }
 
+int SetupLidar(viewer::Viewer *viewer)
+{
+    int i;
+    for(i=0;i<LIDARMAX;i++)
+    {
+        LidarObj * pLidar = new LidarObj;
+        pLidar->m_x = 10000;
+        pLidar->m_y = 10000;
+
+        pLidar->model = viewer->AddLidarModel(osg::Vec3(0.5, 0.5, 0.5),"",0.01,0.01,0.01);
+        glidarobj.push_back(pLidar);
+    }
+    return 0;
+}
+
 int SetupCars(roadmanager::OpenDrive *odrManager, viewer::Viewer *viewer)
 {
 	if (density < 1E-10)
@@ -396,7 +459,7 @@ int SetupCarsSpecial(roadmanager::OpenDrive* odrManager, viewer::Viewer* viewer)
 }
 
 
-void updateADCIVCar(Car * car)
+void updateADCIVCar(Car * car,viewer::Viewer *viewer)
 {
 
     static float x=0;
@@ -406,6 +469,7 @@ void updateADCIVCar(Car * car)
     static float pitch =0;
     static float roll = 0;
 
+
     if (car->model->txNode_ != 0)
     {
 //        car->model->txNode_->setPosition(osg::Vec3(car->pos->GetX(), car->pos->GetY(), car->pos->GetZ()));
@@ -418,7 +482,7 @@ void updateADCIVCar(Car * car)
         y = y+ 0.1;
         x = gfrel_x;
         y = gfrel_y;
-        z = gfrel_z - gvehicleheight;
+        z = gfrel_z + gvehicleheight;
         head = gvehicle_hdg;
         car->model->txNode_->setPosition(osg::Vec3(x, y, z));
 
@@ -429,6 +493,7 @@ void updateADCIVCar(Car * car)
 
         car->model->txNode_->setAttitude(car->model->quat_);
 
+
         int k;
  //       head = head +M_PI/2.0;
         for(k=0;k<gradarobj.size();k++)
@@ -447,6 +512,69 @@ void updateADCIVCar(Car * car)
             gradarobj.at(k)->model->txNode_->setAttitude(car->model->quat_);
         }
 
+        bool bLidarUpdate = gbLidarupdate;
+        if(bLidarUpdate)
+        {
+            gMutexLidar.lock();
+
+            for(k=0;k<glidar.obj_size();k++)
+            {
+                iv::lidar::lidarobject * pobj = glidar.mutable_obj(k);
+                if(k<LIDARMAX)
+                {
+                    glidarobj.at(k)->m_x = pobj->position().x();
+                    glidarobj.at(k)->m_y = pobj->position().y();
+                    glidarobj.at(k)->m_z = pobj->position().z();
+                    glidarobj.at(k)->mlength = pobj->dimensions().x();
+                    glidarobj.at(k)->mwidth = pobj->dimensions().y();
+                    glidarobj.at(k)->mheight = pobj->dimensions().z();
+                    glidarobj.at(k)->mfYaw = pobj->tyaw();
+                }
+            }
+            for(k=glidar.obj_size();k<LIDARMAX;k++)
+            {
+                glidarobj.at(k)->m_x = 10000;
+                glidarobj.at(k)->m_y = 10000;
+                glidarobj.at(k)->m_z = 10000;
+                glidarobj.at(k)->mlength = 0.01;
+                glidarobj.at(k)->mwidth = 0.01;
+                glidarobj.at(k)->mheight = 0.01;
+            }
+            gbLidarupdate = false;
+            gMutexLidar.unlock();
+
+            for(k=0;k<LIDARMAX;k++)
+            {
+                double rel_x,rel_y,rel_z;
+//                rel_x = glidarobj.at(k)->m_x;
+//                rel_y = glidarobj.at(k)->m_y;
+                rel_z = glidarobj.at(k)->m_z;
+
+                rel_x = glidarobj.at(k)->m_y*cos(head) + glidarobj.at(k)->m_x * sin(head);
+                rel_y = glidarobj.at(k)->m_y*sin(head) - glidarobj.at(k)->m_x*cos(head);
+
+                glidarobj.at(k)->m_x = x + rel_x;
+                glidarobj.at(k)->m_y = y + rel_y;
+                glidarobj.at(k)->m_z = z + rel_z;
+
+
+
+
+//                if(glidarobj.at(k)->mlength>0.1)qDebug("len is %f width is %f ,height is %f",glidarobj.at(k)->mlength,glidarobj.at(k)->mwidth,glidarobj.at(k)->mheight);
+                glidarobj.at(k)->model->ChangeScale(glidarobj.at(k)->mlength,glidarobj.at(k)->mwidth,glidarobj.at(k)->mheight);
+
+                glidarobj.at(k)->model->txNode_->setPosition(osg::Vec3(glidarobj.at(k)->m_x, glidarobj.at(k)->m_y, glidarobj.at(k)->m_z));
+
+                glidarobj.at(k)->model->quat_.makeRotate(
+                            roll, osg::Vec3(1, 0, 0),
+                            pitch, osg::Vec3(0, 1, 0),
+                            head + glidarobj.at(k)->mfYaw, osg::Vec3(0, 0, 1));
+
+                glidarobj.at(k)->model->txNode_->setAttitude(car->model->quat_);
+            }
+
+        }
+
 
     }
 
@@ -484,20 +612,43 @@ void updateCar(roadmanager::OpenDrive *odrManager, Car *car, double dt)
 	}
 }
 
-int main(int argc, char** argv)
+int main(int argcx, char** argvx)
 {
 	static char str_buf[128];
 	SE_Options opt;
 
+    int argc;
+    char** argv;
     argc = 3;
     argv = new char*[3];
-    argv[0] = "testodrviewer";
+    argv[0] = "ui_osgviewer";
     argv[1] = "--odr";
-    argv[2] = "/home/nvidia/map/models/y.xodr";
+    argv[2] = "/home/nvidia/map/map.xodr";
+
+    std::string mappath = getenv("HOME");
+    mappath = mappath + "/map/map.xodr";
+    argv[2] =  new char[256];
+    strncpy(argv[2],mappath.c_str(),256);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+//    QString apppath = strpath;
+    if(argcx < 2)
+        strpath = strpath + "/ui_osgviewer.xml";
+    else
+        strpath = argvx[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+
+    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
+    std::string strradarmsg = xp.GetParam("radarmsg","radar");
+    std::string strcnnmsg = xp.GetParam("cnnmsg","lidar_track");
+    std::string strheightajust = xp.GetParam("heightajust","-2.8");
+    gvehicleheight = atof(strheightajust.data());
 
 
-    void * pa = iv::modulecomm::RegisterRecv("ins550d_gpsimu",Listengpsimu);
-    pa = iv::modulecomm::RegisterRecv("radar",ListenRADAR);
+    void * pa = iv::modulecomm::RegisterRecv(strgpsmsg.data(),Listengpsimu);
+    pa = iv::modulecomm::RegisterRecv(strradarmsg.data(),ListenRADAR);
+    pa = iv::modulecomm::RegisterRecv(strcnnmsg.data(),Listenlidarcnndectect);
     (void)pa;
 
 	// Use logger callback
@@ -632,6 +783,7 @@ int main(int argc, char** argv)
 
          SetupADCIVCar(viewer);
          SetupRadar(viewer);
+         SetupLidar(viewer);
 //         gtestRadar = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
 //         gtestRadar = viewer->AddEntityModel(carModelsFiles_[0], osg::Vec3(0.5, 0.5, 0.5),
 //                                             viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0);
@@ -666,7 +818,8 @@ int main(int argc, char** argv)
 
 			if (!(run_only_once && !first_time))
 			{
-                updateADCIVCar(gADCIV_car);
+
+                updateADCIVCar(gADCIV_car,viewer);
                 for (size_t i = 0; i < cars.size(); i++)
                 {
                     updateCar(odrManager, cars[i], deltaSimTime);

src/ui/ui_osgviewer/ui_osgviewer.pro

@@ -16,7 +16,7 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
         ../../../thirdpartylib/esminilib/CommonMini.cpp \
-        ../../../thirdpartylib/esminilib/RoadManager.cpp \
+        RoadManager.cpp \
         ../../../thirdpartylib/esminilib/RubberbandManipulator.cpp \
         ../../include/msgtype/gps.pb.cc \
         ../../include/msgtype/gpsimu.pb.cc \
@@ -29,7 +29,9 @@ SOURCES += \
         ../../../thirdpartylib/esminilib/pugixml.cpp \
         ../../../thirdpartylib/esminilib/roadgeom.cpp \
         ../../../thirdpartylib/esminilib/vehicle.cpp \
-        viewer.cpp
+        viewer.cpp \
+    ../../include/msgtype/objectarray.pb.cc \
+    ../../include/msgtype/object.pb.cc
 
 
 # Default rules for deployment.
@@ -75,4 +77,6 @@ HEADERS += \
     ../../include/msgtype/imu.pb.h \
     ../../include/msgtype/radarobject.pb.h \
     ../../include/msgtype/radarobjectarray.pb.h \
-    gnss_coordinate_convert.h
+    gnss_coordinate_convert.h \
+    ../../include/msgtype/objectarray.pb.h \
+    ../../include/msgtype/object.pb.h

src/ui/ui_osgviewer/viewer.cpp

@@ -513,6 +513,21 @@ osg::ref_ptr<osg::PositionAttitudeTransform> CarModel::AddWheel(osg::ref_ptr<osg
 	return tx_node;
 }
 
+EntityModel::~EntityModel()
+{
+   delete trail_;
+    mparent->removeChild(txNode_);
+    txNode_->removeChild(lod_);
+
+
+//    delete lod_;
+}
+
+void EntityModel::ChangeScale(int length, int width, int height)
+{
+    mtx->setScale(osg::Vec3(length,width,height));
+}
+
 EntityModel::EntityModel(osgViewer::Viewer *viewer, osg::ref_ptr<osg::Group> group, osg::ref_ptr<osg::Group> parent, 
 	osg::ref_ptr<osg::Group> trail_parent, osg::ref_ptr<osg::Node> dot_node, osg::Vec3 trail_color,std::string name)
 {
@@ -520,6 +535,7 @@ EntityModel::EntityModel(osgViewer::Viewer *viewer, osg::ref_ptr<osg::Group> gro
 	{
 		return;
 	}
+    mparent = parent;
 	name_ = name;
 	group_ = group;
 
@@ -542,6 +558,10 @@ EntityModel::EntityModel(osgViewer::Viewer *viewer, osg::ref_ptr<osg::Group> gro
 		if (group->getChild(i)->getName() == "BoundingBox")
 		{
 			bb_ = (osg::Group*)group->getChild(i);
+            if(bb_ != 0)
+            {
+                mtx = (osg::PositionAttitudeTransform *)bb_->getChild(0);
+            }
 			break;
 		}
 	}
@@ -1024,6 +1044,15 @@ Viewer::Viewer(roadmanager::OpenDrive* odrManager, const char* modelFilename, co
 
 }
 
+void Viewer::clearLidar()
+{
+    for (size_t i=0; i< entities_Lidar.size(); i++)
+    {
+        delete(entities_Lidar[i]);
+    }
+    entities_Lidar.clear();
+}
+
 Viewer::~Viewer()
 {
 	osgViewer_->setDone(true);
@@ -1047,6 +1076,78 @@ void Viewer::SetCameraMode(int mode)
 	rubberbandManipulator_->setMode(camMode_);
 }
 
+EntityModel* Viewer::AddLidarModel(osg::Vec3 trail_color,std::string name,double length,double width, double height)
+{
+    osg::ref_ptr<osg::Group> group = 0;
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(new osg::ShapeDrawable(new osg::Box()));
+
+        osg::ref_ptr<osg::Group> bbGroup = new osg::Group;
+        osg::ref_ptr<osg::PositionAttitudeTransform> tx = new osg::PositionAttitudeTransform;
+
+        osg::Material* material = new osg::Material();
+
+        // Set color of vehicle based on its index
+        double* color;
+        double b = 1.5;  // brighness
+        color = color_green;
+
+        material->setDiffuse(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
+        material->setAmbient(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
+
+        if (group == 0)
+        {
+            // If no model loaded, make a shaded copy of bounding box as model
+            osg::ref_ptr<osg::Geode> geodeCopy = dynamic_cast<osg::Geode*>(geode->clone(osg::CopyOp::DEEP_COPY_ALL));
+            group = new osg::Group;
+            tx->addChild(geodeCopy);
+
+ //           geodeCopy->setNodeMask(NodeMask::NODE_MASK_OSI_LINES);
+            geodeCopy->setNodeMask(NodeMask::NODE_MASK_ENTITY_MODEL);
+        }
+
+        // Set dimensions of the entity "box"
+
+            // Scale to something car-ish
+            tx->setScale(osg::Vec3(length, width, height));
+            tx->setPosition(osg::Vec3(1.5, 0, 0.75));
+
+        tx->addChild(geode);
+        tx->getOrCreateStateSet()->setAttribute(material);
+        bbGroup->setName("BoundingBox");
+        bbGroup->addChild(tx);
+
+        group->addChild(bbGroup);
+        group->setName(name);
+
+    EntityModel* model;
+//	if (type == EntityModel::EntityType::ENTITY_TYPE_VEHICLE)
+//	{
+        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+//        model = new CarModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+
+//	}
+//	else
+//	{
+//        model = new EntityModel(osgViewer_, group, rootnode_, trails_, dot_node_, trail_color, name);
+//	}
+
+    model->state_set_ = model->lod_->getOrCreateStateSet(); // Creating material
+    model->blend_color_ = new osg::BlendColor(osg::Vec4(1, 1, 1, 1));
+    model->state_set_->setAttributeAndModes(model->blend_color_);
+    model->blend_color_->setDataVariance(osg::Object::DYNAMIC);
+
+    osg::BlendFunc* bf = new osg::BlendFunc(osg::BlendFunc::CONSTANT_ALPHA, osg::BlendFunc::ONE_MINUS_CONSTANT_ALPHA);
+    model->state_set_->setAttributeAndModes(bf);
+    model->state_set_->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
+    model->state_set_->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
+
+    entities_Lidar.push_back(model);
+
+    return entities_Lidar.back();
+}
+
 EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
 {
 
@@ -1113,7 +1214,7 @@ EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
     model->state_set_->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
     model->state_set_->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
 
-    entities_.push_back(model);
+    entities_Radar.push_back(model);
 
 //	// Focus on first added car
 //	if (entities_.size() == 1)
@@ -1139,7 +1240,7 @@ EntityModel* Viewer::AddRadarModel(osg::Vec3 trail_color,std::string name)
 //		}
 //	}
 
-    return entities_.back();
+    return entities_Radar.back();
 }
 
 EntityModel* Viewer::AddEntityModel(std::string modelFilepath, osg::Vec3 trail_color, EntityModel::EntityType type, 
@@ -1158,6 +1259,11 @@ EntityModel* Viewer::AddEntityModel(std::string modelFilepath, osg::Vec3 trail_c
 		file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[i], path));
 		file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[i], std::string("../models/" + path)));
 	}
+    for (size_t i = 0; i < SE_Env::Inst().GetPaths().size(); i++)
+    {
+        file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[i], path));
+        file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[i], std::string("./models/" + path)));
+    }
 	for (size_t i = 0; i < file_name_candidates.size(); i++)
 	{
 		if (FileExists(file_name_candidates[i].c_str()))

src/ui/ui_osgviewer/viewer.hpp

@@ -180,10 +180,12 @@ namespace viewer
 			ENTITY_TYPE_OTHER
 		} EntityType;
 
+
 		osg::ref_ptr<osg::Group> group_;
 		osg::ref_ptr<osg::LOD> lod_;
 		osg::ref_ptr<osg::PositionAttitudeTransform> txNode_;
 		osg::ref_ptr<osg::Group> bb_;
+                osg::ref_ptr<osg::Group> mparent;
 		osg::Quat quat_;
 		double size_x;
 		double size_y;
@@ -204,9 +206,16 @@ namespace viewer
 
 		void SetTransparency(double factor);
 
+                ~EntityModel();
+
+                void ChangeScale(int length,int width,int height);
+
 
 		Trail* trail_;
 		osgViewer::Viewer* viewer_;
+
+        private:
+                osg::ref_ptr<osg::PositionAttitudeTransform> mtx = 0;
 	};
 
 	class CarModel : public EntityModel
@@ -247,6 +256,7 @@ namespace viewer
 	private:
 		scenarioengine::Object* object_;
 		EntityModel* entity_;
+
 		osg::LOD* node_;
 	};
 
@@ -295,6 +305,8 @@ namespace viewer
 		osg::ref_ptr<osgGA::RubberbandManipulator> rubberbandManipulator_;
 		osg::ref_ptr<osgGA::NodeTrackerManipulator> nodeTrackerManipulator_;
 		std::vector<EntityModel*> entities_;
+                std::vector<EntityModel*> entities_Radar;
+                std::vector<EntityModel*> entities_Lidar;
 		float lodScale_;
 		osgViewer::Viewer *osgViewer_;
 		osg::MatrixTransform* rootnode_;
@@ -318,6 +330,8 @@ namespace viewer
 		EntityModel* AddEntityModel(std::string modelFilepath, osg::Vec3 trail_color, EntityModel::EntityType type, 
 			bool road_sensor, std::string name, OSCBoundingBox *boundingBox);
                 EntityModel* AddRadarModel(osg::Vec3 trail_color,std::string name);
+                void clearLidar();
+                EntityModel* AddLidarModel(osg::Vec3 trail_color,std::string name,double length,double width, double height);
 		void RemoveCar(std::string name);
 		int LoadShadowfile(std::string vehicleModelFilename);
 		int AddEnvironment(const char* filename);