diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h index 0e083dedf01bb0a5257b68f3a0752f0f8c07775c..e006cb5cf12c8dd44b5a7df820b75a6011b3ecfc 100644 --- a/include/opendrive_road_node.h +++ b/include/opendrive_road_node.h @@ -44,11 +44,14 @@ class COpendriveRoadNode TOpendriveWorldPoint get_start_pose(void) const; unsigned int get_num_links(void) const; const COpendriveLink &get_link(unsigned int index) const; + const COpendriveLink &get_link_ending_at(unsigned int node_index) const; unsigned int get_num_lanes(void) const; const COpendriveLane &get_lane(unsigned int index) const; unsigned int get_num_geometries(void) const; const COpendriveGeometry &get_geometry(unsigned int index) const; - double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1); + unsigned int get_closest_link(double x, double y,double heading,double angle_tol=0.1) const; + double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const; + void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const; friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node); ~COpendriveRoadNode(); }; diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp index 3c0dfeaf6befd43b642b2f4dabc39ab7620469bf..31c212895ce180ce9fd812740914fed8a7717398 100644 --- a/src/opendrive_road_node.cpp +++ b/src/opendrive_road_node.cpp @@ -249,6 +249,17 @@ const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const throw CException(_HERE_,"Invalid link index"); } +const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_index) const +{ + for(unsigned int i=0;i<this->links.size();i++) + { + if(this->links[i]->get_next().get_index()==node_index) + return *this->links[i]; + } + + throw CException(_HERE_,"No link in this node ends at the desired node"); +} + unsigned int COpendriveRoadNode::get_num_lanes(void) const { return this->lanes.size(); @@ -275,7 +286,39 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c throw CException(_HERE_,"Invalid geometry index"); } -double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) +unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double heading,double angle_tol)const +{ + double dist,min_dist=std::numeric_limits<double>::max(); + TOpendriveWorldPoint target; + unsigned int closest_index; + double angle; + TPoint point; + + target.x=x; + target.y=y; + target.heading=heading; + for(unsigned int i=0;i<this->links.size();i++) + { + if(&this->links[i]->get_prev()==this)// links starting at this node + { + this->links[i]->find_closest_world_point(target,point); + angle=diff_angle(normalize_angle(heading),point.heading); + if(fabs(angle)<angle_tol) + { + dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0)); + if(dist<min_dist) + { + min_dist=dist; + closest_index=i; + } + } + } + } + + return closest_index; +} + +double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) const { double dist,min_dist=std::numeric_limits<double>::max(); double angle; @@ -291,7 +334,6 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T if(&this->links[i]->get_prev()==this)// links starting at this node { length=this->links[i]->find_closest_world_point(target,point); - std::cout << " " << point.x << "," << point.y << "," << point.heading << "," << length << std::endl; angle=diff_angle(normalize_angle(heading),point.heading); if(fabs(angle)<angle_tol) { @@ -311,6 +353,41 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T return closest_length; } +void COpendriveRoadNode::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const +{ + double dist; + double angle; + double length; + TOpendriveWorldPoint target,new_point; + TPoint point; + + target.x=x; + target.y=y; + target.heading=heading; + closest_points.clear(); + closest_lengths.clear(); + for(unsigned int i=0;i<this->links.size();i++) + { + if(&this->links[i]->get_prev()==this)// links starting at this node + { + length=this->links[i]->find_closest_world_point(target,point); + angle=diff_angle(normalize_angle(heading),point.heading); + if(fabs(angle)<angle_tol) + { + dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0)); + if(dist<=dist_tol) + { + new_point.x=point.x; + new_point.y=point.y; + new_point.heading=point.heading; + closest_points.push_back(new_point); + closest_lengths.push_back(length); + } + } + } + } +} + std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node) { out << " Node: " << node.get_index() << std::endl;