Commit 19d30ab3 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added several functions:

  * A function to get the link ending at a given node.
  * A function to get the closest link to a point.
  * A function to get the set to closest points to a given point.
parent 885d9dfa
......@@ -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();
};
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment