diff --git a/include/opendrive_road.h b/include/opendrive_road.h index 7d3ec7eeabdeda4cfa60c3638fda321e4fbd70f1..a4d7ae398526fc934dc92cbffc65568138365215 100644 --- a/include/opendrive_road.h +++ b/include/opendrive_road.h @@ -42,6 +42,7 @@ class COpendriveRoad unsigned int get_num_nodes(void) const; const COpendriveRoadNode& get_node(unsigned int index) const; const COpendriveRoadSegment& operator[](std::size_t index); + double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point); void operator=(const COpendriveRoad& object); friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road); ~COpendriveRoad(); diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp index c65d1864276acb952b87e0386e65314be40d4739..0025da6db2dcc49ba5e3262fa98798cd72e57d29 100644 --- a/src/opendrive_road.cpp +++ b/src/opendrive_road.cpp @@ -1,7 +1,8 @@ #include "opendrive_road.h" +#include "exceptions.h" #include <sys/types.h> #include <sys/stat.h> -#include "exceptions.h" +#include <math.h> COpendriveRoad::COpendriveRoad() { @@ -61,6 +62,7 @@ COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key) void COpendriveRoad::link_segments(OpenDRIVE &open_drive) { std::string predecessor_id,successor_id; + std::string predecessor_contact,successor_contact; for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it) { @@ -72,12 +74,18 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive) if(road_it->lane_link().get().predecessor().present())// predecessor present { if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road + { predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get(); + predecessor_contact=road_it->lane_link().get().predecessor().get().contactPoint().get(); + } } if(road_it->lane_link().get().successor().present())// successor present { if(road_it->lane_link().get().successor().get().elementType().get()=="road") + { successor_id=road_it->lane_link().get().successor().get().elementId().get(); + successor_contact=road_it->lane_link().get().successor().get().contactPoint().get(); + } } } if(std::stoi(road_it->junction().get())==-1)// non junction road segments @@ -293,6 +301,33 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index) throw CException(_HERE_,"Invalid segment index"); } +double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point) +{ + double dist,min_dist=std::numeric_limits<double>::max(); + double angle,min_angle=std::numeric_limits<double>::max(); + TOpendriveWorldPoint point; + double length,closest_length; + + for(unsigned int i=0;i<this->nodes.size();i++) + { + length=this->nodes[i]->get_closest_point(x,y,heading,point); + angle=diff_angle(normalize_angle(heading),point.heading); + if(angle<min_angle) + { + dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0)); + if(dist<min_dist) + { + min_angle=angle; + min_dist=dist; + closest_point=point; + closest_length=length; + } + } + } + + return closest_length; +} + void COpendriveRoad::operator=(const COpendriveRoad& object) { COpendriveRoadSegment *segment;